産業機械をパソコンの中でひも解く・多関節ロボット ―― まっすぐ動かしたいだけなのに、腕が自分を放り出す

6つの関節を持つ腕。関節の角度から手先の位置を求め、各コマを立体で描いています。赤い点線は手先がなぞる経路です。

ツールの先を、ゆっくり、まっすぐ動かしたいだけでした。速さは秒速12センチで、人が歩くよりずっと遅い。それなのに、腕は自分を放り出しました。

これは、プログラムを書く人なら一度は見たことのある壊れ方です。分母がゼロに近づいて、答えが無限大に飛ぶ、あの割り算です。今回はその割り算が、6本の関節を持つ腕の上で起きます。直し方も、ソフトのバグと同じ発想でした。以下、順に見ていきます。

前回は充填機を扱いました。今回は多関節ロボットです。私はこの腕を触ったことがありません。実機も用意していません。パソコンの中に腕を建てて、もっとも単純な指令、つまり「まっすぐ動かす」が、なぜ腕を暴れさせるのかを見ます。

腕をパソコンの中に建てる

腕は6つの関節でできています。付け根から順に、肩の高さが360mm、上腕が420mm、前腕が400mm。手先の届く範囲はおよそ900mmです。これらの寸法はコードに直書きしていません。一枚の表にまとめてあって、腕の形はそこから全部計算されます。数字を一つ変えれば、絵も、後で出てくる暴走の起き方も、いっせいに変わります。

関節の角度を決めれば、手先がどこに来るかは掛け算だけで出ます。これを順運動学と呼びます。全部の関節を0度にすると、手先は基準点から見て横に820mm、少し手前、高さ360mmのところに来ます。ここまでは素直です。

逆に、手先を動かしたいとき

やりたいのは、たいてい逆です。手先をこの向きにこの速さで動かしたい。そのために、各関節をどれだけの速さで回せばいいか。

関節の回る速さと手先の動く速さは、ヤコビ行列という一枚の行列でつながっています。順方向は、関節の速さを入れると手先の速さが出る。逆をやりたければ、この行列の逆を取って、手先の速さから関節の速さを求めます。

ここに落とし穴があります。逆行列の中には、行列式で割る計算が隠れています。行列式がゼロに近づくと、割り算の答えが無限大に飛ぶ。ソフトで分母がゼロに近いときと、まったく同じことが起きます。

たった0.5度のねじれで、腕が暴れる

行列式がゼロになる姿勢を、特異点と呼びます。この腕の特異点にはいくつかの形があります。腕を伸ばしきったとき、手首の中心が付け根の回転軸の真上に来たとき、そして手首の軸が一直線に並んだとき。今回の経路が通るのは、最後の手首のものです。軸が並んだ瞬間、腕は回る自由をひとつ失って、行列式がゼロに落ちます。

私は最初、まっすぐ動かすだけなら特異点も怖くないだろうと思っていました。実際にやってみると、そうではありませんでした。意外だったのは、まっすぐなピッチ運動そのものは無害なことです。特異姿勢をなぞるだけなら、何も起きません。ところが、その姿勢を横切る向きに、目で見えないほどの0.5度のねじれを足すと、関節の速さが一気に跳ね上がります。手首の二本の軸が、秒速7490度で逆向きに回ろうとする。関節の速さの上限は秒速720度なので、その10倍以上を要求している計算です。

手首を曲げる関節（軸5）だけを動かしたところ。軸4と軸6のなす角は、そのままこの関節の角度です。0度に近づくと、二本の回転軸が一直線に重なります。

なぜ小さなねじれで関節の速さが発散するのか。この図で見当がつきます。手首は三つの関節でできていて、真ん中の関節（軸5）が手首を曲げる役です。この曲げ角がゼロに近づくと、両側にある二本の回転軸、つまり手首ロールの軸4と工具ロールの軸6が、一直線に重なります。図で二本の矢印がそろっていくところです。重なってしまうと、軸4を回しても軸6を回しても、手先は同じ向きにしか回りません。二つあったはずの回転が、一つに減る。これが、腕が回る自由をひとつ失うということで、行列式がゼロに落ちる正体です。

