siar

Snowy Institute

IKと過ごしたひと夏の思い出

最適化 IK

逆運動学ソルバを自分で実装してみたお話です.

夏の始まり

過ち きっかけ

マニピュレータやヒューマノイドのような多自由度系を理論的に構成するにあたり, 逆運動学(IK: Inverse Kinematics)や最適化に興味を持ち始めていた頃, 産総研のHRP-2が膝を伸ばして歩いたという情報が流れてきました.

確かこの梶田先生のツイートあたり.

「うわっ」ってなるじゃないですか.
それで色々調べてみると, 特異点近傍でも逆運動学が解けるということを知り, じゃあ自分で実装してみるかとなりました.

当時は7月末. 夏休みが丸ごと溶けるとも知らずに……

最終的に実装したもの

今回実装した逆運動学は, 大阪大学の杉原先生による論文『Levenberg-Marquardt法による可解性を問わない逆運動学』(日本ロボット学会誌29巻3号, 2011)による提案手法です.


理論

モデル

逆運動学に興味のある人が沼に浸かってもらえるように, 今回は図のような簡単な2次元2自由度で考えていきます.

(このため, 先の論文の中で次元に関わる3という数字は, 基本的に2へと読み替えてください.)

f:id:ssr-yuki:20181022180853p:plain
マニピュレータ・モデル

座標系の設定としては, 画像右方向に{x}軸, 画像上方向に{y}軸を取り, マニピュレータの第1ジョイント(床との固定点)を原点とします.

リンク, ジョイントは原点に近い方から順に1, 2と数字を振ります.

また, 各パラメータの定義は以下のようにします.

文字 定義
{l_1} リンク1の長さ
{l_2} リンク2の長さ
{\theta_1} リンク1と{x}軸のなす角
{\theta_2} リンク1とリンク2がなす角

このとき, 手先座標{\,\boldsymbol{p}}は幾何学的に次のように表現することができます.

\boldsymbol{p} = \begin{pmatrix} x \\ y \end{pmatrix} = \begin{pmatrix} l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2) \\ l_1 \sin\theta_1 + l_2 \sin(\theta_1+\theta_2) \end{pmatrix}

また, 回転行列{R_1,\,R_2}を

R_1 = \begin{pmatrix} \cos\theta_1 & -\sin\theta_1 \\\ \sin\theta_1 & \cos\theta_1 \end{pmatrix},\\ R_2 = \begin{pmatrix} \cos\theta_2 & -\sin\theta_2 \\\ \sin\theta_2 & \cos\theta_2 \end{pmatrix}

のように定義すれば,

\boldsymbol{p} = R_1 \begin{pmatrix} l_1 \\\ 0 \end{pmatrix} + R_1 R_2 \begin{pmatrix} l_2 \\\ 0 \end{pmatrix}

とも表現できます. さて, 手先位置座標系が求まりました.

次にマニピュレータの一般化座標{\,\boldsymbol{q}}を

\boldsymbol{q} = \begin{pmatrix} \theta_1 & \theta_2 \end{pmatrix}^\mathsf{T}

とすると, 手先座標の一般化座標に対するヤコビ行列{J}は

J = \frac{\partial \boldsymbol{p}}{\partial \boldsymbol{q}} = \begin{pmatrix} -l_1\sin\theta_1 -l_2\sin(\theta_1+\theta_2) & -l_2\sin(\theta_1+\theta_2) \\\ l_1\cos\theta_1 + l_2\cos(\theta_1+\theta_2) & l_2\cos(\theta_1+\theta_2) \end{pmatrix}

と求めることができます.

\dot{\boldsymbol{p}} = J\dot{\boldsymbol{q}}

という関係も示すことができます.

最適化問題への置き換え

目標位置を{\,\boldsymbol{p}_d}として, 誤差ベクトル{\,\boldsymbol{e}}は

\boldsymbol{e} = \boldsymbol{p}_d - \boldsymbol{p}

と定義できます.

