机器人清洁轨迹规划，使用圆柱坐标系

在视觉点云因釉面反射、水雾遮挡、管路盲区而不可靠时，规划清洁轨迹的核心思路需要从 “精确几何复现” 转向 “约束空间覆盖 + 力觉自适应” ，可以使用一种或几种通用轨迹模板，能够以不变应万变，实现对不同设备的高效清洁。

一、轨迹规划方案分类（从依赖模型到模型无关）

二、恒力贴合的盲清洁轨迹

可以设计基于近似几何基元（圆柱/椭圆柱/锥面）的参数化螺旋或往复轨迹，并结合力控保持恒力贴合。这种轨迹对所有常见都适用，因为它抓住了一个共同特征：内壁和座圈都是一个连续、近似回转的曲面，即使曲率变化，整体呈环形或半环形。

1. 环形螺旋线（适合内壁周围）

• 原理：将内壁抽象为一个近似圆柱或圆锥面，机械臂末端在恒定高度（或逐步下降）的同时，绕中心轴线做圆周运动，形成螺旋线。
• 关键参数：
  • 中心轴线位置与方向（由基座相机粗定位或手动设置，或通过力觉探测找到边缘来估计）
  • 螺旋半径范围（由尺寸预估，或通过力觉触碰内外边界自适应确定）
  • 螺距（螺旋线每圈下降的距离，取决于刷头覆盖宽度）
  • 切向速度
• 贴合保持：由导纳控制负责法向力恒定，即使实际曲面偏离理想圆柱，力觉会实时调整工具进退，保证接触。

2. 往复折线/回形线（适合座圈、较平坦的曲面）

• 原理：在座圈平面的二维投影面上，生成来回扫描的往复路径（类似扫地机的弓字形），路径超出座圈边缘的部分由力控自然悬空或检测边缘后回退。
• 关键参数：
  • 扫描方向与行距
  • 区域边界（可由基座相机粗略框定座圈区域，或触觉探索边界）
• 内壁几乎都是连续曲面，螺旋线可以遍历整个内表面而不需要知道精确方程。
• 力控保证无论曲面如何起伏，刷头始终贴合且压力恒定。
• 螺旋线的中心轴和半径参数，可以在清洁开始前通过一次简单的触觉边界探测自动获取：让机械臂在力控模式下，从中心向外缓慢移动，直到力突然消失（边缘），从而找到边界，依此设定螺旋半径。

三、具体流程

阶段1：获取轨迹基准参数

1. 确定大致位置和中心轴：用基座深度相机（即使反射强烈，也能得到大致轮廓和位置，不需要精细点云）定位包围盒和中心点，或使用触觉中心探测，让清洁模组在力控模式下，以极低期望力（0.5N）轻触内壁不同方位，通过触碰点拟合圆柱中心。
2. 确定边界：在内壁某高度，沿径向向外缓慢移动，当接触力消失时记录边界点，从而估算内壁的最大半径范围。
3. 设定螺旋参数：
   • 给定中心轴线、起始高度、终止高度（由深度设定）、螺距（如刷头宽度的80%）、切向线速度（如50mm/s）。

阶段2：生成螺旋轨迹（运动学正逆解）

• 在圆柱坐标系下生成轨迹点（角度、半径、高度），半径可以先使用常值（如探测到的最大半径），或在一个范围内变化。
• 通过Pinocchio逆运动学将工具位姿（位置+法向对齐）解算为关节角，法向方向设定为指向中心轴（近似垂直内壁）。
• 由于力控会补偿法向偏差，即使设定法向不完美，实际接触时力控会通过J6的微调自动修正。

阶段3：清洁执行（力控与轨迹联动）

• 机械臂沿螺旋轨迹运动，同时开启导纳控制（法向恒力）。
• 如果实际曲面比预估的更凸或更凹，力控会自动推进或回退工具，保持贴合。
• 当到达底部时，可反向螺旋上升，确保覆盖。

阶段4：特殊区域处理

• 凹陷：螺旋线可能无法深入，可在螺旋结束后，单独以更小半径的螺旋或定点往复清洁排污口。
• 座圈：切换为往复弓字形轨迹，座圈平面近似水平，由基座相机提供边界框，工具保持垂直向下贴合，力控保证压力。

四、与视觉的协同

即使精细点云不能用，基座深度相机仍能提供粗粒度几何信息，这足以确定轨迹的基准参数：

• 包围盒：确定中心轴大致位置、内壁的大致深度和口径。
• 座圈区域：通过红外或RGB分割出座圈区域（座圈通常有清晰的边缘和颜色），给出二维扫描边界。
• 障碍物检测：避免机械臂碰撞等。

这比单纯触觉探测更快、更安全，而且不需要稠密点云。

五、方案优势总结

• 通用性强：所有都可以用一个圆柱螺旋+座圈弓字形的组合完成，仅需调整几个参数（中心轴、半径范围、深度），这些参数可通过简单的触觉或粗视觉自动获取。
• 无视觉盲区影响：轨迹生成不依赖在线点云，力控保证贴合。
• 高效稳定：螺旋轨迹运动平滑，无需频繁加减速，对电机和釉面友好。
• 易于实现：在Pinocchio里只需实时计算螺旋线上的目标位姿，逆解后发给机器人，导纳环处理力修正。

一句话结论：在点云不可靠的清洁场景，参数化螺旋/弓字覆盖 + 导纳恒力贴合就是通用轨迹方案，把复杂的曲面跟踪问题，转化成了简单的圆柱坐标系的几何基元逼近与力觉自适应问题，是实现多型号通用清洁的解。