逆運動学とヤコビ法、特異点

はじめに

ロボットなどの機械を動作させるためには、3次元空間上でどのように動いてほしいのかを定めて、その通りに動くようなアクチュエータへの指令値を求めなければなりません。例えばアクチュエータがモータならば、モータは回転という動作しかできませんが、私達が今制御したいのはモータと棒が連なって作られた手先の位置であり、水平移動などもしてほしいのだとしましょう。

順運動と逆運動

私達が与えたいのは手先の位置であるにも関わらず、実際に動かすことができるのはモータの回転ということです。

このとき、（私達が実際に動かすことのできる）モータの角度から、（実際に動かしたい）手先の位置を指定するような関係式を順運動と呼びます。要するに角度 $\bf \theta$ を与えると手先位置 $\bf r$ が決まるという関数

$$ {\bf r} = f({\bf \theta}) $$

のことです。一方で、本来の目的に立ち返れば、ある手先の位置 $\bf r$ に行ってほしいというのが我々の望みであり、その位置に至らしめるようなモータの角度 $\bf \theta$ が知りたいというのが実際の状況である場合多々あります。これを逆運動と呼び

$$ {\bf \theta} = g({\bf r}) $$

という関数のことを指します。

さて、ここで「順運動」だとか「逆運動」だとかいうのは、私達が何を直接制御できて、何を達成したいのかに起因した言葉であることは強く認識しておいたほうが良いでしょう。よく「逆運動学は順運動学より難しい（逆問題は順問題より難しい）」という表現を目にする気がしますが、それもそのはずで、制御できる変数を制御したら結果として制御したい従属変数がどうなるのかを計算することを順運動と定義しているのに対して、それをひっくり返しているのが逆運動だからです（要は数式を用いたり実験すればすぐに確認できるような自明の関係の方を「順」と言って、その逆となっている自明では無い関係を「逆」と定義しているのだ）。

順運動と逆運動の一般的な表記

ここで、我々が直接的に値を制御できるような変数を $p$ と書き、直接制御はできないものの $p$ の動きに合わせて変化するような制御したいと考える変数を $q$ と書くことにします。（面倒なのでボールド体は用いませんが、一般にベクトルの形をしているということで認識しておいてください）

そうして、順運動を

$$ q = f(p) $$

逆運動を

$$ p= g(q) $$

と表記することにします。また、直接制御できる $p$ の値を刻一刻と変化させるようなこともできるため、$p$ というのは時間変化をするということを明記するために、順問題に関して

$$ q(t) = f(p(t)) $$

であり、また逆問題も同様であるとします（時間変化する）。

逆運動の解法

解析的手法

まず、逆運動というのは順運動の関係式をひっくり返しているだけのものでした。すなわち

$$ p(t) = g(q(t)) $$

という関係性を知ることができれば目的が達成できるのです。そうしたら、簡単で順運動

$$ q(t) = f(p(t)) $$

の逆変換 $f ^ {(-1)}$ を求めてこれを $g = f ^ {(-1)}$ としてしまえば良さそうです。これは半分正解で半分間違いです。 なぜなら、必ずしも逆変換が存在するとは限らないからです。

逆変換が存在するためには関数 $f$ が全単射である必要があります。全単射というのは $f$ の定義域の任意の点 $p _ i$ に対して、その変換先 $q _ i$ が必ず存在してただ1つでなければならず、またそれぞれの集合を完全に網羅した形にならなければいけません。

例えば、$x, y$ 平面上の点 $p$ を、たった1つの制御可能な変数 $\theta$ を使って

$$ \begin{align} x &= \cos \theta \\ y & = \sin \theta \end{align} $$

と順運動が書けたとしましょう。今、$x, y$ の二次元平面のすべての点を含んだ集合を考えたときに、$\theta$ をいくら調整したところでたどり着けない$x, y$ の点があることが容易に想像できるはずです。なぜならこれは、単位円の円周上の点しか取らないからです。これは、回転するモータを原点に固定し、そこに単位長さの棒を取り付けて、棒の先端（モータとの取り付け先とは反対）の位置が、二次元平面の任意の場所に来るように制御できますか？と聞いていることに相当します。

もしも、二次元平面の任意の点を縦横無尽に取れるようにしたければ、制御可能な変数 $l$ を追加して

