遞歸牛頓-歐拉動力學算法是一種高效的動力學計算方法，它有正和逆兩種形式，我們先來看正動力學，也就是給定關節(jié)力矩求機器人的運動。下面的算法來自于論文《Lie Group Formulation of Articulated Rigid Body Dynamics》，我們更正了原文中的一處錯誤。該算法使用了李群的表述，其優(yōu)點是形式簡潔、并有清晰的數(shù)學含義。該算法適用于三維空間，每步正動力學計算過程包含三個遞歸過程：1. 前向計算位姿和速度；2. 反向計算廣義慣量和偏置力；3. 前向計算加速度。

具體實現(xiàn)(Mathematica代碼)

(*Initialization 運動參數(shù)初始化*)

time = 2000; dt = 0.0005;

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個連桿的例子進行仿真試驗，連桿之間用轉動關節(jié)連接，機器人初始處于水平靜止狀態(tài)，所有關節(jié)的力矩為0，所以機器人將在重力作用下自由下落，仿真結果如下圖所示(顯示了Y-Z平面)。

正確性驗證

為了驗證算法的正確性，和第三方的仿真軟件進行對比，這里我們選取了Working Model軟件(一款商業(yè)二維動力學仿真軟件)。Working Model中的仿真過程如下圖所示。

我們選擇末端連桿(也就是第4個連桿)質心的Y坐標進行對比，結果如下圖左所示。二者的誤差(下圖右)在-0.001m~0.001m之間(每個連桿的長度為10cm，誤差<1mm)，說明我們算法基本是正確的。

??

下圖中的例子是10自由度的連桿只在重力作用下(關節(jié)力矩為0)的運動，每相鄰的兩個關節(jié)旋轉軸相互垂直。聞到了一絲混沌(Chaos)的味道！這時要對比驗證可以借助三維動力學仿真軟件，例如MSC Adams。但是太繁瑣，我懶得做了：-)

總結

以上是生活随笔為你收集整理的双盘转子动力学仿真c语言程序,递归牛顿欧拉（正）动力学仿真的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內容還不錯，歡迎將生活随笔推薦給好友。