遞迴牛頓-尤拉方法（Recursive Newton-Euler Method）是一種高效的動力學計算方法，尤其適用於串聯多剛體系統，例如串聯機械臂。遞迴牛頓-尤拉方法有正和逆兩種形式，本文我們先來看正動力學。正動力學又稱為前向動力學，如果給你機器人各關節的力矩後，通過正動力學可以求出機器人的運動。在這裡，機器人的運動指各個關節（或者各連桿）的位置、速度和加速度。

下面的演算法來自於論文《Lie Group Formulation of Articulated Rigid Body Dynamics》，我們更正了原文中的一處小錯誤。該演算法使用了李群和李代數表示機器人連桿的座標、速度和受力，其優點是形式簡潔、線速度和角速度可以合寫在一塊，力也是如此，並有清晰的數學含義。該演算法適用於三維空間，每一步正動力學計算過程包含三個遞迴子過程：1. 前向計算連桿的位姿和速度；2. 反向計算連桿系的廣義慣量和偏置力；3. 前向計算加速度。在定義連桿的記號時，通常將固定的基座記為 0，與基座

具體實現（Mathematica程式碼）

    (*Initialization 部分引數初始化*)
    time = 2000; dt = 0.005;  
    Table[mass[i] = 1; Gravity[i] = grav*mass[i]*{0, 0, -1, 0, 0, 0}, {i, 0, n, 1}];  
    Table[g[i, i + 1, 0] = RPToH[Id[3], {0, 0, (La[i] + La[i + 1])/2}], {i, 0, n - 1, 1}];  
    q = dq = ddq = ConstantArray[0, n];  
    Table[V[i] = dV[i] = ConstantArray[0, 6], {i, 0, n, 1}];  
    Table[M[i] = Id[6]; \[Tau][i] = 0, {i, n}];  
    F[n + 1] = ConstantArray[0, 6];  
    g[n, n + 1] = g[0, 0] = Id[4];  
    q = ConstantArray[Pi/2, n];  
    \[CapitalPi][n + 1] = Id[6]*0.0;  
    \[Beta][n + 1] = ConstantArray[0, 6];  
    Table[  
       qList = {qList, q};  
       gList = {gList, g[0, 4]};  
       (*Forward 前向遞迴*)  
       dq = dq + ddq*dt;   (*尤拉積分*)
       q = q + dq*dt;  
       For[i = 1, i <= n, i++,  
        g[i - 1, i] = TwistExp[\[Xi]r[i], q[[i]]].g[i - 1, i, 0];  
        g[0, i] = g[0, i - 1].g[i - 1, i];  
        V[i] = Ad[Iv[g[i - 1, i]]].V[i - 1] + \[Xi]s[i]*dq[[i]];  
        \[Eta][i] = ad[V[i] - \[Xi]s[i]*dq[[i]]].\[Xi]s[i]*dq[[i]];  
        ];  
       (*Backward 反向遞迴*)  
       For[i = n, i >= 1, i--,  
        \[Tau][i] = 0;  
        Mh[i] = M[i] + T[Ad[Iv[g[i, i + 1]]]].\[CapitalPi][i + 1].Ad[Iv[g[i, i + 1]]];  
        Fext[i] = T[Ad[RPToH[R[g[0, i]], {0, 0, 0}]]].Gravity[i];  
        \[ScriptCapitalB][i] = -T[ad[V[i]]].M[i].V[i] - Fext[i] + T[Ad[Iv[g[i, i + 1]]]].\[Beta][i + 1];  
        \[CapitalPsi][i] = 1/(\[Xi]s[i].Mh[i].\[Xi]s[i]);  
        \[CapitalPi][i] = Mh[i] - \[CapitalPsi][i]*KroneckerProduct[Mh[i].\[Xi]s[i], \[Xi]s[i].Mh[i]];  
        \[Beta][i] = \[ScriptCapitalB][i] + Mh[i].(\[Eta][i] + \[Xi]s[i]*\[CapitalPsi][i]*(\[Tau][i] - \[Xi]s[i].(Mh[i].\[Eta][i] + \[ScriptCapitalB][i])));  
        ];  
       (*Forward 前向遞迴*)  
       For[i = 1, i <= n, i++,  
        ddq[[i]] = \[CapitalPsi][i]*(\[Tau][i] - \[Xi]s[i].Mh[i].(Ad[Iv[g[i - 1, i]]].dV[i - 1] + \[Eta][i]) - \[Xi]s[i].\[ScriptCapitalB][i]);  
        dV[i] = Ad[Iv[g[i - 1, i]]].dV[i - 1] + \[Xi]s[i]*ddq[[i]] + \[Eta][i]];  
       , {t, time}]; 
 

模擬結果

我們首先以簡單的 4 個連桿組成的機械臂為例進行模擬試驗，連桿之間用轉動關節連線，機器人初始時刻處於水平的靜止狀態，所有關節的力矩設為0。理論上機器人應該在重力作用下自由下落，我們看看用正向動力學步驟計算得到的模擬結果是什麼樣的。結果如下動畫所示（只顯示了Y-Z平面）。從結果看好像是對的，但是我還不敢100%保證。

正確性驗證

為了驗證演算法的正確性，我和第三方的模擬軟體進行對比。這裡我選取了Working Model軟體（是一款商業的二維動力學模擬軟體）。在Working Model中設定相同的引數和初始狀態，模擬過程如下圖所示。

我們選擇末端連桿（也就是第4個連桿）質心的Y座標（重力加速度的反方向）進行對比，結果如下圖左所示。二者的誤差（下圖右）在-0.001m~0.001m之間。考慮到每個連桿的長度為10cm，誤差<1mm 說明我們演算法基本是正確的。因為都是數值計算，有誤差應該是正常的。至於為什麼誤差在0.5mm這個量級，這可能和我們採用的積分方法有關。在上面的演算法中，我們採用了最簡單的尤拉積分。而在Working Model中，因為這個軟體不是開源的，我們也看不到它的積分方法是怎樣實現的。不過我們倒是可以設定，在Edit選單下的Accuracy選項中即可選擇積分方法並設定積分步長。

下圖中的例子是10自由度的連桿同樣只在重力作用下（關節力矩為0）的運動，只是初始狀態不同，而且每相鄰的兩個關節旋轉軸相互垂直。有沒有聞到一絲混沌（Chaos）的味道