位置・姿勢に関する運動学

各関節にアクチュエータが搭載されたロボットアームを考える．関節数を $n$ として，各関節変位を $n$ 次元ベクトル $\boldsymbol{q}$ で表す．アームの手先位置と姿勢をベクトル $\boldsymbol{x}$ で表す． $\boldsymbol{x}$ は $m$ 次元ベクトルとしておく．ふつう $m=6$ であるが，運動が平面上に限られて，かつ姿勢角度を問わない場合， $m=2$ となるといった状況もある．逆に双腕アームを有するロボットで，両手先の位置姿勢を指定したい場合は $6\times 2 = 12$ 自由度になったりする．

ロボットアームを使用する上で，我々はアーム手先の位置・姿勢を直接指定したり，もしくはそれらが何らかの時間的な軌道を追従するように動かすことで，作業を行わせたい．我々が直接操作できるのは各関節のアクチュエータの変位や速度といった値であるから，各関節を動かしたときに手先がどのように動くか，または手先に目的とする動作を行わせるためには各関節の変位や速度をどのように指定すればよいのかといった，関節-手先の関係を把握しておく必要がある．

手先位置・姿勢は各関節変位の関数として次のように表すことができる． \begin{align} \boldsymbol{x}(t)=\boldsymbol{f}\left(\boldsymbol{q}(t)\right) \end{align} これが順運動学であり，関数 $\boldsymbol{f}$ は一般に非線形関数である．たとえば簡単なケースとして，2つの回転関節で構成された平面2自由度アームの手先位置を関節角 $\theta _ 1$ と $\theta _ 2$ で表そうとすると，これらを変数とする $\sin$ と $\cos$ が出現することからイメージがつく．順運動学はロボットの幾何学的な設計パラメータから求めることができる．隣接する関節同士の座標変換は簡単に行えるので，たとえばDH記法によって各座標系間のねじれや並進のずれを表現し，関節間の同次変換行列を求めることで任意の座標系から見た手先位置・姿勢を関節変位の関数として表すことが可能である．

逆運動学は $\boldsymbol{f}$ の逆関数を手にすることであって，すなわち \begin{align} \boldsymbol{q}(t)=\boldsymbol{f}^{-1}\left(\boldsymbol{x}(t)\right) \end{align} という関係がわかればよい．しかしながら，先ほどの2自由度アームの例からわかるように，逆三角関数を用いて必要に応じて場合分けをしながら $\boldsymbol{q}(t)$ を求めなければならず，これが6自由度くらいになると大変である．そこで解析的に逆運動学を解くのではなく，ニュートン法などの反復計算によって解を求めるということがふつう行われる．そこで役に立つのが微分運動学である．

微分運動学

手先自由度と関節数が等しい場合

手先と関節の（角）速度間の関係は線形で書ける．位置についての順運動学の式を時間微分し，表れた係数部分を行列としてまとめることで次の微分順運動学の式が得られる． \begin{align} \dot{\boldsymbol{x}}(t)=\boldsymbol{J}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{q}}(t) \end{align} 行列 $\boldsymbol{J}\left(\boldsymbol{q}(t)\right)$ はヤコビ行列である．手先6自由度に対して関節数が6，といったように $m=n$ となっている状況であって，さらに特異姿勢でなければ，ヤコビ行列は正方かつ正則となるから，逆行列が存在して，微分逆運動学を求めることができる． \begin{align} \dot{\boldsymbol{q}}(t)=\boldsymbol{J}^{-1}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{x}}(t) \end{align} 説明順序が前後するが，特異姿勢とはヤコビ行列が非正則となる状態を意味する．この場合，ヤコビ行列の逆行列は存在しないから，目的とする手先速度を実現するための関節速度が得られず，ロボットは動作することができない．また，逆行列の各要素には行列式の逆数がかかることを思い起こすと，特異姿勢に近い状態では行列式の値がほぼ零であるため，逆行列の各要素は過大な値となる．したがって， $\dot{\boldsymbol{x}}$ を微小に変化させるだけであっても $\dot{\boldsymbol{q}}$ が極めて大きい値になり得る．関節速度が過大になるのは安全性やロボット本体にかかる負荷といった観点から考えてもよろしくない．

手先自由度と関節数が異なる場合

