如何對機器人動力學矩陣M、C和G進行實時計算？

01-08

各位知乎大神，小弟最近開始做機器人的控制，目前是建立機器人的動力學方程，我用Maple推導出了機器人動力學方程中M、C、和G矩陣的解析表達式，但由於我們的機器人（Baxter）是7自由度的，算出來的矩陣的每一項超級長，以至於計算M矩陣要耗時幾十毫秒，感覺在後面的控制器實施時很難做到模型的實時補償。請問各位知乎大神有什麼好的方法沒？比如簡化這些矩陣的表達式？我目前嘗試的方法是用程序提取出矩陣元素表達式的某一項，比如M11是很長的表達式，先把M11完全展開，然後提取出展開後的式子的每一項，比較每項的係數，如果某些項的係數比另一些項的係數小几個數量級，直接把小的那些項忽略掉。
請問各位知乎大神給一些指導和意見。
謝謝！

不要把整個機器人的M,C,G matrix 全部寫出來展開計算. 可以對每一條kinematics chain,從root link開始,一個關節一個關節向下迭代運算.這樣可以重複利用很多之前的結果。

因為M,C,G矩陣的運算可以考慮成一連串矩陣相乘，考慮計算一串矩陣的乘積:

$T=A_1 imes A_2 imeshdots imes A_n$

如果你把乘積展開算這個T矩陣的每一項,那麼是很耗時間的; 但是如果你先算了 $A_1$ 和 $A_2$ 的乘積,再乘以 $A_3$ ,那麼有很多中間結果是可以反覆利用的.

迭代計算的方法是

$egin{align} T_1 = A_1\ T_2 = T_1 imes A_2\ T_3 = T_2 imes A_3\ vdots\ T_n = T_{n-1} imes A_{n-1} end{align}$

最後的 $T_n$ 就是需要的結果 $T$ 。

如果每個矩陣 $A_i$ 都是的大小都是 $m imes m$ ，並且假設兩個矩陣相乘的複雜度是 $o(m^3)$ ,那麼這樣迭代計算下來的複雜度是 $o(nm^3)$ 。

但是如果把 $T$ 給展開寫成 $A_i$ 的每一項相乘，那麼複雜度是指數形式的。

推薦看看Roy Featherstone的Rigid Body Dynamics Algorithm.他的網頁上有程序算M,C,G矩陣,也是一個link一個link算的. Spatial Vectors

最後安利一下,我們組在開發一個機器人建模和控制的軟體,程序在這裡RobotLocomotion/drake · GitHub. 如果你有baxter的URDF文件,那麼就可以直接算出M,C,G矩陣.

可以轉換問題，用化簡的方法解決，也許是一個思路。不要完全實時計算獲得，有一部分數據從存儲裡面直接獲得。

如果動作是重複性的，或者是配合視覺等感測器可以預見性的，可以在控制裡面可以加入幾個位置記憶點，每個點用末端（機械手）標註，每個點都有各個關節位置記憶，如果可能也可以是多關節時間序列記憶。
比如，目標是將盤子里的工件放到傳送帶上，需要記憶機械手在盤子上方將要開始抓取的位置，和機械手完成抓取在最高點的位置，以及機械手在傳送帶上將要放手的位置。然後可以來回重複播放這兩點的位置,其他位置插補計算得出。每個關節完全可以與其他關節解耦，用2點之間的各自關節的角度差就可以計算平均角速度。即使目標盤子里有很多工件，位置不固定，需要實時計算，也只需要在接近目標時，計算和控制3個自由度就行了。不需要每個關節都動，把冗餘的自由度在軌跡規劃里固定，系統實時性能保證。
如果記憶了多位置點、多關節時間序列，靈活性更高，不過需要視覺反饋和關節參數及時調整，對硬體要求更高。

這個問題很容易解決。

你現在的程序，本質上還是一個符號運算，而且是腳本程序。運算時涉及到的動態東西太多，和純數值運算的速度沒法比。

