转载：抛光机器人力位混合控制思路与方式
2022-09-13 22:35:07 0次
如果我们对位置闭环，那就无法控制力。如果我们对力闭环，那就无法控制位置。那么，如何实现位置和力的同时控制呢？就像我们的人手一样。于是，于是就有了阻抗控制的概念，即我们用位置闭环去控制机械臂末端，但是末端具有一定的刚度，与外界接触时会表现出力的特征。这种刚度可以从机械设计上实现，比如柔性关节SEA。也可以从控制算法上实现，比如阻抗/导纳控制。
我们统称为“阻抗控制”；一般的，阻抗控制是直接控制力（电流）的方式；导纳控制是通过控制位置来控制力；无论是阻抗控制还是导纳控制，都可以调节阻抗特征的，只是阻抗控制无法模拟高刚度，导纳控制无法模拟低刚度，而且这种限制并不来源于理论，而是由于控制量不相同，工程中的传感器测量有误差和延时导致；
理论分析：任何物体间的接触，都可以等效为两个“阻尼质量弹簧”模型，之间的接触举个例子，人擦玻璃，玻璃的弹簧刚度K可以认为是无穷大（没有弹性），人手的刚度K就需要比较小（有弹性），这样才能适应玻璃表面，完成擦拭工作；再比如，拿刀切肉，肉的刚度K可以认为是很小，刀的刚度认为是很大，这样才能让肉随着刀的运动，而变化；现实世界里，我们人与外界接触的刚度往往不是固定的，而是变化的，举个例子，拍篮球，手在刚接触球的时候是偏柔性的，让手随着球而动，然后，手会变得刚性起来，让球随着手而动，这个刚度的控制，是人对自己手臂的主动控制，我们也希望机器人在处理力控任务的时候，能有这种自由控制刚度的能力；为什么需要这个“阻尼质量弹簧”模型呢？ 擦玻璃的话，给玻璃施加一个恒力不就行了吗？想象一下，如果现在机械臂是对一块玻璃施加一个恒力F，然后突然撤开玻璃会怎样呢？
显然机械臂会失控，以最快速度向F的方向运动；但是如果有了“阻尼质量弹簧（BMK）”模型，当MBK三个参数调节得当的时候，机械臂就会以平滑的运动回到虚拟弹簧的原点；在这种情况下，我们控制机械臂与环境的接触，是通过控制机械臂末端的四个参数：虚拟质量M，虚拟阻尼B，虚拟刚度K，虚拟弹簧原点x0；控制这四个参数，我们可以让机械臂对外呈现各种力特征，来满足打磨抛光，精密装配，协作交互等需求现在我们目标明确了，要设计一个BMK模型，对机械臂进行控制，接下来怎么做呢？还是按照图9-6来分析，其中f是施加到系统的外力，xdot是速度，xdotdot是加速度，这是一个典型的二阶系统，有以下公式来描述系统的运动：
即已知MBK以及测得的外力、速度、位置，我们可以求出当前需要的加速度；这个加速度就是我们要控制的量；那怎么控制加速度？由于我们是离散系统，通过将xd积分两次，就得到了位置，所以，最终我们可以通过控制位置x，去实现这个模型，而且在伺服系统中，控制量通常是位置环，所以这种方式是合理的；那么既然最后都要控制位置x，那为什么刚开始不直接把公式，化成位置的表达式呢？因为化成位置的表达式，式中有加速度项，需要测量加速度项，而通常伺服系统中位置信息是由编码器得到的，而编码器只能获得位置反馈，其速度反馈是通过对位置的微分得到，而加速度是对速度微分得到，这个时候，由于经过了两次微分，得到的加速度信息往往是噪声非常大的，不适合使用；可以看到，我们这种方法，实际上，是通过控制位置来控制力的，俗称“导纳控制”；其实常用的还有另一种方法，是直接控制力，俗称“阻抗控制”，但这需要有关节力传感器的反馈，也要进行动力学建模，并且要能直接控制伺服驱动器的电流环，实现成本较大，这里就不叙述了；
一个例子：
控制器的位置环程序为3ms周期，其中包括一个导纳控制模块
导纳控制模块如下：
//x轴导纳模型
xdd = (fx0 - Bx*vx_now - Kx*(xnow-xbase))/Mx;
//加速度积分得到速度
xd = xd + xdd;
//y轴导纳模型
ydd = (fy0 - By*vy_now - Ky*(ynow-ybase))/My;
//加速度积分得到速度
yd = yd + ydd;
//将导纳模型的位置偏移计入，得到新的x轴y轴位置
xt = xd + xnow;
yt = yd + ynow;分别计算出x和y轴的导纳模型，将位置偏移量更新到新的目标位置
其中，fx0是笛卡尔坐标系中x轴向的外力，Bx是x轴向的阻尼值，vx_now是末端在x轴向的瞬时速度，Kx是x轴向的刚度，xnow是末端在x轴的瞬时位置，xbase是x轴的虚拟阻抗原点，Mx是x轴向的质量值。
通过导纳模型计算出加速度，由于此模块，是与位置环同周期的，在离散数学中，积分可简化为加法（类似于PID，如果不明白可以自行百度一下）
那么，怎样得到fx0呢？我使用了一个末端的六维力传感器，对传感器受的力进行坐标变换，得到x轴和y轴的外力fx0和fy0；
如何得到vx_now？首先从驱动器获得关节速度，通过雅克比变换，得到末端的x轴和y轴速度；
如何得到xnow？首先从驱动器获得关节位置，通过正运动学计算，得到末端的x和y轴位置
所以，大概是这样：
我们假设机械臂的末端是一个阻尼质量弹簧系统，当有了外力作用时，我们通过模型计算，这个虚拟的MBK系统的加速度，速度，位置应处于什么状态。最后，通过直接篡改位置环，去模拟一个MBK系统；
并且，你可以看出，x和y轴的MBK系统是解耦的，各自独立计算的。由于我的实验机械臂，只具有在xy轴平面内运动的能力，所以只考虑了x和y轴，一般的六自由度机械臂，应分别对x，y，z，rx，ry，rz六个自由度进行MBK解耦；
