清洁机械臂柔性跟随控制技术

柔性跟随控制需要解决的核心矛盾：位置控制 vs 力控制的天然冲突

位置控制（传统机械臂默认模式）：
├─ 机械臂严格按照预设轨迹运动
├─ 末端位置精确到达目标点
├─ 问题：如果目标表面位置有偏差（±5–10mm）
│   └─ 末端会"硬顶"进表面 → 接触力急剧增大
│   └─ 可能损坏表面或机械臂
└─ 结论：纯位置控制不适合接触作业

力控制（理想模式）：
├─ 机械臂根据接触力实时调整位置
├─ 保持恒定接触力（5–10N）
├─ 末端自动跟随表面形状变化
└─ 问题：需要复杂的力控算法（阻抗/导纳控制）

本方案的解决思路：
├─ 机械被动柔顺：弹簧机构吸收位置误差（解决80%问题）
├─ 简化力控：基于力传感器的位置微调（解决剩余20%）
└─ 目标：用最简单的算法实现足够好的柔性跟随

各清洁面的几何特征分析：
─────────────────────────────────────────────────────
内壁（最复杂）：
├─ 形状：旋转曲面（近似椭圆截面）
├─ 曲率半径：约 R=150–200mm（水平截面）
├─ 高度方向：从上到下逐渐收窄（锥形）
├─ 表面粗糙度：陶瓷釉面，Ra < 0.8μm（光滑）
├─ 位置不确定性：安装位置偏差 ±10–20mm
└─ 跟随难度：最难，三维曲面

外壁（中等）：
├─ 形状：近似圆柱面（局部平面）
├─ 曲率半径：约 R=200–250mm（水平截面）
├─ 高度方向：基本垂直（轻微弧度）
├─ 位置不确定性：±10–20mm
└─ 跟随难度：中等，近似平面

座垫（较简单）：
├─ 形状：水平平面（座圈上表面）
├─ 高度：约 H=400mm（固定）
├─ 位置不确定性：±5–10mm（高度方向）
└─ 跟随难度：较简单，平面跟随

底座（较简单）：
├─ 形状：近似水平面（地面附近）
├─ 高度：约 H=0–50mm
├─ 位置不确定性：±5mm
└─ 跟随难度：较简单

柔性跟随控制四层架构：
─────────────────────────────────────────────────────

Layer 4：任务层（轨迹规划）
├─ 功能：规划擦拭路径（覆盖全部清洁面）
├─ 输入：几何模型（预设或视觉测量）
├─ 输出：末端目标位置序列
└─ 算法难度：★★☆☆☆

Layer 3：控制层（位置补偿）
├─ 功能：根据力传感器信号微调末端位置
├─ 输入：力传感器读数、当前关节角度
├─ 输出：关节位置修正量
└─ 算法难度：★★★☆☆

Layer 2：传感层（力感知）
├─ 功能：实时感知末端接触力
├─ 输入：力传感器原始信号
├─ 输出：滤波后的接触力值
└─ 算法难度：★★☆☆☆

Layer 1：结构层（机械被动柔顺）
├─ 功能：被动吸收位置误差，自动贴合曲面
├─ 输入：无（纯机械）
├─ 输出：末端自动适应表面形状
└─ 算法难度：★☆☆☆☆（无需算法）

─────────────────────────────────────────────────────
关键原则：
├─ Layer 1（机械）解决 80% 的柔顺问题
├─ Layer 2+3（传感+控制）解决剩余 20%
└─ Layer 4（轨迹）决定清洁覆盖率

被动柔顺机构：
─────────────────────────────────────────────────────
├─ 安装位置：力传感器 → 弹簧机构 → 磁吸快换 → 清洁模组
├─ 弹簧刚度：k = 5N/mm
├─ 预压量：1–2mm（初始接触力 5–10N）
└─ 最大压缩量：5mm（限位保护）

但这只是法向（Z轴）的柔顺，还需要考虑：
─────────────────────────────────────────────────────
需要补充的柔顺自由度：

法向柔顺（已有）：
├─ 方向：垂直于清洁面（压入/弹出）
├─ 弹簧刚度：k = 5N/mm
├─ 作用：吸收末端到表面的距离误差（±5mm）
└─ 效果：接触力自动维持在 5–10N 

切向柔顺（需要补充）：
├─ 方向：平行于清洁面（擦拭方向）
├─ 问题：擦拭时末端沿表面移动，如果表面有凸起/凹陷
│   └─ 末端会被"绊住"或"跌落"
├─ 解决方案：擦拭头底部增加柔性海绵（已有）
│   └─ 海绵厚度20mm，Shore A 30–40
│   └─ 海绵可以自适应表面微小起伏（±3mm）
└─ 效果：切向方向的微小起伏由海绵吸收 