$$ \begin{align} x &= l\cos \theta \\ y & = l\sin \theta \end{align} $$

としてやればいいでしょう。これは長さを自在に伸縮できる棒をモータに取り付けたことになります。 すると、$x, y$ の適当な位置を指定したときに、その位置に棒の先端が来るようなモータの回転角 $\theta$ と棒の長さ $l$ を決めることができそうです。

このようなときには、単に上記の非線形連立方程式を $\theta, l$ に関して解いてやれば、逆運動を記述できたことになります。

ちなみに、このとき $p = (l, \theta) ^ T$ というベクトル、$q = (x, y) ^ T$ というベクトルを考えており、これらのベクトルの変換の関係を

$$ q = f (p) $$

としていたのでした。これが解けるような関係（全単射）ならば

$$ p = f ^ {(-1)} (q) $$

と逆問題が求まるという仕組みです。

局所線形近似法

さて、上記の程度の式ならば単純です。しかし一般の機械システムはもっと沢山の棒とモータが連なっており、一目では逆問題を解くことができないかもしれません。そこで、局所的に線形な関係を作って、その近傍では線形の連立方程式に従っているとしてしまう方法があります。

方法は単純で、順問題

$$ q = f (p) $$

の微分

$$ {\rm d} q = f'(p){\rm d}p $$

を考えます。これは具体的には、$p = (p_1, p_2) ^ T$ と $q = (q_1, q_2) ^ T$ のようなベクトルである場合には、左辺の成分ごとに

$$ {\rm d}q _ i = \sum _ j \frac {\partial q _ i}{\partial p _ j} {\rm d} p _ j $$

という微分を考えてやればよく、要するに、$p _ i$ が $q $ の成分をそれぞれちょっとだけ動かすという局所的な範囲で、どのように増加するのかを表した関係式になります。これは全微分の定義そのものです。

そうすると、${\rm d} p$ が小さければひとまず全微分の接空間で議論しても良さそうということになり、これは上記で書いたように局所的には

$$ \begin{align} {\rm d} q = f'(p){\rm d}p \\ where \\ f'(p) = (\frac {\partial q _ i}{\partial p _ j}) _ {ij} \end {align} $$

という線形の関係だと考えられることになります。このとき、行列 $f'(p) = (\frac {\partial q _ i}{\partial p _ j}) _ {ij}$ のことをヤコビ行列と呼びます。とにもかくにも、この局所的な空間の中では単に線形方程式 $y = A x$ みたいな単純な関係で扱えるという点が強みです。

この局所的な空間における逆運動は、ヤコビ行列の逆行列$f'^{-1}(p)$を使って

$$ {\rm d} p = f'^{-1}(p){\rm d}q $$

と解いてやればよいことになります。非線形の連立方程式を解くよりはだいぶ楽になっているでしょう。 この手法はヤコビ行列を用いることから運動学の世界では「ヤコビ法」と呼ばれているようです。

ただし、当然のことながら、線形近似で誤差が乗ることを認識しておかなければなりません。特に重要なのは、 順運動の線形近似

$$ {\rm d} q = f'(p){\rm d}p $$

において当然「微分」であるのだから ${\rm d} p$ は小さな値を取らなければならないにも関わらず、大きな値を入れてしまうような場合です。これは、思い返せば制御できる変数 $p$ は時間波形 $p(t)$ であったので、時刻の経過に伴って $p$ をどれだけ増加させるのか、すなわち

$$ \frac {dp}{dt} $$

が大きな値をとってしまう場合があることに起因します。時間の経過というのは人間がコントロールできるものではありません。例えばロボット制御であるならば、最小の時間経過は制御のサンプリング周期に依存するわけです。今、自分が与える $p(t)$ の波形が激しく振動するような振る舞いをする場合には、その時間に対する傾きが大きな場所では「サンプリング時間をもっと細かく」取ることで、${\rm d}p(t)$ という経過時間依存の微分の大きさを抑えられるかもしれませんが、なかなか現実的ではないでしょう（なぜなら、普通は最初から可能な限りサンプリング周期は細かくしているはずだから）。

そして更に、私達が実際に行いたいのは逆問題の

