清洁机器人在基座和末端，增加深度相机的技术方案

机械臂清洁使用两个深度相机，基座深度相机提供全局视野，末端深度相机提供局部高精度感知，可以将之前的清洁流程从触觉为主、视觉为辅升级为视觉预判-力觉执行-视觉校验的闭环系统。这能大幅提升清洁的流畅度、效率和稳定性。

一、双相机协同感知

1. 基座深度相机（全局）

• 固定安装在机械臂基座或移动底盘上方，视场覆盖整个机械臂工作空间。
• 核心作用：环境理解、粗定位、机械臂自碰撞监测、清洁区域宏观划分。

2. 末端深度相机（局部）

• 安装在末端连杆，距清洁模组约20cm，随工具运动。
• 核心作用：精细曲面重建、法向量预估、接触前预对准、污渍检测与洁净度评估。

协同原则：

• “粗-精”接力：全局相机引导机械臂到达目标区域附近，末端相机接管进行精确伺服。
• “外-内”互补：全局相机解决“在哪”的问题，末端相机解决“怎么贴”的问题。
• “静-动”分工：底盘停驻时，全局相机实时监测状态和机械臂运动；末端相机在贴合前、清洁后短暂静止时进行高精度采集。

二、清洁算法与流程

阶段1：系统标定与模型构建（离线）

阶段2：全局定位与粗规划（基座相机）

1. 底盘就位：移动底盘锁死后，基座相机拍摄，与预存语义模型进行3D点云配准（如ICP或基于特征的配准），得到当前精确位姿。
2. 清洁区域自动分割与路径粗规划：根据类型和污渍分布（可由基座相机初步识别大块污渍），自动规划清洁顺序，生成各区域的安全接近点和大致的清洁路径框架。
3. 机械臂自碰撞规避：实时利用基座相机监测机械臂姿态，在规划路径时加入自碰撞约束，确保臂杆不碰。

相比纯触觉方案的优势：

• 无需先验模型，自动适应不同形状和摆放姿态。
• 避免机械臂在未知空间盲目伸展，缩短粗定位时间。

阶段3：视觉引导快速趋近（双相机接力）

1. 基座相机引导大范围移动：将末端工具移动到目标区域上15-20cm的安全点（此时末端相机距目标约35-40cm，处于有效视距内）。
2. 末端相机精确伺服：末端相机识别目标区域的局部曲面，通过点云匹配或边缘特征跟踪，实时估计工具与表面的相对位姿（位置+姿态）。
3. 视觉伺服趋近：以30-50mm/s的速度，根据末端相机的位姿反馈，平滑控制机械臂，使工具面与釉面平行且距离缩短至5mm。此时，视觉估计的法向量已发送给J5/J6进行预对准。
4. 视觉-力觉切换点：当距离达到5mm，或视觉置信度下降（如太近失焦），切换到力控的“低速柔性逼近阶段”。

核心算法：

• 基于图像的视觉伺服（IBVS）：利用末端相机图像特征点（或点云边缘）的移动来直接控制关节速度，避免复杂的3D重建。
• 融合策略：视觉负责速度前馈，力控负责最终接触，视觉预估一个初始法向，力控在接触瞬间（0.5N）微调，实现“无感切换”。

阶段4：力控贴合与曲面跟随（力控为主，视觉为辅）

视觉前馈让姿态调整从“力误差驱动”变成“几何预测驱动”，提升稳定性和效率。

• 恒力贴合控制：保持“J5宏动、J6微动”导纳控制架构。
• 视觉前馈：
  • 曲面预测：末端相机在工具侧前方约20cm处，能提前看到即将清洁的20cm外的曲面形状。在工具移动时，相机连续采集前方点云，计算出未来数秒内的曲面法向和曲率变化序列。
  • 前馈控制：将该序列作为前馈量，提前驱动J5（大俯仰）进行慢速姿态调整，让J6（小俯仰）始终工作在零位附近，仅需补偿微小力误差，避免力控被动响应的大滞后，使贴合更加稳定。
• 切向速度自适应：根据曲率变化剧烈程度，自动调节切向清洁速度——平坦区高速（80mm/s），拐角区低速（20mm/s），决策由末端相机预估的曲率直接驱动。

阶段5：清洁质量评估与补擦（末端相机主导）

1. 区域清洁完成后回退拍照：机械臂将工具回退8-10cm，使末端相机正对刚清洁的釉面。
2. 多模态污渍检测：
   • 采集高分辨率RGB图+深度图，利用异常检测模型识别残留污渍和水垢，不受水膜反光影响。
   • 或用差分法：对比清洁前后同一区域的纹理特征变化。
3. 自适应补擦策略：
   • 若检测到局部未达标，仅对该小区域进行重规划（短路径），用更慢速、稍高力（如4N）进行二次清洁。
   • 补擦后再次评估，直至通过或达到最大重复次数。

阶段6：全局安全与故障处理（基座相机持续监控）

• 全局障碍物检测：基座相机实时监测周围是否有人员靠近或异物落入，触发减速或暂停。
• 模组状态视觉确认：更换模组后，基座相机可拍摄工具架，视觉确认模组是否锁紧在位，防止脱落。
• 底盘漂移监测：若底盘因长时间工作产生微小位移，基座相机可通过比对固定参考物（如特征点）的位姿变化，及时提醒或触发重定位。

三、算法架构

[系统启动]
  ↓
[离线：双相机标定 & 语义模型构建]
  ↓
[清洁任务开始]
  ├─→ 基座相机：3D配准当前位姿 → 全局路径规划
  ├─→ 基座相机：引导机械臂至目标区域安全点
  ├─→ 末端相机：视觉伺服快速趋近（位置+姿态预对准）
  ├─→ 力传感器：切换至力控，柔性接触
  ├─→ 贴合清洁循环：
  │     ├─ 末端相机：前瞻曲面法向/曲率 → 前馈给J5姿态
  │     ├─ 力传感器+导纳：J6微动恒力控制
  │     └─ 速度自适应：根据曲率调整切向速度
  ├─→ 区域清洁完成
  ├─→ 末端相机：洁净度评估 → (不通过) 补擦
  ├─→ 切换下一区域
  ↓
[清洁完成] → 基座相机：全局对比确认

四、相机作用

• 清洁周期缩短：视觉引导快速趋近+速度自适应+精准补擦，减少空跑和重复覆盖。
• 力控稳定性提升：曲面法向前馈让恒力贴合误差从±0.5N 降至 ±0.2N，保护釉面。
• 自主化水平跃升：无需先验模型，适应不同；自动质量评估，无需人工干预。
• 安全性增强：全局相机提供第二道安全防线，防止臂杆碰撞和外部干扰。

双深度相机的引入，让复合机器人从“触觉敏感”进化到“视觉敏锐、触觉精准”，实现了真正意义上的智能清洁闭环。