角度柔顺（需要补充）：
├─ 方向：擦拭头相对于表面的倾斜角度
├─ 问题：如果末端姿态与表面法向有偏差（±5°）
│   └─ 擦拭头只有边缘接触，清洁效果差
├─ 解决方案：万向节（球铰）+ 弹簧复位
│   ├─ 在弹簧机构末端增加小型球铰
│   ├─ 球铰允许擦拭头自由倾斜 ±10°
│   ├─ 弹簧复位力矩：约 0.1N·m/°
│   └─ 效果：擦拭头自动对齐表面法向 
└─ 成本：球铰约 50–100元

完整被动柔顺机构（三自由度柔顺）：
─────────────────────────────────────────────────────

结构层次（从J6末端到擦拭布）
J6输出轴
    ▼
[力传感器]（测量接触力）
    ▼
[法向弹簧机构]（Z轴柔顺，k=5N/mm，行程5mm）
    ▼
[球铰万向节]（角度柔顺，±10°，弹簧复位）
    ▼
[磁吸快换接口]（模组更换）
    ▼
[清洁模组外壳]（铝合金）
    ▼
[柔性海绵基底]（切向柔顺，20mm厚，Shore A 30–40）
    ▼
[擦拭布]（直接接触表面）

三层柔顺的分工：
├─ 法向弹簧：吸收 ±5mm 的法向位置误差
├─ 球铰万向节：吸收 ±10° 的姿态角度误差
└─ 柔性海绵：吸收 ±3mm 的表面微小起伏

关键参数：
├─ 法向弹簧刚度：k = 5N/mm（接触力5–10N对应压缩1–2mm）
├─ 球铰复位刚度：k_angle = 0.1N·m/°（±10°范围内）
├─ 海绵压缩量：最大5mm（超过则海绵底板接触）
└─ 整体柔顺行程：法向最大 5+5=10mm（弹簧+海绵）

被动柔顺无法解决的问题：
─────────────────────────────────────────────────────
局限1：大范围位置偏差（>10mm）
├─ 被动柔顺行程有限（最大10mm）
├─ 如果安装位置偏差超过10mm
│   └─ 弹簧压缩到底，变成刚性接触
│   └─ 接触力急剧增大（可能损坏）
└─ 解决：需要主动位置补偿（Layer 3）

局限2：持续的接触力漂移
├─ 擦拭过程中，弹簧压缩量会随表面曲率变化
├─ 接触力在 5–10N 范围内波动（不恒定）
├─ 对于清洁效果影响不大（可接受）
└─ 如果需要更精确的力控：需要主动控制

局限3：无法感知"已清洁"状态
├─ 被动柔顺只管接触，不管清洁效果
├─ 需要传感层（Layer 2）判断清洁状态
└─ 解决：力传感器 + 清洁完成判断逻辑

力传感器信号处理链路：
─────────────────────────────────────────────────────
原始信号 → 滤波 → 重力补偿 → 接触力提取 → 控制器

Step 1：原始信号采集
├─ 传感器类型：单轴力传感器（法向力）
├─ 量程：0–50N
├─ 精度：±0.1N
├─ 采样频率：1000Hz（1ms采样一次）
└─ 输出：原始力值 F_raw(t)

Step 2：低通滤波（去除高频噪声）
├─ 滤波器类型：一阶低通滤波（IIR）
├─ 截止频率：10–20Hz（擦拭动作频率约1–2Hz）
├─ 公式：F_filtered(t) = α × F_raw(t) + (1-α) × F_filtered(t-1)
│   └─ α = 2πfc / (2πfc + fs) ≈ 0.06（fc=10Hz, fs=1000Hz）
└─ 效果：滤除机械振动、电气噪声

Step 3：重力补偿（关键步骤）
─────────────────────────────────────────────────────
问题：力传感器安装在末端，随J5/J6俯仰运动
      末端执行器自重（1.3kg）会随姿态变化产生重力分量
      这个重力分量会叠加在接触力上，导致读数错误

重力补偿原理：
├─ 末端执行器重量：G = 1.3kg × 9.8 = 12.74N
├─ 力传感器测量轴方向：沿末端法向（随J5/J6姿态变化）
├─ 重力在测量轴上的分量：
│   └─ F_gravity = G × cos(θ_pitch)
│   └─ θ_pitch = J5 + J6（当前俯仰角）
│
├─ 补偿公式：
│   └─ F_contact = F_filtered - G × cos(θ_pitch)
│
├─ 输入：J5/J6当前关节角度（从关节编码器读取）
├─ 计算：实时计算重力分量并减去
└─ 效果：得到真实的接触力 F_contact

关键注意事项：
├─ 重力补偿必须实时计算（每个控制周期都要更新）
├─ 如果忘记重力补偿，力控会严重失准
│   └─ 例：J5=-90°时，重力分量=0，读数正确
│   └─ 例：J5=-45°时，重力分量=9N，不补偿则误差9N
└─ 这是力控系统中最容易出错的地方 