では、失われた向きへ、それでも手先を回そうとするとどうなるか。残された方法はひとつだけです。軸4と軸6を逆向きに回して、その差でわずかに向きを変える。ところが二本が重なっているほど、同じ変化を作るのに要る逆回転は大きくなります。重なりきった瞬間には、どれだけ速く回しても届かない。だから、消えかけた向きへのほんのわずかな指令に対して、関節の速さが青天井に跳ね上がるのです。まっすぐ動かすこと自体ではなく、この失われた向きへの過敏さが、特異点の近くで起きていることでした。

歯止めをかける

直し方は、ゼロ割にガードを入れるのと同じ発想です。分母がゼロに近くても、そこに小さな数をひとつ足しておけば、答えは無限には飛ばない。この腕でも同じことをします。逆行列を作るときに、対角に小さな数を足しておく。すると、特異点の近くでも関節の速さが跳ね上がらずにすみます。減衰付き最小二乗、略してDLSと呼ばれる手法です。

効き目ははっきり出ました。同じ指令に対して、素朴な逆算では秒速7490度だった関節速度が、歯止めをかけると秒速40度に収まります。7490度が40度。およそ180倍の差です。歯止めをかけると、腕は同じ経路をすうっと通り抜けます。

数値計算をやる人には、これはLevenberg-Marquardt法と同じ発想の正則化だと言えば早いと思います。病的な逆行列に対角の小さな数を足して安定させる、あの手です。

ただ、歯止めには代償がある

いいことばかりではありません。特異点の近くでは、関節の速さを抑えることと、手先を指令どおりの向きに保つことは、両立しません。歯止めをかけると、腕は暴れないかわりに、狙った向きから21度ほどずれて通過します。速さの安全を、正確さで買っているわけです。ここは選ぶしかないところで、どちらが正しいという話ではありません。

正直な境界も書いておきます。このモデルは腕の幾何だけを扱っています。関節のたわみ、摩擦、がたつき、熱、そういうものは入っていません。指令した角度がそのまま実現する、という前提です。本物の腕では個体ごとに較正が要りますし、狙った場所にいつも来る精度と、同じ場所に戻る精度は別物です。ここから先は実機の仕事になります。

実機がなくても、確かめられること

実機がないので、この計算が本物と合っているかは、確かめられません。そこは最後まで分かりません。それでも、内部で辻褄が合っているかは確かめられます。

たとえば逆運動学、つまり手先の位置と向きから関節の角度を求める計算は、ひとつの姿勢に対して最大8通りの答えを出します。肩を前に出すか後ろに引くか、肘を上げるか下げるか、手首を表にするか裏にするか。その8通りを、もう一度順運動学に通して手先に戻すと、全部が元の位置と向きに、小数点以下12桁までそろって戻りました。答えが8つあっても、どれも同じ手先を指しているということです。

もうひとつ。手首の幾何をわざと1箇所だけ壊して、腕を組み直してみます。すると、閉じた式で解いていた逆運動学が、200姿勢すべてで答えを返せなくなりました。閉じた式で解けていたのは、手首の三本の軸が一点で交わる、その形のおかげだったわけです。前提を崩すとテストが通らなくなる、という形で確かめてあります。

分かったこと

書いておきたかったのは、いちばんやさしい指令ほど、あぶない場所を通ることがある、ということです。まっすぐ動かすだけ。速さも控えめ。それでも経路が特異点をかすめると、腕は自分を放り出す。

そして、その壊れ方はソフトの世界のものとまったく同じ数学でした。分母がゼロに近づく割り算です。だから直し方も同じで、分母に小さな数を足して歯止めをかける。ただしその歯止めは、正確さを少し手放すことで買っています。ここも、ソフトで数値を安定させるときに払う代償とよく似ています。

触ったことのない機械でも、パソコンの中でならここまではたどれます。別の機械でも同じ手順を試しているところです。

この記事で使ったコードは、腕を建てる順運動学から、逆運動学、ヤコビ行列、特異点とDLS、それに図の生成まで、すべて自作で GitHub に置いてあります（logicia32/articulated-robot-lab）。params.json の数字を一つ変えると、腕の形も特異点の起き方も変わります。MIT ライセンスです。

同じ指令に対して、左（素朴な逆算）は手先の向きをほぼ保ったまま、関節4と6が逆向きに回って速さの上限（秒速720度）を大きく超えます。右（歯止めをかけた側）は同じ経路をなめらかに通り抜ける。違いは、分母に小さな数を一つ足したかどうかだけです。