このとき, 誤差ベクトル{\,\boldsymbol{e}}と{\,\boldsymbol{q}}との関係は, {\,\boldsymbol{p}_d}が{\,\boldsymbol{q}}に依存しないとすれば,

\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{q}} \simeq -\frac{\partial \boldsymbol{p}}{\partial \boldsymbol{q}} = -J
となることを覚えておきましょう.

この誤差ベクトルを零ベクトルにしたい, すなわち次の方程式

\boldsymbol{e} = \boldsymbol{0}

を解きたいわけですが, 目標手先位置から関節の角度を導出する逆運動学は高次の非線形方程式のため, 低次方程式のような代数的解法で求解することは困難です. (高次の問題になることは, 三角関数のテイラー展開がべき級数になることからも明らかでしょう.)

そこで今回は, 目標位置と現在位置との誤差ベクトルの最小化を図る最小化問題への置き換えを行っています.

注意するべき点として, 要素は負値を取る場合があるので, このままでは最小化できません. (このまま最小化すると, {\boldsymbol{e}})の要素は負の方向に発散し, 誤差は最終的に無限に増大してしまいます.)

そこで, {\boldsymbol{e}}のノルムを最小化する方向で問題を設定します.

\min_\boldsymbol{q} \boldsymbol{e}^\mathsf{T} \boldsymbol{e}

{\boldsymbol{e}}の要素ごとに収束速度をチューニングしたい場合は, {i}番目の要素の重み{w_{E,i} > 0}を対角成分とする重み行列{W_e = \mathrm{diag}(w_{E,i})}を利用して

\min_\boldsymbol{q} \boldsymbol{e}^\mathsf{T} W_E \boldsymbol{e}

とすればよいでしょう. ただし, 今回は{W_E}は単位行列とします.

LM法

今回は最適化の手法として, LM法(Levenberg-Marquardt 法)を利用しました.

ここでは数学的に詳細な議論はしませんが, LM法は参考論文で採用されていたほか,

  • 特殊な場合を除き, 二次収束性がある(高速に解ける)
  • 大域的に解を求めることに向いている

といった特徴があることがわかりました. 下で簡単に説明をします.

従来のニュートン法(Gauss-Newton 法)の更新規則は, 今回の問題を例にすると

\boldsymbol{q}_{k+1} = \boldsymbol{q}_k + \left[ \left( \frac{\partial \boldsymbol{e}_k}{\partial \boldsymbol{q}_k} \right)^\mathsf{T} \frac{\partial \boldsymbol{e}_k}{\partial \boldsymbol{q}_k} \right]^{-1} (- \frac{\partial \boldsymbol{e}_k}{\partial \boldsymbol{q}_k} \boldsymbol{e}_k ) = \boldsymbol{q}_k + (J_k^\mathrm{T} J_k)^{-1} J_k \boldsymbol{e}_k

のように定義されます. これは{J_k}, つまりヤコビ行列が階数落ちしている, つまりマニピュレータが特異姿勢近傍にあるときの逆行列が不安定な値になってしまい, 計算が発散してしまうことが危惧されるので, 嬉しくありません.

※冗長性等によってヤコビ行列が非正方な場合の問題は, 疑似逆行列の適用で解決します.

※ {(J_k^\mathsf{T} J_k)}は半正定値性を有します.

LM法では, ニュートン法に減衰因子行列{W_N}を導入することで, この不安定性を取り除いています. LM法の更新規則は次の通りです.

\boldsymbol{q}_{k+1} = \boldsymbol{q}_k + (J_k^\mathsf{T} J_k + W_{N,\,k})^{-1} J_k \boldsymbol{e}_k

{W_{N,\,k}}は正実数の減衰因子による対角行列で定義すれば, {(J_k^\mathsf{T} J_k + W_{N,\,k})}は正定値性を有し, つまり正則となることが保証されるので, 逆行列が安定することがわかります.

論文で提案されている手法

{W_N}をどう設計するかによって収束性能が変化するようですが, 今回は論文に載っている内容で素直に実装しました.

