复合清洁机器人底座偏差时，机械臂对准方案

底座偏差的来源：
─────────────────────────────────────────────────────
偏差类型 典型范围 来源
位置偏差X/Y ±1–2cm 机器人每次停靠位置略有不同
位置偏差Z ±0.5cm 地面不平、轮子压缩
朝向偏差Yaw ±1–2° 停靠朝向不精确
俯仰/翻滚偏差 ±0.5° 地面倾斜

偏差对清洁的影响分析：
─────────────────────────────────────────────────────
示教状态：
├─ 机械臂末端正确贴合内壁
├─ 擦拭头朝向正确（垂直于内壁）
└─ 接触力适中（3–8N）

回放状态（无补偿）：
├─ 底座偏移1–2cm → 末端位置偏移1–2cm
├─ 底座旋转2° → 末端在远端（200mm）产生约7mm位置偏差
├─ 叠加效果：末端位置偏差可达2–3cm
└─ 结果：擦拭头可能远离（清洁不充分）
 或切入（损坏或卡住）

关键洞察：
├─ 内壁是固定曲面（不可移动）
├─ 机械臂底座位置决定了末端可达范围
├─ 1–2cm的位置偏差 + 2°的朝向偏差
│ 会导致末端轨迹与内壁产生明显偏离
└─ 必须通过底座标定 + 轨迹补偿 + 力控制三重保障

解决方案总体框架（三层保障）：
─────────────────────────────────────────────────────
第一层：底座标定（预防）
├─ 目的：精确测量底座相对于的实际位置
├─ 方法：示教标定点 → 计算实际坐标变换
├─ 效果：将位置偏差从1–2cm降低到<2mm
└─ 重要性：必须

第二层：轨迹自适应调整（校准）
├─ 目的：将示教轨迹转换到当前底座坐标系
├─ 方法：坐标变换 + 轨迹重映射
├─ 效果：轨迹与内壁对齐
└─ 重要性：关键

第三层：力控制柔性贴合（兜底）
├─ 目的：吸收残余误差，保持恒定接触力
├─ 方法：力传感器反馈 + 位置修正
├─ 效果：即使轨迹有偏差，接触力仍保持稳定
└─ 重要性：必须

三层保障的协同：
├─ 标定消除大部分系统性偏差
├─ 轨迹调整消除坐标系变换偏差
├─ 力控制吸收残余误差（<5mm）
└─ 组合效果：清洁贴合可靠、稳定

底座标定的核心思想：
─────────────────────────────────────────────────────
问题：示教时底座在位置A，回放时底座在位置B
 如何让轨迹在位置B也能正确贴合？

答案：建立"固定坐标系"，将所有轨迹都映射到这个坐标系

坐标系的定义：
─────────────────────────────────────────────────────
坐标系1：坐标系（W）
├─ 原点：中心（底部中心）
├─ X轴：指向前方
├─ Y轴：指向左侧
├─ Z轴：垂直向上
└─ 特点：不动，坐标系固定

坐标系2：底座坐标系（B）
├─ 原点：底座安装中心
├─ X轴：指向底座前方
├─ Y轴：指向底座左侧
└─ 特点：每次停靠后位置不同

坐标系3：机器人坐标系（R）
├─ 原点：J1关节中心（J1轴线与底座平面交点）
├─ 与底座坐标系的关系：固定偏移（设计确定）
└─ 特点：随底座移动

标定的目标：
├─ 建立底座坐标系B到坐标系W的变换
├─ 示教时：记录末端在W中的轨迹
├─ 回放时：将轨迹转换到当前B'坐标系
└─ 执行转换后的轨迹

标定方法：示教标定法（推荐）
─────────────────────────────────────────────────────
原理：通过示教几个已知的特征点
 计算底座到的坐标变换

标定步骤：
─────────────────────────────────────────────────────
Step 1：确定特征点
├─ 选择上3个易于示教的特征点
│ ├─ P1：前壁中心（正前方）
│ ├─ P2：左壁中心（左侧90°）
│ └─ P3：右壁中心（右侧90°）
│
├─ 特征点要求：
│ ├─ 位置固定（本体）
│ ├─ 易于识别和触碰
│ └─ 3个点不共线（保证计算唯一性）
│
└─ 替代方案：
 ├─ 如果有固定标记点（如安装孔）
 ├─ 使用这些标记点作为标定参照
 └─ 或者使用外形的已知几何参数

