機械臂的動力學模型是如何解耦和線性化的？常用的線性化解耦方法有哪些？

12-29

機械臂的動力學模型是非線性時變二階系統，機械臂關節之前還存在耦合作用，那它是如何被一步步的變成了常用的線性時不變系統？

$,$ $, ,$ $,,,,$ $,$ 在學機器人控制的時候，通常情況下有個默認前提，就是已經對控制理論有了足夠的了解，包括線性控制和非線性控制，所以在眾多機器人學的書籍資料當中並沒有對控制理論做深入分析，從而也就導致了這類問題的出現。

$qquad qquad qquad$ $qquad$ $qquad$ 而將非線性系統線性化，是非線性控制理論的一個永恆的主題。而機器人控制系統除了針對自身機電一體化的模型外，另一條主線也是如何對機器人的非線性化模型進行補償，從而儘可能的實現線性化控制，由此，又引出魯棒控制、自適應控制等控制理論。

======================================================================

$qquadqquad$ $qquad$ $qquad$ 下面步入正題，現考慮機器人的動力學方程模型:

$H(x)ddot{x}+n(x,dot{x})=u$ (1)

其中 $[x^{T} ,dot{x}^{T}]^{T}$ 表示系統的狀態向量（可以理解為關節空間或任務空間變數）, $u$ 是輸入向量(可以理解為輸入力矩), $H(x)$ 是依賴於 $x$ 的正定矩陣, $n(x,dot{x})$ 是依賴於系統狀態的n維向量(也就是科式力項、向心力、摩擦力和重力等非線性耦合項). 該模型是非線性多變數控制系統。

$qquad$ $qquad$ $qquad$ $qquad$ 一下方法的基本思想是找到控制量 $u$ ,該向量是系統狀態的函數，可以以此實現線性形式的輸入輸出關係。換句話說，可以通過非線性狀態反饋方法(nonlinear state feedback，逆動力學控制, inverse dynamics control) 實現系統動力學的線性化。通過非線性補償可以得到如下控制律：

$u=hat{H}(x)v+hat{n}(x,dot{x})$ (2)

其中 $hat{H}(x)$ 和 $hat{n}(x,dot{x})$ 分別表示 $H(x)$ 和 $n(x,dot{x})$ 的估計量，他們是基於對系統狀態的測量經計算得到的，而 $v$ 是要設計的新的控制輸入。通常由於建模時不可避免的要做一些近似，又或者是有意的對補償操作進行簡化，因而有：

$hat{H}(x)=H(x)+Delta H(x)$ (3)

$hat{n}(x,dot{x}) = n(x,dot{x})+Delta n(x,dot{x})$ (4)

將式(2)帶入式(1)並聯立(3)(4)，可得：

$ddot{x}=v + (H^{-1} (x)hat{H}(x)-I)v+H^{-1} (x)Delta n(x,dot{x})=v-eta$ (5)

其中 $eta$ 是系統狀態 $[x,dot{x}]^{T}$ 的非線性函數，

$eta =(I-H^{-1} (x)hat{H}(x))v-H^{-1} (x)Delta n(x,dot{x})$ (6)

若希望跟蹤軌跡為 $(x_{d}(t),dot{x}_{d}(t),ddot{x}_{d}(t) )$ ，位置誤差可以定義為 $e=x_{d}-x$ , 其一次時間導數為 $dot{e}=dot{x}_{d}-dot{x}$ ,二次時間導數可由式(5)得到，

$ddot{e}=ddot{x}_{d}-v+eta$ (7)

$qquad$ $qquad$ $qquad$ $qquad$ 有必要推導誤差的動力學方程以研究實際狀態向期望狀態的收斂性。為此設計系統狀態為

$xi =[e,dot{e}]^{T}$ (8)

我們希望系統儘可能的簡化，並且方便處理，所以設計的輸入輸出通常都是線性化表達式，也就是設計輸入 $v$ 應該是跟蹤軌跡和反饋的線性組合，為此不妨令

$v=ddot{x}_d+w(xi)$ (9)

（註：控制系統輸入通常表示為 輸入=前饋+反饋 的形式）

$qquad$ $qquad$ $qquad$ $qquad$ 結合式（5）得到誤差的狀態方程

$dot{xi } = Fxi-Gw(xi)-Geta$ (10)

上式中 $eta (xi,x_{d},dot{x}_{d},ddot{x}_d)$ 和 $eta (x,dot{x})$ 是等價的。

其中 $2n$ 階矩陣 $F$ 和 $2n imes n$ 矩陣 $G$ 服從誤差定義式(8),分別具有如下形式

$F=egin{bmatrix} 0 I \ 0 0 end{bmatrix}quad$ $G=egin{bmatrix} 0 \ I end{bmatrix}quad$ (11)