Step 4：接触状态判断
├─ F_contact > 1N：已接触（接触阈值）
├─ F_contact < 0.5N：未接触（脱离阈值）
├─ 1N ≤ F_contact ≤ 15N：正常接触范围
├─ F_contact > 15N：过载（需要立即退让）
└─ 迟滞设计：接触阈值1N，脱离阈值0.5N（防止抖动）

力传感器安装的关键注意事项：
─────────────────────────────────────────────────────
注意1：安装方向（最重要）
├─ 力传感器的测量轴必须与清洁模组的法向方向一致
├─ 即：力传感器测量的是"垂直于清洁面"的力
├─ 如果安装偏斜（>5°），测量误差显著增大
└─ 安装精度要求：轴向偏差 < 2°

注意2：预紧力
├─ 安装时不要给力传感器施加预紧力
├─ 预紧力会导致零点偏移
├─ 安装后需要重新标定零点
└─ 标定方法：末端悬空（不接触任何表面），读取零点值

注意3：线缆走线
├─ 力传感器的信号线不能有拉力（影响测量）
├─ 线缆需要留有足够的松弛量
├─ 随J5/J6运动时，线缆不能绷紧
└─ 推荐：使用柔性扁平电缆（FFC）+ 拖链保护

注意4：防水密封
├─ 力传感器本体需要IP65以上防护
├─ 接线端子需要防水连接器
├─ 清洁液不能渗入传感器内部
└─ 推荐：传感器外加硅胶套保护

注意5：量程选择
├─ 量程过小：容易过载损坏（清洁时偶发冲击力）
├─ 量程过大：分辨率不足（0.1N精度要求）
├─ 推荐量程：50N（正常接触力10N，安全系数5）
└─ 过载保护：量程的150%（75N）不损坏

控制策略对比（从简单到复杂）：
─────────────────────────────────────────────────────
策略A：纯位置控制（不推荐）
├─ 原理：按预设轨迹运动，不考虑接触力
├─ 优点：最简单
├─ 缺点：无法适应表面位置偏差，可能损坏
└─ 推荐度：❌

策略B：力阈值开关控制（推荐入门）
├─ 原理：
│   ├─ F_contact < F_min（5N）→ 末端向前移动（靠近表面）
│   ├─ F_contact > F_max（10N）→ 末端向后移动（远离表面）
│   └─ F_min ≤ F_contact ≤ F_max → 保持当前位置
├─ 优点：逻辑简单
├─ 缺点：接触力在F_min和F_max之间震荡（Bang-Bang控制）
└─ 推荐度：✅ 适合算法能力有限的团队

策略C：比例力控制（推荐主方案）
├─ 原理：
│   ├─ 误差：e = F_target - F_contact（目标力 - 实际力）
│   ├─ 位置修正量：Δx = Kp × e
│   │   └─ Kp = 0.1mm/N（每1N误差修正0.1mm位置）
│   └─ 末端位置：x_new = x_current + Δx
├─ 优点：平滑，无震荡
├─ 缺点：稳态误差（需要加积分项消除）
└─ 推荐度：✅✅ 最适合本项目

策略D：PID力控制（推荐进阶）
├─ 原理：
│   ├─ 比例项：Kp × e（快速响应）
│   ├─ 积分项：Ki × ∫e dt（消除稳态误差）
│   └─ 微分项：Kd × de/dt（抑制超调）
├─ 优点：精度高，稳态误差小
├─ 缺点：需要调参（Kp/Ki/Kd三个参数）
└─ 推荐度：✅✅✅ 能力允许时推荐

策略E：阻抗/导纳控制（不推荐）
├─ 原理：将机械臂末端建模为弹簧-质量-阻尼系统
├─ 优点：理论上最优
├─ 缺点：需要精确的动力学模型，算法复杂
└─ 推荐度：❌ 超出普通团队能力

比例力控制（P控制）详细设计：
─────────────────────────────────────────────────────
控制目标：
├─ 目标接触力：F_target = 7.5N（5–10N范围的中值）
├─ 允许误差：±2.5N（即接触力在5–10N范围内均可接受）
└─ 控制频率：100Hz（每10ms执行一次）

控制算法（伪代码）：
─────────────────────────────────────────────────────

// 参数定义
F_target = 7.5      // 目标接触力（N）
Kp = 0.05           // 比例增益（mm/N）
max_correction = 2  // 单次最大修正量（mm，防止突变）
dead_zone = 1.0     // 死区（N，小于此误差不修正）

