复合机器人自主清洁的技术路线

索旭东

发布于 2026-07-13 20:21:03

1360

文章被收录于专栏：具身小站具身小站

本方案按照从标定到清洁执行的完整流程，覆盖了运动学/动力学、力觉感知、控制策略、任务规划及模组切换等全部核心环节。

一、算法架构与逻辑流程

整个系统以状态机串联四阶段：系统标定、任务规划与定位、清洁执行（分阶段控制）、安全与质量管理，核心逻辑如下：

标定阶段：确定机器人自身参数和传感器零位。
粗定位阶段：利用底盘激光雷达和机械臂视觉/Aruco码，将工具送至目标面附近。
精定位阶段：基于六维力传感器的触觉探测，找到距离釉面精确 5mm 的“柔性逼近起点”。
贴合与清洁阶段：采用位置控制高速趋近 → 导纳控制低速柔性逼近 → 导纳恒力贴合与曲面跟随的分段策略，并依据模组类型和接触面特性，调用不同清洁轨迹和力控参数。
退出与转场：完成一个区域后，安全脱附并移动至下一区域，直至全清洁完毕。

二、标定（一次性或定期执行）

本阶段所有操作均在工具无接触、机器人静止（底盘锁死）条件下进行。

步骤	输入/设备	算法/方法	输出/参数	工程落地要点
传感器零位校准	六维力传感器悬空静止数据	采集500ms数据取平均，作为当前零漂向量 F_bias, M_bias	零漂值	每次上电、每30分钟或温度变化超过5°C时自动触发；设计固定的“校零姿态”
工具负载辨识	6~8个不同倾斜姿态下的关节角 q_i 及传感器读数 F_raw, M_raw	最小二乘法线性系统辨识，求解 m, r_com	工具总质量 m，质心在传感器坐标系下的位置 r_com	必须包含传感器下安装面以下全部部件；管路内液体状态需一致
工具惯量获取	CAD装配体模型（以传感器下安装面为原点）	质量属性读取或平行轴定理近似	惯性张量 I_sensor（6个独立分量）	若用质点近似，低速清洁可接受
运动学参数标定	关节实际臂长、零点偏移等；激光跟踪仪或拉线编码器（可选）	修正DH参数，消除加工装配误差	精确的URDF模型	即使使用Pinocchio自动生成正逆解，也需输入真实几何参数
六维力传感器安装偏差标定	让机械臂末端法兰精确竖直向下（由水平仪保证），悬空读取力数据	计算使非竖直轴力归零的旋转修正矩阵 R_align	安装偏差补偿矩阵	集成到重力补偿的姿态计算中
关节摩擦参数辨识（可选）	各关节单独以多种恒定速度运动，记录驱动器力矩	线性回归拟合 τ_f = f_csign(ẋ)+ f_vẋ	库仑摩擦系数 f_c，粘滞系数 f_v	用于未来力矩前馈，当前可不做

三、任务规划与全局定位（在线实时）

步骤	输入/设备	算法/方法	输出/效果
全局地图构建	移动底盘2D激光雷达	SLAM构建环境地图，识别轮廓，拟合其中心轴和朝向	坐标系在全局地图中的位姿
底盘粗定位	上一步结果，机械臂关节编码器	将底盘导航至预设的清洁站点（如正前方），伸出刚性支腿锁死悬架	机械臂基座与清洁面建立固定的空间关系
末端工具视觉辅助定位（可选）	末端RGB-D相机或Aruco码	识别标记或直接点云匹配，获取目标区域的大致位姿	引导大俯仰关节将末端送至安全区域（上空10cm处）
触觉自定位（核心）	六维力传感器，柔性小俯仰关节	力觉探测：设定期望接触力0.5N，以极慢速度(5mm/s)逼近釉面，接触后回退5mm	精确的柔性逼近起点位姿 P_start，绝对误差 <2mm

四、力信号实时预处理（1kHz）

每一控制周期，对六维力传感器原始数据执行以下管线，输出纯净的接触力/力矩 F_contact, M_contact：

零漂去除：F = F_raw - F_bias
温漂补偿（若有温度传感器）：F -= k_T * (T - T_ref)
重力补偿：利用 m, r_com 和当前姿态 R 计算 F_g, M_g，扣除。
惯性补偿：利用 m, r_com, I_sensor 和运动量 a, ω, α 计算 F_i, M_i，扣除。
陷波滤波（若有固定转动）：自适应滤除刷头转频。
低通滤波：截止频率30~50Hz的二阶巴特沃斯滤波器。

