力位混合控制：自然约束与人工约束、选择矩阵、力控与位控的切换逻辑
力位混合控制，这个名字听起来挺唬人的。说白了，就是让机器人一边用力干活，一边还得管好自己的位置。我最早接触这个概念是在做打磨抛光项目的时候，那时候我就想：能不能让机器人既保证接触力稳定，又不把工件给怼飞了？嗯，力位混合控制就是干这个的。
自然约束与人工约束
要理解力位混合，得先搞清楚两个概念：自然约束和人工约束。这是Mason在1981年提出来的，到现在依然是这个领域的基石。
自然约束，是环境强加给机器人的限制。你想想看，当机器人的末端执行器贴在桌面上时，它能不能往桌面里钻？不能。能不能沿着桌面法线方向自由移动？也不能。这些就是自然约束。它们由几何接触关系决定，跟你的控制算法无关。
举个例子，我在做装配任务时，轴要插入孔里。轴在孔内的径向移动就是自然约束——它被孔壁限制住了。但轴向的移动是自由的，只要你不撞到底。
人工约束，则是我们人为设定的控制目标。比如，在打磨时，我们希望法向力保持10N。这个10N不是环境强制的，是我们自己想要的。又比如，在跟踪轨迹时，我们希望切向速度是0.1m/s，这也是人工约束。
这里有个关键点：自然约束和人工约束必须互补。什么意思？
- 在自然约束限制自由度的方向上，我们做力控
- 在自然约束允许自由度的方向上，我们做位控
我踩过这个坑。有一次做力控抛光，我在法线方向既做了力控又做了位置限制，结果两个控制器打架，系统直接抖成筛子。后来才明白，自然约束已经限制了法向位置，你再去控制位置就是画蛇添足。
核心原则：自然约束和人工约束在同一个自由度上不能同时存在。一个自由度上，要么是自然约束（环境限制），要么是人工约束（控制目标），不能两者兼得。
选择矩阵：谁干活谁休息
选择矩阵，就是用来分配每个自由度上到底谁说了算。它是一个对角矩阵，对角线上的元素是0或1。
- S = diag(s₁, s₂, ..., s₆)，sᵢ ∈ {0, 1}
- sᵢ = 1：这个自由度做位置控制
- sᵢ = 0：这个自由度做力控制
等等，你可能会问：为什么1是位置控制，0是力控制？其实反过来也行，只是约定俗成。我个人习惯用1表示位置控制，因为位置控制是默认的，力控制是特殊情况。
来看一个实际的例子。假设我们要做平面打磨，工具在Z方向接触工件，在X和Y方向移动。
| 自由度 | 自然约束 | 人工约束 | 选择矩阵S |
|---|---|---|---|
| X平移 | 自由 | 位置控制（轨迹跟踪） | 1 |
| Y平移 | 自由 | 位置控制（轨迹跟踪） | 1 |
| Z平移 | 受限（接触面） | 力控制（恒力10N） | 0 |
| 绕X旋转 | 自由 | 位置控制（姿态保持） | 1 |
| 绕Y旋转 | 自由 | 位置控制（姿态保持） | 1 |
| 绕Z旋转 | 自由 | 位置控制（姿态保持） | 1 |
你看，只有Z方向的选择矩阵元素是0，其他都是1。这意味着只有Z方向做力控，其他五个自由度都做位置控。
实战技巧：选择矩阵不是一成不变的。在装配过程中，随着接触状态的变化，选择矩阵需要动态切换。比如，轴插入孔前，Z方向是位置控制；插入后，Z方向就要切换成力控制，防止撞底。
力控与位控的切换逻辑
切换逻辑，说白了就是什么时候该用力控，什么时候该用位控。这取决于任务阶段和接触状态。
常见的切换策略有三种：
- 基于接触检测的切换
这是最直观的方法。当检测到接触力超过阈值时，从位控切到力控。我在做精密装配时就用这个：
if (|F_z| > F_threshold) {
// 检测到接触，切换到力控
S[2] = 0; // Z方向力控
F_desired = 5.0; // 目标力5N
} else {
// 自由空间，位置控制
S[2] = 1; // Z方向位控
X_desired = X_target;
}但这里有个坑：阈值设得太小容易误触发，设得太大又可能已经撞上了。我建议阈值设为额定接触力的20%-30%，同时加一个滤波，防止噪声干扰。
- 基于状态机的切换
对于复杂的任务，比如装配，用状态机来管理切换更靠谱。每个状态对应一组选择矩阵和控制器参数。
enum AssemblyState {
APPROACH, // 接近阶段：位控
CONTACT, // 接触阶段：力控
INSERT, // 插入阶段：力控+位控混合
DONE // 完成
};
void assembly_control(AssemblyState state) {
switch(state) {
case APPROACH:
S = diag(1,1,1,1,1,1); // 全位置控制
break;
case CONTACT:
S = diag(1,1,0,1,1,1); // Z方向力控
F_desired = 2.0; // 轻柔接触
break;
case INSERT:
S = diag(1,1,0,1,1,1); // 保持Z方向力控
F_desired = 10.0; // 增大插入力
break;
case DONE:
S = diag(1,1,1,1,1,1); // 回到位置控制
break;
}
}我曾经在一个轴孔装配项目中，状态机写了7个状态。每个状态的切换条件都经过上百次测试才稳定下来。嗯，调试过程很痛苦，但结果很值得。
- 连续平滑切换
硬切换会导致力或位置的突变。更优雅的做法是用一个平滑函数来过渡。比如用sigmoid函数：
// 平滑切换函数
double smooth_transition(double t, double t_start, double t_end) {
double x = (t - t_start) / (t_end - t_start);
x = clamp(x, 0.0, 1.0);
return 1.0 / (1.0 + exp(-10.0 * (x - 0.5)));
}
// 在切换过程中，选择矩阵连续变化
double alpha = smooth_transition(t, 2.0, 2.5); // 2秒到2.5秒完成切换
S[2] = alpha; // 从0（力控）渐变到1（位控）注意：平滑切换虽然减少了冲击，但会引入一段时间的混合控制状态。这段时间内，位置和力都不完全受控。我建议切换时间控制在0.3-0.5秒，太短没效果，太长容易出问题。
力位混合控制的完整框架
说了这么多，我们来画一张图，把整个逻辑串起来。
这张图展示了力位混合控制的完整流程。从任务规划开始，先分析自然约束，再设定人工约束，然后生成选择矩阵，最后分别计算位置控制和力控制的输出，合并成最终的关节力矩。
实际项目中的避坑指南
我曾经在一个打磨项目中，力位混合控制怎么调都不对。后来发现是选择矩阵搞反了——我把力控和位控的自由度弄颠倒了。检查了三天才找到问题，那叫一个郁闷。
这里给大家几个实用建议：
- 先做仿真验证：在真机运行前，先用MATLAB或Simulink仿真跑一遍。我习惯把选择矩阵的每个元素都打印出来，肉眼检查一遍。
- 力控方向加软限位：力控自由度虽然不做位置控制，但还是要加一个位置软限位，防止意外情况导致机器人飞车。
- 切换时注意积分器：PID控制器的积分项在切换时容易出问题。我建议在切换瞬间把积分项清零，或者用条件积分。
- 调试时先低速：第一次跑力位混合控制，速度设到正常值的10%。确认没问题了再慢慢提速。
总结一下：力位混合控制的核心就是搞清楚每个自由度上，到底是环境说了算（自然约束），还是我们说了算（人工约束）。选择矩阵就是分配权力的工具。切换逻辑则是根据任务阶段动态调整这个分配。搞懂了这三件事，力位混合控制就入门了。
公众号:蓝海资料掘金营，微信deep3321