手先自由度 $m$ と関節の数 $n$ が異なる場合はどうすればよいのか．このときヤコビ行列は非正方となり，逆行列を考えることができない．冗長系，すなわち関節の数が多く $m \lt n$ となっている場合はヤコビ行列は横長の長方形となるし， $m>n$ であれば縦長となる．必要な手先自由度を実現するためには，関節数は自由度の数以上でなければならないから，ふつうは $m \leq n$ となるようにロボットアームを作る．運動学的にはこれで十分である．2次元平面上において手先位置を自在に指定したいのに，もし関節が一つしか無いのであれば，手先位置はリンク長で決まる円周上に限定されてしまう（手先位置は2次元ベクトルで表すことができるけれども，運動は1次元である）．また，関節数 $n$ はアクチュエータの個数とイコールであるとは限らないことに注意する． $m \leq n$ となっているロボットアームにおいても，全ての関節にアクチュエータが搭載されているとは限らない．関節数 $n$ 未満の個数のアクチュエータを用いる系を劣駆動系と呼ぶ．平面2自由度アームの二つの回転関節のうち，一つにしかアクチュエータが搭載されておらず，もう一つはフリージョイントであるとする．このような場合においても，手先位置は二つの関節角で決まる．すなわち冗長系と劣駆動系は両立する．一つのアクチュエータによって生成される運動によってフリージョイント側の関節角をどのように制御するか，というのはまた別の問題である．

関節数の方が少ない場合：最小二乗法

このときヤコビ行列は縦長となり，連立方程式の数（手先自由度）が未知数（関節速度）の数よりも多い．この場合，すべての方程式を満たす関節速度のセットは存在せず（不能）， $\dot{\boldsymbol{x}}(t)=\boldsymbol{J}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{q}}(t)$ の関係がもはや成り立たない．そこで， $\dot{\boldsymbol{x}}(t)$ と $\boldsymbol{J}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{q}}(t)$ の差をなるべく小さくするような関節速度 $\hat{\dot{\boldsymbol{q}}}(t)$ を望ましい答えだとみなすという方針を採る．すなわち最小二乗法を用いればよい． $\hat{\dot{\boldsymbol{q}}}(t)=\boldsymbol{J}^{+}\dot{\boldsymbol{x}}(t)$ と表すことができたとき，行列 $\boldsymbol{J}^{+}$ を擬似逆行列と呼ぶ．最小二乗法では差の二乗和を最小化する．ベクトルの大きさの二乗は自身との内積であるから，目的関数を \begin{align} Q=\left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right)^{T}\left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right) \end{align} とする．合成関数の微分によって \begin{align} \frac{\partial Q}{\partial \dot{\boldsymbol{q}}}&= \frac{\partial}{\partial \dot{\boldsymbol{q}}} \left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right) \frac{\partial}{\partial \left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right)} \left[ \left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right)^{T}\left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right) \right] \\ &=\boldsymbol{J}\left(\boldsymbol{q}\right)^{T} 2\left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right)\\ &=2\boldsymbol{J}\left(\boldsymbol{q}\right)^{T}\left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right) \end{align} これがゼロになるときの $\dot{\boldsymbol{q}}$ を求めればよいから， \begin{align} 2\boldsymbol{J}\left(\boldsymbol{q}\right)^{T}\left(\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right) &= 0 \\ \boldsymbol{J}\left(\boldsymbol{q}\right)^{T}\boldsymbol{J}\left(\boldsymbol{q}\right)\dot{\boldsymbol{q}} &=\boldsymbol{J}\left(\boldsymbol{q}\right)^{T} \dot{\boldsymbol{x}} \end{align} ここでグラム行列 $\boldsymbol{J}\left(\boldsymbol{q}\right)^{T}\boldsymbol{J}\left(\boldsymbol{q}\right)$ は $n\times n$ の正方行列であり， $\boldsymbol{J}$ が列フルランクであれば， ${\rm rank}(\boldsymbol{J}^{T}\boldsymbol{J})=n$ であって逆行列を持つ．したがって \begin{align} \dot{\boldsymbol{q}} &=\left(\boldsymbol{J}\left(\boldsymbol{q}\right)^{T}\boldsymbol{J}\left(\boldsymbol{q}\right)\right)^{-1}\boldsymbol{J}\left(\boldsymbol{q}\right)^{T} \dot{\boldsymbol{x}} \end{align} としてそれらしい $\dot{\boldsymbol{q}}$ を得ることができた．このときの $\left(\boldsymbol{J}\left(\boldsymbol{q}\right)^{T}\boldsymbol{J}\left(\boldsymbol{q}\right)\right)^{-1}\boldsymbol{J}\left(\boldsymbol{q}\right)^{T}$ が， $\boldsymbol{J}$ が縦長で列フルランクであるときの擬似逆行列である．これは順運動学を満たしている（順運動学の式の両辺に左から擬似逆行列を作用させてみればよい）．