// 控制循环（100Hz）
while cleaning:
    // Step 1：读取力传感器
    F_raw = read_force_sensor()

    // Step 2：低通滤波
    F_filtered = low_pass_filter(F_raw, alpha=0.06)

    // Step 3：重力补偿
    theta_pitch = J5.angle + J6.angle
    F_gravity = 12.74 * cos(theta_pitch)
    F_contact = F_filtered - F_gravity

    // Step 4：安全检查
    if F_contact > 20:          // 过载保护
        emergency_retreat(5mm)  // 立即退让5mm
        continue

    // Step 5：计算误差
    error = F_target - F_contact

    // Step 6：死区处理（小误差不修正，避免抖动）
    if abs(error) < dead_zone:
        continue

    // Step 7：计算位置修正量
    delta_x = Kp * error
    delta_x = clamp(delta_x, -max_correction, max_correction)

    // Step 8：修正末端位置（沿法向方向）
    x_normal_new = x_normal_current + delta_x
    move_to(x_normal_new)  // 发送给关节控制器

    sleep(10ms)  // 等待下一个控制周期

关键参数调整方法：
├─ Kp太大：末端位置震荡（来回抖动）→ 减小Kp
├─ Kp太小：响应慢，接触力偏差大 → 增大Kp
├─ 推荐调参步骤：
│   ├─ 从Kp=0.01开始
│   ├─ 逐步增大到出现轻微震荡
│   └─ 退回到震荡临界值的50%
└─ 典型值：Kp = 0.03–0.08mm/N

法向方向计算（关键技术点）：
─────────────────────────────────────────────────────
问题：力控修正量是沿"清洁面法向"方向的，
      但机械臂控制的是关节角度（或笛卡尔坐标）。
      需要将法向修正量转换为关节修正量。

方法1：直接法向修正（推荐，最简单）
─────────────────────────────────────────────────────
原理：
├─ 对于每种清洁任务，法向方向是已知的：
│   ├─ 内壁清洁：法向 = 指向轴心方向（水平）
│   ├─ 外壁清洁：法向 = 背向轴心方向（水平）
│   ├─ 座垫清洁：法向 = 垂直向下（Z轴负方向）
│   └─ 底座清洁：法向 = 垂直向下（Z轴负方向）
│
├─ 法向修正量 Δx 直接转换为笛卡尔坐标修正：
│   ├─ 内壁/外壁：Δx_cartesian = Δx × (cos(θ_arm), sin(θ_arm), 0)
│   │   └─ θ_arm = J1 + J2（当前平面角度）
│   └─ 座垫/底座：Δx_cartesian = (0, 0, Δx)
│
└─ 再通过逆运动学将笛卡尔修正量转换为关节修正量

方法2：雅可比矩阵法（进阶，精度更高）
─────────────────────────────────────────────────────
原理：
├─ 雅可比矩阵 J 描述关节速度到末端速度的映射
├─ Δq = J⁻¹ × Δx_cartesian
├─ 优点：精确，适用于任意姿态
├─ 缺点：需要计算雅可比矩阵及其伪逆
└─ 推荐：在算法能力允许时使用

本项目推荐：方法1（直接法向修正）
├─ 清洁任务的法向方向固定且已知
└─ 无需复杂的雅可比计算

控制层关键注意事项：
─────────────────────────────────────────────────────
注意1：控制频率与机械臂响应速度匹配
├─ 力控频率：100Hz（推荐）
├─ 关节控制器响应频率：通常1000Hz（一体化关节内置）
├─ 两者频率差10倍，保证力控指令能被及时执行
└─ 如果力控频率过高（>500Hz），可能超过通信带宽

注意2：积分饱和
├─ 如果使用PID控制，积分项可能累积过大
├─ 当末端离开表面时，积分项继续累积
├─ 重新接触时，积分项导致过大的初始力
└─ 解决：积分项限幅

注意3：接触/脱离的状态切换
├─ 未接触状态：使用位置控制（按轨迹运动）
├─ 接触状态：切换到力控（保持接触力）
├─ 切换时机：F_contact > 1N（接触阈值）
├─ 切换时需要平滑过渡（避免突变）
└─ 实现：加权混合（位置控制权重从1→0，力控权重从0→1）

注意4：末端速度限制
├─ 力控修正时，末端移动速度不能过快
├─ 过快会导致冲击力（超过过载阈值）
├─ 推荐最大修正速度：10mm/s
└─ 实现：对Δx进行速度限幅

注意5：奇异点附近的处理
├─ 当J5/J6接近0°时（俯仰链伸直），雅可比矩阵接近奇异
├─ 此时力控修正可能导致关节角度突变
├─ 解决：在奇异点附近降低Kp（减小控制增益）
└─ 检测方法：计算雅可比矩阵行列式，接近0时降增益

轨迹规划的目标：确保擦拭头覆盖目标清洁面的全部区域

覆盖率要求：
├─ 内壁：≥ 95%（关键清洁面）
├─ 外壁：≥ 90%
├─ 座垫：≥ 98%（直接接触皮肤）
└─ 底座：≥ 85%（视觉不明显区域）

轨迹规划的输入：
├─ 几何模型（预设参数或视觉测量）
├─ 擦拭头尺寸（决定轨迹间距）
├─ 清洁起始点和终止点
└─ 关节运动约束（速度/加速度限制）

