机械臂串联关节，柔顺控制策略

对于具备同一方向不同行程俯仰的关节，以机械臂在末端有两个同向俯仰关节（J5：大行程，J6：小行程）为例，这创造了一个非常有价值的运动学冗余。合理分配和利用这个冗余，是提升贴合柔顺性、提高效率的关键，下面介绍运动学分配原则、Pinocchio 配置方法、以及清洁场景的最佳实践

一、运动学分配的核心原则：宏动 + 微动

两个同方向俯仰轴可以合成一个总俯仰角 θtotal=θ5+θ6，但它们的物理特性完全不同，因此必须按功能分工：

分配原则：

• 准静态曲面跟随：让 J5 承担大部分角度，J6 接近其自身行程中间点，这样 J6 两边都有充足的运动裕量。
• 动态力控贴合：导纳控制器输出的高频姿态修正量（基于力误差）只发送给 J6，对于J5 保持静止或以极慢速度调整平均臂形。
• 零空间优化：在逆运动学求解时，利用冗余，最小化 J6 偏离中心的距离（

），同时满足末端姿态约束。

二、在 Pinocchio 中设置

Pinocchio 基于 URDF 或直接构建模型，支持这种串联冗余关节。

1. URDF 定义

按实际机械结构顺序定义关节和连杆：

<!-- 前面 SCARA 的 J1~J4 ... -->
<joint name="joint5" type="revolute">
  <axis xyz="0 1 0"/>  <!-- 俯仰轴方向，与J6一致 -->
  <parent link="link4"/>
  <child link="link5"/>
  <limit lower="-1.5" upper="1.5" effort="..." velocity="..."/>
</joint>
<joint name="joint6" type="revolute">
  <axis xyz="0 1 0"/>  <!-- 与J5同向 -->
  <parent link="link5"/>
  <child link="link6"/>
  <limit lower="-0.3" upper="0.3" /> <!-- 小行程 -->
</joint>
<!-- 末端工具固连在 link6 之后 -->

这样 Pinocchio 的正运动学自动包含两个俯仰角的总和效果。

2. 逆运动学求解中的冗余处理

Pinocchio 没有内置的带零空间优化的 IK 求解器，但可以基于它提供的雅可比矩阵实现，基本实现逻辑（数值迭代法）：

• 定义任务空间误差：末端位置和姿态（至少保证工具 Z 轴对齐目标法向）。
• 计算任务雅可比 J_task（通过 pinocchio.computeFrameJacobian 或全模型雅可比选取相关行）。
• 使用阻尼最小二乘（DLS）或加权伪逆求解关节增量：

其中零空间速度 q˙0 用于实现次任务。

• 保持 J6 居中：q˙0=k⋅∇(代价)，代价函数为

，梯度只在 J6 维度非零。

• 避免关节限位：可加入 J5 的限位惩罚项。
• 最小化运动：也可直接最小化各关节速度的范数。

具体到 Pinocchio 实现：

1. 用 pinocchio.computeJointJacobians(model, data, q) 获得全雅可比。
2. 提取任务相关的 5 行（3 位置 + 2 姿态，因为绕工具 Z 轴的旋转通常不需要约束）。
3. 用 np.linalg.pinv 或自己实现 DLS 求解。
4. 将零空间投影项作为额外的关节增量叠加上去。

这样，就在 Pinocchio 上构建了既能满足主任务（末端姿态对齐法向），又能利用 J6 居中优化冗余的 IK。

三、清洁场景注意事项

1. 贴合控制

彻底解耦宏动与微动，是最核心的实践，在进入恒力贴合模式后：

• J5 切换到慢速位置保持或速度控制：根据视觉或先验曲率，以低频（如 2~5 Hz）缓慢调整 J5，使其跟随曲面大致法向，保持 J6 在中心附近。
• J6 单独负责导纳输出的姿态修正：导纳控制器输出一个绕俯仰轴的姿态修正角速率 Δθ˙_adm，直接叠加到 J6 的速度或位置指令上。由于 J6 惯量小，力控带宽可以做到很高（>20 Hz），对釉面的微小起伏和力扰动响应极快。
• 接口清晰：在控制逻辑中，力控外环只输出给 J6，而路径规划/曲面前馈输出给 J5，两者不打架。

2. 曲面切换时的协同

当清洁头从座垫（平坦）过渡到内壁（大倾角）时，仅靠 J6 可能不够。

• 前馈引导：利用前方曲面估计（视觉或力觉），提前计算所需的总俯仰角变化，将其中的大部分分配给 J5，J6 保持中线附近。
• 平滑过渡：J5 的粗调指令经过低通滤波，避免激起惯性冲击；J6 同时做相反的补偿运动，保证末端总角度连续平稳，工具不抖动。

3. 利用冗余增强安全

• 限位管理：在零空间优化中加入强惩罚，确保 J6 绝不触及机械限位。一旦 J6 被迫接近极限，立即通过 J5 的慢速运动来解救，使 J6 回到安全区域。
• 碰撞缓冲：若清洁模组意外撞到边缘，J6 的低惯量使其能迅速后退（导纳效应），同时 J5 可保持不动，碰撞动能被最大限度吸收，保护釉面。

4. 姿态保持与覆盖优化

在需要工具 Z 轴严格对齐法向的任务中，总俯仰角是刚性约束，但有时为了提高清洁覆盖，可能需牺牲一点法向对齐来换取可达性（如内壁拐角）。此时可放松姿态约束（例如用加权矩阵），允许小范围姿态误差，此时 J5、J6 可共同分担总角度，通过零空间优化找到既不违反限位又能保证接触的配置。

5. Pinocchio 中的实时运行建议

• 在控制循环（如 1 kHz）中：
  • 用当前的 q 更新 Pinocchio 数据。
  • 计算任务雅可比，运行上述 IK 得到目标 q_des。
  • 将 q_des作为位置指令下发给关节伺服（J5、J6 分别有自己的目标）。
  • 导纳环输出的微动角度直接加到 J6 的目标值上（在 IK 输出之后叠加，或作为扰动前馈）。
• 由于 Pinocchio 的雅可比计算极快，该流程完全能在 1ms 内完成。

四、总结

• 运动学分配：J5 宏动粗调，J6 微动柔顺，利用零空间优化让 J6 保持居中。
• Pinocchio 设置：URDF 定义两个连续同向俯仰关节，编写带零空间优化项的逆运动学求解器，利用 Pinocchio 的雅可比实现冗余解析。
• 最优实践：贴合时彻底解耦——J5 慢速跟随，J6 高频力控，两者通过前馈和零空间协调，达到“大臂稳、手腕灵”的仿人效果。

这样，双俯仰机构就不再是简单的串联，而是一套高带宽宏微结合的柔顺贴合系统，完美适配曲面清洁的需求。