Step 2：示教标定
├─ 操作者拖动末端依次触碰3个特征点
├─ 记录每个点的关节角度
├─ 通过正运动学计算末端的机器人坐标系位置
└─ 得到：B空间中的3个点 {b1, b2, b3}

Step 3：计算坐标变换
├─ 坐标系是已知的（基于型号参数）
├─ 已知：坐标系的3个点 {w1, w2, w3}（设计值）
├─ 已知：机器人坐标系的3个点 {b1, b2, b3}（示教值）
├─ 计算：从B到W的旋转矩阵R和平移向量T
│ └─ 使 min Σ||(R·bi + T) - wi||² 最小
└─ 输出：R（3×3）、T（3×1）

坐标变换验证：
├─ 将标定后的{R, T}应用到示教的3个点
├─ 验证转换后的位置与坐标系理论值偏差
├─ 偏差 < 2mm 标定成功
└─ 偏差 > 5mm 重新标定

前置条件：
├─ 机械臂已完成重力补偿标定
├─ 末端已安装标定探针（便于接触）
└─ 位置固定不动

流程：
─────────────────────────────────────────────────────
阶段1：标定准备
├─ 1.1 将机械臂移动到初始姿态
├─ 1.2 进入零力矩示教模式
├─ 1.3 提示操作者准备触碰第一个标定点
└─ 1.4 确认力传感器零点

阶段2：触碰标定点（3次）
├─ 2.1 操作者拖动末端触碰P1（前壁中心）
├─ 2.2 按下"记录"按钮，记录机器人坐标系位置 b1
├─ 2.3 重复P2、P3，记录 b2、b3
│
└─ 标定点记录数据格式：
 {
 "P1": {"joints": [...], "robot_pos": [x, y, z]},
 "P2": {"joints": [...], "robot_pos": [x, y, z]},
 "P3": {"joints": [...], "robot_pos": [x, y, z]}
 }

阶段3：计算变换矩阵
├─ 3.1 加载型号参数（标定点的坐标 w1, w2, w3）
├─ 3.2 使用Kabsch算法计算最佳旋转矩阵R和平移向量T
├─ 3.3 验证标定精度
└─ 3.4 保存标定结果

阶段4：标定验证
├─ 4.1 拖动末端触碰内壁某点
├─ 4.2 系统自动转换到坐标系
├─ 4.3 显示转换后的位置
├─ 4.4 操作者确认与实际位置一致
└─ 4.5 标定完成

标定精度要求：
├─ 标定点触碰误差 < 1mm（力传感器确认接触）
├─ 标定变换误差 < 2mm
├─ 验证点偏差 < 3mm
└─ 不满足则重新标定

轨迹坐标系转换原理：
─────────────────────────────────────────────────────
示教时的坐标系：
├─ 示教轨迹存储格式：机器人坐标系R中的关节角度序列
├─ 通过正运动学转换为机器人坐标系R中的末端位置
└─ 通过标定变换转换为坐标系W中的末端位置