内壁清洁轨迹（螺旋线）：
─────────────────────────────────────────────────────
轨迹类型：螺旋向下（从上到下）
├─ 起始高度：H_start = 400mm（座圈下方）
├─ 终止高度：H_end = 100mm（内壁底部）
├─ 螺旋圈数：(400-100) / 擦拭头宽度 = 300/80 ≈ 4圈
├─ 每圈角度：360°
├─ 每圈下降量：80mm（擦拭头宽度，保证无遗漏）
└─ 总轨迹长度：4圈 × 2π × 190mm ≈ 4.8m

轨迹生成（伪代码）：

R_inner = 190          // 内壁半径（mm）
H_start = 400          // 起始高度（mm）
H_end = 100            // 终止高度（mm）
brush_width = 80       // 擦拭头宽度（mm）
n_turns = (H_start - H_end) / brush_width  // 圈数

waypoints = []
for i in range(n_points):
    angle = i * (2π * n_turns / n_points)  // 螺旋角度
    height = H_start - i * (H_start-H_end) / n_points
    x = R_inner * cos(angle)
    y = R_inner * sin(angle)
    z = height
    waypoints.append((x, y, z))

外壁清洁轨迹（竖向往复）：
─────────────────────────────────────────────────────
轨迹类型：竖向往复（从上到下，再从下到上）
├─ 外壁半径：R_outer = 260mm
├─ 高度范围：H = 0–450mm
├─ 每列间距：80mm（擦拭头宽度）
├─ 列数：2π × 260 / 80 ≈ 20列
└─ 每列行程：450mm（上下各一次）

注意：有扶手侧和无扶手侧分开规划
├─ 有扶手侧：J5+J6折叠，轨迹点需要考虑空间约束
└─ 无扶手侧：J6单独工作，轨迹点更简单

座垫清洁轨迹（同心圆弧）：
─────────────────────────────────────────────────────
轨迹类型：沿座圈形状的环形路径

参数：
├─ 座圈内径：R_inner_seat = 150mm
├─ 座圈外径：R_outer_seat = 220mm
├─ 座圈宽度：70mm
├─ 擦拭头宽度：100mm（覆盖整个座圈宽度）
└─ 轨迹：沿座圈中心线（R=185mm）绕一圈

特点：
├─ 擦拭头宽度（100mm）> 座圈宽度（70mm）
├─ 一圈即可覆盖全部座圈面积
└─ 轨迹最简单（单圆弧）

位置标定（轨迹规划的前提）：
─────────────────────────────────────────────────────
问题：每个的安装位置不同（偏差±20mm）
      轨迹规划需要知道的精确位置

方案A：固定位置（最简单）
├─ 假设安装位置固定（机器人底盘对准）
├─ 使用预设的轨迹参数
├─ 被动柔顺机构吸收 ±10mm 的位置偏差
├─ 优点：无需视觉，最简单
├─ 缺点：位置偏差>10mm时清洁效果下降
└─ 推荐度：适合初期版本

方案B：接触式标定（推荐）
├─ 清洁前，机械臂缓慢向移动
├─ 力传感器检测到接触（F_contact > 1N）时停止
├─ 记录当前末端位置作为表面参考点
├─ 根据参考点修正轨迹参数
├─ 优点：自动适应位置偏差
├─ 缺点：需要额外的标定动作（约10秒）
└─ 推荐度：推荐主方案

方案C：视觉标定（进阶）
├─ 使用深度相机（RealSense D435i）扫描
├─ 点云处理，提取几何参数
├─ 自动生成适配当前的轨迹
├─ 优点：最精确，适应任意形状
├─ 缺点：算法复杂，需要点云处理能力
└─ 推荐度：未来版本考虑

接触式标定流程（方案B详细步骤）：
─────────────────────────────────────────────────────
Step 1：标定轴心位置
├─ 机械臂从3个方向（0°/120°/240°）分别接触外壁
├─ 记录3个接触点坐标 P1(x1,y1), P2(x2,y2), P3(x3,y3)
├─ 计算圆心（三点确定圆，解析公式）：
│
│ 已知三点，圆心 (cx, cy) 满足：
│ (x1-cx)² + (y1-cy)² = R²
│ (x2-cx)² + (y2-cy)² = R²
│ (x3-cx)² + (y3-cy)² = R²
│
│ 联立前两式：
│ 2(x2-x1)·cx + 2(y2-y1)·cy = x2²-x1² + y2²-y1²
│ 联立后两式：
│ 2(x3-x2)·cx + 2(y3-y2)·cy = x3²-x2² + y3²-y2²
│
│ 解二元一次方程组即得 (cx, cy)，得到轴心 (x_center, y_center)，精度 ±2mm

