4. 经典力位混合控制框架：Raibert-Craig算法
聊到力位混合控制，就绕不开Raibert-Craig算法。说实话，这个算法在机器人控制领域，就像经典力学里的牛顿定律一样——基础、优雅、但有时候也会让人头疼。
我记得刚入行那会儿，第一次在六轴机器人上跑这个算法，结果力控方向直接震荡到报警。后来才明白，不是算法不好，是我对它的理解太浅了。今天咱们就把这个框架掰开揉碎了讲清楚。
4.1 核心思想：选通矩阵
Raibert-Craig算法的精髓，说白了就是一句话：在任务空间里，把自由度分成两拨——一拨控位置，一拨控力。
你想想看，机械臂末端执行器在拧螺丝的时候，沿着螺丝轴向要用力控（防止拧坏螺纹），垂直于轴向要用位置控（保证对准）。这两种需求同时存在，怎么处理？
答案就是选通矩阵 S。它是一个对角矩阵，对角线元素是0或1：
- S = 1：这个自由度走位置控制
- S = 0：这个自由度走力控制
关键公式：
τ = J^T [ S * (位置控制律) + (I - S) * (力控制律) ]其中 J 是雅可比矩阵，τ 是关节力矩指令。
我在项目中遇到过一种情况：操作员想同时控制末端的位置和接触力，但选通矩阵设计得不好，导致力控方向上的位置误差一直在累积。嗯，这里要注意——选通矩阵不是随便设的，它必须和任务几何严格对应。
4.2 内环/外环结构
实际工程实现时，我习惯把Raibert-Craig算法拆成两层：
4.2.1 外环：任务规划层
外环负责两件事：
- 根据选通矩阵 S，生成位置控制的目标轨迹
- 根据 (I - S)，生成力控制的目标力/力矩
说白了，外环就是告诉机器人：「你在这个方向上要走到哪，在那个方向上要用多大力」。
4.2.2 内环：伺服执行层
内环是真正的「干活」层。它接收外环的指令，通过关节伺服驱动电机。内环通常跑在1kHz以上的频率，而外环可以慢一些，100Hz左右就够了。
我曾经调试过一个打磨机器人，外环频率设得太高（500Hz），结果力控指令频繁跳变，内环根本跟不上。后来把外环降到100Hz，内环保持2kHz，系统就稳了。这个经验值你可以记一下。
我的建议：内环用PD控制+重力补偿，外环用阻抗控制或直接力控制。这样内外分工明确，调试起来也方便。
4.3 稳定性分析
稳定性是力位混合控制的老大难问题。为什么？因为力控本质上是和环境交互，环境刚度一变，系统特性就变了。
我总结了三类常见的失稳模式：
| 失稳类型 | 原因 | 表现 |
|---|---|---|
| 高频振荡 | 力控增益过高 + 环境刚度过大 | 末端高频抖动，声音尖锐 |
| 低频漂移 | 力控积分项累积误差 | 接触力缓慢增大或减小 |
| 耦合失稳 | 位置控和力控方向耦合 | 一个方向震荡引发另一个方向失控 |
避坑指南：我曾经在装配机器人上遇到过耦合失稳。原因是选通矩阵S没有和实际任务坐标系对齐——力控方向其实和位置控方向有5度的偏差。这5度在仿真里看不出来，一上真实硬件就炸了。所以，一定要做坐标系标定。
4.4 稳定性判据
从控制理论角度，Raibert-Craig算法的稳定性取决于两个条件：
- 被动性条件：力控环路的阻抗必须是被动的（passive），即能量不会凭空产生。说白了，就是力控增益不能太大，否则系统会「自激」。
- 解耦条件：位置控和力控的带宽要拉开差距。我一般让位置控带宽是力控带宽的3-5倍。这样两个环路不会互相干扰。
你想想看，如果位置控和力控的响应速度差不多，那它们就会「打架」——位置控想快速到位，力控想缓慢施力，结果就是震荡。
4.5 一个简单的实现示例
下面是一个简化版的Raibert-Craig算法伪代码。注意，这只是教学演示，实际工程中要考虑更多细节：
// 伪代码：Raibert-Craig 力位混合控制
// 输入：期望位置 xd, 期望力 Fd, 选通矩阵 S
// 输出：关节力矩 tau
void raibert_craig_control(Vector3 xd, Vector3 Fd, Matrix3 S) {
// 1. 获取当前状态
Vector3 x = get_end_effector_position();
Vector3 dx = get_end_effector_velocity();
Vector3 F = get_force_sensor_reading();
// 2. 位置控制律（PD）
Vector3 pos_error = xd - x;
Vector3 pos_control = Kp * pos_error + Kd * (0 - dx);
// 3. 力控制律（PI）
Vector3 force_error = Fd - F;
Vector3 force_control = Kf_p * force_error + Kf_i * integral(force_error);
// 4. 选通矩阵合成
Vector3 task_control = S * pos_control + (I - S) * force_control;
// 5. 映射到关节空间
Matrix3 J = compute_jacobian();
Vector3 tau = J.transpose() * task_control;
// 6. 发送力矩指令
send_torque_command(tau);
}注意：上面的代码里，力控用了积分项。我建议你在实际使用时，给积分项加一个限幅（anti-windup），否则碰到硬墙时积分会疯狂累积，导致「积分饱和」——我吃过这个亏。
4.6 本章小结
Raibert-Craig算法虽然经典，但它有个隐含假设：环境是刚性的。如果环境是柔性的（比如打磨海绵、装配橡胶件），这个框架就需要做调整。不过，作为理解力位混合控制的起点，它绝对值得你花时间吃透。
我个人习惯在调试这个算法时，先用仿真跑一遍，重点观察选通矩阵切换瞬间的力/位置跳变。如果仿真里都跳得厉害，那实机上肯定更糟。
好了，关于Raibert-Craig算法就聊这么多。记住：选通矩阵是灵魂，内外环分工是骨架，稳定性是底线。把这三点抓住了，这个框架你就掌握了八成。