力控算法全面解析：阻抗控制、导纳控制与混合策略
一、为什么需要力控？
传统机器人控制的目标很单纯：让末端执行器准确到达某个位置。这在搬运、焊接这类"不碰东西"的任务里没问题。
但现实世界充满接触：
- 机械臂打磨工件时，力太大会划伤表面，力太小又打磨不净
- 外科手术机器人缝合组织时，多一分力可能划破血管
- 无人机在风中悬停或抓取物体时，需要感知并响应外部力
这时候，纯粹的位置控制就失效了。我们需要让机器感知力、响应力、控制力------这就是力控（Force Control）。
二、力控的本质：力与位移的关系
在讲具体算法之前，先建立一个直觉。
想象你用手指压弹簧：
- 压得越深（位移 x），弹力越大（力 F）
- F = k · x，这就是胡克定律
弹簧天然地将力和位移关联起来了。力控算法的核心思想，就是人为给机器人末端设计一个"虚拟弹簧-阻尼-质量"系统，让它在受力时产生合理的位移响应，或者在运动时产生合理的力。
这个系统的动力学方程是力控的基础，写出来是：
Mx¨+Bx˙+Kx=FextM \ddot{x} + B \dot{x} + K x = F_{ext}Mx¨+Bx˙+Kx=Fext
其中：
- MMM：虚拟惯性（质量）
- BBB：虚拟阻尼
- KKK：虚拟刚度（弹簧系数）
- FextF_{ext}Fext：外部接触力
- xxx：末端位移（相对于期望位置的偏差）
这个方程描述的，就是目标阻抗。
三、阻抗控制（Impedance Control）
3.1 概念
阻抗（Impedance） 在电路里是电压与电流的比值。类比到力控领域，就是力与运动（速度、加速度、位移）的比值关系。
阻抗高 → 外力难以改变运动（刚性大）
阻抗低 → 轻轻一碰就偏移（柔顺性好）
阻抗控制的核心思路：
已知末端受到的外力 FextF_{ext}Fext，计算末端应该产生多大的位移修正量，再让位置控制器去跟踪这个修正后的目标位置。
3.2 控制框图
期望轨迹 x_d ──→ [阻抗模型] ──→ 修正目标位置 x_ref
↑
外部接触力 F_ext
x_ref ──→ [位置控制器] ──→ 关节力矩 τ ──→ [机器人本体] ──→ 实际位置 x3.3 数学推导
目标阻抗方程：
Md(x¨−x¨d)+Bd(x˙−x˙d)+Kd(x−xd)=FextM_d(\ddot{x} - \ddot{x}_d) + B_d(\dot{x} - \dot{x}d) + K_d(x - x_d) = F{ext}Md(x¨−x¨d)+Bd(x˙−x˙d)+Kd(x−xd)=Fext
其中下标 ddd 表示期望值。令位置误差 e=x−xde = x - x_de=x−xd，则：
Mde¨+Bde˙+Kde=FextM_d \ddot{e} + B_d \dot{e} + K_d e = F_{ext}Mde¨+Bde˙+Kde=Fext
从这个方程解出 eee（通过拉普拉斯变换或数值积分），再叠加到期望轨迹上，得到修正后的参考轨迹：
xref=xd+ex_{ref} = x_d + exref=xd+e
3.4 MATLAB 代码示例
matlab
% 阻抗控制位置修正量计算（离散仿真）
% 参数设置
Md = 1.0; % 虚拟质量 [kg]
Bd = 20.0; % 虚拟阻尼 [N·s/m]
Kd = 100.0; % 虚拟刚度 [N/m]
dt = 0.001; % 仿真步长 [s]
T = 2.0; % 仿真时长 [s]
N = T/dt;
% 初始化
e = 0; de = 0; % 位置误差及其导数
e_hist = zeros(1,N);
for i = 1:N
t = i * dt;
% 模拟一个阶跃外力（0.5s 后接触）
F_ext = (t > 0.5) * 10; % 10N 外力
% 阻抗方程离散化（欧拉法）
dde = (F_ext - Bd*de - Kd*e) / Md;
de = de + dde * dt;
e = e + de * dt;
e_hist(i) = e;
end
% 绘图
t_vec = (1:N) * dt;
plot(t_vec, e_hist, 'b', 'LineWidth', 1.5);
xlabel('时间 [s]'); ylabel('位置修正量 e [m]');
title('阻抗控制：外力作用下的位置修正');
grid on;
运行结果：在 0.5 秒接触后，末端位置会柔顺地偏移约 0.1 m（10N / 100 N/m），并在阻尼作用下稳定，不会剧烈反弹。
3.5 关键特性
| 参数 | 增大效果 | 减小效果 |
|---|---|---|
| KdK_dKd（刚度） | 更抗外力，位移小 | 更柔顺，位移大 |
| BdB_dBd（阻尼） | 振荡小，响应慢 | 响应快，但可能振荡 |
| MdM_dMd（惯性） | 响应更迟钝 | 响应灵敏，但难稳定 |
阻抗控制的适用场景： 末端有可靠的力传感器，且底层有稳定的位置控制器（大多数工业机械臂）。
四、导纳控制（Admittance Control）
4.1 与阻抗控制的区别
很多人容易把这两者搞混，用一句话区分：
| 输入 | 输出 | 底层依赖 | |
|---|---|---|---|
| 阻抗控制 | 运动（位置/速度） | 力 | 底层力控（力矩控制） |
| 导纳控制 | 力 | 运动（位置/速度） | 底层位置控制 |
导纳（Admittance）是阻抗的倒数。如果说阻抗控制是"给我一个力，我按刚度关系产生位移"，那导纳控制就是"给我一个力，我按导纳关系修正运动指令"。
一句话记住：阻抗控制需要力矩控制底层（力源），导纳控制需要位置控制底层（运动源）。
4.2 控制框图
期望位置 x_d ──→ [+] ──→ x_ref ──→ [位置控制器] ──→ 机器人
↑
Δx（修正量）
↑
[导纳模型：F → Δx]
↑
力传感器 F_ext4.3 数学原理
导纳模型与阻抗模型本质是同一个方程，只是求解方向不同：
MdΔx¨+BdΔx˙+KdΔx=FextM_d \ddot{\Delta x} + B_d \dot{\Delta x} + K_d \Delta x = F_{ext}MdΔx¨+BdΔx˙+KdΔx=Fext
这里把 FextF_{ext}Fext 作为输入 ，Δx\Delta xΔx 作为输出，在拉普拉斯域写成：
ΔX(s)F(s)=1Mds2+Bds+Kd\frac{\Delta X(s)}{F(s)} = \frac{1}{M_d s^2 + B_d s + K_d}F(s)ΔX(s)=Mds2+Bds+Kd1
这个传递函数就是导纳（阻抗的倒数）。
4.4 MATLAB 代码示例
matlab
% 导纳控制仿真：力输入 → 位置修正量输出
Md = 2.0; Bd = 30.0; Kd = 200.0;
dt = 0.001; T = 3.0; N = T/dt;
dx = 0; ddx = 0; x_corr = 0;
x_hist = zeros(1,N);
F_hist = zeros(1,N);
for i = 1:N
t = i * dt;
% 模拟正弦变化的外力（如打磨时的周期性接触力）
F_ext = 5 * sin(2*pi*1*t) * (t > 0.5);
ddx = (F_ext - Bd*dx - Kd*x_corr) / Md;
dx = dx + ddx * dt;
x_corr = x_corr + dx * dt;
x_hist(i) = x_corr;
F_hist(i) = F_ext;
end
t_vec = (1:N)*dt;
subplot(2,1,1); plot(t_vec, F_hist,'r','LineWidth',1.5);
ylabel('外力 [N]'); title('导纳控制仿真'); grid on;
subplot(2,1,2); plot(t_vec, x_hist,'b','LineWidth',1.5);
ylabel('位置修正量 [m]'); xlabel('时间 [s]'); grid on;
五、纯力控制（Force Control）
5.1 直接力控
不通过位置间接实现，而是直接控制末端接触力 跟踪期望力 FdF_dFd。
最简单的实现：PI 力控制器
τ=JTFd+Kp(Fd−Fmeas)+Ki∫(Fd−Fmeas)dt\tau = J^T \left F_d + K_p(F_d - F_{meas}) + K_i \\int(F_d - F_{meas})dt \\rightτ=JTFd+Kp(Fd−Fmeas)+Ki∫(Fd−Fmeas)dt
其中 JTJ^TJT 是雅可比矩阵的转置，将操作空间力映射到关节力矩。
缺点： 在自由空间（不接触时）力传感器读数为零，无法产生运动，容易不稳定。
5.2 混合力/位置控制（Hybrid Force/Position Control）
这是实际工程中最常用的方案。
核心思想：将任务空间分解为两个子空间：
- 位置控制子空间：沿不受约束的方向做位置控制（如沿曲面切线方向移动）
- 力控制子空间：沿受约束的方向做力控制（如垂直接触面方向控制压力）
用选择矩阵 SSS 和 I−SI-SI−S 来分离：
τ=JTS⋅Fforce+(I−S)⋅Fposition\tau = J^T \left S \\cdot F_{force} + (I-S) \\cdot F_{position} \\rightτ=JTS⋅Fforce+(I−S)⋅Fposition
典型例子： 机械臂在曲面上打磨
-
Z 方向（垂直面）：力控，维持恒定接触力 10N
-
X/Y 方向（沿面切线）：位置控制，按规划路径移动
任务空间
↑ Z 方向：力控（F_z = 10N）
|
+──→ X/Y 方向：位置控制
六、三种算法的对比总结
| 对比维度 | 阻抗控制 | 导纳控制 | 混合力/位置控制 |
|---|---|---|---|
| 底层控制 | 力矩控制 | 位置控制 | 力矩/位置均可 |
| 传感器需求 | 力传感器 | 力传感器 | 力传感器 |
| 自由空间性能 | 较好 | 较好 | 较好 |
| 接触过渡 | 自然 | 自然 | 需切换逻辑 |
| 参数整定难度 | 中 | 中 | 较高 |
| 工程普及程度 | 高（科研） | 高（工业） | 高（工业） |
| 典型应用 | 协作机器人 | 工业机械臂 | 打磨、装配 |
七、参数整定的直觉指南
很多新手拿到这三个参数（MdM_dMd、BdB_dBd、KdK_dKd）无从下手，这里给出一个直觉框架：
第一步：确定刚度 KdK_dKd
- 如果任务是柔顺接触（如擦拭、按摩）：KdK_dKd 取小值，10 ~ 100 N/m
- 如果任务需要精确力（如装配对齐）：KdK_dKd 取中值，200 ~ 1000 N/m
第二步：确定阻尼 BdB_dBd（避免振荡）
经验公式：Bd=2Md⋅KdB_d = 2\sqrt{M_d \cdot K_d}Bd=2Md⋅Kd （临界阻尼条件）
在此基础上调整：
- 嫌响应太慢 → 适当减小 BdB_dBd（接受轻微超调）
- 接触有振荡 → 适当增大 BdB_dBd
第三步：设定虚拟质量 MdM_dMd
MdM_dMd 决定动态响应速度，一般初始设为机器人末端有效负载的 1 ~ 5 倍即可。
八、应用领域
8.1 协作机器人（Cobot）
UR、KUKA LBR iiwa 等协作机器人大量使用导纳控制。当人类推动机械臂时，力传感器感知外力，导纳模型将其转化为顺从运动------这就是"牵引示教（Lead-through Teaching）"的底层原理。
同时，碰撞检测也依赖力控：关节力矩突然超过阈值，判定为碰撞，立即切换到低刚度阻抗模式保护人员安全。
8.2 机械臂精密装配
在电子制造领域（如 PCB 插针、连接器对接），零件公差往往只有微米级。混合力/位置控制在 Z 方向施加恒定插入力，在 X/Y 方向做精确定位，实现"找正-插入"一体化控制。
8.3 手术机器人
达芬奇手术系统的精细操作背后，需要将医生手部的大幅运动缩比为毫米级动作，同时对组织的接触力进行精确限制。高带宽阻抗控制可以在切割、缝合过程中实时调整刚度参数。
8.4 无人机抓取与接触
四旋翼无人机搭载机械臂进行空中作业时（如管道检测、外立面清洗），无人机平台本身会在接触时产生扰动。导纳控制使机械臂末端对接触力表现出顺从性，同时无人机飞控维持位置，两者协同工作。
近年来兴起的**空地协作（Aerial-Ground Collaboration）**场景中，力控算法是实现无人机与地面机器人协同搬运大型物体的核心技术。
8.5 腿足式机器人
波士顿动力 Atlas、宇树 H1 等双足机器人在行走和跳跃时，脚部与地面的接触力控制至关重要。每个支撑相都需要根据地形反力实时调整腿部阻抗，以保持稳定。过硬（KdK_dKd 太大）会导致腿部"弹起"，过软会导致支撑不稳。
九、常见问题
Q：阻抗控制和 PD 位置控制有什么本质区别？
PD 控制只管"跟位置"，不关心力。阻抗控制则是在力的作用下主动调整目标位置，本质是给机器人装了一个"弹簧-阻尼缓冲器"。
Q：刚度 KdK_dKd 设为零会怎样？
方程退化为 Mde¨+Bde˙=FextM_d \ddot{e} + B_d \dot{e} = F_{ext}Mde¨+Bde˙=Fext，末端变成纯阻尼系统------受力会持续漂移，适合"引导示教"场景（人拉着机械臂走）。
Q：力传感器噪声大怎么办？
增大虚拟惯性 MdM_dMd 可以自然滤波（惯性越大，高频噪声对加速度的影响越小）。也可以在力信号上加一个低通滤波器，但会引入相位延迟。
Q：为什么说导纳控制更适合工业机械臂？
工业机械臂的底层驱动器通常是位置控制模式，很难切换成力矩控制。导纳控制只需要在位置指令上叠加修正量，无需改动底层，工程落地更容易。
如果这篇文章对你有帮助，欢迎点赞收藏 🌟，有问题欢迎在评论区交流。