Step 2：标定高度
├─ 机械臂从上方缓慢下降（速度5mm/s）
├─ 力传感器检测到座圈接触（F_contact > 1N）时停止
├─ 记录接触高度 H_seat
└─ 得到座圈高度参考，精度 ±1mm

Step 3：修正轨迹参数
├─ 平移修正：Δx = x_center - x_preset
│ Δy = y_center - y_preset
│ ΔH = H_seat - H_preset
├─ 将预设轨迹所有点加上修正量
└─ 修正后的轨迹即为实际执行轨迹

内壁清洁柔性跟随实现：
─────────────────────────────────────────────────────
特点：三维曲面，法向方向随位置变化

法向方向计算：
├─ 内壁是旋转曲面，法向始终指向轴心
├─ 在位置 (x, y) 处，法向方向 = (-x, -y, 0) / |(x,y)|
├─ 即：法向 = 从当前点指向轴心的方向
└─ 这个方向随J1+J2的角度变化而变化

力控修正方向：
├─ 修正量 Δr 沿径向方向（指向/远离轴心）
├─ F_contact > F_target → 末端向外移动（远离轴心，减小接触力）
├─ F_contact < F_target → 末端向内移动（靠近轴心，增大接触力）
└─ 转换为关节修正：调整J1+J2的合成半径

J4的配合：
├─ J4始终保持：θ₄ = θ_arm + 180°（工具朝向轴心）
│ └─ θ_arm = θ₁ + θ₂（J1+J2累积角度）
├─ 随J1+J2运动实时更新J4角度
└─ 保证擦拭头始终正对内壁

J5/J6的配合（随高度变化的预设曲线）：
├─ 内壁上部（H=400mm）：J5≈-10°，J6≈-5°（微俯）
├─ 内壁中部（H=250mm）：J5≈-30°，J6≈-15°（中俯）
├─ 内壁下部（H=100mm）：J5≈-55°，J6≈-25°（大俯）
└─ J5/J6角度按高度线性插值，不参与力控（固定姿态）

擦拭运动：
├─ J1+J2联动，末端沿内壁螺旋向下
├─ 螺旋间距：80mm（擦拭头宽度）
├─ 擦拭速度：50mm/s（切向速度）
└─ 力控频率：100Hz（法向实时补偿）

外壁清洁柔性跟随实现：
─────────────────────────────────────────────────────
有扶手侧（J5+J6折叠进入）：
├─ J5俯仰：约-30°（折叠进入有限空间）
├─ J6俯仰：约-30°（进一步折叠）
├─ J4朝向：背向轴心（θ₄ = θ_arm）
├─ 力控方向：沿外壁法向（背向轴心方向）
│
├─ 注意：J5+J6折叠后，末端法向方向需要重新计算
│ └─ 末端法向 ≠ 水平方向（因为J5/J6有俯仰角）
│ └─ 末端法向 = 旋转矩阵(J5,J6) × 水平法向
│ └─ 需要通过正运动学计算末端实际朝向
│
└─ 力控修正：沿末端实际法向方向修正位置

无扶手侧（J6单独工作）：
├─ J5保持0°（不俯仰，固定）
├─ J6俯仰：约-20°（轻微俯仰，适应外壁角度）
├─ J4朝向：背向轴心（θ₄ = θ_arm）
├─ 力控方向：水平法向（背向轴心）
└─ 控制最简单（J5固定，法向方向为水平）

外壁法向方向：
├─ 外壁法向 = 从轴心指向当前点的方向（与内壁相反）
├─ 在位置 (x, y) 处，法向方向 = (x, y, 0) / |(x,y)|
└─ 力控修正：F_contact > F_target → 末端向内移动（靠近轴心）

擦拭运动：
├─ J3升降，末端沿外壁竖向往复
├─ 每列间距：80mm（擦拭头宽度）
├─ 擦拭速度：50mm/s（竖向速度）
└─ 列间切换：J1+J2联动，末端绕移动80mm

座垫清洁柔性跟随实现（最简单）：
─────────────────────────────────────────────────────
特点：水平平面，法向方向固定（垂直向下）

法向方向：固定为 (0, 0, -1)（Z轴负方向）

力控修正（最简单的力控）：
├─ F_contact > F_target → J3上升（末端远离座圈）
├─ F_contact < F_target → J3下降（末端靠近座圈）
└─ 修正量直接转换为J3升降量（Δd3 = Kp × error）

J5/J6的配合：
├─ J5俯仰：约-90°（末端垂直向下）
├─ J6俯仰：约0°（末端保持垂直）
└─ 两者固定，不参与力控

J4的配合：
├─ 座垫清洁时，J4角度不影响清洁效果
└─ J4保持固定角度（如0°）

擦拭运动：
├─ J1+J2联动，末端沿座圈中心线（R=185mm）绕一圈
├─ 擦拭头宽度（100mm）> 座圈宽度（70mm）
├─ 一圈即可覆盖全部座圈面积
└─ 擦拭速度：50mm/s（切向速度）

