4脚ロボットのモデル予測制御(MPC)part2 : 歩容生成

はじめに

ここでは、4脚ロボットの歩容生成について説明します。ロボットの歩容（ほよう）生成は、四足歩行ロボットや二足歩行ロボットなどの移動型ロボットが、安定して効果的に地面を歩くための 脚の動きのパターンを設計する プロセスです。四足歩行ロボット「MIT Cheetah」では、独立した位相変数を用いた歩容生成が行われており、トロットやギャロップなどの動物のような自然な歩容を実現しています。要するに行きたい方向へ動くための足の動かし方を決める部分です。ここでは歩容生成から、swing modeまでの説明をします。正直MITの論文がすごく丁寧に書いてくれているので、割とそのままになってしまいましたが、MITの手法は足の3次元の動きには対応していないので、そういった点でオリジナリティがあるのかなと思っています。

概要　ざっくりとした流れ

今回は赤丸の囲った部分を説明します。ざっくり説明すると、

• 上位の指令指令を受け取る
• 歩容に合わせて、足の位相を生成
• 足の踏み場所を速度に合わせて生成
• 始点と終点を補間
• 逆運動学で座標を関節角度へ変換

といった流れで行っています。注意されたいのが、解析的に逆運動学で関節角度を明示的に指定するのはswing modeのときだけです。stance modeのときはmpcとwbcを頑張って解いて動かしています。

上位の指令生成(desired command)

リモコンやmove baseなどの上位層から(\(v_x,v_y,\omega_x\))の速度ベクトルを受け取ります。移動に関してはこの3つだけなのですが、MPCへの目標軌道としては、\(\mathbf{x} = [\Theta , \mathbf{p} , \omega , \dot{\mathbf{p}} \)]の12個のベクトルで与えられます。MPCの目標軌道生成で後々ここの指令がまた必要になるので留意してください。速度を生成するとき、リモコンやmovebaseだとステップ上の速度が入ってしまうため、ローパスフィルタなどをかけてあげることでなめらかにするといいと思います。

$$\mathbf{v_{filtered}} = \frac{g}{s+g}\mathbf{v_{ref}}$$

足の位相生成(phase generator)

今回で一番大切な部分です。4脚ロボットが動いている動画を見ると4本の足を、独立して複雑な動作をしていると行った印象を受けると思います。しかし実際のところは、足一本ごとに独立したswing modeとstance modeが0~1の位相を持ちそれぞれの位相を少しずらすだけで、トロット(trot)、ギャロップ(gallop)、静歩行(static walk)を実現することができます。これはmarc raibertのホッピングロボットで提唱された、ステートマシーンの考え方と同じです。そこの実装の部分と各位相の話をします。詳しくは[1]を参照してください。

位相生成に必要なもの

位相の生成には、ロボット本体の時間が必要です。ROSのタイマーや、ほかにはstd::chrono::steady_clockを利用して現在時間を位相生成器に送り続けます。

位相生成式

定義の仕方により、いろいろ位相生成の式は変わるのですがここではMITで提唱された方法を記載します。

パラメータ	単位	説明
\(T_{st}\)	[s]	接地期間
\(T_{sw}\)	[s]	遊脚期間
\(T_{stride}\)	[s]	接地と遊脚の合計
\(t_{last}\)	[s]	リセットされたときの時刻
\(t_{i}\)	[s]	各足ごとの経過時間
\(t\)	[s]	pc クロック
\(t_{i,elapsed}\)	[s]	最後に足をついてからの経過時間
\(S_{i,st}\)	[]	stance modeの位相
\(S_{i,sw}\)	[]	swing modeの位相
\(\Delta S_{FR,i}\)	[]	右前足を基準とした各足の位相遅れ

