エクサウィザーズMLエンジニアの柳元です。あけましておめでとうございます（遅い）。
エクサウィザーズのRobotTechチームはこれまでにマニピュレータロボットを使って粉体秤量、液体秤量、パレタイジング、ピッキング、コンプライアンス制御などの動作を機械学習させることに成功してきました。そして、これらの学習済みモデルを COREVERY によってデリバーしています。
学習と制御のフローを考えたりデバッグをする上で、念頭に置かなければならないことの１つとして、特異点の問題があります。今回はロボット制御におけるこの「特異点問題」についてお話しします。

ロボットの特異点(Singularity)とは？

特異点と聞いて何を想像するでしょうか？多くの人が技術的特異点を想像するかもしれませんが、数学・物理学・制御学においては

特異点（とくいてん、英: singular point、シンギュラー・ポイント）は、一般解の点ではなく特異解の点こと。ある基準 (regulation)を適用できない、あるいは一般的な手順では求まらない(singular) 点である。特異点は、基準・手順に対して「—に於ける特異点」「—に関する特異点」という呼び方をする。

とあります（Wikipediaより）。
ロボット制御における特異点は、構造的に制御できない姿勢を指します。軌道に特異点が含まれている場合、ロボットは特異点付近において高速に移動（暴走）し、そして特異点で停止してしまいます。なので、制御する際にはこれを避ける必要があります。

ロボットの姿勢の表現

ロボットがどんな体勢になっているかの表現は、ふつう以下のいずれかを使用します。

関節変位 (Joint space) $q$: 関節の角度の値
姿勢 (Pose, Task space) $r$: TCP(Tool Center Point, ロボットの手先の位置)を表す3次元空間の値

例えば、URのような6DoFのマニピュレータは、6個の関節(joint)を回転させて姿勢(pose)を変化させることができます。

この場合は関節変位$q$と姿勢$r$はそれぞれ \begin{equation} q = [\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6] \\ r = [x, y, z, r_x, r_y, r_z] \end{equation} というベクトルで表現できます。
例えばこのロボットのエンドエフェクターにグリッパーを取り付けて、物体のピッキングをさせる場合について考えます。物体は深度カメラで撮影されていて、その位置$P=[x_P, y_P, z_P]$は既に計算されています。あとはロボットのTCPを位置$P$付近にくるように姿勢を制御してグリッパーを閉じれば、物体を掴むことができます。
ただ、ロボットそのものにできることは基本的には関節を動かすことだけです。目的地点の位置や姿勢を指示されても、関節をどう動かせば目的の姿勢になるのかがわかりません。そこで運動学を考える必要が出てきます。

運動学

運動学(kinematics)とは関節変位$q$と姿勢$r$の関係を表す数式であり、

順運動学(forward kinematics): 関節変位$q$から姿勢$r$を求める, $r = f(q)$
逆運動学(inverse kinematics): 姿勢$r$から関節変位$q$を求める, $q = f^{-1}(r)$

の2種類があります。上述のピッキングの例だと、逆運動学を使って$r$を$q$に変換する必要があります。

同次変換行列