底座清洁柔性跟随实现：
─────────────────────────────────────────────────────
特点：地面附近，空间最受限

末端位置：
├─ 径向：R=280–350mm（最宽处以外）
├─ 高度：H=0–50mm（地面附近）
└─ J5/J6：斜向下（约-45°），末端接近地面

法向方向：近似垂直向下（Z轴负方向）

力控修正：
├─ 同座垫清洁，J3升降补偿
└─ 但高度极低，J3下降量有限（注意碰地保护）

碰地保护（关键）：
├─ J3最低位置限制：H_min = 20mm（距地面安全距离）
├─ 如果力控要求J3继续下降但已到限位
│ └─ 停止下降，保持当前高度
└─ 避免机械臂末端碰到地面

擦拭运动：
├─ J1+J2联动，末端绕底座一圈
└─ 擦拭速度：30mm/s（低速，空间受限）

关键问题1：力控与位置控制的切换时机 
─────────────────────────────────────────────────────
问题描述：
├─ 机械臂从空中移动到接触表面的过程中
├─ 需要从"位置控制"切换到"力控"
├─ 切换时机不对会导致：
│ ├─ 太早切换：末端还没接触表面，力控无效
│ └─ 太晚切换：末端已经"硬顶"进表面，冲击力过大

解决方案：
├─ 接近阶段：位置控制（按轨迹移动），速度降低到5mm/s
├─ 接触检测：F_contact > 1N（接触阈值）
├─ 切换：立即切换到力控模式
└─ 切换后：力控维持接触力在5–10N

关键参数：
├─ 接近速度：5mm/s（慢速接近，减小冲击）
├─ 接触阈值：1N（灵敏检测）
└─ 切换时间：< 10ms（一个控制周期内完成）

关键问题2：擦拭运动与力控的耦合 
─────────────────────────────────────────────────────
问题描述：
├─ 擦拭时，机械臂沿表面做往复运动（切向运动）
├─ 同时力控在法向方向做位置补偿（法向运动）
├─ 两个运动方向耦合，可能相互干扰

解决方案：运动解耦
├─ 切向运动：由轨迹规划控制（J1+J2+J3联动）
├─ 法向运动：由力控控制（沿法向方向微调）
├─ 两者在不同方向上独立控制，互不干扰
└─ 实现：在笛卡尔空间中分解运动方向

具体实现：
├─ 每个控制周期（10ms）：
│ ├─ 计算轨迹规划的切向位移 Δx_tangential
│ ├─ 计算力控的法向修正 Δx_normal
│ ├─ 合并：Δx_total = Δx_tangential + Δx_normal
│ └─ 通过逆运动学转换为关节修正量
└─ 关键：切向和法向方向必须正交（垂直）

关键问题3：曲面的不连续性 
─────────────────────────────────────────────────────
问题描述：
├─ 内壁不是完美的旋转曲面
├─ 存在：排水孔边缘、冲水口、内壁接缝等不连续点
├─ 末端经过这些位置时，接触力会突变

解决方案：
├─ 力控死区：±2N内不修正（避免对小突变过度响应）
├─ 过载保护：F_contact > 20N → 立即退让5mm
├─ 速度降低：经过不连续点时降低擦拭速度（50→20mm/s）
└─ 预设不连续点位置：在轨迹规划中标记，经过时特殊处理

关键问题4：清洁液对力传感器的影响 
─────────────────────────────────────────────────────
问题描述：
├─ 清洁液喷洒后，表面有液膜
├─ 液膜会降低擦拭摩擦系数（从0.3降到0.1）
├─ 摩擦力减小 → 擦拭效果下降
└─ 液膜可能渗入力传感器

解决方案：
├─ 清洁液喷洒时机：擦拭前5秒喷洒（让液体均匀分布）
├─ 擦拭时不喷洒（避免液体飞溅）
├─ 力传感器防水：IP65以上，硅胶套保护
└─ 接触力补偿：液膜存在时，适当增大目标接触力（7.5N→10N）

关键问题5：重力补偿是力控系统最大陷阱 
─────────────────────────────────────────────────────
问题描述：
├─ J5/J6俯仰时，末端自重（1.3kg）在力传感器
│ 测量轴上产生重力分量
├─ 重力分量随俯仰角变化（0°时最大，90°时为0）
└─ 不补偿则力控完全失准

重力分量计算：
├─ F_gravity = G × cos(θ_pitch)
│ └─ G = 12.74N，θ_pitch = J5 + J6
├─ θ_pitch = 0°：F_gravity = 12.74N（最大误差）
├─ θ_pitch = -45°：F_gravity = 9.01N
├─ θ_pitch = -90°：F_gravity = 0N（无误差）
└─ 补偿公式：F_contact = F_filtered - F_gravity

