今天，本菜通过一个平面二连杆机械臂的仿真表达一下对于阻抗控制的理解。

1. 关于阻抗模型的几点总结

首先，我们知道阻抗模型的基本形式是

关于这个公式，有如下几点说明：

1. 如果左边的状态和右边力是是笛卡尔空间的，就称为笛卡尔空间阻抗模型；如果都是关节空间的，就称为关节空间阻抗模型
2. 阻抗模型一般是针对机器人来考虑的，因此左边的状态量差如果定义为期望值减去当前值，即  ，那么右边的力就定义为机器人施加给环境的力；如果左边反过来，即  ，那么右边的力就定义为机器人受到来自环境的力。我们这里采用后一种定义，因为力传感器测量的是其所受的力
3. 阻抗模型并不意味着什么时候都是三项齐全的，比如零力拖动或者直接示教时，期望位置。速度及加速度未定义，此时阻抗模型中  均为零，机器人只需要补偿重力矩，科氏力向心力矩以及摩擦力矩
4. 一般来说，我们可以获得机器人当前的关节位置，关节速度。在此基础上，如果我们可以得到关节加速度，那么带入阻抗模型就可以计算出力，然后把这个力纳入到关节驱动力矩中，这种就是阻抗控制。此时注意必须采用2中前一种定义，因为所谓阻抗就是基于状态量偏差输出力。阻抗控制不需要检测或者估计外力。阻抗控制和计算力矩控制是一回事。另一种情况是已知外力，那么可以计算出加速度差，如果机器人是速度控制，那么把加速度差乘以控制周期叠加到期望速度上就得到控制输入，即  ，如果机器人是位置控制，那么把加速度差乘以控制周期叠加到当前速度差得到更新的速度差，接着再把更新的速度差乘以控制周期叠加到当前期望位置得到控制输入，即  。导纳控制的时候千万要记得是把由于外力引起的速度或者位置偏差叠加到期望速度或者位置上，而不是做系统仿真或者轨迹跟踪控制，有很多小白看见积分就想到仿真，看见差值就想到跟踪，之所以混为一谈还是没搞清楚你的研究对象
5. 基于前面几点，可以总结出如下阻抗控制与导纳控制的实现方式（用  表示笛卡尔空间位姿，  表示关节空间广义坐标，  表示笛卡尔空间外力/力矩，  表示关节空间外力/力矩）

• 笛卡尔阻抗控制+关节力矩指令输出

• 关节阻抗控制+关节力矩指令输出

• 笛卡尔导纳控制+关节速度指令输出

或者

• 笛卡尔导纳控制+关节位置指令输出

或者

• 关节空间导纳控制+关节速度指令输出

• 关节空间导纳控制+关节位置指令输出

这几种实现形式中，阻抗控制没有奇异性，但是要求关节可以力控；笛卡尔导纳控制牵涉到雅克比矩阵求逆以及逆向运动学，存在奇异性问题。

2. 场景文件

几点说明：

1. 最左侧的紫色小球在仿真过程中可以移动位置(鼠标选中小球，然后选中菜单栏中的移动物体图标即可用鼠标移动小球)，从其初始位置到其当前位置的向量表示人为在二连杆末端小球上施加的力(方向与该向量相同，大小与其长度成正比)
2. 两个长方体是固定障碍物，当二连杆与其接触时有接触力作用，无接触时没有接触力
3. 黑色圆是二连杆末端期望路径
4. 关节设为motor enabled且control loop disabled，即支持速度与力矩控制
5. 仿真阻抗控制采用笛卡尔阻抗加关节力矩指令输出形式(主要还是本菜太懒，懒得写雅克比，懒得写逆向运动学)，其他形式仿真后续补充

3. 运行

1. 末端期望轨迹为匀速圆周

• 无人为添加外力

有人为添加外力

2. 末端期望轨迹为匀速水平直线

• 无人为添加外力

• 有人为添加外力

--------------------------------- 更新 -------------------------------

话说评论区大佬 

 敏锐地发现了笛卡尔和关节空间的阻抗控制公式少了期望关节惯性力项。如果期望的运动轨迹对应的期望关节加速度不是0的话，少了这一项会影响轨迹跟踪的精度，但是不影响稳定性。碰巧的是，第一个匀速圆周轨迹恰好是以机器人基座原点为圆心，初始机械臂末端到基座距离为半径的匀速圆周，这样期望的关节加速度恰好是0，所以圆周轨迹的仿真看起来效果不错。至于匀速直线的情况，此时虽然末端期望轨迹是匀速直线，但是期望的关节加速度是变化的，因为  。所以考虑在体现柔顺性的同时有好的轨迹跟踪精度，我们对第1部分的阻抗控制进行如下修正，让前馈项更好。从这里我们可以深刻体会一个好的前馈对于提高控制精度的意义。

• 笛卡尔阻抗控制+关节力矩指令输出

• 关节阻抗控制+关节力矩指令输出

补上无惯性力前馈和有惯性力前馈的阻抗控制器在机械臂末端匀速直线运动且有恒定人为扰动力情况下的仿真动画。

• 无惯性力前馈

• 有惯性力前馈

欢迎大佬们继续指教本菜！

-----------------------------------------------------

本例程源文件链接(切记只可用于学习，科研等非商业用途)

-----------------------------------------------------

做人呐，最重要的就是开心喽。能帮到别人是最大的快乐，希望本菜鸡能对小白们有所帮助，希望大佬们的批评能帮助本菜鸡成长！