這裡分享一下，搬個磚，一個有大神已經針對這個知識給了詳細的解答。

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

(1)

其中表示系統的狀態向量（可以理解為關節空間或任務空間變數）, 是輸入向量(可以理解為輸入力矩), 是依賴於的正定矩陣,是依賴於系統狀態的n維向量(也就是科式力項、向心力、摩擦力和重力等非線性耦合項). 該模型是非線性多變數控制系統。一下方法的基本思想是找到控制量,該向量是系統狀態的函式，可以以此實現線性形式的輸入\輸出關係。換句話說，可以透過非線性狀態反饋方法(nonlinear state feedback，逆動力學控制, inverse dynamics control) 實現系統動力學的線性化。透過非線性補償可以得到如下控制律：

(2)

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

(3)

(4)

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

(5)

其中是系統狀態的非線性函式，

(6)

若希望跟蹤軌跡為，位置誤差可以定義為, 其一次時間導數為 ,二次時間導數可由式(5)得到，

(7)

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

(8)

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

(9)

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

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

(10)

上式中和是等價的。

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

(11)

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

(12)

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

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

對完全的非線性補償(), 控制方式最簡單的選擇是如下線性控制：

(13)

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

(14)

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

(15)

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

將式(14)化簡展開可得

(16)

該式表示跟蹤給定軌跡的過程中，位置誤差的動態變化。該誤差只有當時存在,其收斂到零的速度與所選矩陣和相關。

此時的控制框圖為：

線性化逆動力學控制框圖

所示框圖中用到了兩個反饋迴路：基於機器人動力學模型的內迴路和處理跟蹤誤差的外迴路。內迴路函式是為了得到線性、解耦的輸入輸出關係，而外迴路是為了穩定整個系統。因為外迴路是線性定常系統，控制器設計可以簡化。主要這種基於逆動力學的完全線性化技術需要實時計算慣性陣和哥式力、向心力、重力項，因為控制是以當前系統狀態的非線性反饋為基礎的。當然為了應用簡化，可以採用部分前饋補償，以補償機器人運動時最相關的分量項，通常透過計算重力轉矩和慣性陣對角元素相應的慣性轉矩來實現部分靜態補償，而那些來自於其它關節互動的動態項就不能補償。對於重複軌跡，部分補償分量可以實現離線計算。