szmlb.net

tips for robotics

ムーア・ペンローズの疑似逆行列を用いた逆運動学の一般解の導出方法

{ \displaystyle \dot{{\bf p}} = {\bf J}_{aco}\dot{{\bf q}} }に対する{ \displaystyle \dot{{\bf q} }}の一般解は,

{ \displaystyle \dot{{\bf q}} = {\bf J}_{aco}^{\dagger}\dot{{\bf p}} + ({\bf I} - {\bf J}_{aco}^{\dagger}{\bf J}_{aco})\dot{\bf q}_0 } - ①

となる. ここで, { \displaystyle {\bf J}_{aco}^\dagger }{ \displaystyle {\bf J}_{aco} }の疑似逆行列である.

上記の関係式は, 冗長マニピュレータの逆運動学計算において多用されます.
この一般解の導出過程を理解できていなかったので, 今回まとめてみます.

問題の再解釈

そもそも, 疑似逆行列は, 以下の最適化問題ラグランジュの未定乗数法を用いることで解析的に求められます.

{ \displaystyle min \ \ J = \frac{1}{2}\dot{{\bf q}}^T\dot{{\bf q}} }
{ \displaystyle s.t \ \ \ \dot{{\bf p}} = {\bf J}_{aco}\dot{{\bf q}} }

上記問題を解くと, 式①において右辺第2項が{ \displaystyle {\bf 0}}になった解が求まります.
{ \displaystyle \dot{{\bf q}_0 }}の項を導出するためには, 最適化問題を以下のように定義します.

{ \displaystyle min \ \ J = \frac{1}{2}(\dot{{\bf q}}-\dot{{\bf q}}_0)^T(\dot{{\bf q}}-\dot{{\bf q}}_0) }
{ \displaystyle s.t \ \ \ \dot{{\bf p}} = {\bf J}_{aco}\dot{{\bf q}} }

ラグランジュの未定乗数法を用いて解いていけば, 一般解が求められます.

{ \displaystyle {\bf g} = \frac{1}{2}(\dot{{\bf q}}-\dot{{\bf q}}_0)^T(\dot{{\bf q}}-\dot{{\bf q}}_0) + {\bf \lambda}^T({\bf J}_{aco}\dot{{\bf q}} - \dot{{\bf p}})}とおくと,

{ \displaystyle \frac{{\partial\bf g}}{\partial {\bf \lambda}} = {\bf J}_{aco}\dot{{\bf q}} - \dot{{\bf p}} = {\bf 0} } - ②
{ \displaystyle \frac{{\partial\bf g}}{\partial \dot{{\bf q}}} = \dot{\bf q} - \dot{\bf q}_0 + {\bf J}_{aco}^T{\bf \lambda} = {\bf 0} } - ③

式③より,

{ \displaystyle \dot{\bf q} = \dot{\bf q}_0 + {\bf J}_{aco}^T{\bf \lambda}} - ④

式②の関係式に式④を代入すると,

{ \displaystyle {\bf J}_{aco}({\bf J}_{aco}^T{\bf \lambda + \dot{\bf q}_0}) = \dot{\bf p}}
{ \displaystyle {\bf J}_{aco}{\bf J}_{aco}^T\bf \lambda = \dot{\bf p} - {\bf J}_{aco}\dot{{\bf q}}_0 }
{ \displaystyle \bf \lambda = ({\bf J}_{aco}{\bf J}_{aco}^T)^{-1}(\dot{\bf p} - {\bf J}_{aco}\dot{{\bf q}}_0) } - ⑤

式⑤を式④に再度代入すると, 式①が得られる.

{ \displaystyle \bf \dot{\bf q} = {\bf J}_{aco}^T({\bf J}_{aco}{\bf J}_{aco}^T)^{-1}(\dot{\bf p} - {\bf J}_{aco}\dot{{\bf q}}_0) + \dot{\bf q}_0 \\
 \ = {\bf J}_{aco}^{\dagger}\dot{{\bf p}} + ({\bf I} - {\bf J}_{aco}^{\dagger}{\bf J}_{aco})\dot{\bf q}_0
}

次回は, 拘束条件が複数存在する場合の解法を導出します.