関節数の方が多い場合：ラグランジュ未定乗数法

この場合はヤコビ行列が横長となり，連立方程式の数（手先自由度）よりも未知数（関節速度）の数が多い．この連立方程式をがんばって解こうとしても，どうしても未知数が定まらないまま残ってしまう（不定）．つまり等式 $\dot{\boldsymbol{x}}(t)=\boldsymbol{J}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{q}}(t)$ を満たす $\dot{\boldsymbol{q}}$ が無数に存在する．このように解が不定の状況では，無数に存在する解に何らかの制約を課すことで一つに絞り込むことができる．そこでノルム $|\dot{\boldsymbol{q}}|$ を最小化することを考える．ただし，このとき等式 $\dot{\boldsymbol{x}}(t)=\boldsymbol{J}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{q}}(t)$ が満たされていなければならない．すなわち等式制約付きの最小化問題を解くことになる．ラグランジュ未定乗数法によって目的関数は \begin{align} Q=\dot{\boldsymbol{q}}^{T}\dot{\boldsymbol{q}}+\boldsymbol{\lambda}^{T} \left( \dot{\boldsymbol{x}}(t)-\boldsymbol{J}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{q}}(t) \right) \end{align} と書ける． \begin{align} \frac{\partial Q}{\partial \dot{\boldsymbol{q}}}= 2\dot{\boldsymbol{q}} - \boldsymbol{J}^{T}\boldsymbol{\lambda} &= 0 \\ \dot{\boldsymbol{q}} &= \frac{1}{2} \boldsymbol{J}^{T}\boldsymbol{\lambda}\\ \end{align} となるから， $\dot{\boldsymbol{x}}(t)=\boldsymbol{J}\left(\boldsymbol{q}(t)\right)\dot{\boldsymbol{q}}(t)= \frac{1}{2} \boldsymbol{J} \boldsymbol{J}^{T}\boldsymbol{\lambda}$ ．行列 $\boldsymbol{J}\left(\boldsymbol{q}\right)\boldsymbol{J}\left(\boldsymbol{q}\right)^{T}$ は $m\times m$ の正方行列であり， $\boldsymbol{J}$ が行フルランクであれば， ${\rm rank}(\boldsymbol{J}\boldsymbol{J})^{T}=m$ であって逆行列を持つ．したがって， $\boldsymbol{\lambda}=2\left( \boldsymbol{J} \left(\boldsymbol{q}\right) \boldsymbol{J}\left(\boldsymbol{q}\right)^{T} \right)^{-1}\dot{\boldsymbol{x}}$ となる．これにより， \begin{align} \dot{\boldsymbol{q}} = \frac{1}{2} \boldsymbol{J}^{T}\boldsymbol{\lambda} = \frac{1}{2} \boldsymbol{J}^{T}2\left( \boldsymbol{J} \boldsymbol{J}^{T} \right)^{-1}\dot{\boldsymbol{x}} =\boldsymbol{J}^{T}\left( \boldsymbol{J} \boldsymbol{J}^{T} \right)^{-1}\dot{\boldsymbol{x}} \end{align} を得る．行列 $\boldsymbol{J}^{T}\left( \boldsymbol{J} \boldsymbol{J}^{T} \right)^{-1}$ が， $\boldsymbol{J}$ が横長で行フルランクであるときの擬似逆行列である．

\begin{align} \dot{\boldsymbol{q}} =\boldsymbol{J}^{T}\left( \boldsymbol{J} \boldsymbol{J}^{T} \right)^{-1}\dot{\boldsymbol{x}} \end{align} の両辺に左から $\boldsymbol{J}$ を作用させると，順運動学の式となることがわかる．

特異姿勢となってランク落ちした場合

階数因数分解による方法

