【机器人基础】阻抗/导纳控制深度解析

之前写过一篇阻抗控制的文章，【机器人基础】机器人阻抗控制概念写的比较浅也不是很专业，最近上了桂凯博士的动力学课程之后有了更深入的认识，认真整理总结了一下，这次详细地分析一下阻抗/导纳控制。

1、柔顺控制

柔顺控制概念对于理解阻抗/导纳控制非常重要，所以单独做一节来讨论。首先大家要明确一点，位置控制是有局限性的，在位置精度需求很高且环境刚度很大的情况下会出问题，为什么呢？因为位置一旦有偏差，环境的刚度很大，再加上机器人本身极大的刚度，硬碰硬，两者之间会产生很大的力，这不论对机器人还是环境都会带来很大的损伤。邵总写过机器人开门的文章，可以去看看，会有帮助的。阻抗控制的实现方法应该是多种多样的，请能否概述一下大概有几种方法
柔顺控制光看名字就能想象出来了，机械臂正在移动，如果突然来了一个外力，机器人将顺着这个外力运动，外力撤销之后，再回复到之前的状态。就像一个人突然被推了一下，人会产生一个惯性运动，这叫做柔顺。
再往细点讲，为什么位置控制它不柔顺呢？看下面这张带干扰的位置控制框图，PID控制器的一个功能是消除扰动，外力加上来想要改变机器人原本运动的位置，可PID控制器获得反馈信号之后，输出的效果是让抵消外力的干扰，这两者之间是一个对抗的关系，这就不柔顺了！，在其他很多控制问题中是应该要尽可能快速消除扰动信号的，但如果机器人的任务中需要做到柔顺，这不是一个好的结果。

2、阻抗/导纳控制的设计目的

阻抗/导纳控制在有些论文里面会混作一谈，都称为阻抗控制，因为阻抗控制和导纳控制有相同的地方，环境接触力（或者说外力）和位置偏差之间满足公式（2）的关系，这个公式反映了阻抗控制器（这里暂时把阻抗/导纳都称为阻抗控制器）的阻抗特性。控制器就是要根据1）对象模型和2）阻抗特性来设计，这也是阻抗/导纳控制设计的根本目的（设计控制器/控制率）。Md,Dd,KdM_d,D_d,K_dMd,Dd,Kd代表期望阻抗参数，分别是惯性特性、阻尼特性和刚度特性，通过调节这三个参数，从而改变阻抗的效果，或者可以理解成改变机器人末端的柔顺度。
Mde¨+Dde˙+Kde=Fext（2）\begin{aligned} M_d \ddot e+D_d \dot e+K_de=F_{ext} （2） \end{aligned} Mde¨+Dde˙+Kde=Fext（2）
e=x−x0\begin{aligned} e = x-x_0 \end{aligned} e=x−x0

举一个具体的案例，让下图的机器人去按压海绵，如果设置海绵表面的位置为x0x_0x0，当机械臂挤压海绵的时候，产生了接触力FextF_{ext}Fext。如果我拉着机械臂往下摁，接触力会变大，对于导纳控制的策略而言，误差eee会变大，期望的位置也会随之变大，位置控制环里面有了新的控制目标。

阻抗/导纳控制的核心思想可以用公式（2）来表达，外力和位置偏差是一个动态的关系，以此来达到柔顺的目的。

下面以一个实际的案例来分析，考虑一个单自由度系统，它的动力学模型可以表示为：
mx¨=F+Fext（1）\begin{aligned} m \ddot x=F+F_{ext}（1） \end{aligned} mx¨=F+Fext（1）

FFF代表控制器计算出来的输出力，FextF_{ext}Fext是外力或者说是机器人与环境之间的接触力（external force)。先记一下，后面的仿真要拿这个系统来分析。

3、阻抗控制

为了使得位置偏差和外力之间呈柔顺的关系，结合公式（1）和公式（2），计算出阻抗控制率:
F=(mMd−1)Fext+mx¨0−mMd(Dde˙+Kde)(3)\begin{aligned} F=(\frac{m}{M_d}-1)F_{ext}+m\ddot x_0-\frac{m}{M_d}(D_d\dot e+K_de) (3) \end{aligned} F=(Mdm−1)Fext+mx¨0−Mdm(Dde˙+Kde)(3)

阻抗控制根据期望位置x0x_0x0，实际位置x和外力FextF_{ext}Fext算出输出力F，输出力F作用在对象模型上，可以看到阻抗控制的内部回路是一个力环，而接下来看到的导纳控制它的内部回路是一个位置环。

