第25章 力控插补：力传感器接口、力位混合控制、柔顺控制

各位同学，今天我们来聊聊力控插补。说实话，这个主题在运动控制里算是比较“高阶”的内容了。我刚开始做机器人控制那会儿，一听到“力控”两个字就觉得头大——位置控制还没搞明白呢，怎么又来一个力？

但后来我明白了。你想想看，一个机械臂如果只会“硬碰硬”地走位置，那它就是个莽夫。真正的智能，在于知道什么时候该用力，什么时候该收力。这就是力控插补要解决的问题。

25.1 力传感器接口：让机器“有感觉”

力控的第一步，是让控制器能“感知”到力。这就像人没有触觉，就没办法拿鸡蛋一样。

25.1.1 常见的力传感器类型

我接触过的力传感器主要有这么几类：

• 六维力传感器：能测Fx、Fy、Fz三个力和Mx、My、Mz三个力矩。装在机器人手腕处最常用。
• 单维力传感器：只测一个方向的力，比如末端夹爪的夹持力。
• 扭矩传感器：装在关节处，测关节力矩。协作机器人用得比较多。

我个人习惯在机器人末端装六维力传感器，因为信息最全。但要注意——六维力传感器贵啊，一个进口的动辄几万块。我在一个项目里为了省钱用了国产的，结果温漂大得离谱，最后还得换回来。嗯，有些钱真不能省。

25.1.2 传感器数据读取与滤波

力传感器接口，说白了就是怎么把传感器的原始数据读进来，变成可用的力信息。

典型的读取流程：

1. 通过SPI/I2C/CAN总线读取原始数据
2. 进行零点校准（减去空载时的读数）
3. 进行温度补偿（如果传感器支持）
4. 应用低通滤波器（去除高频噪声）

这里我特别想强调滤波。力传感器天生噪声大，尤其是电机振动会耦合进来。我曾经在一个打磨项目里，力信号毛刺特别多，导致机器人一直在抖。后来加了个截止频率20Hz的巴特沃斯低通滤波器，问题就解决了。

25.2 力位混合控制：左手抓位置，右手抓力

力位混合控制，说白了就是“在需要控位置的地方控位置，在需要控力的地方控力”。

为什么会需要这个？你想想看，机器人去擦玻璃——沿着玻璃表面移动需要控位置（走直线），但垂直压向玻璃的方向需要控力（不能压太紧，也不能离开）。

25.2.1 自然约束与人工约束

这里有个经典的理论框架，叫“自然约束”和“人工约束”。

约束类型	含义	例子
自然约束	由环境几何决定的约束	接触面法线方向不能穿透
人工约束	人为设定的运动/力轨迹	沿接触面切向以速度v运动

力位混合控制的核心思想就是：在自然约束的方向上控力，在人工约束的方向上控位。

25.2.2 选择矩阵法

实现力位混合控制，最经典的方法是用选择矩阵S。S是一个对角矩阵，对角线元素为1表示该方向控位置，为0表示该方向控力。

// 力位混合控制伪代码
// 假设在任务空间有6个自由度（3个位置+3个姿态）

// 选择矩阵S：1=位置控制，0=力控制
float S[6] = {1, 1, 0, 1, 1, 1};  
// 比如：Z方向控力，其他方向控位置

// 位置控制律
float pos_error[6] = {0};
float pos_output[6] = {0};
for (int i = 0; i < 6; i++) {
    if (S[i] == 1) {
        pos_error[i] = target_pos[i] - current_pos[i];
        pos_output[i] = Kp_pos[i] * pos_error[i];
    }
}

// 力控制律
float force_error[6] = {0};
float force_output[6] = {0};
for (int i = 0; i < 6; i++) {
    if (S[i] == 0) {
        force_error[i] = target_force[i] - current_force[i];
        force_output[i] = Kp_force[i] * force_error[i];
    }
}

// 最终输出 = 位置控制输出 + 力控制输出
float final_output[6] = {0};
for (int i = 0; i < 6; i++) {
    final_output[i] = pos_output[i] + force_output[i];
}

25.3 柔顺控制：让机器人学会“顺着来”

柔顺控制，说白了就是让机器人表现出“弹性”。你推它，它让一让；你拉它，它跟一跟。

这和力位混合控制有什么区别？力位混合是“硬切换”——这个方向控力，那个方向控位。而柔顺控制是“软调节”——通过调节刚度，让机器人在所有方向上都表现出一定的柔顺性。

25.3.1 阻抗控制

阻抗控制是最常用的柔顺控制方法。它的核心思想是：把机器人模拟成一个“质量-弹簧-阻尼”系统。

数学表达式很简单：

M * (x_ddot - x_ddot_d) + B * (x_dot - x_dot_d) + K * (x - x_d) = -F_ext

其中：

• M：惯性矩阵（质量）
• B：阻尼矩阵
• K：刚度矩阵
• x_d：期望位置
• F_ext：外部施加的力

你想想看，如果K很大，机器人就很“硬”，稍微偏离期望位置就会产生很大的回复力。如果K很小，机器人就很“软”，外力一推就跑。

25.3.2 刚度参数的整定

阻抗控制里，参数怎么调？我分享一个我自己的经验：

1. 先调刚度K：K决定了机器人的“硬度”。装配任务K取小（500-1000 N/m），搬运任务K取大（5000+ N/m）。
2. 再调阻尼B：B决定了机器人的“粘性”。B太小会震荡，B太大响应慢。一般取B = 2 * sqrt(M*K)（临界阻尼）。
3. 最后调惯性M：M影响动态响应。M太大，机器人反应迟钝；M太小，容易受噪声影响。

25.4 力控插补的RTOS实现

好了，理论讲完了，咱们看看在RTOS里怎么落地。

25.4.1 任务划分

我通常把力控插补分成三个任务：

任务名称	优先级	周期	功能
力传感器读取任务	高	1ms	读取传感器、滤波、校准
力控插补计算任务	中	2ms	力位混合/阻抗控制计算
轨迹规划任务	低	10ms	生成期望轨迹、更新目标

这里要注意：力传感器读取任务必须保证实时性。如果读取延迟超过2ms，力控的稳定性就会下降。我在FreeRTOS里把这个任务的中断优先级设得最高，并且用DMA方式读取SPI数据，尽量减少CPU占用。

25.4.2 力控插补的整体流程

下面这张图是我自己画的力控插补流程，你可以参考一下：

25.5 实战经验总结

最后，我把自己这些年做力控插补踩过的坑总结一下：

• 传感器安装要刚性：力传感器和机器人末端之间如果有间隙，力信号会失真。我见过有人用橡胶垫片减震，结果力控完全没法用。
• 重力补偿必须做：机器人姿态变化时，传感器自身的重力分量会变化。不做补偿的话，力控会“飘”。
• 力控增益不能太大：力控的增益如果太大，系统容易不稳定。我一般从很小的增益开始调，慢慢往上加。
• 注意任务优先级：在RTOS里，力传感器读取任务的优先级一定要高于其他任务。否则一旦有任务卡住，力控就会断档。

---