上一篇中我們講到了在座標系{A}和座標系{B}原點重合時，B Q ^BQ B Q
A V Q ^AV_Q A V Q B A R B V Q ^A_BR^BV_Q B A R B V Q A Ω B ^A\Omega_B A Ω B × \times × B A R B Q ^A_BR^BQ B A R B Q
我們可以將左側改寫爲如下形式：
d d t \frac{\mathrm{d}} {\mathrm{d}t} d t d B A R B Q ^A_BR^BQ B A R B Q B A R B V Q ^A_BR^BV_Q B A R B V Q A Ω B ^A\Omega_B A Ω B × \times × B A R B Q ^A_BR^BQ B A R B Q
記住這種形式的方程，會在求解加速度方程時很有用。B Q ^BQ B Q
A V ˙ Q ^A\dot{V}_Q A V ˙ Q d d t \frac{\mathrm{d}} {\mathrm{d}t} d t d B A R B V Q ^A_BR^BV_Q B A R B V Q A Ω ˙ B ^A\dot{\Omega}_B A Ω ˙ B × \times × B A R B Q ^A_BR^BQ B A R B Q A Ω B ^A\Omega_B A Ω B × \times × d d t \frac{\mathrm{d}} {\mathrm{d}t} d t d B A R B Q ^A_BR^BQ B A R B Q
對式5.3右側的第一項和最後一項代入式5.2，整理後可得，
A V ˙ Q ^A\dot{V}_Q A V ˙ Q B A R B V ˙ Q ^A_BR^B\dot{V}_Q B A R B V ˙ Q A Ω B ^A\Omega_B A Ω B × \times × B A R B V Q ^A_BR^BV_Q B A R B V Q A Ω ˙ ^A\dot{\Omega} A Ω ˙ × \times × B A R B Q ^A_BR^BQ B A R B Q A Ω B ^A\Omega_B A Ω B × \times × A Ω B ^A\Omega_B A Ω B × \times × B A R B Q ^A_BR^BQ B A R B Q
將上式擴展到原點不重合的情況，就得到了一般的表達式：
A V ˙ Q ^A\dot{V}_Q A V ˙ Q A V ˙ B O R G ^A\dot{V}_{BORG} A V ˙ B O R G B A R B V ˙ Q ^A_BR^B\dot{V}_Q B A R B V ˙ Q A Ω B ^A\Omega_B A Ω B × \times × B A R B V Q ^A_BR^BV_Q B A R B V Q A Ω ˙ ^A\dot{\Omega} A Ω ˙ × \times × B A R B Q ^A_BR^BQ B A R B Q A Ω B ^A\Omega_B A Ω B × \times × A Ω B ^A\Omega_B A Ω B × \times × B A R B Q ^A_BR^BQ B A R B Q
另外要指出，當計算旋轉關節時，B Q ^BQ B Q
B V Q ^BV_Q B V Q B V ˙ Q ^B\dot{V}_Q B V ˙ Q
A V ˙ Q ^A\dot{V}_Q A V ˙ Q A V ˙ B O R G ^A\dot{V}_{BORG} A V ˙ B O R G A Ω ˙ ^A\dot{\Omega} A Ω ˙ × \times × B A R B Q ^A_BR^BQ B A R B Q A Ω B ^A\Omega_B A Ω B × \times × A Ω B ^A\Omega_B A Ω B × \times × B A R B Q ^A_BR^BQ B A R B Q
接下來我們推導角加速度公式。假設座標系{B}以角速度A Ω B ^A\Omega_B A Ω B B Ω C ^B\Omega_C B Ω C
A Ω C ^A\Omega_C A Ω C A Ω B ^A\Omega_B A Ω B B A R B Ω C ^A_BR^B\Omega_C B A R B Ω C
對式5.6求導可得，
A Ω ˙ C ^A\dot{\Omega}_C A Ω ˙ C A Ω ˙ B ^A\dot{\Omega}_B A Ω ˙ B d d t ( B A R B Ω C ) \frac{\mathrm{d}}{\mathrm{d}t}(^A_BR^B\Omega_C) d t d ( B A R B Ω C )
同樣我們將式5.2帶入上式最後一項，可以得到，
A Ω ˙ C ^A\dot{\Omega}_C A Ω ˙ C A Ω ˙ B ^A\dot{\Omega}_B A Ω ˙ B B A R B Ω ˙ C ^A_BR^B\dot{\Omega}_C B A R B Ω ˙ C A Ω B ^A\Omega_B A Ω B × \times × B A R B Ω C ^A_BR^B\Omega_C B A R B Ω C
上式用於計算機械臂連桿的角加速度。
這裏引入慣性張量（inertia tensor） 的概念，他可以表徵剛體質量分佈的方式，是對物體慣性矩的廣義度量。給出如下3 × \times ×
A I ^AI A I ( I x x − I x y − I x z − I x y I y y − I y z − I x z − I y z I z z ) \begin{pmatrix}
I_{xx} & -I_{xy} & -I_{xz}\\
-I_{xy} & I_{yy} & -I_{yz}\\
-I_{xz} & -I_{yz} & I_{zz}
\end{pmatrix} ⎝ ⎛ I x x − I x y − I x z − I x y I y y − I y z − I x z − I y z I z z ⎠ ⎞
其中各個元素分別爲：
I x x I_{xx} I x x ∭ V ( y 2 + z 2 ) ρ  d v \iiint_{V}^{}(y^2 + z^2)\rho\,dv ∭ V ( y 2 + z 2 ) ρ d v I y y I_{yy} I y y ∭ V ( x 2 + z 2 ) ρ  d v \iiint_{V}^{}(x^2 + z^2)\rho\,dv ∭ V ( x 2 + z 2 ) ρ d v I z z I_{zz} I z z ∭ V ( x 2 + y 2 ) ρ  d v \iiint_{V}^{}(x^2 + y^2)\rho\,dv ∭ V ( x 2 + y 2 ) ρ d v I x y I_{xy} I x y ∭ V x y ρ  d v \iiint_{V}^{}xy\rho\,dv ∭ V x y ρ d v I y z I_{yz} I y z ∭ V x z ρ  d v \iiint_{V}^{}xz\rho\,dv ∭ V x z ρ d v I z x I_{zx} I z x ∭ V y z ρ  d v \iiint_{V}^{}yz\rho\,dv ∭ V y z ρ d v
I x x I_{xx} I x x I y y I_{yy} I y y I z z I_{zz} I z z 慣性矩（mass moments of inertia） ，其餘三個稱爲慣量積(mass products of inertia) 。我們在選擇座標軸時，最好使得剛體的慣量積爲零，此時座標系的軸也被稱爲主軸（principal axes） ，相應的慣量矩被稱爲主慣性矩（principal moments of inertia） 。
這個方法就是假設已知關節的位置、速度、加速度（Θ \Theta Θ Θ ˙ \dot{\Theta} Θ ˙ Θ ¨ \ddot{\Theta} Θ ¨ 外推法 ，運用運動學知識，通過對每個連桿運用牛頓-歐拉方程依次從連桿1到連桿n向外遞推計算連桿的速度和加速度；牛頓歐拉方程 ，通過每個連桿質心的速度和加速度，計算作用在連桿質心上的慣性力和力矩；內推法 ，通過連桿間的相互作用力和力矩，從連桿n到連桿1向內迭代計算關節驅動力矩。從算法角度的話，算到外推法裏很合理，同一個循環算了速度和加速度，接着算力和力矩；但是從理解角度，把這部分算到內推法裏也很合理，外推法運動學算速度和加速度，內推法算力和力矩 。老鐵沒毛病！所以怎麼理解就看大家了。。。
上一篇雅可比矩陣部分我們已經講過了速度的“傳遞”問題：
i + 1 ω i + 1 ^{i+1}\omega_{i+1} i + 1 ω i + 1 i i + 1 R _i^{i+1}R i i + 1 R i ω i ^i\omega_i i ω i θ ˙ i + 1 \dot{θ}_{i+1} θ ˙ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1
那麼再根據式5.2，
d d t \frac{\mathrm{d}} {\mathrm{d}t} d t d B A R B Q ^A_BR^BQ B A R B Q B A R B V Q ^A_BR^BV_Q B A R B V Q A Ω B ^A\Omega_B A Ω B × \times × B A R B Q ^A_BR^BQ B A R B Q
就可以推導出角加速度的變換公式：
i + 1 ω ˙ i + 1 ^{i+1}\dot{\omega}_{i+1} i + 1 ω ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R i ω ˙ i ^i\dot{\omega}_i i ω ˙ i θ ¨ i + 1 \ddot{θ}_{i+1} θ ¨ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1 i i + 1 R i ω i ^{i+1}_iR^i\omega_i i i + 1 R i ω i × \times × θ ¨ i + 1 \ddot{\theta}_{i+1} θ ¨ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1
如果第i+1個關節式移動關節，那麼上式就可以化簡爲：
i + 1 ω ˙ i + 1 ^{i+1}\dot{\omega}_{i+1} i + 1 ω ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R i ω ˙ i ^i\dot{\omega}_i i ω ˙ i
接下來是推導線加速度公式，首先以免大家遺忘，再給出線速度公式，
i + 1 v i + 1 ^{i+1}v_{i+1} i + 1 v i + 1 i i + 1 R _i^{i+1}R i i + 1 R ( i v i + i ω i × i P i + 1 ) \left( \ ^iv_i+ ^i\omega_i \times ^iP_{i+1} \right) ( i v i + i ω i × i P i + 1 )
應用式5.6如下，
A V ˙ Q ^A\dot{V}_Q A V ˙ Q A Ω ˙ ^A\dot{\Omega} A Ω ˙ × \times × B A R B Q ^A_BR^BQ B A R B Q A Ω B ^A\Omega_B A Ω B × \times × A Ω B ^A\Omega_B A Ω B × \times × B A R B Q ^A_BR^BQ B A R B Q
就可以得到每個連桿座標系原點的線加速度，
i + 1 v ˙ i + 1 ^{i+1}\dot{v}_{i+1} i + 1 v ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ] \left[\ ^i\dot{v}_i+ ^i\dot{\omega}_i \times ^iP_{i+1} + ^i\omega_i \times \left( ^i\omega_i \times ^iP_{i+1}\right)\right] [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ]
如果第i+1個關節是移動關節，那麼上式就應當爲：
i + 1 v ˙ i + 1 ^{i+1}\dot{v}_{i+1} i + 1 v ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ] + 2 i + 1 ω i + 1 × d ˙ i + 1 i + 1 Z ^ i + 1 + d ¨ i + 1 i + 1 Z ^ i + 1 \left[\ ^i\dot{v}_i+ ^i\dot{\omega}_i \times ^iP_{i+1} + ^i\omega_i \times \left( ^i\omega_i \times ^iP_{i+1}\right)\right] + 2^{i+1}\omega_{i+1} \times \dot{d}_{i+1}\\^{i+1}\hat{Z}_{i+1} + \ddot{d}_{i+1}\\^{i+1}\hat{Z}_{i+1} [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ] + 2 i + 1 ω i + 1 × d ˙ i + 1 i + 1 Z ^ i + 1 + d ¨ i + 1 i + 1 Z ^ i + 1
同理我們可以得到每個連桿質心的線加速度：
i v ˙ C i ^i\dot{v}_{C_i} i v ˙ C i i ω ˙ i × i P C i + i ω i × ( i ω i × i P C I ) + i v ˙ i ^i\dot{\omega}_i \times \\^iP_{C_i} + \\^i\omega_i \times \left( ^i\omega_i \times \\^iP_{C_I}\right) + ^i\dot{v}_i i ω ˙ i × i P C i + i ω i × ( i ω i × i P C I ) + i v ˙ i
整理一下外推法，就是以下公式：
i + 1 ω i + 1 ^{i+1}\omega_{i+1} i + 1 ω i + 1 i i + 1 R _i^{i+1}R i i + 1 R i ω i ^i\omega_i i ω i θ ˙ i + 1 \dot{θ}_{i+1} θ ˙ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1
i + 1 ω ˙ i + 1 ^{i+1}\dot{\omega}_{i+1} i + 1 ω ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R i ω ˙ i ^i\dot{\omega}_i i ω ˙ i θ ¨ i + 1 \ddot{θ}_{i+1} θ ¨ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1 i i + 1 R i ω i ^{i+1}_iR^i\omega_i i i + 1 R i ω i × \times × θ ¨ i + 1 \ddot{\theta}_{i+1} θ ¨ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1
i + 1 v i + 1 ^{i+1}v_{i+1} i + 1 v i + 1 i i + 1 R _i^{i+1}R i i + 1 R ( i v i + i ω i × i P i + 1 ) \left( \ ^iv_i+ ^i\omega_i \times ^iP_{i+1} \right) ( i v i + i ω i × i P i + 1 )
i + 1 v ˙ i + 1 ^{i+1}\dot{v}_{i+1} i + 1 v ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ] \left[\ ^i\dot{v}_i+ ^i\dot{\omega}_i \times ^iP_{i+1} + ^i\omega_i \times \left( ^i\omega_i \times ^iP_{i+1}\right)\right] [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ]
i + 1 v ˙ C i + 1 ^{i+1}\dot{v}_{C_{i+1}} i + 1 v ˙ C i + 1 i + 1 ω ˙ i + 1 × i + 1 P C i + 1 + i + 1 ω i + 1 × ( i + 1 ω i + 1 × i + 1 P C i + 1 ) + i + 1 v ˙ i + 1 ^{i+1}\dot{\omega}_{i+1} \times \\^{i+1}P_{C_{i+1}} + \\^{i+1}\omega_{i+1} \times \left( ^{i+1}\omega_{i+1} \times \\^{i+1}P_{C_{i+1}}\right) + ^{i+1}\dot{v}_{i+1} i + 1 ω ˙ i + 1 × i + 1 P C i + 1 + i + 1 ω i + 1 × ( i + 1 ω i + 1 × i + 1 P C i + 1 ) + i + 1 v ˙ i + 1
前面通過外推法計算出每個連桿質心的線加速度和角加速度後，運用牛頓-歐拉公式便可計算出作用在連桿質心上的慣性力和力矩：
F i F_i F i m v ˙ C i m\dot{v}_{C_i} m v ˙ C i
N i N_i N i C i I ω ˙ i + ω i × C i I ω i ^{C_i}I\dot{\omega}_i + \omega_i \times \\^{C_i}I\omega_i C i I ω ˙ i + ω i × C i I ω i
式中座標系{C i C_i C i i i i
上一節計算出每個連桿上的力和力矩後，我們就要計算出產生這些力和力矩的需要施加在關節上的關節力矩。f i f_i f i i − 1 i-1 i − 1 i i i n i n_i n i i − 1 i-1 i − 1 i i i i i i
i F i ^iF_i i F i i f i − i + 1 i R ^if_i - ^i_{i+1}R i f i − i + 1 i R i + 1 f i + 1 ^{i+1}f_{i+1} i + 1 f i + 1
將所有作用在質心上的力矩相加，得到力矩平衡方程：
i N i ^iN_i i N i i n i − i n i + 1 + ( − i P C i ) × i f i − ( i P i + 1 − i P C i ) × i f i + 1 ^in_i - \\^in_{i+1} + \left(-^iP_{C_i}\right) \times \\^if_i - \left(^iP_{i+1} -\\^iP_{C_i}\right) \times\\^if_{i+1} i n i − i n i + 1 + ( − i P C i ) × i f i − ( i P i + 1 − i P C i ) × i f i + 1
利用力平衡方程和附加旋轉矩陣的辦法，可以將上式化簡爲
i N i ^iN_i i N i i n i − i + 1 i R i n i + 1 − i P C i × i F i − i P i + 1 × i + 1 i R ^in_i - \\^i_{i+1}R \\ ^in_{i+1} -\\^iP_{C_i} \times \\^iF_i - ^iP_{i+1}\times\\ ^i_{i+1}R i n i − i + 1 i R i n i + 1 − i P C i × i F i − i P i + 1 × i + 1 i R i f i + 1 ^if_{i+1} i f i + 1
最後我們排列一下力和力矩方程，讓相鄰連桿按照序號大的向序號小的順序（也就是內推）來迭代：
i f i ^if_i i f i i F i + i + 1 i R ^iF_i + \\^i_{i+1}R i F i + i + 1 i R i + 1 f i + 1 ^{i+1}f_{i+1} i + 1 f i + 1
i n i ^in_i i n i i N i + i + 1 i R i n i + 1 + i P C i × i F i + i P i + 1 × i + 1 i R ^iN_i + \\^i_{i+1}R \\ ^in_{i+1} +\\^iP_{C_i} \times \\^iF_i + \\^iP_{i+1}\times\\ ^i_{i+1}R i N i + i + 1 i R i n i + 1 + i P C i × i F i + i P i + 1 × i + 1 i R i f i + 1 ^if_{i+1} i f i + 1
那麼怎麼求得關節力矩呢？Z ^ \hat{Z} Z ^
τ i \tau_i τ i i n i T i Z ^ i ^in_i\\^{Ti}\hat{Z}_i i n i T i Z ^ i
如果是移動關節，則有τ i \tau_i τ i i f i T i Z ^ i ^if_i\\^{Ti}\hat{Z}_i i f i T i Z ^ i
式5.23和式5.24中的τ \tau τ
我們把前面三節推導的結果整理以下，就是這個算法啦！外推：i : 0 → 5 i : 0 \to 5 i : 0 → 5
i + 1 ω i + 1 ^{i+1}\omega_{i+1} i + 1 ω i + 1 i i + 1 R _i^{i+1}R i i + 1 R i ω i ^i\omega_i i ω i θ ˙ i + 1 \dot{θ}_{i+1} θ ˙ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1
i + 1 ω ˙ i + 1 ^{i+1}\dot{\omega}_{i+1} i + 1 ω ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R i ω ˙ i ^i\dot{\omega}_i i ω ˙ i θ ¨ i + 1 \ddot{θ}_{i+1} θ ¨ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1 i i + 1 R i ω i ^{i+1}_iR^i\omega_i i i + 1 R i ω i × \times × θ ¨ i + 1 \ddot{\theta}_{i+1} θ ¨ i + 1 i + 1 Z ^ i + 1 ^{i+1}\hat{Z}_{i+1} i + 1 Z ^ i + 1
i + 1 v i + 1 ^{i+1}v_{i+1} i + 1 v i + 1 i i + 1 R _i^{i+1}R i i + 1 R ( i v i + i ω i × i P i + 1 ) \left( \ ^iv_i+ ^i\omega_i \times ^iP_{i+1} \right) ( i v i + i ω i × i P i + 1 )
i + 1 v ˙ i + 1 ^{i+1}\dot{v}_{i+1} i + 1 v ˙ i + 1 i i + 1 R _i^{i+1}R i i + 1 R [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ] \left[\ ^i\dot{v}_i+ ^i\dot{\omega}_i \times ^iP_{i+1} + ^i\omega_i \times \left( ^i\omega_i \times ^iP_{i+1}\right)\right] [ i v ˙ i + i ω ˙ i × i P i + 1 + i ω i × ( i ω i × i P i + 1 ) ]
i + 1 v ˙ C i + 1 ^{i+1}\dot{v}_{C_{i+1}} i + 1 v ˙ C i + 1 i + 1 ω ˙ i + 1 × i + 1 P C i + 1 + i + 1 ω i + 1 × ( i + 1 ω i + 1 × i + 1 P C i + 1 ) + i + 1 v ˙ i + 1 ^{i+1}\dot{\omega}_{i+1} \times \\^{i+1}P_{C_{i+1}} + \\^{i+1}\omega_{i+1} \times \left( ^{i+1}\omega_{i+1} \times \\^{i+1}P_{C_{i+1}}\right) + ^{i+1}\dot{v}_{i+1} i + 1 ω ˙ i + 1 × i + 1 P C i + 1 + i + 1 ω i + 1 × ( i + 1 ω i + 1 × i + 1 P C i + 1 ) + i + 1 v ˙ i + 1
i + 1 F i + 1 ^{i+1}F_{i+1} i + 1 F i + 1 m i + 1 i + 1 v ˙ C i + 1 m_{i+1}\\^{i+1}\dot{v}_{C_{i+1}} m i + 1 i + 1 v ˙ C i + 1
i + 1 N i + 1 ^{i+1}N_{i+1} i + 1 N i + 1 C i + 1 I i + 1 i + 1 ω ˙ i + 1 + i + 1 ω i + 1 × C i + 1 I i + 1 ω i + 1 ^{C_{i+1}}I_{i+1}\\^{i+1}\dot{\omega}_{i+1} + \\^{i+1}\omega_{i+1} \times \\^{C_{i+1}}I\\^{i+1}\omega_{i+1} C i + 1 I i + 1 i + 1 ω ˙ i + 1 + i + 1 ω i + 1 × C i + 1 I i + 1 ω i + 1
內推：i : 6 → 1 i : 6 \to 1 i : 6 → 1
i f i ^if_i i f i i F i + i + 1 i R ^iF_i + \\^i_{i+1}R i F i + i + 1 i R i + 1 f i + 1 ^{i+1}f_{i+1} i + 1 f i + 1
i n i ^in_i i n i i N i + i + 1 i R i n i + 1 + i P C i × i F i + i P i + 1 × i + 1 i R ^iN_i + \\^i_{i+1}R \\ ^in_{i+1} +\\^iP_{C_i} \times \\^iF_i + \\^iP_{i+1}\times\\ ^i_{i+1}R i N i + i + 1 i R i n i + 1 + i P C i × i F i + i P i + 1 × i + 1 i R i f i + 1 ^if_{i+1} i f i + 1
τ i \tau_i τ i i n i T i Z ^ i ^in_i\\^{Ti}\hat{Z}_i i n i T i Z ^ i
如果要考慮重力的話，只需要令0 v ˙ 0 ^0\dot{v}_0 0 v ˙ 0 G G G
用牛頓-歐拉方程對機械臂進行分析時，動力學方程可以表示爲如下形式
τ \tau τ M ( Θ ) Θ ¨ + V ( Θ , Θ ˙ ) + G ( Θ ) M\left(\Theta \right) \ddot{\Theta} \ +\ V\left(\Theta, \dot{\Theta}\right) + \ G\left(\Theta \right) M ( Θ ) Θ ¨ + V ( Θ , Θ ˙ ) + G ( Θ )
式中M ( Θ ) M\left(\Theta \right) M ( Θ ) n × n n \times n n × n 質量矩陣 ，V ( Θ , Θ ˙ ) V\left(\Theta, \dot{\Theta}\right) V ( Θ , Θ ˙ ) n × 1 n \times 1 n × 1 離心力 和哥氏力矢量 ，G ( Θ ) G\left(\Theta \right) G ( Θ ) n × 1 n \times 1 n × 1 重力矢量 。質量矩陣可分爲四塊，主對角線的兩塊爲有效慣量，副對角線上的兩塊爲耦合慣量；哥氏力部分在高速時影響較爲顯著。
總結 ：牛頓歐拉法邏輯還是很嚴密的，如果跟着一起推導的話，理解起來並不是太難，就怕一上來看到算法的一大坨公式就第一印象覺得很難，這樣的心態在學習新知識的時候是最要不得的。說起來，剛學完運動學部分覺得很簡單，然後甫一接觸動力學部分，看到成片公式也舉得有點煩躁。但是如我所說，一條條自己推導，結合實際去理解也就過去了。學習本就不是一件容易的事，在這個過程裏體會數學和力學的魅力豈不美哉（Or2）。。。
牛頓-歐拉方法是建立在動力學基本公式 以及作用在連桿之間的約束力和力矩 之上的。而另一種方法，也就是這一節要講的拉格朗日動力學方程法，則是從能量角度 分析的，當然，最終兩種分析方法得到的結果應該是相同的。
首先分析第i i i k i k_i k i
k i k_i k i 1 2 m i v C i T v C i + 1 2 i ω i T C i I i i ω i \frac{1}{2}m_iv_{C_i}^T v_{C_i} + \frac{1}{2}^i\omega_i^T \ ^{C_i}I_i \ ^i\omega_i 2 1 m i v C i T v C i + 2 1 i ω i T C i I i i ω i
式中第一項是基於連桿質心線速度的動能，第二項是連桿的角速度的動能。機械臂的動能是所有連桿動能之和，亦即
k k k ∑ i = 1 n k i \sum_{i=1}^n k_i ∑ i = 1 n k i
式5.24中的v C i v_{C_i} v C i i ω i ^i\omega_i i ω i Θ \Theta Θ Θ ˙ \dot{\Theta} Θ ˙
k ( Θ , Θ ˙ ) k\left(\Theta, \dot{\Theta}\right) k ( Θ , Θ ˙ ) 1 2 Θ ˙ T M Θ ˙ \frac{1} {2} \dot{\Theta}^TM\dot{\Theta} 2 1 Θ ˙ T M Θ ˙
接下來我們考慮連桿的勢能。第i i i u i u_i u i
u i u_i u i − m i 0 g T 0 P C i + u r e f -m_i \ ^0g^T \ ^{0}P_{C_i} + u_{ref} − m i 0 g T 0 P C i + u r e f
注意這裏的0 g ^0g 0 g 3 × 1 3 \times 1 3 × 1 0 P C i ^0P_{C_i} 0 P C i i i i
機械臂的總勢能爲各個連桿勢能之和，即
u u u ∑ i = 1 n u i \sum_{i=1}^nu_i ∑ i = 1 n u i
拉格朗日動力學公式爲我們提供了一種利用標量函數來推導動力學方程的方法，這個標量函數就是拉格朗日函數，亦即一個機械系統的動能和勢能之差，它可以表示爲
L ( Θ , Θ ˙ ) \mathcal{L}\left(\Theta,\dot{\Theta}\right) L ( Θ , Θ ˙ ) k ( Θ , Θ ˙ ) − u ( Θ , Θ ˙ ) k\left(\Theta,\dot{\Theta}\right) - u\left(\Theta,\dot{\Theta}\right) k ( Θ , Θ ˙ ) − u ( Θ , Θ ˙ )
則機械臂的運動方程爲
d d t ∂ L ∂ Θ ˙ − ∂ L ∂ Θ = τ \cfrac{\mathrm{d}}{\mathrm{d}t}\cfrac{\partial \mathcal{L}}{\partial \dot{\Theta}} - \cfrac{\partial \mathcal{L}}{\partial \Theta} = \tau d t d ∂ Θ ˙ ∂ L − ∂ Θ ∂ L = τ
進一步得到
d d t ∂ k ∂ Θ ˙ − ∂ k ∂ Θ + ∂ u ∂ Θ = τ \cfrac{\mathrm{d}}{\mathrm{d}t}\cfrac{\partial k}{\partial \dot{\Theta}} - \cfrac{\partial k}{\partial \Theta} + \cfrac{\partial u}{\partial \Theta}= \tau d t d ∂ Θ ˙ ∂ k − ∂ Θ ∂ k + ∂ Θ ∂ u = τ
以上就是機械臂動力學的拉格朗日方程。當時自己學完這一部分突然聯想到，高中做力學物理題的時候（就是那些小物塊，斜面，圓坑一類的）往往按受力分析去做會很麻煩，但是如果用能量守恆的動能勢能方程做就很簡單。這一點在本章分析機械臂的動力學方程也是這樣，算是一點挺有趣的事情。
動力學部分終於也總結完了。相比運動學部分來說確實更有難度一點，但是還是那句話，自己多手算多推導，多聯想。這一部分耗費心力太多，到現在已經懶於舉一個實例或者MATLAB代碼了。另外一個收穫，就是這幾天上手了一點LaTex輸入公式的技巧，算是意外收穫吧。
