はじめに
最近ロボットアームを動かしていたところ、
ある姿勢から別の姿勢に動こうとする時に限って、
毎回エラーで停止してしまうという事象に遭遇しました。
そこでログを見てみると出てきたのが特異点というキーワード。
恥ずかしながらロボットに関してあまり知識が無く、
特異って何？ヤバくない？
と物々しい響きに面食らいましたが、
せっかくなのでこの機会にそれなりに理解しようと思い、調べてみました。
事象から背後の理屈までさかのぼっていくと、
大学で習う線形代数が何の役に立つかをよく表していて面白かったので、
今回記事にまとめます。
特異点って何よ？
ひとことで言うと、、、
・ヤコビ行列が特異行列のような性質を持つ点
・ヤコビ行列が逆行列を持たなくなる点、
・ヤコビ行列の行列式が0になる点
・ヤコビ行列がフルランクでなくなる点
これらは全て特異点です。
というか、実はどれも同じ現象を違う言葉で言っているだけです。
一応、言葉の意味をひとつずつ確認していきましょう。
ある正方行列Aがあるとします。このとき、、、
・Aは特異行列である ⇒ Aは逆行列を持たないということ。
・Aの行列式 ⇒ Aをベクトルにかけた時、ベクトルの張る空間がどうなるかを表す数。
行列式によって空間が押しつぶされるか広げられるかが分かる。
・Aのランク ⇒ Aをベクトルにかけた時、結果として残る次元の数。
ちょっと抽象的ですね。もう少し直感的に言うと、
・Aは特異行列である ⇒ Aは逆行列を持たない ⇒ Aは元の状態に戻れなくなっている
元の状態というのは何かというと、
ベクトルの張る空間が元通りにならないということです。すなわち、
・Aの行列式が0のとき、Aをかけられたベクトルは空間が押しつぶされた状態になる
空間が押しつぶされた状態になるというのはつまり、
3次元空間が2次元空間になるようなイメージです。なので、
・空間が押しつぶされて元のベクトルより次元が減っている ⇒ ランク落ちしている
これらをまとめて、改めて特異点をひとことで言うと、
「動作空間が押しつぶされて自由度(=次元)が減り、元の動きに戻れない状態になる点」
ということですね。
これは物理的には
「急にロボットの動ける方向が制限される点」
という現象になって現れます。
ロボットの姿勢とヤコビ行列
簡単にオイラー角とヤコビ行列について説明します。
・オイラー角 ⇒ X,Y,Z 3つの直交する座標周りの連続した3回の回転角度で姿勢を表す方法。
またしてもちょっと抽象的ですね。
オイラー角で姿勢を表す場合、通常は↓に示す(1), (2), (3)の順番で回転を行います。
(1)ヨー
(2)ピッチ
(3)ロール
ここで大事なことですが、(2)の回転後の姿勢は(1)の回転の影響を受け、
(3)の回転後の姿勢は(2)の回転の影響を受けます。
つまり姿勢の変化の仕方、すなわちロボットの動作を表す時、
オイラー角は空間に固定された座標に対する動作とはなりません。
なので装置内でロボットがワークをピックアップする場合等、
空間に固定された座標軸上のポイントを指定して、ロボットを制御したい時は、
ロボットの姿勢変化を空間固定座標系で書き直してあげる必要があります。
そのためのツールがヤコビ行列です。
早速、ロボットの動きを空間固定座標系に書き直してみましょう。
ロボットの動きとはすなわちオイラー角の変化率なので、まず三軸それぞれを微分します。
$$
\begin{matrix}
ヨー(Z軸)：\dot{\psi}\\
ピッチ(Y軸)：\dot{\theta}\\
ロール(X軸)：\dot{\phi}
\end{matrix}
$$
ここでヤコビ行列$J(\phi, \theta, \psi)$を定義して、空間固定座標系の角速度
$$
\begin{matrix}
X軸：\omega_x\\
Y軸：\omega_y\\
Z軸：\omega_z
\end{matrix}
$$
を得ることを考えると、座標を変換する式は
$$
\begin{pmatrix}
\omega_x\\
\omega_y\\
\omega_z
\end{pmatrix}=
J(\phi, \theta, \psi)
\begin{pmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{pmatrix}
$$
と書けて、ヤコビ行列を用いて空間固定座標系へ変換できたことになります。
ツールとしてのヤコビ行列の使い方は上記の通りですが、その成分はどうなるでしょうか？
過程は省いて結果だけを出します。
$$
J(\phi, \theta, \psi) =
\begin{pmatrix}
\cos\theta\cos\psi & -\sin\psi & 0 \\
\cos\theta\sin\psi & \cos\psi & 0 \\
-\sin\theta & 0 & 1
\end{pmatrix}
$$
このヤコビ行列を見ると、$\phi$の項がありませんね。
これはつまり、オイラー角を空間固定座標系へ変換する際に、
ロールの変化率は不要ということです。
なぜなら、ヨー、ピッチと動作した後で最後のロール動作を行うため、
その時点ですでに$\phi$は傾いているからです。
ヤコビ行列は、多変数関数のすべての一階の偏微分を行列にまとめたものなので、
もちろん複数種類存在します。
今回は省略しますが、空間固定座標系ではなく機体固定座標系に変換する場合の
ヤコビ行列は上記とは違った形になり、ロールではなくヨーの変化率、
すなわち$\psi$の項が無くなった形で表されます。
ヤコビ行列と特異点
さて、前節までで、ロボットの姿勢は通常オイラー角で表され、
空間固定座標系にしたければヤコビ行列が必要になる、ということが分かりました。
ですがロボットを動かすにあたって重要なのはその逆の操作です。
すなわち、我々がロボットを動かそうとする際は空間固定座標系でポイントを指定し、
それをロボットがオイラー角に変換して動作する、というプロセスを踏むはずです。
指定された空間固定座標系からオイラー角に変換する場合を考えてみましょう。
先ほどの変換式を再掲します。
$$
\begin{pmatrix}
\omega_x\\
\omega_y\\
\omega_z
\end{pmatrix}=
J(\phi, \theta, \psi)
\begin{pmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{pmatrix}
$$
この式からオイラー角を求めるためには、
両辺に左からヤコビ行列の逆行列をかければよさそうです。
すなわち、
$$
J(\phi, \theta, \psi)^{-1}
\begin{pmatrix}
\omega_x\\
\omega_y\\
\omega_z
\end{pmatrix}=
J(\phi, \theta, \psi)^{-1}
J(\phi, \theta, \psi)
\begin{pmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{pmatrix}=
\begin{pmatrix}
\dot{\phi}\\
\dot{\theta}\\
\dot{\psi}
\end{pmatrix}
$$
というわけで、ロボットの動作には座標系の変換が必要で、
そのツールとしてヤコビ行列と、その逆行列は必須であることが分かります。
しかし、もしもヤコビ行列が逆行列を持たなかったとしたらどうなるでしょうか？
空間固定座標系で指定したポイントをオイラー角に変換できなくなるので、
ロボットはどう動いたら良いのか分からず、少なくとも正常な動作はできなくなります。
もっと悪ければ加速度は無限大に発散して、ロボットは暴走します。
というわけで、はじめに挙げた特異点とは？の内、
・特異点とは、ヤコビ行列が特異行列のような性質を持つ点
・特異点とは、ヤコビ行列が逆行列を持たなくなる点、
以上の意味はこれで説明できました。
そして、ヤコビ行列が逆行列を持たずオイラー角に変換できない、ということは
はじめにヤコビ行列をかけることで空間固定座標系に変換した結果、
不可逆的な変化をもたらしてしまったと言うことができます。
不可逆的な変化とは幾何学的に見ると、
空間が押しつぶされて、次元が減ってしまった状態に相当します。
そして、空間が押しつぶされてしまっているかどうかは行列式が0かどうかで分かります。
さらに、次元が減ることはすなわちランク落ちしているということなので、
特異点とは？の説明の内、
・特異点とは、ヤコビ行列の行列式が0になる点
・特異点とは、ヤコビ行列がフルランクでなくなる点
以上ふたつも結局はヤコビ行列に逆行列が存在しない、
すなわち座標系の相互な変換ができない状態というのを言い換えただけである、
ということが分かります。
特異点の発生条件
では、どんな時にヤコビ行列の行列式が0になる、つまり特異点が発生するのでしょうか？
ヤコビ行列の行列式を計算すると、実は下記の通り非常にシンプルな形になります。
$$
det(J)=cos\theta
$$
このとき前節の通り、$\theta$はピッチ角です。
つまり、$det(J)=cos\theta=0$になるのはピッチ角が±90°になるときということですね。
物理現象としては$90°$のピッチ回転によってヨー軸とロール軸が一直線に重なって、
それぞれの軸の回転が独立して扱えない状態になっている状態で、数学的には
まさしく空間が押しつぶされて次元が減った、ランク落ちした状態ということです。
上記の現象こそ、いわゆるジンバルロックと呼ばれるものです。
この状態になったロボットに対して、Y軸周りに回るよう指令を出したとすると、
逆行列が存在しないためロボットに対しては無限大の速度を指令したことになり、
結果的には、ロボットは安全のためエラー停止します。
これが私の遭遇した事象の背景でした。
まとめ
今回は特異点について書いてきましたが、私自身ロボットに関してあまり詳しくないので、
間違いや不足を見つけられた方は、すみませんが是非ご指摘頂けるとありがたいです。
またいずれ、特異点を回避する手法についても書いていこうと思います。