順運動学$f$について知るために、まず同次変換行列(Homogeneous transformation matrix)について説明します。
3次元空間に原点座標系$O$と座標系$A$があり、点$Q$が座標系$A$上の $r_{Q}=[x_{Q}, y_{Q}, z_{Q}]$に存在していたとします。ここで、座標系$A$を原点座標系$O$に対して$p=[x_p, y_p, z_p]$移動させ、さらに$R_{OA}$回転させます²。移動と回転後の点$Q'$の位置$r_Q'=[x_Q', y_Q', z_Q']$は \begin{equation} r_Q' = p + R_{OA}r_{Q} \end{equation} となり、これは下のように書けます: \begin{equation} \begin{bmatrix} r_Q' \\ 1 \\ \end{bmatrix}=\begin{bmatrix} & & & \\ &R_{OA}& &p \\ & & & \\ 0&0 &0&1 \end{bmatrix} \begin{bmatrix} r_{Q} \\ 1 \\ \end{bmatrix} = H_{OA} \begin{bmatrix} r_{Q} \\ 1 \end{bmatrix} \end{equation} この$H_{OA}$を同次変換行列といいます。

リンクとジョイント

ロボットはリンクとジョイント（関節）で構成されています。人間の腕で考えると、脇や肘、手首が回転の中心があるジョイントで、ジョイント同士の間にある上腕や前腕がリンクです。
再びURについて考えると、リンクとジョイントは下の図のようになっています。「ジョイントn」の関節角度は$\theta_n$で、TCPはリンク6の上にある点です。

このように、ロボットは複数の座標系が連なりあって構成されていて、各ジョイントの関節変位と姿勢によってTCPの姿勢、つまり順運動学が計算できると予想できます。

Denavit–Hartenberg parameters

Denavit–Hartenberg parameters（DHパラメータ）は、上のような同次変換によってロボットの順運動学を計算する際に必要な4つのパラメータ$\alpha, a, d, \theta$を指します。

上の図のようにDHパラメータと座標系が決まっている時、座標系$n-1$から座標系$n$への同次変換は次のようになります: \begin{equation} T_{n}^{n-1}={\rm Trans}_{z_{n-1}}(d_n) \cdot {\rm Rot}_{z_{n-1}}(\theta_n) \cdot {\rm Trans}_{x_{n}}(a_{n}) \cdot {\rm Rot}_{x_n}(\alpha_n) \end{equation} ただし、 \begin{equation} {\rm Trans}_{z_{n-1}}(d_n)= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_n \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \hspace{30pt} {\rm Trans}_{x_n}(a_n)= \begin{bmatrix} 1 & 0 & 0 & a_n \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \\ {\rm Rot}_{z_{n-1}}(\theta_n)= \begin{bmatrix} \cos\theta_n & -\sin\theta_n & 0 & 0 \\ \sin\theta_n & \cos\theta_n & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \hspace{15pt} {\rm Rot}_{x_n}(\alpha_n)= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\alpha_n & -\sin\alpha_n & 0 \\ 0 & \sin\alpha_n & \cos\alpha_n & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \end{equation} であり、それぞれが各パラメータによる並進または回転の同次変換となっていることがわかります。また、Modified Denavit–Hartenberg parametersというものもあり、上記の古典的なDHパラメータと比較して各座標系の原点と変換の順序が異なっています。詳しくは Wikipediaの記事などを参照してください。

順運動学

URのようなマニピュレータの順運動学を導出する場合、リンク0とジョイント0が乗っている座標系0からリンク6とジョイント6が乗っている座標系6への同次変換$T_6^0$を導出する必要があり、 $$T_6^0=T_1^0\cdot T_2^1\cdot T_3^2\cdot T_4^3\cdot T_5^4\cdot T_6^5$$ によって求められます。 $T_n^{n-1}$は$\alpha_{n-1}, a_{n-1}, \theta_n, d_n$を変数とする関数とみなせます。そしてURの場合、$\theta_n$が独立の変数なので、実質的には$T_6^0$は$q=[ \theta_1, \cdots, \theta_6]$を変数とする関数となります。 $T_6^0=T(q)$とすると、順運動学は \begin{equation} \begin{bmatrix} r \\ 1 \end{bmatrix}=T(q)\begin{bmatrix} r_0 \\ 1 \end{bmatrix} \end{equation} から、最初に書いた$r=f(q)$を導けます。

逆運動学

順運動学から逆運動学$f^{-1}$を求めるためには、上の式から逆算で求めれば良さそうですが、$T_N^0$の$N$が大きくなるほど逆算は困難になります。 $N=6$にもなると、ほとんど無理です。そのため、順運動学から数値解析的に求めるのがふつうです。以下は姿勢$r_{tgt}$の時の関節変位$q_{tgt}$を求める手順です。

$q=q_0$（初期値の設定）
$r=f(q)$（順運動学で$r$を求める）
$| r_{tgt} - r |$の大きさを調べる

$| r_{tgt} - r |$が十分小さければ、$q_{tgt}=q$として終了する
そうでなければ、順運動学のヤコビ行列の逆行列によって$q$を修正する。手順2に戻る。

ヤコビ行列とは、

一変数スカラー値関数における接線の傾きおよび一変数ベクトル値函数の勾配の、多変数ベクトル値関数に対する拡張、高次元化

です（Wikipediaより）。つまり、勾配法の要領でヤコビ行列から$q$の修正すべき向きと量が決まって、$q_{tgt}$に近づけていけます。
$r=[r_1, \cdots, r_M], q=[q_1, \cdots, q_N]$の場合、順運動学は \begin{equation} x_i=f_i(q_1, \cdots, q_N) \end{equation} であり、ヤコビ行列の要素は \begin{equation} J_i(q_j)=\frac{\partial f_i(q_1, \cdots q_N)}{\partial q_j} \end{equation} と表せます。
姿勢の空間は$r=[x, y, z, r_x, r_y, r_z]$のため$M=6$となります。また、URのような6DoFマニピュレータの場合だと$q=[\theta_1, ..., \theta_6]$のため$N=6$となります。よってヤコビ行列は$6\times 6$の正方行列となります。

特異点

問題は、順運動学のヤコビ行列の逆行列が存在していない場合です。逆行列を持たない正方行列の特徴として行列式が0(${\rm det} J=0$)となることが挙げられます。ヤコビ行列の逆行列は \begin{equation} J^{-1}=\frac{\Delta_{ij}}{{\rm det} J} \end{equation} となるので、${\rm det} J$が0に近づくほど$J^{-1}$の要素の絶対値は大きくなります。姿勢$r$と$J^{-1}$から関節変位$q$を求めるので、このときの関節変位の速度も急上昇し、結果的にロボットが暴走しているように見えます。そして、${\rm det} J$が0となると、逆行列を算出できなくなり停止してしまいます。これがロボット制御における特異点の問題です。

擬似逆行列

今日、様々な解決策が提案されています。もっともよく知られているのは、逆行列の代わりに擬似逆行列を使う方法です。擬似逆行列の利点は、上のような例における特異点を避けること、そして7DoFのような冗長ロボットのように$N\neq6$でないためにヤコビ行列が正方でない場合についても求められることです。欠点は計算量が多いことで、スピードを重視して転置行列で近似してしまう方法もあります。

おわりに

エクサウィザーズは優秀なエンジニア、社会課題を一緒に解決してくれる魔法使い”ウィザーズ”を募集していますので、ご興味を持たれた方はぜひご応募ください。
採用情報｜株式会社エクサウィザーズ

注釈

1: ロボット（マニピュレータ）には直列タイプと並列タイプがあり、ここでは直列タイプについて扱う。直列タイプに含まれるのは、例えば UR, Panda, LBR iiwaなど、関節が直列な構造を持つものである。

2: $R_{OA}$は$3\times 3$の回転行列である。

エクサウィザーズ Engineer Blog

株式会社エクサウィザーズのエンジニアチームブログ

ロボット制御における特異点