$$ {\rm d} p = f'^{-1}(p){\rm d}q $$

という関係式を使うことでした。このとき、$q$ を少しだけ動かしたい、すなわち ${\rm d} q$ だけ動かしたい、と思ったときに、上記の式で定まる ${\rm d} p $ が小さな値になってくれているかなど、考慮のしようもないのです（もしも大きくなっていたならば $q$ を動かすことを諦めるのだろうか？それは制御の意味がないだろう）。

通常はヤコビ法を素直に使うと、線形近似の誤差が避けられないため、その誤差を埋めるために「前回の線形近似誤差をフィードバックする」というclosed loop ヤコビ法が使われるようです。

ヤコビ法と特異点

上記のように、ヤコビ法では局所線形空間で逆問題を解いて少しだけ制御を進め、また進んだ先で局所線形空間を作り直して制御を進め…という方法を使います（要するに大局的な関数の形状を描かずに、局所的な接平面を切り貼りしてつないでいくというイメージだ）。

このとき、先に述べたとおり、${\rm d} p$ は細かく取らなければならないのですが、$t$ に依存してしまうということでした。しかし、$t$ というものに無関係に、どう頑張っても ${\rm d} p$ を小さくできないケースがあるという話をします。

それは

$$ {\rm d} p = f'^{-1}(p){\rm d}q $$

という逆運動を記述した際に、制御したい変数の微分 ${\rm d} q$ をどれだけ小さくしたところで制御できる変数 ${\rm d} p$が無限大に発散してしまうことに起因します（これは、先の項目の最後に触れたことの更に深刻な場合である）。

このような点を特異点と呼び、通常、この点でのヤコビ法による制御は不可能です。これは行列 $f'(p) = (\frac {\partial q _ i}{\partial p _ j}) _ {ij}$ が退化することに起因します。

退化とは、行列の各行が線形従属の関係にある、すなわちランク落ちを起こすケースがあるということを意味しています。このとき、順運動

$$ {\rm d} q = f'(p){\rm d}p $$

において、適当な線形変換（これは単に座標を傾けたり、軸を拡大縮小したりしているだけで、制御したい項目を物理的に変化させているわけではない）をしたときにヤコビ行列 $f'(p)$ のある行がすべて $0$ になってしまい、その行は ${\rm d}p$ をどれだけ大きくとっても ${\rm d} q$ に影響を及ぼせなくなっているケースになります。

要するにヤコビ行列の正則性が重要であるということです。厄介なことに、特異点の近傍では、いくら正則であろうとも、先に述べたように${\rm d}p$ をかなり大きく取らなければ行けないのが通常のケースで、線形近似が破綻してしまうようなことがしばしば起こります。これは予期せぬ暴走（近似があらすぎる、あるいは大きな速度が入力される機械的負荷に起因）を助長するため、特異点にはそもそも近づかないのがベストです。

直感的には、どれだけ制御可能な変数 $p$ を動かしても、動かすことのできない $q$ の点があるというのは、例えば自分の腕をピンと伸ばした状態で、肘や肩の関節 $p$ をどのような具合いで動かしてもソレ以上手先 $q$ を奥には動かせないという物理的な現象に紐付けることができます。なので必ずしもヤコビ行列を数学的に綿密に計算しなければ予期できないものでもありません（二足歩行ロボットが、いつも膝をわずかに曲げているのは、おそらくヤコビ行列によって逆問題を解いているからでしょう）。

それでも座標の取り方（角度の原点の与え方）などによっては予期せぬ特異点が生ずるケースもあります。 例えば、三次元デカルト座標を、三次元球面極座標で表現し、原点からの距離 $r$ が固定の場合を考えてみてください。これはちょうど、モータを原点に２つ配置し、腕を一本取り付けた場合の手先位置の制御を考えることに相当します。

このとき経度と緯度という角度を考えた場合に、北極と南極において特異点が生じます（当然、球面は完全に対称な図形であるのだから、北極も南極も、その他の点も等価である。ただただ、座標のとり方で物理的実態とは関係なく生ずる特異点があるということだ）。

最後に

ちなみにロボットの制御は下記の本をパラッと読んだ程度で、実は真面目に勉強をしたことはないので、ぜひご教授していただけると嬉しいです。

ヒューマノイドロボット