控制器的設計包括找到使式(10)能夠全局漸進穩定的誤差函數 $w(e)$ ,即有

$lim_{t ightarrow infty}{xi (t)} =0$ (12)

由式(10)描述的系統仍然是非線性耦合系統，因為 $eta$ 是非線性函數。

====================================================================

$qquad$ $qquad$ $qquad$ $qquad$ 下面我們再來討論對非線性項的補償，分為完全補償和非完全補償兩種情況。

對完全的非線性補償( $eta (x,dot{x})=0$ ), 控制方式最簡單的選擇是如下線性控制：

$w(xi )=-K_{P}e-K_{D}dot{e}=egin{bmatrix} -K_{P} -K_{D} end{bmatrix}xi$ (13)

其中誤差方程的漸進穩定性通過選擇正定矩陣 $K_{P}$ 和 $K_{D}$ 提供保證，則動態誤差可以表示為

$dot{xi }=Axi$ (14)

誤差的瞬態表現由矩陣 $A$ 的特徵值來刻畫

$A=egin{bmatrix} 0 I \ -K_{P} -K_{D} end{bmatrix}quad$ (15)

$qquad$ $qquad$ $qquad$ $qquad$ 通常 $K_{P}$ 和 $K_{D}$ 選擇為對角陣，實現關節間的解耦, 二者與系統的自然頻率和阻尼比相關。

將式(14)化簡展開可得

$ddot{e}+K_{D}dot{e}+K_{P}e=0$ (16)

該式表示跟蹤給定軌跡的過程中，位置誤差 $e=x_{d}-x$ 的動態變化。該誤差只有當 $xi (0) e 0$ 時存在,其收斂到零的速度與所選矩陣 $K_{P}$ 和 $K_{D}$ 相關。

$qquad$ $qquad$ $qquad$ $qquad$ 此時的控制框圖為：

線性化逆動力學控制框圖

$qquad$ $qquad$ $qquad$ $qquad$ 所示框圖中用到了兩個反饋迴路：基於機器人動力學模型的內迴路和處理跟蹤誤差的外迴路。內迴路函數是為了得到線性、解耦的輸入輸出關係，而外迴路是為了穩定整個系統。因為外迴路是線性定常系統，控制器設計可以簡化。主要這種基於逆動力學的完全線性化技術需要實時計算慣性陣 $H(x)$ 和哥式力、向心力、重力項 $n(x)$ ，因為控制是以當前系統狀態的非線性反饋為基礎的。

$qquad$ $qquad$ $qquad$ $qquad$ 當然為了應用簡化，可以採用部分前饋補償，以補償機器人運動時最相關的分量項，通常通過計算重力轉矩和慣性陣對角元素相應的慣性轉矩來實現部分靜態補償，而那些來自於其它關節交互的動態項就不能補償。對於重複軌跡，部分補償分量可以實現離線計算。

=====================================================================

$qquad$ $qquad$ $qquad$ $qquad$ 以上部分和 @夕陽的回答是一致的，但從實現的觀點來看，模型的不確定性和逆動力學在線計算的近似補償都是有缺陷的。等有時間的時候，再來補充針對模型不確定的不完全補償（imperfect compensation)的線性化控制做出解釋。

建議想弄明白的同學還是要手動推導一遍上邊的公式，哪怕只是抄寫一遍，也會發現比看n多遍理解要深刻很多~~~

也歡迎各位大神一起討論，共同進步。

參考文獻：

1. Bruno Siciliano, Lorenzo Sciavicco et al. Robotics:Modelling, Planning, and Control

2. Jean-Jacques E. Slotine Applied Nonlinear Control

拋個磚，請各位大神指點。

一、線性化

研究一個問題往往需要一個明確的控制對象，姑且以六自由度機械臂作為該題的研究對象。

通常作為被控對象的機械系統是一個非線性系統，假設排除雜訊的影響（雜訊有可能使系統變為高階），姑且認為該系統是非線性二階系統。

該系統中存在的非線性項主要有科氏力（哥氏力和離心力）、重力、摩擦力等。

因此，一般的動力方程如下：

那麼，對於一個非線性二階系統來說，一般會有以下步驟：

1、設計一個非線性的基於模型的控制率，用來「抵消」被控系統的非線性，即可以設計的控制率中，盡量將科氏力、重力、摩擦力補償掉。

2、將系統化為線性系統，用對應於單位質量系統的簡單線性伺服控制律進行控制。

下圖描述了基本模型的機械臂控制系統：

二、求解方法