回放时的坐标系：
├─ 当前底座坐标系B' ≠ 示教时底座坐标系B
├─ 需要将W中的轨迹转换到B'中
└─ 通过逆变换：P_B' = R'⁻¹ · (P_W - T')

坐标转换流程：
─────────────────────────────────────────────────────
示教时（建立坐标系轨迹）：
├─ 1. 记录示教轨迹的关节角度序列
├─ 2. 正运动学转换为机器人坐标R
├─ 3. 标定变换转换为坐标W
└─ 4. 保存坐标系中的轨迹 {W轨迹}

回放时（适配当前底座位置）：
├─ 1. 当前标定：计算 B' → W 的变换 {R', T'}
├─ 2. 读取 W 轨迹
├─ 3. 逆变换：P_B' = R'^T · (P_W - T')
├─ 4. 逆运动学转换为关节角度
└─ 5. 执行调整后的关节角度轨迹

关键：每次回放都需要重新标定
 因为每次停靠后底座位置不同

轨迹重映射方法（两种方案）：
─────────────────────────────────────────────────────
方案A：末端位置重映射（推荐）
├─ 原理：保持末端在坐标系中的位置不变
├─ 转换：
│ ├─ W轨迹（坐标）→ B'轨迹（当前机器人坐标）
│ └─ 需要逆运动学转换为关节角度
├─ 优点：末端轨迹精确贴合内壁
├─ 缺点：需要逆运动学（关节角度可能有抖动）
└─ 适用：清洁贴合（末端必须贴合）

方案B：关节角度偏移（备选）
├─ 原理：计算示教位姿与当前位姿的偏差
│ 将此偏差应用到整个轨迹
├─ 转换：
│ ├─ 计算：Δq = q_current_home - q_teach_home
│ │ └─ 底座位置偏差导致的关节角度偏移
│ ├─ 应用：q_playback[i] = q_teach[i] + Δq
│ └─ 直接在关节空间添加偏移
├─ 优点：无需逆运动学
├─ 缺点：累积误差可能较大（各关节放大比例不同）
└─ 适用：初步补偿，不适合高精度场景

方案A的详细步骤：
─────────────────────────────────────────────────────
Step 1：读取示教轨迹（坐标系）
├─ 轨迹点：{W_P0, W_P1, W_P2, ...}
└─ 每个点：末端位置[x, y, z] + 姿态[roll, pitch, yaw]

Step 2：当前标定（每次回放前）
├─ 计算当前底座到的变换 {R', T'}
└─ 保存到当前回放会话

Step 3：转换每个轨迹点到当前机器人坐标
├─ 对于每个 W_Pi：
│ ├─ B'_Pi = R'^T · (W_Pi - T') // 位置
│ └─ 姿态：相对的朝向保持不变
│ 但需要叠加底座旋转的影响
└─ 输出：B'轨迹

Step 4：逆运动学求解关节角度
├─ 对于每个 B'_Pi：
│ └─ q_i = IK(B'_Pi) // 逆运动学求解
└─ 输出：关节角度轨迹 {q0, q1, q2, ...}

Step 5：轨迹平滑检查
├─ 检查关节角度序列是否平滑
├─ 检查是否有关节限位冲突
├─ 检查是否有奇异点姿态
└─ 如有问题，调整或重新标定

逆运动学处理要点：
─────────────────────────────────────────────────────
问题：逆运动学求解可能产生多解或奇异点

多解处理：
├─ 6轴机械臂的逆运动学有多个解（通常2–8个）
├─ 选择标准：
│ ├─ 优先选择与示教时关节角度接近的解
│ ├─ 避免关节限位边界的解
│ └─ 避免奇异点的解
│
└─ 实现方法：
 ├─ 预设关节角度优先级权重
 ├─ 计算每个解的"代价函数"
 ├─ 选择代价最小的解
 └─ 必要时进行轨迹级平滑

奇异点处理：
├─ 奇异点：雅可比矩阵秩不足，导致关节速度趋向无穷
├─ 发生位置：J5接近0°时（J5+J6俯仰链伸直）
├─ 影响：奇异点附近关节速度可能突变
│
└─ 处理方法：
 ├─ 检测：计算当前姿态的奇异值
 ├─ 回避：规划时避免经过奇异点
 ├─ 平滑：如果必须经过奇异点
 │ ├─ 降低奇异点附近的运动速度
 │ └─ 切换到关节空间插值（避开奇异）
 └─ 警告：如果奇异点无法回避，提示重新示教

逆运动学计算的实时性：
├─ 每次回放需要计算N次逆运动学（N=轨迹点数）
├─ 100Hz轨迹，10秒 = 1000个点
├─ 逆运动学计算量：约1–5ms/点（取决于算法）
├─ 总时间：1–5秒（离线计算）
└─ 建议：回放前离线计算，不在实时循环中计算

力控制柔贴合原理：
─────────────────────────────────────────────────────
为什么需要力控制？
├─ 标定 + 轨迹调整只能消除大部分系统性偏差
├─ 残余误差（<5mm）仍可能导致：
│ ├─ 接触力过大 → 损坏擦拭头或
│ ├─ 接触力过小 → 清洁不充分
│ └─ 姿态不正 → 擦拭效果差
├─ 力控制作为最后一道保障
└─ 实时修正位置，保持恒定接触力

力控制的工作原理：
─────────────────────────────────────────────────────
控制器结构：
├─ 输入：期望接触力 F_desired（3–8N）
├─ 反馈：实际接触力 F_actual（力传感器读数）
├─ 控制量：位置修正量 ΔP
│
└─ 控制律：
 ├─ 误差 e = F_desired - F_actual
 ├─ 位置修正 ΔP = Kp × e
 │ └─ Kp：比例增益（如0.5mm/N）
 └─ 修正后的目标位置 P_target = P_trajectory + ΔP

方向判断：
├─ F_actual > F_desired → 末端过深 → 向外修正
├─ F_actual < F_desired → 末端过浅 → 向内修正
└─ 修正量与误差成正比

力控制参数推荐值：
─────────────────────────────────────────────────────
目标接触力 F_desired：
├─ 内壁清洁：3–6N
│ └─ 理由：擦拭头需要一定压力保证清洁效果
│ 但不能过大以免损坏釉面
└─ 外壁/座垫清洁：2–5N
 └─ 理由：外壁和座垫表面相对平整
 接触力可以稍小

比例增益 Kp：
├─ 推荐值：0.5–1.0 mm/N
├─ Kp过大（>2mm/N）：
│ ├─ 反应灵敏，但容易震荡
│ └─ 末端抖动，清洁效果差
├─ Kp过小（<0.2mm/N）：
│ ├─ 反应迟钝，修正不足
│ └─ 接触力偏差大
│
└─ 推荐调试方法：
 ├─ 从 Kp=0.5mm/N 开始
 ├─ 观察接触力曲线
 ├─ 如果震荡，增加阻尼
 └─ 如果迟钝，增大Kp

死区设置：
├─ 推荐值：±1.5N
├─ 含义：|误差| < 1.5N时不进行修正
├─ 目的：避免小幅震荡导致频繁修正
└─ 注意：死区过大（>3N）会导致修正不及时

最大修正量：
├─ 推荐值：±10mm
├─ 含义：单次修正不超过10mm
├─ 目的：防止大幅修正导致轨迹失真
└─ 超出时：报警提示，可能需要重新标定

力控制启用条件：
├─ 示教时记录了接触点（contact=true）
├─ 当前姿态末端朝向内壁
└─ 接触力传感器读数 > 0.5N（确认已接触）

力控制实施流程：
─────────────────────────────────────────────────────
示教时记录接触信息：
├─ 记录每个点的接触状态（contact: true/false）
├─ 记录接触时的力传感器读数（F_contact）
├─ 记录接触时末端的法向方向（n：指向内）
└─ 作为回放时的参考

回放时力控制执行：
─────────────────────────────────────────────────────
控制频率：100Hz（与位置控制同步）

每个控制周期：
├─ Step 1：读取当前关节角度
│ └─ q_current = get_joint_angles()
│
├─ Step 2：查表获取轨迹目标角度
│ └─ q_trajectory = trajectory[index]
│
├─ Step 3：计算末端位置和姿态
│ └─ FK(q_current) → P_current
│ └─ FK(q_trajectory) → P_trajectory
│
├─ Step 4：读取力传感器
│ └─ F_actual = get_end_force()
│ └─ 计算法向分力 F_normal（指向内壁）
│
├─ Step 5：力控制计算
│ ├─ e = F_desired - F_actual
│ ├─ 如果 |e| > deadzone（1.5N）：
│ │ └─ 计算修正量 ΔP = Kp × e × n
│ │ └─ P_modified = P_trajectory + ΔP
│ │ └─ ΔP_clamped = clamp(ΔP, max=10mm)
│ └─ 否则：P_modified = P_trajectory
│
├─ Step 6：逆运动学求解修正后的关节角度
│ └─ q_modified = IK(P_modified)
│
├─ Step 7：发送关节目标角度
│ └─ set_joint_targets(q_modified)
│
└─ Step 8：记录日志
 ├─ 记录当前时刻的接触力
 ├─ 记录修正量
 └─ 如果异常，记录报警

力控制的安全保护：
├─ 最大接触力限制：15N（超过则立即停止）
├─ 最大修正量限制：±10mm（超出则报警）
├─ 修正量变化率限制：±5mm/周期（防止突变）
└─ 力传感器异常检测：读数突变 > 10N → 停止