导纳控制器的阻尼矩阵 - 阻尼椭圆

在标准 CSP 模式下，导纳控制器阻尼矩阵的空间几何表示称为阻尼椭圆，用于让机器人末端在不同运动方向上表现出不同的粘滞感，从而在力控贴合任务中实现稳定与敏捷的最佳折中。对于清洁机器人，这是一个非常实用的高级调优手段。

一、目标

在清洁的恒力贴合中，导纳控制器负责将力误差转换为位置修正量。如果没有方向性设计，机器人在所有方向上的阻尼都是一样的，这会导致：

• 法向（接触方向）：如果阻尼太小，接触力会出现振荡或过冲，划伤釉面。
• 切向（清洁移动方向）：如果阻尼太大，曲面跟踪会变得迟钝，力误差纠正慢，容易脱力。

阻尼椭圆正是为了解耦这两个方向的动态特性：

• 法向高阻尼：保证接触稳定，消除振荡，让刷头像被蜂蜜裹住一样，运动平缓。
• 切向低阻尼：允许快速响应曲面变化，让刷头灵活地跟随轮廓移动。

二、原理

导纳控制律（K=0 时）可以写为：

这里的 D 通常是一个 6×6 矩阵，当 D 不是单位矩阵的倍数时，它在各个方向上的阻尼值不同，在三维空间中，从一个点出发，沿不同方向单位速度所需克服的阻力可以画成一个椭球，这就是阻尼椭圆/椭球。

物理意义：

• 椭球的长轴方向表示低阻尼，相同力下运动速度更快。
• 椭球的短轴方向表示高阻尼，运动更粘稠，需要更大的力才能推动。

在贴合任务中，将阻尼椭圆的短轴对齐法向，长轴对齐切平面。这样，力误差在法向会引起较慢的修正（避免冲击），在切向则会引起较快的修正（及时跟上曲面）。

三、优劣势

四、实现步骤

步骤 1：在工具坐标系下设计目标阻尼特性

设定工具坐标系的 Z 轴为法向（接触方向），X、Y 轴为切向，在工具系下，期望的阻尼矩阵为对角阵：

• dz：法向阻尼，设为大值（如 200 Ns/m）。
• dxy：切向阻尼，设为较小值（如 50 Ns/m）。
• 旋转部分同理，可设绕法向的阻尼较大（防止刷头扭转），绕切向轴的阻尼稍小。

步骤 2：实时获取工具到基座的旋转矩阵

通过正运动学，每个控制周期（1kHz）得到工具坐标系相对于基座标系的旋转矩阵 R

R=pinocchio.forwardKinematics(model,data,qactual)

步骤 3：将阻尼矩阵变换到基座标系（或直接在工具系计算）

导纳控制通常在工具坐标系下计算（因为力误差在工具系下表达最自然），可以在工具系下直接应用 Dtool，无需变换。但如果导纳控制器在基座标系下实现，则需要变换：

推荐做法：在工具坐标系下进行导纳计算，这样 Dtool就是常数对角阵，代码简洁。

步骤 4：在导纳控制器中使用方向阻尼

在工具系下计算力误差和导纳修正：

# 力误差（工具系，Z轴法向）
F_err = F_des - F_contact

# 工具系下的阻尼矩阵
D_tool = np.diag([d_xy, d_xy, d_z, d_rxy, d_rxy, d_rz])

# 导纳加速度计算（简化版，仅平动）
# M * dx_ddot + D * dx_dot = F_err
dx_ddot = (F_err - D_tool @ dx_dot) / M
dx_dot += dx_ddot * dt
delta_x += dx_dot * dt

此时，delta_x 的 Z 分量修正会因高阻尼而变得平滑，而 X、Y 分量修正会因低阻尼而更灵敏。

步骤 5：姿态跟随时的阻尼椭圆旋转

如果工具姿态会变化（例如清洁内壁时工具法向始终指向圆心），那么工具系下的常数对角阻尼矩阵自然随工具旋转，始终是法向高阻尼，这正是我们想要的。

无需额外处理：只要导纳计算在工具坐标系下进行，阻尼椭圆自动跟随工具姿态。

步骤 6：参数整定建议

1. 先调试法向阻尼 dz，使之在平直釉面上恒力贴合无振荡（过阻尼）。
2. 再调试切向阻尼 dxy，使之在曲面起伏时能快速跟上、力误差小。
3. 两者比例通常取 dz/dxy≈3∼5开始。

五、清洁系统示例

[六维力传感器] → 工具系力误差 → [阻尼椭圆导纳控制器]
                                      ↓
                                ΔX_tool (Z向慢，XY向快)
                                      ↓
                            叠加到轨迹 → 逆运动学 → CSP指令

这个框架让清洁模组实现一种仿生触感：像手指轻抚曲面一样，法向稳重不冲击，切向灵活不脱开，极大提升了清洁的质量和安全性。