W_{N, k} = (\boldsymbol{e}_k^\mathsf{T} \boldsymbol{e}_k + 0.001) I

なお, 等価な大域解が複数存在する場合, LM法では初期条件に近い解へ, つまり関節角度が最小変位になるように収束するそうです.

実装

コード

アルゴリズムが存在するので, 書くだけです

Eigenを使うと簡単に行列計算ができるので便利ですね. 検証ではMATLABも併用していました.

サンプルを公開しています.

github.com

注意点

今回の実装例では, 目標値が原点の場合かつ特異姿勢近傍から計算を開始する場合, 不可解となります.

これは{J^\mathsf{T} \boldsymbol{e}}が{0}となってしまう幾何的な問題のためです.

また, デバッグ中苦しんだこととして,

  • 反復回数の増大に伴って更新加算時に発生する計算機上のオーバーフローが発生すること.
  • RCサーボモータ等, 実機の可動角(稼働範囲)は有限(コンフィギュレーション空間が有界)であり, 解(一般角)から代表角へ変換する必要があること.
  • 最小化問題を解いているため, 解が出るまでの時間は一定でないこと.

などが挙げられます. ご留意を.

評価

公開しているコードでの評価です.

製作していた実機に合わせたため, {l_1 = 0.1,\, l_2 = 0.1}としています.

また計算開始時の姿勢は常に{\,\boldsymbol{q}_0 = \begin{pmatrix} \pi/2 & 0 \end{pmatrix}^\mathsf{T}}としています. 明らかに特異姿勢です.

計算終了の収束判定の閾値は, 何となく気分で{0.000001}にしています.

まずは目標位置に対する計算終了までの反復回数です.
f:id:ssr-yuki:20181026042106p:plain

可到達な円形領域の境界近傍(特異姿勢)と原点付近(大域解が無数に存在)で収束性能が格別に悪化しています.

f:id:ssr-yuki:20181026042554p:plain

上記以外の部分では, 高々40回程度の反復で求解できていることがわかります.

続いて, 実時間処理を検討するための評価です. 反復回数が100を超えたときに強制的に計算を打ち切り, そのときの誤差のノルムを計算しています.

f:id:ssr-yuki:20181026043533p:plain

可到達空間の外側は当然誤差が増加します.

f:id:ssr-yuki:20181026043547p:plain

こちらも原点と領域境界を除けば十分な精度で結果を出せています.

夏の終わりに

できたこと

自慢コーナーです.

自分の書いた逆運動学で宴会芸(End-Effector Position Control)ができたので, 学祭でドヤ顔で展示してました.

あと, 先週のWRSという大会でもこの逆運動学でマニピュレータ動かしてました.

感想

もうすぐ冬だが！？

ひと夏どころか秋も終わろうとしています.

伝えるべきこととして, 既存のライブラリを使うことを強く推奨します.

ロボットを動かすだけならそれで十分です. 目的と手段を間違えないようにしましょう.

私は今回, 非線形最適化の勉強の初歩の初歩から始めて, 7月末から9月の半ばまで論文や本の数式とにらめっこ, その後実装自体は1週間程度でできました.

夏休みが丸ごと消えてますね. 楽しいはずの大学3年生の夏……どこへ……?

もし物好きの方がいたら, 時間を十分に確保して臨んでください.

逆運動学を自分で実装すれば! 次元拡張や収束速度チューニングなどのマニアックなことができます(?) 今回の経験もきっとどこかで役に立つと信じて.

IKと過ごしたひと夏の思い出は, いつまでも心の中で反復計算されることでしょう.

出典など

  • 実装したもの.

doi.org

  • LM法の勉強をしたもの

http://www.kurims.kyoto-u.ac.jp/~kyodo/kokyuroku/contents/1174.html

の中の『Levenberg-Marquardt法の局所収束性について (最適化の数理科学)』(山下信雄/福島雅夫).

www.amazon.co.jp