二足歩行ロボットのモーション制御としてポーズとポーズの間を分割してつなぐといった方法がありますが、この分割された中間のポーズがやっかいなヤツで、実は思ったとおりの動きをしていません。

これはロボットのサーボが角速度で制御されているからです。
それが、足先の点の線速度で目標までの軌跡の制御が出来たらどんなにすばらしいでしょう。

これを可能にするのが逆運動学です。
ただし逆運動学の一般的解法は定まっておらず、DH法による計算は煩雑です。

実は小型二足歩行ロボット程度の足の制御は高校数学+α（逆三角関数の基礎）で説明が出来ます。
ここでは「逆三角関数」と「極座標」を中心にプログラムを説明します。

これは、僕が解いてみた二足歩行ロボットにおける逆運動学の幾何学的解法です。

■　MENU　■

1. 膝を原点とする足のXYZの計算
2. ＸＹＺの計算における補足事項
3. ヨー軸について考える
4. ヨー軸についての補足事項
5. 膝ダブルサーボ構成について考える
6. 「腰から膝」「膝から足首」の長さが違う場合
7. その他のパターンについてと、まとめ
8. 応用編1) 非直交軸の平行リンク足の逆運動学 (NEW)

公開 2006.12.20
追記 (3),(4) 2006.12.27
追記 (5) 2007.03.12
追記 (6),(7) 2007.04.07
追記 (8) 2011.03.11

精度に関しては足の長さの定数をmmでとって、倍精度で計算すれば、だいたいmm単位の制御は出来ます。
分解能に関しては、サーボのタイマ分解能とサーボコントローラーの分解能に依存するので、一概には言えません。
　

各角度を定義する。

また足の長さを2aとし、腰から膝、膝から足の長さをそれぞれaとする。

ちなみにaは定数である。

腰から足までの直線距離を変数 l とおく。

x,y,zは目標値。

体に対して、足裏が常に地面に平行であるためδ を計算する必要がある

xは与えられたx座標の変位である。

ここでピッチ角が決定する。

正面図

また、足裏が地面に平行であるには、ロール方向も同様に ε を計算することが必要

y,zは目標値である。

プログラムへの実装

いざ、プログラムに実装するわけですが、その計算はといえば引数をxyzと考えた場合、プログラム的な順番では下記の計算を毎ループ両足分計算することになります。

また、足と腰に関しては直交していることが前提となります。（非直交軸に関しては後述）
直交でない場合にも応用はできますが、そのままでは正確な距離計算は出来なくなり、値は目安のものとなります。
mm（ミリメートル）単位で制御する場合は、直交軸をお勧めします。

使い方としては、例えば足の長さ 2a = 200(mm)だった場合。
x = 0 , y = 0 , z = 200 で直立となります。
足を曲げるならzを小さくし、前に出すならxを増やす、横に出すならyを増加。って感じで、数値はそのままmm（ミリメートル）に置き換えられます。 腰が原点になっています。

あと、この計算はmath.hを使って計算するわけですが、マイコンでリアルタイムでやると結構重いです。
ＳＨ系ならまぁ問題ないですが、Ｈ８だと秒間数十回くらいか、それ以下だと思います。
（僕はH8/3052(CISC・25MHz)を愛用していたのですが、歩行の軌跡計算も含めると１秒に４分割程度しかできず、SH2/7144(RISC・48MHz)に乗り換えました。）
RISCのCPUで40MHz以上での使用をお勧めします。
　

そこにヨー軸を加えることで、足をひねることが出来ます。
応用として出した足をその場で回転させたり、旋回歩行したりできます。

その計算について説明します。
　

ヨー軸を考える

目的のXYZ座標（絶対座標）とヨー軸の角度から、ヨー軸に与える角度と、上記のプログラムに与えるべきXYZ座標（ヨー軸を基準にした相対座標）を極座標平面状で変換します。

 γ の位置の足をそのままの状態で γ 時計回りに回転させれば、下図のようになり、前に出した足と比べると、その場で γ だけ回転したように見えます。

簡単に言えば、腰ヨー軸で足裏ヨー軸と同じ働きをさせよう！！ってことです。
（逆に言えば、構造的に足裏ヨー軸が可能ならその方が楽です。）

まず、移動させたい絶対座標xin、yinを、計算のために極座標に変換する。
θ は極座標平面に従って定義。

・・・ （atan2関数を使って計算）

γ 足をひねりたいので、欲しい足のひねりを得るθ をγ 分回転させてヨー軸を基準としたXY座標を求める。

このままでは、γ 分多く回転しているので、ヨー軸を反対方向にγ 分回転させて、絶対座標を元に戻す。 ここで、θout はヨー軸に与えられる角度である。

