【機器人基礎】阻抗/導納控制深度解析

之前寫過一篇阻抗控制的文章，【機器人基礎】機器人阻抗控制概念寫的比較淺也不是很專業，最近上了桂凱博士的動力學課程之後有了更深入的認識，認真整理總結了一下，這次詳細地分析一下阻抗/導納控制。

1、柔順控制

柔順控制概念對於理解阻抗/導納控制非常重要，所以單獨做一節來討論。首先大家要明確一點，位置控制是有侷限性的，在位置精度需求很高且環境剛度很大的情況下會出問題，爲什麼呢？因爲位置一旦有偏差，環境的剛度很大，再加上機器人本身極大的剛度，硬碰硬，兩者之間會產生很大的力，這不論對機器人還是環境都會帶來很大的損傷。邵總寫過機器人開門的文章，可以去看看，會有幫助的。阻抗控制的實現方法應該是多種多樣的，請能否概述一下大概有幾種方法
柔順控制光看名字就能想象出來了，機械臂正在移動，如果突然來了一個外力，機器人將順着這個外力運動，外力撤銷之後，再回復到之前的狀態。就像一個人突然被推了一下，人會產生一個慣性運動，這叫做柔順。
再往細點講，爲什麼位置控制它不柔順呢？看下面這張帶干擾的位置控制框圖，PID控制器的一個功能是消除擾動，外力加上來想要改變機器人原本運動的位置，可PID控制器獲得反饋信號之後，輸出的效果是讓抵消外力的干擾，這兩者之間是一個對抗的關係，這就不柔順了！，在其他很多控制問題中是應該要儘可能快速消除擾動信號的， 但如果機器人的任務中需要做到柔順，這不是一個好的結果。

2、阻抗/導納控制的設計目的

阻抗/導納控制在有些論文裏面會混作一談，都稱爲阻抗控制，因爲阻抗控制和導納控制有相同的地方，環境接觸力（或者說外力）和位置偏差之間滿足公式（2）的關係，這個公式反映了阻抗控制器（這裏暫時把阻抗/導納都稱爲阻抗控制器）的阻抗特性。控制器就是要根據1）對象模型和2）阻抗特性來設計，這也是阻抗/導納控制設計的根本目的（設計控制器/控制率）。$M_d,D_d,K_d$代表期望阻抗參數，分別是慣性特性、阻尼特性和剛度特性，通過調節這三個參數，從而改變阻抗的效果，或者可以理解成改變機器人末端的柔順度。
$\begin{aligned} M_d \ddot e+D_d \dot e+K_de=F_{ext} （2） \end{aligned}$
$\begin{aligned} e = x-x_0 \end{aligned}$

舉一個具體的案例，讓下圖的機器人去按壓海綿，如果設置海綿表面的位置爲$x_0$，當機械臂擠壓海綿的時候，產生了接觸力$F_{ext}$。如果我拉着機械臂往下摁，接觸力會變大， 對於導納控制的策略而言，誤差$e$會變大，期望的位置也會隨之變大，位置控制環裏面有了新的控制目標。

阻抗/導納控制的核心思想可以用公式（2）來表達，外力和位置偏差是一個動態的關係，以此來達到柔順的目的。

下面以一個實際的案例來分析，考慮一個單自由度系統，它的動力學模型可以表示爲：
$\begin{aligned} m \ddot x=F+F_{ext}（1） \end{aligned}$

$F$代表控制器計算出來的輸出力，$F_{ext}$是外力或者說是機器人與環境之間的接觸力（external force)。

3、阻抗控制

爲了使得位置偏差和外力之間呈柔順的關係，結合公式（1）和公式（2），計算出阻抗控制率:
$\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}$

阻抗控制根據期望位置$x_0$，實際位置x和外力$F_{ext}$算出輸出力F，輸出力F作用在對象模型上，可以看到阻抗控制的內部迴路是一個力環，而接下來看到的導納控制它的內部迴路是一個位置環。

導納控制

導納控制的位置控制器用一個PD控制器來實現
$\begin{aligned} F=k_p(x_d-x)-k_d\dot x （4） \end{aligned}$

結合公式（4）和公式（1），結合公式（2）和公式（4）並將$x$替換成$x_d$（假設位置控制內環實現無靜差控制，那麼$x=x_d$），得到完整的動力學公式。
$\begin{aligned} m\ddot x+k_d\dot x+k_p(x-x_d)=F_{ext}（5） \end{aligned}$

$\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}$

從導納控制框圖可以看到導納控制的期望位置不是固定的，而是根據公式（6）計算得到的。

程序仿真

以導納控制爲例，嘗試在matlab裏面實現，假設$x_0$爲階躍響應，其他相關參數如下：

考慮到模型的不確定性，阻抗特性中的$M_d$使用$\hat m$代替而不直接使用$m$。另外考慮到動力學模型中摩擦力的影響，增加了兩個參數$C_v$（粘性摩擦力系數）和$F_c$（庫倫摩擦力系數），摩擦力模型爲：
$F_f = -sign(\dot x)(c_v \left| \dot x \right| +F_c)$
設外力$F_{ext}=-k_e（x-x_0)$，取$k_e=3000$代表環境對象的剛度，此時模型的動力學方程爲：$m\ddot x=F+F_{ext}+F_f$。

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