机械臂泛化性轨迹规划方法

将清洁轨迹定义为一组参数化的运动基元，参数由快速、粗略的在线感知（视觉或力觉）估计并实时更新，轨迹生成器的适应性由参数和导纳层共同完成。

一、总体框架

1. 通用轨迹生成器：根据结构共性（无论何种款式，都有内壁环形曲面、座圈平面、外壁凸曲面等），预定义几类参数化轨迹模板。
   • 内壁 → 圆柱/圆锥螺旋线
   • 座圈 → 平面往复弓字形
   • 外壁 → 竖直往复弓字形
   • 面板 → 矩形区域弓字形
2. 粗补偿（在线参数更新）：在执行某区域清洁前，利用基座深度相机（稀疏点云或轮廓）或末端力觉，快速估计该轨迹模板所需的几何参数（中心轴位置、半径范围、高度范围、平面边界等），并实时更新轨迹生成器的参数。
3. 精调执行：机械臂沿更新后的轨迹运动，同时末端导纳控制保持恒力贴合，自动吸收曲面局部误差和动态扰动。

二、轨迹规划方法

下面几种方法均能生成连续平滑的覆盖轨迹，且只需少量在线可测参数即可泛化。

1. 参数化基元轨迹（首选）

基于近似几何基元（圆柱面、圆锥面、平面等），直接写出轨迹的参数方程。

• 内壁螺旋线 轨迹方程（在圆柱坐标系下，中心轴为 Z 轴）： θ(t)=θ_0+ω_t z(t)=z_0+v_zt r(t)=R （固定半径，或根据预估曲面缓慢变化） 转换为笛卡尔坐标后，工具法向设为指向中心轴。 需要估计的参数：中心轴线（原点、方向）、起始/终止高度、螺旋半径 RR、螺距（由刷头宽度决定）。
• 座圈弓字形 在座圈平面上定义二维扫描路径：沿长轴往复，行距固定。 平面方程和边界由四个角点或轮廓多边形定义。 需要估计的参数：座圈平面方程、扫描边界（二维多边形）。
• 外壁竖直弓字形 将外壁投影到一个近似圆柱面或直接使用竖直平面扫描，路径点保持工具法向大致指向表面。 需要估计的参数：外壁轮廓（在某一高度的水平截面曲线）、清洁高度范围。

粗补偿：利用基座深度相机获取稀疏点云，通过 RANSAC 拟合圆柱（内壁）、平面（座圈）、或轮廓多边形（外壁），提取上述参数，实时更新轨迹方程。即使点云稀疏、有孔洞，也可得到足够精度的参数（厘米级），剩余误差由导纳处理。

2. 基于视觉特征的引导轨迹

不以绝对几何坐标为参照，而是让轨迹生成器跟踪图像或点云中的显著特征。

• 中心轴在线锁定：末端相机持续检测内壁边缘或排水口，估计其中心，引导螺旋轨迹自动居中。
• 边缘引导：对于座圈缝隙、面板边框，通过边缘检测+视觉伺服，生成沿边缘的轮廓跟随轨迹。
• 区域光流覆盖：将一个虚拟的覆盖波在图像中推进，解算出对应的末端运动。

粗补偿：轨迹生成器输出的是一个相对运动指令（如保持该特征在图像中心且沿边缘方向移动），而不是绝对坐标。特征的在线位置直接补偿了不同几何差异，无需显式估计参数。适合视觉特征较稳定、但几何建模困难的区域（如接缝、按键缝隙）。

3. 动态系统/向量场轨迹生成

为清洁区域构造一个光滑向量场，使积分曲线自然形成覆盖图案，同时可叠加障碍物排斥项。

• 极限环吸引子：设计一个绕中心轴旋转、缓慢下降的吸引子，产生光滑螺旋，且对于中心偏移具有自动收敛性。
• 往复扫描场：定义一维周期性方向场 + 横向偏移项，生成弓字形路径。
• 自适应边界排斥：加入根据力反馈或稀疏点云计算的表面接近/远离项，使轨迹始终保持在接触或近接触状态。