どちらもランクが $r$ の $m\times r$ 行列 $\boldsymbol{B}$ と $r\times n$ 行列 $\boldsymbol{C}$ を用いて $\boldsymbol{J}=\boldsymbol{B}\boldsymbol{C}$ と階数因数分解する． $\boldsymbol{J}$ はランク落ちしており，これまでに挙げた手法では擬似逆行列を計算することができない．グラム行列のランクは元の行列のランクに等しく，つまり ${\rm rank}(\boldsymbol{J})={\rm rank}(\boldsymbol{J}^{T}\boldsymbol{J})={\rm rank}(\boldsymbol{J}\boldsymbol{J}^{T})$ だから， $\boldsymbol{J}$ がランク落ちならば $\boldsymbol{J}^{T}\boldsymbol{J}$ と $\boldsymbol{J}\boldsymbol{J}^{T}$ もランク落ちとなり，これらの逆行列が存在しないからである．しかしながら， $\boldsymbol{B}$ は列フルランク， $\boldsymbol{C}$ は行フルランクであるから，これらは擬似逆行列を持つ．したがって \begin{align} \boldsymbol{J}^{+}=\left(\boldsymbol{BC}\right)^{+}=\boldsymbol{C}^{+}\boldsymbol{B}^{+}=\boldsymbol{C}^{T}\left( \boldsymbol{C} \boldsymbol{C}^{T} \right)^{-1}\left(\boldsymbol{B}^{T}\boldsymbol{B}\right)^{-1}\boldsymbol{B}^{T} \end{align} と $\boldsymbol{J}$ の擬似逆行列を計算することができる． $\left(\boldsymbol{BC}\right)^{+}=\boldsymbol{C}^{+}\boldsymbol{B}^{+}$ は $\boldsymbol{B}$ と $\boldsymbol{C}$ のランクが等しいとき成立する．

SR逆行列（Singularity-Robust Inverse）

手先速度を所望の値にすることよりも，特異点付近での安定性を優先する場合，最小化する目的関数を次のように変更すればよい．

\begin{align} Q=\left(\boldsymbol{J}\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right)^{T}\left(\boldsymbol{J}\dot{\boldsymbol{q}}-\dot{\boldsymbol{x}}\right) +\lambda \dot{\boldsymbol{q}}^{T}\dot{\boldsymbol{q}} \end{align}

そうすると結局， \begin{align} \dot{\boldsymbol{q}}=\left( \boldsymbol{J}^{T}\boldsymbol{J} + \lambda \boldsymbol{I}\right)^{-1} \boldsymbol{J}^{T} \dot{\boldsymbol{x}} \end{align} となる． $\boldsymbol{J}^{T}\boldsymbol{J}$ がランク落ちであっても， $\lambda \boldsymbol{I}$ のおかげで $\left( \boldsymbol{J}^{T}\boldsymbol{J} + \lambda \boldsymbol{I}\right)^{-1}$ の逆行列は求まる．減衰因子 $\lambda$ は可操作度を用いるなどして，特異姿勢への近さに応じて調整する．

とりあえず一旦ここまで

擬似逆行列については，特異値分解との関係もまとめたい．可操作度を考えるにあたっても必要だし．

参考書籍

ロボット制御基礎論（Amazon）冗長アームといえばこれ．
実践ロボット制御（Amazon）ロボット制御基礎論をベースにまとめられた新しい本．ロボットアームの入門的内容を概観するのであればこの本から始めてもよいと思う．ただ当たり前のことだが一冊で勉強が完結するということはないので，ロボット制御基礎論などの他の本を一緒に持っておくのがよい．
“巧みさ”とロボットの力学　プレミアムブックス版（Amazon）冗長アームやハンドについて，いかに器用な動作を実現するか？というテーマで2008年に書かれた本．深層学習以後，有本先生たちがどのようなことを考えられているのかは，2017年のロボット制御学ハンドブック（Amazon）最終章にまとめられている．
ヒューマノイドロボット(改訂2版)（Amazon）ヒューマノイドを題材にして逆運動学の数値解法やMATLAB実装例が紹介されており，イメージがつきやすい．

ギャラクシースーパーはてなブログ

ギャラクシースーパーノヴァ子の日記だお

【ロボティクス】運動学・ヤコビ行列・擬似逆行列の覚え書き