验证方法：
├─ 末端悬空，缓慢改变J5/J6角度（0°→-90°）
├─ 补偿正确时：读数始终接近0N（±0.5N以内）
└─ 如果读数随角度变化，说明补偿参数有误

调试阶段的关键注意事项：
─────────────────────────────────────────────────────
注意1：调试顺序（严格遵守）
├─ Step 1：先验证机械被动柔顺（弹簧+球铰）
├─ Step 2：再验证力传感器标定和重力补偿
├─ Step 3：再开启主动力控（P控制）
├─ Step 4：再验证接触式标定
└─ Step 5：最后验证完整轨迹
 原因：每层依赖上一层，顺序错误会导致问题叠加

注意2：力传感器每次开机必须标定零点
├─ 末端悬空，记录零点值（约3秒）
├─ 零点随温度变化（温漂约0.05N/°C）
├─ 不标定直接使用会导致力控偏差
└─ 建议：开机自动执行标定程序

注意3：Kp参数必须在实际上调整
├─ 不同表面（陶瓷/亚克力）刚度不同
├─ 在金属板上调好的Kp，在上可能不适用
├─ 推荐调参步骤：
│ ├─ 从Kp=0.01开始，速度5mm/s
│ ├─ 逐步增大Kp到出现轻微震荡
│ └─ 退回到震荡临界值的50%
└─ 典型值：Kp = 0.03–0.08mm/N

注意4：过载保护必须优先验证
├─ 手动施加>20N的力，确认机械臂立即退让5mm
├─ 过载保护失效会损坏机械臂
├─ 这是安全红线，必须在其他功能之前验证
└─ 验证通过后才能进行后续调试

注意5：末端速度限制
├─ 力控修正时，末端移动速度不能过快
├─ 过快会导致冲击力（超过过载阈值）
├─ 推荐最大修正速度：10mm/s
└─ 实现：对Δx进行速度限幅（rate limiter）

阶段1：机械被动柔顺验证（第1–2周）
─────────────────────────────────────────────────────
任务清单：
├─ 安装法向弹簧机构（k=5N/mm，预压1–2mm）
├─ 安装球铰万向节（±10°角度柔顺）
├─ 安装柔性海绵基底（20mm厚，Shore A 30–40）
└─ 手动测试：推动末端，验证三层柔顺效果

验收测试：
├─ 法向推力5N → 弹簧压缩1mm（±0.2mm）
├─ 法向推力10N → 弹簧压缩2mm（±0.2mm）
├─ 倾斜末端±10° → 球铰自由转动，无卡顿
└─ 松开后 → 弹簧和球铰自动复位到中位

阶段2：力传感器集成与标定（第3周）
─────────────────────────────────────────────────────
任务清单：
├─ 安装力传感器（单轴，量程50N）
├─ 实现低通滤波（截止频率10Hz）
├─ 实现重力补偿（读取J5/J6角度，实时计算）
└─ 实现接触状态判断（阈值1N/0.5N）

验收测试：
├─ 悬空标定：零点误差 < ±0.2N
├─ 重力补偿：J5从0°到-90°，读数变化 < ±0.5N
├─ 接触检测：轻触表面，F_contact > 1N立即触发
└─ 过载检测：施加>20N，立即触发过载报警

阶段3：比例力控实现（第4–5周）
─────────────────────────────────────────────────────
任务清单：
├─ 实现P控制力控算法（约50行代码）
├─ 实现位置控制→力控的平滑切换
├─ 实现过载保护（>20N立即退让5mm）
└─ 在平板上调参（Kp），再在上验证

验收测试：
├─ 平板测试：接触力稳定在5–10N（±2N）
├─ 测试：内壁清洁时接触力稳定
├─ 切换测试：从空中接近，切换平滑无冲击
└─ 过载测试：手动施加>20N，立即退让

阶段4：接触式标定实现（第6周）
─────────────────────────────────────────────────────
任务清单：
├─ 实现3点接触标定（三点拟合圆心）
├─ 实现高度标定（接触座圈）
├─ 实现轨迹参数修正（平移修正）
└─ 验证标定精度

验收测试：
├─ 将偏移±15mm，标定后轨迹仍正确
├─ 标定时间 < 20秒
└─ 标定精度 ±3mm

阶段5：轨迹规划与完整验证（第7–8周）
─────────────────────────────────────────────────────
任务清单：
├─ 实现内壁螺旋轨迹（4圈，间距80mm）
├─ 实现外壁竖向往复轨迹（有扶手/无扶手分开）
├─ 实现座垫环形轨迹（一圈）
├─ 实现底座轨迹（一圈，低速）
└─ 完整清洁流程联调（标定→内壁→外壁→座垫→底座）

验收测试：
├─ 内壁覆盖率 ≥ 95%（染色法验证）
├─ 座垫覆盖率 ≥ 98%
├─ 全程无过载（F_contact < 20N）
├─ 完整清洁时间 < 3分钟
└─ 连续清洁5次，无故障