用Maple可以生成c語言程序(函數)，直接用這個程序進行數值運算，速度快的多，實時無壓力。

看了幾個回答，覺得還是基於我在我們實驗室的經驗補充討論一下。

所謂實時計算在具體應用中我覺得應該在ms左右完成，這樣一般才能滿足實時控制的要求。題主中的7個自由度的解析表達式需要幾十毫秒確實慢了。我們一般處理的是至少20幾個往上的自由度的3D機器人，時間至少可以控制在2ms內。如果機器人的腿的內部仍然有closed chain，就會慢點。通常我們的計算方法和大家說的沒多少差別：

A，一個是生成symbolic expression，然後需要算的時候帶入角度和角速度計算，這個速度其實與怎麼生成表達式還有最後帶入計算表達式的方法有很大關係；

B, 另外就是用迭代，不用symbolic expression，一個link接一個link地算。

具體計算速度和使用的語言、計算步驟還有機器人本身的結構都有很大關係。想說的主要一點就是A可以比B快很多。沒有具體的包不太好比較，就拿兩個用的比較熟的舉例吧：

FROST， 這個是我們實驗室之前畢業的師兄寫的，裡面東西比較多，主要可以用來做trajectory optimization via direct collocation。如果只看生成表達式的方法其實用了Richard Murrary等寫的screw包（mathematica），可以單獨生成M C G的表達式（mex file， c file等），生成C的方法對M進行微分，分成C1 C2 C3三個大的表達式（什麼的1st kind，如果知道是什麼名字的話，我記不得了）。我感覺生成表達式的方法還可以進一步優化。

Spatial Vectors （SVA），這個也有人提到了，方法就是迭代算。

兩個方法我都在一個3D機器人上試過（環境是matlab），用Frost生成的M和G計算比直接用SVA計算要快一個數量級（後者大概需要1ms），計算C用frost比SVA慢五倍（後者還是需要1ms，而前者需要5ms）所以我目前的方法是兩個都用，用frost算M和G，用SVA算C。

所以如果需要快速計算這些矩陣的話，可以都試試。

「用Maple推導出了機器人動力學方程中M、C、和G矩陣的解析表達式」

那就認為你在求一個超長的符號表達式的值。

符號運算是很慢的，即使是Maple，即使是已知完整表達式，僅僅帶入值計算。

我用Matlab時有個方法是把得到的表達式直接列印到文件，然後打開文件，開頭結尾補成一個函數，然後這個文件就直接作為Matlab的一個函數調用，調用時是作為一個算式，而非一個符號表達式，比符號替換快得多（10倍以上）。（我Matlab裝的Maple 的符號引擎，所以速度變化對你的情況也是適用用的）這樣能沿著你的思路計算的同時進行加速。

更好的方法是分步計算， @戴泓楷說的是正道，不需要在程序里存著最終的表達式。

另外，你不能指望拿Maple跑實時控制吧。。

很務實的問題，但很多答案都只是討論拉格朗日演算法與牛頓-歐拉演算法在求解動力學上的區別。要提高計算速度，這種思路是不行的。這裡摘錄《操作機器人的實時動力學》的一些話，最終為了提高計算效率，需要綜合解析-數值來優化求解。

具體使用時確實會對各個關節的慣量矩陣進行簡化，比如只考慮關節軸之間的主要耦合，不考慮次要的交叉耦合作用，具體可以參考「The explicit dynamic model and inertial parameters of the Puma 560 arm」IEEE1986年的文章

據說計算量是完整的10%，但是精度保證在1%以內。

六軸機械臂，用牛頓歐拉迭代法，C語言編寫的動力學函數，在i7處理器下計算173組點的時間是99ms，平均一個點0.57ms，基本可以滿足實時計算要求

牛頓歐拉迭代法參考《Introduction to Robotics, Mechanics and Control Third Edition》, John J. Craig