運動量・角運動量ヤコビアン

シリアルリンクマニピュレータを対象とし， 運動量・角運動量ヤコビアンを導出する． 運動量・原点まわりの角運動量を各関節変数で表現し， その偏微分でヤコビアンの行を計算する．

第$j$関節の運動変数を$\theta_j$とする． まず，回転・並進の1自由度関節を考える．



ここで， $[\tilde{m}_j, \tilde{\mbox{\boldmath {$c$}}}_j, \tilde{\mbox{\boldmath {$I$}}}_j]$は AddMassProperty関数に第$j$関節の子リンクより 末端側のリンクのマスプロパティを与えたものであり， 実際には再帰計算により計算する¹⁷． これらを $\dot{\theta}_j$で割ることにより ヤコビアンの各列ベクトルを得る．



これより慣性行列は次のように計算できる．

$$\mbox{\boldmath {$M$}} _{\dot{\mbox{\boldmath {$\theta$}}}} = [\mbox{\boldmath {$m$}}_1, \hdots, \mbox{\boldmath {$m$}}_{N}]$$ (34)

$$\mbox{\boldmath {$H$}} _{\dot{\mbox{\boldmath {$\theta$}}}} = [\mbox{\boldmath {$h$}}_1,... ...boldmath {$p$}}}_{G} \mbox{\boldmath {$M$}}_{\dot{\mbox{\boldmath {$\theta$}}}}$$ (35)

ここでは，全関節数を$N$とした． また，ベースリンクは 直動関節$x$，$y$，$z$軸， 回転関節$x$，$y$，$z$軸を もつと考え整理し，次のようになる．

$$\left[ \begin{array}{c} \mbox{\boldmath {$M$}}_{B}\\ \mbox{\... ...math {$0$}} & \tilde{\mbox{\boldmath {$I$}}} \end{array}\right]$$ (36)

これを用いて重心まわりの角運動量・運動量は次のようになる．

$$\left[ \begin{array}{c} \mbox{\boldmath {$P$}}\\ \mbox{\bold... ...$}}_{B}\\ \dot{\mbox{\boldmath {$\theta$}}} \end{array}\right]$$ (37)

ここで ヒューマノイドの全質量$M_{r}$， 重心位置 $p$$_{G}$， 慣性テンソル $\tilde{\mbox{\boldmath {$I$}}}$は次のように 全リンクのマスプロパティ演算より求める．

$$[M_{r}, \mbox{\boldmath {$p$}} _{G}, \tilde{\mbox{\boldmath {$I$}}}] = AddMassProperty( [m_{1}, ... ...{1}] ,\hdots, [m_{N}, \mbox{\boldmath {$c$}}_{N}, \mbox{\boldmath {$I$}}_{N}] )$$ (38)