一般而言，正動力學演算法有三種：1、以逆動力學演算法為基本計算方法，化為求解具有對稱係數矩陣的N階方程的組的問題；2、從用Newton-Euler迭代方程導出的機器人模型，直接推導出求關節加速度的遞推公式；3、拉格朗日動力學公式，基於能量的動力學法；

一般地，第一種方法比第二種計算量小，而第二種推導一般採用數學力學工具——Spatial
Notation。而拉格朗日動力學公式則是牛頓-歐拉的一種替代。

1. 逆動力學演算法[1]

該方法又被稱為計算力矩法。

機器人方程的最經典的工作是由M.Walker等學者在20世紀80年代初完成的，基本原理是通過機器人方程：

計算力矩法的基本思想是：先在內控制迴路中引入非線性補償，使機器人控制線性化。具體地說，先引入控制：

2.Newton-Euler迭代動力學方程[2]

3.拉格朗日動力學公式[2]

答主目前在應用第一種方法計算，應用和計算都相對比較簡單；另外以計算力矩法為基礎，可以加上一些自適應等控制方法，會有更好的效果；此外，無論對於哪種方法，模型建的是否準確，參數辨識是否做得好，非常影響最後控制效果。

參考文獻：

[1]霍偉.機器人動力學與控制[M].北京:高等教育出版社,2005

[2]John J.Craig著,貟超等譯.機器人學導論(原書第三版)[M].機械工業出版社.2006

用工業機器人領域的術語來講叫computed torque，用控制理論領域的術語來講叫feedback linearization。

具體的有空了再來補充。

機械臂動力學方程是一類特殊的非線性系統，如上述 @Kaixiang Wang 所屬，其線性化方法在機器人學中被稱作逆動力學問題，在控制領域被稱作反饋線性化。下面分別從這兩個角度對機器人動力學方程進行線性化。

如果不考慮機械臂的關節柔性，一個n連桿機械臂的動力學方程為2*n階非線性系統，動力學方程形式如下：

$M( heta)ddot heta+V( heta,dot heta)+G( heta)=u$

其中， $M( heta)$ 為慣量矩陣， $V( heta,dot heta)$ 為離心力和科氏力， $G( heta)$ 為重力項， $u$ 為各個關節力矩向量。

1. 逆動力學角度

逆動力學的思路是：尋找一個非線性反饋控制率如下：

$u=f(q,dot{q},t)$

當將該控制率應用於機器人的動力學方程中時，會得到一個線性的閉環系統。對於般的非線性系統，要找到這種控制率可能相當困難，或者是不可能的任務。然而，通過考察機器人動力學方程，我們可以看出，如果根據下式選擇控制輸入 $u$

$u=M(q)a_q+C(q,dot{q})dot{q}+g(q)$

那麼，由於慣量矩陣M可逆，因而，機器人動力學方程可以整理為：

$ddot{q}=a_q$

上式中的 $a_q$ 項代表一個尚待選擇的新輸入。

上述控制器的設計過程即被稱作逆動力學控制，通過逆動力學控制，我們可以將非線性的機器人動力學問題進行線性化解耦，然後針對每一個自由度分別設計控制器即可。

2. 控制理論反饋線性化角度

一般情況下，一個n連桿機械臂的運動方程表示一個多輸入非線性系統。多輸入系統可反饋線性化的條件非常難以描述，但是其概念思路與單輸入的情形相同。也就是說，尋求一個坐標系統，其中的非線性可以通過一個或者多個輸入被精確抵消。

考慮上述機器人動力學方程，首先，我們可以將其寫為狀態空間的形式，如下：

$dot{x}_1=x_2$

$dot{x}_2=-M(x_1)^{-1}(C(x_1,x_2)x_2+g(x_1))+M(x_1)^{-1}u$

其中， $x_1= heta,x_2=dot{ heta}$ 。在這種情況下，要求解反饋線性化控制，可以通過下列方式進行簡單檢驗：

$u=M(x_1)v+C(x_1,x_2)x_2+g(x_1)$

將上式代入上述狀態方程中，我們可以得到：

$dot{x}_1=x_2$

$dot{x}_2=v$

對比逆動力學結果和反饋線性化結果，可以看到，對於一個剛性機械臂，反饋線性化控制與逆動力學控制正好相同。

雖然逆動力學和反饋線性化均可以實現對機械臂動力學方程的線性化，但顯然，反饋線性化比逆動力學應用範圍要廣得多。其中反饋線性化可以應用於很多非線性系統，如考慮柔性的機械臂動力學方程，獨輪車系統，跳躍機器人等。

計算扭矩控制，要求伺服系統處於力矩模式下，動力學模型參數辨識是個很複雜的問題，模型也是個很複雜的問題，計算力矩控制的實用穩定需要大量的理論基礎和實驗。