粗补偿：向量场参数（中心点、轴线、扫描间距）由粗略感知在线估计。由于动态系统本身具有吸引特性，即使初始参数有误差，轨迹也会自动调整到稳定覆盖模式。非常适合处理曲率变化、边界不明确的区域。

4. 虚拟夹具引导

定义虚拟的几何约束（管道、曲面），将机械臂末端约束在其上运动，约束本身构成通用轨迹。

• 内壁虚拟漏斗：创建一个与内壁形状近似的虚拟圆锥/圆柱面，末端被约束在该面上并沿螺旋路径滑动。
• 座圈虚拟平面：定义一个虚拟平面，末端被约束在该平面上做弓字运动，平面的高度和倾角由感知估计。
• 外壁虚拟轮廓：从基座相机提取外壁轮廓，生成一个沿轮廓拉伸的虚拟直纹面，末端沿竖直方向扫描。

粗补偿：虚拟夹具的参数（圆锥角度、高度、平面方程等）由在线感知快速估计并更新，机械臂执行时，位置控制使末端保持在虚拟夹具上，导纳则处理与真实表面之间的残余距离和力。

5. 可变形模板轨迹

离线建立一个平均的参数化形状模型（如采用 superquadrics 或 FFDP 形变模型），在线用稀疏点云或几个关键点拟合，得到当前的近似形状，然后在其上生成标准覆盖轨迹。

• 用 5-10 个参数（口径、深度、倾斜角、座圈尺寸等）描述内部，在线拟合后，在其等距面上生成螺旋/弓字形路径。
• 粗补偿由模型参数辨识完成，泛化能力取决于模型自由度。

该方法需要一定的先验建模工作，但一旦建立，适应能力很强。

三、粗补偿的具体实施

无论采用哪种轨迹生成方法，粗补偿都依赖于实时、粗略的几何信息，获取这些信息的手段：

1. 基座深度相机一次粗扫：在清洁开始前，拍摄 2-3 帧稀疏点云，拟合出内壁圆柱参数、座圈平面、外壁轮廓。由于只需厘米级精度，即使反射强、点云稀疏（几百个点即可），RANSAC 拟合也足够稳定。
2. 触觉边界快速探索：在清洁内壁前，让工具在力控模式下以极低压力接触内壁不同方位（4~8 个点），通过触碰点拟合圆柱中心、半径和高度范围。
3. 末端相机近距辅助：当工具趋近某个区域时，末端相机采集局部曲面图像或稀疏深度，修正该区域的轨迹参数（如局部半径、倾角）。
4. 迭代更新：在清洁过程中，可利用导纳控制器的位置修正量来估计实际曲面与当前轨迹的偏差，并缓慢调整轨迹参数（如在螺旋过程中微调半径），实现持续的粗补偿。

四、总结

对于复合机器人，建议采用参数化基元轨迹 + 基座相机/力觉混合参数估计作为核心方案：

• 内壁清洁：基座相机快速拟合圆柱参数（中心、半径、深度），生成螺旋轨迹；螺旋半径可设为估计半径的 95%，由导纳力控自然压紧到真实表面。
• 座圈清洁：基座相机提取座圈平面轮廓，生成弓字形轨迹；轨迹略大于轮廓，边缘处力控会自然悬空或触碰边缘后回退，避免硬接触。
• 外壁清洁：基座相机获取外壁水平截面轮廓，生成竖直往复轨迹。
• 面板清洁：末端相机识别面板边框和按键，生成避让弓字形路径。

方法的优势：

• 轨迹生成器完全通用，所有差异浓缩为几个几何参数。
• 参数获取方式鲁棒，不依赖高精度点云，厘米级误差可被导纳完美吸收。
• 动作流畅，一次生成完整路径，无需在线重新规划。
• 工程实现简单，易于在 Pinocchio + ROS 框架下模块化开发。

最终，通过 通用参数化模板 → 在线粗估计 → 导纳精贴合 三层架构，就能以一套代码适应几乎所有型号，实现高效、稳定的全覆盖清洁。