五、清洁执行的阶段控制算法

将每个区域的清洁过程划分为四个阶段，采用不同的控制模式。

1 高速趋近阶段

目标：从当前位置快速到达 P_start 点。
控制模式：纯关节空间位置控制。
算法：利用Pinocchio逆运动学求解关节角，使用S形速度规划，以最大速度的70%安全趋近。大俯仰负责粗调，小俯仰保持居中。

2 柔性低速逼近阶段

目标：从 P_start 到首次接触釉面，实现零冲击贴合。
控制模式：导纳控制（F_des=0，叠加法向速度前馈）。
算法：
- 激活导纳控制器，参数：K=0，D大（过阻尼），M中等。
- 在末端工具坐标系Z轴方向，施加一个缓慢的恒定速度前馈 v_ff = 8~12mm/s。
- 当检测到 |F_contact_z| > 0.5N 时，判定为可靠接触，立即撤销速度前馈，并将期望力从0斜坡至目标清洁力（如8N），耗时约100ms
- 关节分工：此阶段仅驱动小行程俯仰关节J6，大俯仰J5锁定，充分发挥J6的低惯量高带宽柔顺优势。

3 恒力贴合与曲面跟随阶段

目标：保持恒定法向压力，完整覆盖清洁路径。
控制模式：解耦的宏-微导纳控制。
算法：
- 宏动通道（J5大俯仰）：根据视觉/先验曲率或力反馈平均姿态，以低频（2~5Hz）调整，保持J6在其行程中心附近，实现粗对准。
- 微动通道（J6小俯仰）：负责高带宽（>20Hz）的导纳控制，维持恒力 F_des 并跟随曲面微小起伏。导纳参数：K=0，M稍小、D稍大，保证临界阻尼。
- 切向运动：由SCARA的X,Y轴和升降轴执行位置控制，按清洁路径以恒定线速度移动，保证全覆盖。
- 姿态对齐：工具Z轴需实时对齐曲面法向，此姿态约束由J5和J6的总俯仰角满足，冗余通过J6居中优化解决。

4 安全退出阶段

目标：完成区域清洁后，无冲击脱离釉面。
算法：
- 将期望力从目标值斜坡降至0，同时叠加一个反向速度前馈，使工具柔顺回退5mm。
- 回退后，切换回位置控制模式，快速升至安全转移高度。

六、不同接触面的清洁策略与模组切换

接触面类型	清洁模组特性	导纳/力控参数	轨迹策略
不接触人面	硬质刷头，可辅助清洁液喷射；纯机械固定，无电机	法向力 2.5~3.5N，略高以增强去污；M稍大，D稍大，过阻尼	螺旋线或来回往复路径；在拐角处降速至30%；允许牺牲微小姿态对齐以换取可达性
人接触面	超细纤维软布或海绵模组，可能带轻微旋转电机；力控更灵敏，防划伤	法向力 1.5~2.0N，极轻柔；M小，D大，高度过阻尼	沿座圈内外轮廓的平行轨迹；座圈翻转后清洁背面；严格保持工具Z轴垂直于表面

模组自动切换流程：

机械臂移动至工具架上预设的更换位。
执行快速接头释放/锁紧（气动或电磁），电气接口自动对接。
重新执行负载辨识更新（因惯量和质心改变），或从预设参数库加载该模组的 m, r_com, I_sensor。
执行一次新的触觉自定位，修正 P_start。

七、安全与质量保证

实时碰撞保护：当 |F_contact| 或 |M_contact| 超过安全阈值（如15N）时，立即停止运动并反向柔顺撤离；监测各关节电流/力矩，防止臂杆意外碰撞。
传感器故障诊断：连续若干周期未收到传感器数据或数值异常跳变，触发急停并报错。
清洁质量评估：利用末端相机在清洁前后拍摄釉面图像，通过图像差分或机器学习模型判断洁净度，对未达标区域进行补擦。
管路状态监控：监测清洁液流量和压力，确保有效供应。

八、状态机总结

[系统启动] → [校零与负载辨识] → [底盘导航] → [粗定位(视觉)]→ [触觉自定位] → [加载目标区域清洁路径] → [高速趋近至P_start]→ [柔性逼近至接触] → [恒力贴合清洁] → [安全退出]→ [转至下一区域] → ... → [返回工具架更换模组] → ... → [全任务完成，回充电桩]