力位混合控制基础：自然约束与人工约束、任务空间分解、选择矩阵S与互补矩阵S'
好，我们接着往下聊。上一章我们把机器人运动学和动力学的基础打了一遍，现在该进入力位混合控制的核心了。说实话，这部分内容我当年啃了很久才真正吃透。你想想看，一个机械臂既要控制位置，又要控制力，这两者本身是矛盾的——位置控死了，力就控不住；力控死了，位置就飘了。怎么让它们共存？这就是力位混合控制要解决的问题。
自然约束与人工约束：先搞清楚谁说了算
我第一次接触这个概念时，觉得名字起得挺玄乎。其实说白了很简单：自然约束是环境强加给机械臂的，人工约束是我们人为设定的任务要求。
举个例子。你让机械臂去推一堵墙。墙是硬的，机械臂没法穿过去——这就是自然约束。同时你希望机械臂施加10N的力——这就是人工约束。自然约束决定了哪些方向是位置受限的，人工约束决定了哪些方向是力要控制的。
核心思想：自然约束和人工约束在同一个自由度上不能同时存在。一个自由度上，要么是位置约束，要么是力约束，不能两者都控。
我在项目里遇到过这样的坑：有次调试打磨机器人，我试图在法线方向同时控制位置和力，结果系统直接震荡发散。后来才意识到，法线方向有自然约束（工件表面），只能做力控；切线方向没有自然约束，才适合做位置控。
任务空间分解：把问题拆开来看
为什么要做任务空间分解？因为机械臂的关节空间和任务空间不是一回事。你控制的是关节角度，但你要完成的任务是在笛卡尔空间里定义的。
我个人习惯把任务空间分解成两个子空间：
- 位置控制子空间：机械臂可以自由运动的方向，没有环境约束
- 力控制子空间：机械臂与环境接触的方向，存在自然约束
举个例子，你让机械臂在桌面上画一条线。桌面法线方向是力控（保持接触力），桌面切线方向是位置控（画线轨迹）。这两个子空间是正交的，互不干扰。
我的经验：分解任务空间时，一定要先搞清楚环境的几何特征。平面、曲面、棱边，每种情况对应的自然约束方向都不一样。我曾经因为忽略了工件表面的曲率，导致力控方向算错了，结果机械臂把工件压坏了。
选择矩阵S与互补矩阵S'：数学上的"开关"
好，现在到了最核心的部分。我们怎么在数学上实现这种分解？答案就是选择矩阵S和它的互补矩阵S'。
选择矩阵S是一个对角矩阵，对角线元素是0或1。1表示这个自由度做位置控制，0表示不做位置控制（也就是做力控制）。
互补矩阵S' = I - S，正好反过来。S'的对角线元素中，1表示这个自由度做力控制，0表示不做力控制。
你看，这两个矩阵天然互补，保证了每个自由度上只有一个控制模式。
| 自由度 | S (位置控) | S' (力控) | 说明 |
|---|---|---|---|
| x方向 | 1 | 0 | 自由运动，位置控 |
| y方向 | 1 | 0 | 自由运动，位置控 |
| z方向 | 0 | 1 | 接触环境，力控 |
| 绕x轴 | 1 | 0 | 姿态自由 |
| 绕y轴 | 1 | 0 | 姿态自由 |
| 绕z轴 | 0 | 1 | 姿态受限 |
上面这个表格对应的是机械臂在平面上推一个物体的场景。z方向有接触，所以力控；绕z轴的转动被摩擦力矩限制，所以也是力控。其他方向自由，位置控。
注意：S矩阵的选择不是一成不变的。任务变了，S矩阵就要跟着变。我见过有人把S矩阵写死在代码里，换了个任务就出问题。正确的做法是把S矩阵作为任务参数，动态配置。
力位混合控制的控制律结构
有了S和S'，控制律就可以写成这样：
τ = J^T [ S * (Kp * Δx + Kd * Δẋ) + S' * (Kf * ΔF) ]其中：
- τ 是关节力矩指令
- J 是雅可比矩阵
- Δx, Δẋ 是位置和速度误差
- ΔF 是力误差
- Kp, Kd, Kf 是增益矩阵
这个公式的意思是：位置控的自由度用PD控制，力控的自由度用力控制，两者通过S和S'矩阵叠加在一起，最后通过雅可比矩阵映射到关节空间。
嗯，这里要注意一点：S和S'的维度必须和任务空间的维度一致。如果是6自由度机械臂，S就是6x6的对角矩阵。
一张图看懂力位混合控制
下面我用SVG画一张流程图，把整个逻辑串起来。这张图我当年画过无数次，每次给新同事讲力位混合控制，都是先画这张图。
这张图从左到右展示了完整的流程：先分析自然约束，分解任务空间，然后通过S和S'矩阵分别处理位置控制和力控制，最后合并映射到关节空间。你仔细看，S矩阵在这里起到了"路由"的作用，把不同的控制模式分配到不同的自由度上。
实际调试中的避坑指南
我曾经在调试一个装配机器人时，遇到一个奇怪的问题：机械臂在接近目标位置时突然剧烈抖动。查了半天，发现是S矩阵设置错了——在接近方向同时开启了位置控和力控，两个控制器在打架。
后来我总结了几条经验：
- S矩阵的切换要平滑：不要突然改变S矩阵的值，否则会有冲击。可以用一个过渡函数，让S矩阵的值从0渐变到1。
- 力控方向的增益要保守：力控的Kf增益如果太大，容易引起震荡。我一般从很小的值开始调，慢慢往上加。
- 位置控和力控的带宽要匹配：如果位置控的响应很快，力控的响应很慢，两者耦合时会出现奇怪的现象。尽量让两个回路的带宽接近。
- 注意奇异位形：在奇异位形附近，雅可比矩阵的逆会变得很大，导致控制力矩异常。这时候要限制力矩输出，或者避开奇异区域。
一个小技巧：调试时可以先让S矩阵全为1（纯位置控制），确认位置环没问题后，再逐步把需要力控的自由度切换成0。这样能快速定位问题出在位置环还是力环。
好了，力位混合控制的基础就讲到这里。自然约束和人工约束是前提，任务空间分解是方法，S和S'矩阵是工具。这三者缺一不可。下一节我们会深入具体的控制算法实现，到时候我会拿一个实际的打磨案例来演示。
公众号:蓝海资料掘金营，微信deep3321