导纳控制

导纳控制的位置控制器用一个PD控制器来实现
F=kp(xd−x)−kdx˙（4）\begin{aligned} F=k_p(x_d-x)-k_d\dot x （4） \end{aligned} F=kp(xd−x)−kdx˙（4）

结合公式（4）和公式（1），结合公式（2）和公式（4）并将xxx替换成xdx_dxd（假设位置控制内环实现无静差控制，那么x=xdx=x_dx=xd），得到完整的动力学公式。
mx¨+kdx˙+kp(x−xd)=Fext（5）\begin{aligned} m\ddot x+k_d\dot x+k_p(x-x_d)=F_{ext}（5） \end{aligned} mx¨+kdx˙+kp(x−xd)=Fext（5）

Md(x¨d−x¨0)+Dd(x˙d−x˙0)+Kd(xd−x0)=Fext（6）\begin{aligned} M_d(\ddot x_d-\ddot x_0)+D_d(\dot x_d-\dot x_0)+K_d(x_d-x_0)=F_{ext}（6） \end{aligned} Md(x¨d−x¨0)+Dd(x˙d−x˙0)+Kd(xd−x0)=Fext（6）

从导纳控制框图可以看到导纳控制的期望位置不是固定的，而是根据公式（6）计算得到的。

程序仿真

以导纳控制为例，尝试在matlab里面实现，假设x0x_0x0为阶跃响应，其他相关参数如下：

考虑到模型的不确定性，阻抗特性中的MdM_dMd使用m^\hat mm^代替而不直接使用mmm。另外考虑到动力学模型中摩擦力的影响，增加了两个参数CvC_vCv（粘性摩擦力系数）和FcF_cFc（库伦摩擦力系数），摩擦力模型为：
Ff=−sign(x˙)(cv∣x˙∣+Fc)F_f = -sign(\dot x)(c_v \left| \dot x \right| +F_c)Ff=−sign(x˙)(cv∣x˙∣+Fc)
设外力Fext=−ke（x−x0)F_{ext}=-k_e（x-x_0)Fext=−ke（x−x0)，取ke=3000k_e=3000ke=3000代表环境对象的刚度，此时模型的动力学方程为：mx¨=F+Fext+Ffm\ddot x=F+F_{ext}+F_fmx¨=F+Fext+Ff。

simulink框架如下：

导纳控制器Admittance_ctrl的sfunction如下（只展示mdlDerivatives中的内容）

mhat = 0.8; % 单位：kg
% 环境刚度
ke = 300; % 单位：N/m
% 阻抗参数
Md = mhat; % 单位：kg
Kd = 100; % 单位：N/m
Dd = 2*0.7*sqrt(Kd*Md); % 单位：N*s/m
% 设定值
X0 = u(1); dX0 = u(2); ddX0 = u(3);
X = u(4);
% 外力
Fext = -ke*(X-X0);
% Xd微分方程
Xd = x(1);
dXd = x(2);
ddXd = 1/Md*(Fext - Kd*(Xd-X0) - Dd*(dXd-dX0)) + ddX0;
sys(1) = dXd;
sys(2) = ddXd;

位置控制器Position_ctrl的sfunction如下（只展示mdlOutputs中的内容）

%PD控制系数
kp = 1e3; % 单位：N/m
kd = 2*0.7*sqrt(kp*m); % 单位：N*s/m
Xd = u(1);
X = u(3);  dX = u(4);
% 输出力
F = kp*(Xd-X) - kd*dX;
sys = F;

模型Plant的sfunction如下（只展示mdlOutputs中的内容）

% 质量
m = 1.0; % 单位：kg
% 弹簧刚度
ke = 300; % 单位：N/m
% 粘性系数和库仑摩擦系数（coefficients of viscous and Coulomb friction）
cv = 1.0; % 单位：N*s/m
Fc = 3.0; % 单位：N
X0 = 1;
F = u;
% 外力
Fext = -ke*(x(1)-X0);
% 摩擦力
Ff = -sign(x(2))*(cv*abs(x(2))+Fc);
% 系统动力学，考虑了外力和摩擦力，和工业机器人动力学模型逻辑一致
S = (F + Fext + Ff)/m;
sys(1) = x(2);
sys(2) = S(1);

仿真结果展示如下：

参考文献

Unified Impedance and Admittance Control
论文百度云链接
链接：https://pan.baidu.com/s/1HBJtXriKg3BbQJulXhtz5w
提取码：yoh2