复合机器人自主清洁作业技术方案

介绍一套以力位混合导纳控制+曲面法向在线估计+俯仰冗余优化为核心的方案，可以实现高贴合质量、低损伤风险的清洁。

一、贴合时要注意的关键问题

1. 力控制精度与釉面保护

• 恒力贴合：釉面光滑但相对脆弱，接触力过大会划伤釉面或使模组跳动，过小则清洁不彻底。需要实现法向力伺服，建议法向力控制精度在 ±0.5 N 以内，上限根据刷头材料和清洁液设定（通常在 2~5 N）。
• 动态力干扰抑制：旋转刷头或模组会产生周期性扰动力，力控带宽要高于扰动频率（比如≥20 Hz），且需在工具端增加六维力/力矩传感器或通过关节力矩做力估计，形成闭环。

2. 曲面法向自适应与姿态对齐

• 表面是复杂自由曲面，末端清洁面必须实时对齐釉面法向，否则会产生侧向刮擦或不均匀接触。
• 可以利用大行程俯仰做粗调对准，小行程俯仰做精细柔顺或高频姿态补偿，减少整臂惯量影响。
• 注意两个俯仰轴线的空间位置关系——若它们不共轴，会形成耦合运动，姿态对齐算法需要显式处理这种耦合，同时避免关节进入奇异或限位。

3. 移动底盘的基座不确定性与振动

• 移动底盘即使停驻，也存在悬架沉降、地面不平、轮系间隙带来的基座微动（毫米级平移、微小倾斜），直接导致机械臂末端相对位置漂移。
• 解决方法：
  • 采用末端-环境相对位姿闭环（视觉伺服或力觉引导），而不是仅依赖底盘定位。
  • 底盘需具备稳定支腿或调平机构，减少浮动。
  • 力控模式下，法向力反馈能够部分补偿基座的缓慢漂移，但姿态偏差仍需要视觉/力觉修正。

4. 接触过渡与边缘安全性

• 模组进入、退出边缘时，曲率变化剧烈，容易发生力尖峰或脱离，需要柔顺切入/切出策略，例如降低力控制刚度、限制接近速度。
• 整个系统必须具有碰撞检测和急停能力，尤其在狭小内部，除工具头外，臂杆也不应碰触瓷面，可使用关节力矩监测或电子皮肤。

5. 清洁路径与贴合完整性

• 贴合的目标不只是沿一条曲线运动，而是要保证清洁覆盖面完整。规划路径时需考虑曲面形状，使工具切向进给速度与旋转清洁宽度匹配，重叠率约 20%-30%。
• 在釉面拐角或排水口附近，需切换姿态策略，可能短暂牺牲完美法向对齐以换取可达性，此时接触力阈值和保护机制就格外重要。

6. 防水、化学兼容性与传感器防护

• 清洁液、水雾环境对传感器、电气接口影响大，力传感器和视觉单元必须达到 IP67 以上防护，并考虑水膜对力信号和光学测量的干扰（如光斑折射、力传感器热漂移等）。

二、推荐使用的核心算法框架

综合看，最合适的控制框架是基于力觉的导纳控制 + 在线曲面法向估计 + 冗余运动学优化。

1. 接触力/位混合控制（导纳控制）

• 导纳控制：在工具端建立质量-弹簧-阻尼虚拟动力学，将力误差转换为位置/姿态修正量： MΔx¨+DΔx˙+KΔx=Fd−Fext 其中 Fd 是期望法向力，Fext 是实测力，姿态也可采用类似导纳关系修正。
• 优点：力控稳定，对刚性环境适应好，可以直接输出位置指令给位置环，适合工业机器人驱动器。
• 实现：使用腕部六维力传感器，将力变换到工具坐标系，法向力回路与切向运动解耦，切向做位置控制（覆盖路径），法向做力控制（恒力贴合）。

2. 曲面法向与曲率的在线估计

如果没有精确的CAD模型，需要在线感知曲面特征：

• 基于接触力估计法向：利用力传感器测量接触力方向，结合运动学计算末端位移，用递归最小二乘或卡尔曼滤波估计局部曲面法向量和曲率。此方法在低速、稳定接触时有效。
• 基于视觉增强：在工具端附近安装微型3D相机（如结构光或ToF），实时获取釉面点云，拟合局部平面/二次曲面，直接得到法向和参考轨迹。
• 混合策略：视觉用于前瞻估计（前馈），力觉用于精调反馈，可应对水雾、光照变化。

3. 冗余俯仰关节的姿态分解与优化

工具需要达到“Z轴对齐曲面法向”的姿态约束，两个俯仰关节提供了姿态冗余。可设计如下：

• 设目标姿态矩阵 Rdes 使工具Z轴与估计法向 nn 对齐，X轴沿路径切向。
• 求运动学逆解，当存在冗余时，引入零空间优化，同时满足：
  1. 保持关节远离限位；
  2. 小行程俯仰用于高带宽力控或姿态微调，大行程俯仰负责大范围定向；
  3. 最小化机械臂整体运动能量，提高清洁速度。
• 可以使用加权伪逆或任务优先级框架： 主任务：末端姿态 = 对齐法向 + 切向路径跟踪 次级任务（零空间）：优化关节位置，使小行程俯仰保持在行程中心附近，为精细调整留出裕量。

4. 移动平台-机械臂联合补偿

• 如果底盘装有定位系统（激光SLAM或标记），可以获取底盘/臂基座相对粗略位姿。
• 在清洁过程中，利用末端视觉SLAM或外部位姿测量（如固定的ArUco标记）以 10~30 Hz 更新臂基座与马桶间的变换矩阵，前馈补偿底盘漂移。
• 力控内环以 200~1000 Hz 运行，这样低频外环视觉修正与高频力控互补，保持贴合稳定。

5. 路径生成与学习增强

• 首次可使用拖动示教或遥操作采集贴合路径，保存为末端位姿序列+接触力曲线；复现时用迭代学习控制（ILC）逐步修正路径，减小力误差。
• 若型号固定，可事先三维扫描生成STL模型，离线规划路径，通过仿真验证可达性，实机运行时再用力控/视觉修正。

三、实际系统配置建议

• 传感器：末端六维力/力矩传感器（量程±100 N、±5 Nm，分辨力0.01 N）+ 末端微型RGB-D模组（防护、防雾）+ 关节绝对值编码器 + 底盘2D/3D激光定位。
• 控制架构：外环（50~200 Hz）——视觉估计曲面法向、路径重规划、冗余姿态优化、底盘漂移补偿；内环（1 kHz）——导纳控制、力回路、关节伺服。
• 两个俯仰自由度可以设置不同控制模式：大行程俯仰参与姿态粗调和零空间优化，小行程俯仰可配置为直接力矩控制或高频导纳修正，像“手腕柔顺”一样隔离剩余高频姿态误差，缓解大臂惯量影响。
• 安全层：实时监测关节力矩与末端力，超过阈值立即切换为反向驱动或停止，确保对釉面及环境人机安全。