はじめに
ホンダの人間型二足歩行ロボットP２がロボット研究の歴史的転換点としてIEEEマイルストーンに認定されました。→ リンク1 リンク2
「IEEEマイルストーン」は、電気・電子・情報・通信分野における歴史的偉業をたたえる制度です。確かに当時衝撃的だった。ASIMOのように可愛くなる前だから、ちょっと怖いなと思ったのを覚えています。二足歩行ロボットといえば ZMP制御です。
二足歩行ロボットの制御は全く経験ありませんが、ZMP制御ってどんなものかちょっとトライしてみたいと思います。
二足歩行制御
人が普通に歩くような動歩行は、倒れ続けながら倒れないように制御し続ける運動 であるため、本質的に不安定です。
ZMP（zero-moment point）は、二足歩行時に働く重力と慣性力（加速・減速による力）の合力のモーメント（回転させようとする力）がゼロになる床反力の作用点です。二足歩行時に、ZMP（床反力の作用点）が足裏の範囲からずれるとモーメントが発生し、転倒に至ります。
ZMP制御とは「転倒しないように、ZMP（床反力の作用点）を足裏の範囲に保つように重心を移動させる制御」です。二足歩行制御で最初に必要になるのは、ZMP制御にしたがって、どのように重心を移動するかを決めることになります。
今回は、EXCELでZMP制御、つまり二足歩行における重心移動のシミュレーションをやってみたいと思います。
ダウンロード
ZMP制御のEXCELファイルは、こちら →リンク からダウンロードできます。
二足歩行時の重心移動の状態方程式
二足歩行における重心移動をモデル化していきますが、２次元に簡略化して単質点で考えると、下図のように倒立振子のモデルになります。重心を前方に押し進める具体的な機構については、ここでは考えません。
二足歩行時に、ZMPに作用する重力と慣性力の合力 \(F\) の \(x\) と \(y\) 成分は
\(f_x = M\ddot{x}\tag{2}\)
\(f_y = F\sin\theta\tag{3}\)
\(f_x = F\cos\theta\tag{4}\) \[\sin\theta = \frac{ y }{ \sqrt{ {(x-x_{zmp})}^2 + {y}^2 } } \hspace{2cm} \cos\theta = \frac{ x-x_{zmp} }{ \sqrt{ {(x-x_{zmp})}^2 + {y}^2 } } \tag{5}\]（３）（４）（５）より\[f_y = \frac{y}{ x-x_{zmp} } f_x \tag{6}\]（１）（２）（６）より\[x_{zmp} = \frac{ x(\ddot{y}+g) - y\ddot{x} }{ \ddot{y}+g } \tag{7}\] 重心の高さは一定（\(y=h_{cog}\)）とすると、\(\ddot{y} = 0\) なので \[x_{zmp} = x - \frac{ h_{cog} }{ g } \ddot{x} \tag{8}\] （８）を書き換えると \[\frac{d}{d t} v = \frac{g}{h_{cog}} x - \frac{g}{h_{cog}} x_{zmp} \] \[\frac{d}{d t} x = v\] 状態方程式にまとめると
\[ \frac{d}{dt} \left[ \begin{array}{l}
x \\
v
\end{array} \right]= \left[ \begin{array}{cc}
0 & 1 \\
\frac{g}{h_{cog}} & 0
\end{array} \right]
\left[ \begin{array}{c}
x \\
v \end{array} \right]
+\left[ \begin{array}{c}
0 \\
-\frac{g}{h_{cog}} \end{array} \right] x_{zmp} \tag{9}\]
\(x_{zmp}\) が入力になっています。\(x_{zmp}\) は、二足歩行時に働く重力と慣性力（加速・減速による力）の合力のモーメント（回転させようとする力）をゼロにする床反力の作用点です。二足歩行時は、遊脚（Swing leg）と支持脚（Stance leg）を交互に切り替えて歩行を進めますが、\(x_{zmp}\) は支持脚（Stance leg）の接地位置になります。
この状態方程式は、支持脚（Stance leg）が接地された地点が ZMP になるように、重心の加減速を調整する制御モデルです。
支持脚（Stance leg）と遊脚（Swing leg）の切り替えタイミング
二足歩行を行うためには、上記の状態方程式に加え、\(x_{zmp}\) を更新するタイミング（支持脚（Stance leg）と遊脚（Swing leg）の切り替えタイミング）を決めるロジックが必要です。
歩幅を\(S_{zmp}\) とすると、まずは、重心が歩幅の中心を越えるタイミングで、制御入力\(x_{zmp}\) を更新することにします。 \[x_{zmp}(n+1) = x_{zmp}(n) + S_{zmp} \] 制御入力\(x_{zmp}\) の更新により、歩行状態は支持脚（Stance leg）と遊脚（Swing leg）が切り替わります。
状態方程式を逐次式に変換
ここまで求めてきた二足歩行時の重心移動のモデルですが、時間応答を確認（シミュレーション）するには逐次式に変換するとExcelでの確認が容易になります。また、この状態方程式は、ZMP制御（転倒せずに歩行する）のための目標加減速値を算出しているため、実機を動かす場合には、ターゲットに実装する必要があります。この場合、マイコンベースのデジタル制御システムに実装するため逐次式への変換が必要になります。
二足歩行時の重心移動シミュレーション
Excelのシミュレーションのシートで時間応答を確認しています。
モデルの条件
Swtiming は、制御入力\(x_{zmp}\) の更新タイミング（支持脚（Stance leg）と遊脚（Swing leg）が切り替わるタイミング）で、重心が歩幅の中心を越えるタイミングが 0.5 です。Swtiming を微調整することで、歩行速度の加減速を変更でき、表の下の Swtiming スライダーにより、0.4～0.6の間で調整できます。
逐次式により離散時間ΔT毎の時間応答を計算します。Excelの1行の中で行列演算を展開しています。
時間応答の結果は下のグラフのようになりました。
重心に作用して前方に押し進める力 \(F\) は、式（８）から加速度 \( \ddot{x} \) を求め算出しています。歩行中は、\( \pm 0.02 [m/s^2] \) の加減速を繰り返し、その慣性力を利用して支持脚（Stance leg）と遊脚（Swing leg）を切り替え歩行を継続しています。
Startボタンをクリックすると重心移動モデルの時間応答を歩行イメージで確認できます。
支持脚（Stance leg）は、前方に押し進める力 \(F\) が ＋（加速）時には赤に、－（減速）時には青になります。また、Sim speed の値により再生速度を変えることができます。
支持脚（Stance leg）と遊脚（Swing leg）を切り替えるタイミングを変えてみる
支持脚（Stance leg）と遊脚（Swing leg）を切り替わるタイミングを Swtiming スライダーで変更してみます。
Swtiming を 0.6 にすると次第に歩行速度が上がっていきます。逆に 0.4 では以下のようになります。
最後までお読みいただき、ありがとうございました。
今回は２次元に簡略化したモデルで、具体的な機構については考えていません。実際の歩行機構では、動く部品ごとにモーメントが3次元で発生するし、踵からつま先の大きさも影響するし、リアルワールドでは床面の傾斜や障害物等も考慮が必要で、必然的にAI/強化学習が必要になってきたのだと思います。それにしても、最近のヒューマノイドロボットの進化はすごいですねぇ。
0 件のコメント:
コメントを投稿