\(t_{i,elapsed}\)は足が地面についてから経過した時間です。\(T_{stride}\)が終了した瞬間にリセットして再び計測を開始します。\(T_{stride}\)は\(T_{st}\)+\(T_{sw}\)で表され、足を動かす1周期の合計時間です。\(T_{st}\),\(T_{sw}\)はそれぞれstance mode , swing modeの周期です。例えば\(T_{st}\)が0.4[s]の場合は、足の地面の接地期間は0.4[s]になり設置してから0.4秒後には地面から足が離れます。\(S_{i}\)は各足の位相で0~1の間の周期的な数値で表現されます。\(\Delta S_{FR,i}\)は足の初期位相です。はじめから足の位相を各足ごとに\(\Delta S_{FR,i}\)位相を少しずらして上げることで様々な歩容を実現します。下付き文字\(i\)は\(i \in {FR,FL,RR,RL}\) の順で記載される足のことを表しています。\(t_{i}\)は\(t_{i,elapsed}\)から初期位相に相当する時間を差し引いた経過時間です。これを用いてswingの軌道を作成します。以下に数式的な処理を示します。

$$ t_{i,elapsed} = \left\{
\begin{array}{ll}
t – t_{last}\\
T_{stride} & (if \quad t_{i,elapsed} > T_{stride})
\end{array}
\right. $$

$$ t_{last}=t \quad (if \quad t – t_{last} > T_{stride})$$

$$ t_{i}=t_{i,elapsed} – \Delta S_{FR,i}T_{stride} $$

$$ S_{i,st} = \left\{
\begin{array}{ll}
\frac{t_i }{T_{st}} & (if \quad 0 < t_i < T_{st}) \\
\frac{t_i + T_{stride}}{T_{st}} & (if \quad -T_{stride}\Delta S_{FR,i} < t_i < -T_{sw})
\end{array}
\right. $$

$$ S_{i,sw} = \left\{
\begin{array}{ll}
\frac{t_i + T_{sw}}{T_{sw}} & (if \quad -T_{sw} < t_{i} < 0) \\
\frac{t_i – T_{st}}{T_{sw}} & (if \quad T_{st} < t_i < T_{stride})
\end{array}
\right. $$

試しにstatic walkの各足の時間の流れの図を載せてみます。

\(t_i\)が\(0\)~\(T_{stride}\)を周期的に往復する中で、stance modeとswing modeの位相が0~1で交互に変化します。この位相を用いて補間式を用いて足のswing 軌道を実現します。位相生成の式が参考論文と少し違うのですが、論文そのまま実装すると位相の変更に対応できなかったので修正を加えました。詳しく言うと位相の設定によっては、stance modeがswing modeより先に来るときに対応できていなかったのでその部分を修正しました。

各歩容ごとの位相遅れ

trotが体の対角線上の足が同期して歩くのに対し、paceは体の横の足が同期して動きます。これはラクダと同じ歩き方なのですが、どうしてそんなふうに進化したんですかね…。ラクダの歩容について調査した研究とかあるのでしょうか？？

位相余談

位相は線形的に増やさないと行けない感じがしますがそんなことないです。例えば0.5までは三角関数を使って増やしてそこから先は線形的に1まで増やす…という方法もあるので、カスタマイズ次第でいろいろな歩容を再現できると思います。

足の置き場決定(step planner)

次の足の位置を\(p_{step}\)と定義します。実際につかうときは足のhip座標系かbase座標系に直して使ってあげます。

raibert Heuristic method

raibert heuristic methodはおそらくmark raibertが提唱したのでしょうか…？足の置き場を決める手法としてはシンプルでとても使い勝手がいいと思います。

$$ p_{step} = \frac{T_{st}\mathbf{v}_{ref}}{2} $$

MIT method

こちらはmit cheetah3の論文中で提唱された方法です。

$$　p_{\text{step}} = \frac{T_{\text{st}} \mathbf{v}_{\text{ref}}}{2} – k (\mathbf{v}_{\text{ref}} – \mathbf{v}) + \frac{1}{2} \sqrt{\frac{h}{g}} (\mathbf{v} \times \mathbf{\omega}_{\text{ref}})　$$

一行目はraibert heuristic methodと同じです。2項目は速度調整のダンピング項です。なぜ符号が負になるのかは不明です。3項目は例えばロボットが回転しているときに、足が遠心力を受けるのですがそれを高さ\(h\)でスケーリングして、足の位置を調整している項だと思います。しかし、この項について単位を計算するとm/sと速度の次元になってしまいます。なぜルートをつけて1/2で補正しているのかは不明です。遠心力に合わせてスケールしているだけの係数ですが、単位が合わないと違和感がありますね。論文のミスプリントかもしれないですね。

補間アルゴリズム(interpolation)

step plannerにより、swingのスタートとゴールの位置をゲットすることが出来ました。では実際に足を振る時は、空中のパスに沿って動かさなくてはいけません。足のスタートとゴールを設定し、その間を0~1の媒介変数表示で補間する必要があります。以下に3つの例を示します。補間式はこれだけではないので色々調べてみてください。

スプライン補間と線形補間(2次)

zについてz座標の開始点を\( z_0 \)、中間点を\( z_{mid} \)、最終点を\( z_{f} \)として、拘束条件\( S_i = 0でz_0\)、\( S_i = S_{mid}でz_mid\)、\( S_i = 1でz_f\)となるように立式し、連立方程式を解くことで係数を導出します。

$$ \mathbf{footp}_{\text{z,ref}} = a_0 + a_1 S_i + a_2 S_i^2$$

$$ \begin{aligned}
a_0 &= z_0 \\
a_1 &= (z_f – z_0) + a_2 \\
a_2 &= \frac{(z_{mid} – z_0) – S_{mid}(z_f – z_0)}{S_{mid}^2 – S_{mid}}
\end{aligned}$$

x,,yについて

$$ \mathbf{footp}_{\text{x,y,ref}} = (1-S_i)\mathbf{p}_{x,y,start} + S_i\mathbf{p}_{x,y,end} $$

スプライン補間はn次まで表現可能ですが、4次以降はあまり意味がないですし、発散しやすいのでおすすめしません。実際は2次か3次の利用が多いです。x,y,zすべてをスプライン補間すると軌道がグニョグニョになるのでx,y座標においてはシンプルな線形補間をします。

ベジェ補間(n次)

$$ \mathbf{footp}_{\text{ref}} = \sum_{i=0}^{n} \frac{n!}{i!(n-i)!} (1 – S_i)^{n-i} S_i^i p_i$$

制御点 \(p_i = [ p_0, p_1, …p_i…p_{n}]\)

mini cheetahの一番最初[1]がこの式を使っています。n次式だとn+1個の制御点を持ちます。制御点をいくつか自分で用意しないと行けないですが、0~1の媒介変数表示で使用できるので楽だと思います。それと始点と終点を必ず通るので調整しやすいです。

これらの共通点は、始点から終点を0~1で媒介変数表示できるところです。例えばスプライン補間だと複数の点が必要で各点を0~1で補間してしまうため、点全体で0~1で正規化をする必要があるため、少し面倒です。正弦波も使えるのですが、平地でしか使えないのであまり向いていないと思います。

どれをつかうべきか

選択肢が多くてどれをつかうべきか迷うと思いますが、指標として以下が考えられると思います。

• (\(v_x,v_y,\omega_x\))の変動による補間のパスのスケールの容易さ
• パスの成形にこだわりがある

足の逆運動学(inverse kinematics)

補間アルゴリズムによって得られた軌道(\(x,y,z\))に対応する角度を逆運動学により導出します。逆運動学によって角度を取得する方法は2種類あります。1つめはヤコビアンを用いた数値的解法です。このヤコビアンは、数学のヤコビ行列と違うので注意してください。混同するとロボット警察に叩かれます。2つめは同次変換行列や図形的性質を用いて解析的に解く方法です。4脚ロボットのような簡単な構造であれば座標を用いてそのまま逆運動学が解ける解析的解法がおすすめです。ここでは解析的解法で頑張って解いてみようと思います。先端座標(\(x,y,z\))を与えて(\(\theta_{hip},\theta_{upper},\theta_{lower}\))を導出します。

座標系・ロボットモデル

上図が4脚ロボットのリンク・ジョイント名です。ロボットでは右手座標を用いた方が得することが多いです。

モデル簡略化

実際の私が作成したPSI1の足はリンクとしてあちこち曲がったりして複雑な形状をしています。なのでいったん簡略化してみます。

左図の点線部分をそのまま、1本の足とみなして計算を簡略化します。(\(\theta_{upper},\theta_{lower}\))の修正式を示します。

$$ \theta_{uoff} = cos^{-1}\frac{x_u}{l_u} – \frac{\pi}{2}$$

$$ \theta_{loff} = cos^{-1}\frac{x_l}{l_l} – \frac{\pi}{2}$$

$$ \theta_{upper} = \theta_{utemp}+\theta_{uoff}$$

$$ \theta_{lower} = \theta_{ltemp}+\theta_{loff}-\theta_{uoff}$$

\(\theta_{hip}\)導出

4脚ロボットを真正面から見たときの図です。\(\theta_{hip}\)をはじめに導出します。

図より

$$\theta_{htemp} = tan^{-1}\frac{y}{z}$$

$$ \theta_{hoff} = \frac{\pi}{2} – cos^{-1}\frac{l_{hy}}{\sqrt{y^{2} + z^{2}}} $$

$$ \theta_{hip} = -\theta_{htemp} -\theta_{hoff} $$

\(\theta_{upper},\theta_{lower}\)の導出

足の真横から見た図形を用いて、\(\theta_{upper}\)、\(\theta_{lower}\)を導出します。

上のような視点で見た足の図形を示します。\(-\theta_{hip}\)分回転させることで、足がまっすぐになるようにします。

$$
\begin{bmatrix}
x^{‘} \\
y^{‘} \\
z^{‘}
\end{bmatrix}
=
\begin{bmatrix}1 & 0 & 0 \\0 & \cos(-\theta_{hip}) & -\sin(-\theta_{hip}) \\0 & \sin(-\theta_{hip})& \cos(-\theta_{hip})\end{bmatrix}
\begin{bmatrix}x\\y\\z \end{bmatrix}
$$

$$ \theta_{ltemp} = cos^{-1} \frac{x^{‘2} + z^{‘2} – l_u^{2} – l_l^{2}}{2l_ul_l} $$

$$ \theta_{utemp} = tan^{-1}\frac{x^{‘}}{z^{‘}} – tan^{-1}\frac{l_lsin\theta_{ltemp}}{l_u + l_lcos\theta_{ltemp}} $$

これで、ある座標を与えたときの角度を求めることが出来ました。ほとんどの4脚ロボットはこのやり方で解くことができます。

全体制御フロー

最後に指令を受け取ってから足をswingするまでの流れを示します。

速度コマンドを受け取っている間位相を更新し続けて、速度コマンドの大小で歩幅を変えます。複雑そうに見えてシンプルになっています。最後にこの章でやったことを実装すると以下の動作になります。

おわりに

今回はここまでです。stance modeで得られる足の位相を用いて逆運動学を解けばそれだけで4脚ロボットの歩行ができます。このやり方は4脚に限らず、ヒューマノイドでも実現可能です。ただ、バランスをとったり、重心軌道が一定の歩行をしたい場合はMPCを組み合わせる必要があります。次の記事ではMPCの部分についてガッツリ解説しようと思います。swing modeにおいてもwbcを使って一括に解いても面白そうですがあまりメリットはないのかなとも思っています。これソースコード載せたほうがいいでしょうか？コメントがあったら修正してソースも載せようと思います。

参考文献
Hierarchical controller for highly dynamic locomotion utilizing pattern modulation and impedance control : implementation on the MIT Cheetah robot [1]

inverse kinematic analysis of a quadruped robot [2]