ここで、Z座標には影響しないので

・・・　（ヨー軸にZ座標は影響しない）

最終的にここではXY座標を求めただけなので、求めた座標をXout、Yout、Zout を「腰を原点とする足のＸＹＺの計算」の計算に放り込 めば、思いのままの足裏XYZ+ヨー軸制御が出来ます。

また、このヨー軸の計算を用いるには、あくまでも腰の部分で３軸が直交していることが条件になります。
　

そんなこんなで、ダブルサーボの計算方法をまとめてみました。
前回とは違って、前に曲げても後ろに曲げても見た目が変にならない計算方法です。
　

結論から言うと膝がシングルサーボの時とダブルサーボの時の計算の違いは α 、β の変数の計算が違うだけです。

膝の計算

腰から足までの直線距離を変数 l とおく。

x,y,zは目標値。

この計算によりダブルサーボの膝は、腰と足首を結んだ直線（l）に常に平行を保ちます。

前述したとおり α 、β 以外の変数は基本的にシングルサーボと同じ考え方でOKです。

そこで、どんな長さのロボットでもこの計算が適用できるように改変したものをここに紹介します。
ここでも、高校数学で説明できるように、ヘロンの公式を用いて展開する方法を用いました。

ヘロンの公式ってなに？って人はこちら
　

「腰から膝」「膝から足首」の長さが違っていてもピッチ軸周りの計算がちゃんとできれば、ロール軸関係の計算は特に違いはない。

違いは α 、βの部分だけです。

ヘロンの公式を用いて膝に出来た三角形の面積を求める

三角形の底辺を l と考えると面積を底辺で割れば高さが求められる

２つの三角形に分けて角度を求めていく

この α 、β 、α' 、β' を用いれば、あとは１章の「膝を原点とする足のXYZの計算」を用いてXYZのモジュールを全てそのまま使用できます。

腰ピッチの回転が逆転する瞬間の図（黒）
SISO JUNK STUDIO/ SISO-LAB　より抜粋

ここまでのことが理解できれば、『「腰から膝」「膝から足首」の長さが違うダブルサーボ』等、応用はいくらでも出来ます。

また、ここでは直交軸についてしか触れませんでしたが、同じ方法を用いて、直交軸以外のKHR-1や、ROBONOVA-1のようなロボットにも応用することが出来ます。

ポイントは、ピッチ軸は当然足を前後するためにあるのですが、それと同時にピッチ軸には、腰のロール軸と足首のロール軸の距離を制御する役割もあるということです。

この２つの要素を別々に考えることが出来れば、ここで紹介した逆運動学の計算をどんな構造のロボットにでも応用できると思います。

-------------

というわけで、２足歩行ロボットをやる上で、これから逆運動学の計算はどんどん重要になってくると思います。

その重要性をどう伝えようかと考えていたのですが、SISO JUNK STUDIO/ SISO-LABのSISOさんのブログで素晴らしい記事があったのでここに抜粋させていただきます。

なぜ、逆運動学を使わないと安定した歩行ができないかを図で説明されています。

この制御は、角速度一定でプログラムされた教示機能や、ポジションキャプチャリングにはまず不可能なことです。

二足歩行の総合格闘技大会のROBO-ONEに置いても、「相手を倒すアイディア」も重要ですが、それ以上にスリップダウンがダウンにカウントされるようになった今、歩行において既に差は見え始めていると思います。

というわけで、「是非逆運動学を普及させたいな」と思い、この記事を書きました。

とにかく、少しでも興味を持たれましたら、頑張って実装してみてください。
きっとロボットの動きのスマートさに、感動するでしょう。

軸と角度の定義

基本的には直交軸によく似ているのですが、腰と足首の関節が直交ではないためｂ，ｃの定数を設定します。

また、腰ピッチと足首ピッチを結ぶ直線をｌ’と定義します。

ｌは原点とx,y,zの最終的な制御点を結ぶ直線です。

非直交軸の特徴はｌとｌ’が一致しないので、そこを計算に含めるのがポイントです。

２つの三角形に注目することがポイントです。

また、この平面はx-z平面ではなく、ピッチ軸に直交する平面上であるという点もポイントです。
なので、√(z^2+y^2)の部分をzとしないこと！

これでｌ’が計算できます。

ｌ’を用いて計算する点を除けば、直交軸と考え方は同じです。

また、足裏が地面に平行であるには、ロール方向も同様に ε を計算することが必要 。これは直交軸の時と同じです。

ちなみにこのときはｌ’ではなくｌに注目している点がポイントです。