ZMP

永井 忠一 2019.8.17

Zero-Moment Point

床反力の中心点(side view)

バランスが取れているときは、ZMPが支持多角形のほぼ中央にある。転倒しているときには、ZMPが支持多角形の縁に達して、その縁を中心にして回転している。ZMPをできるだけ支持多角形の中央にある状態にするのが、バランス制御の基本的な戦略になる。

支持多角形(top view)

ZMPは支持多角形の領域の外に出ることはない。

３次元のモーメント

外積(cross product)\[ \boldsymbol{\tau}=\boldsymbol{p}\times\boldsymbol{f} \]

モーメント=位置×力\[ \begin{bmatrix} \tau_x \\ \tau_y \\ \tau_z \end{bmatrix}=\begin{bmatrix} x \\ y \\ z \end{bmatrix}\times\begin{bmatrix} f_x \\ f_y \\ f_z \end{bmatrix}=\begin{bmatrix} yf_{z}-zf_{y} \\ zf_{x}-xf_{z} \\ xf_{y}-yf_{x} \end{bmatrix} \]

（右手座標系）

２次元のモーメント

３次元の１成分が０になったと考える\[ \boldsymbol{\tau}=\boldsymbol{p}\times\boldsymbol{f} \]

モーメント=位置×力\[ \begin{bmatrix} 0 \\ \tau_y \\ 0 \end{bmatrix}=\begin{bmatrix} x \\ 0 \\ z \end{bmatrix}\times\begin{bmatrix} f_x \\ 0 \\ f_z \end{bmatrix}=\begin{bmatrix} 0 \\ zf_{x}-xf_{z} \\ 0 \end{bmatrix} \]\[ \tau=\tau_y=zf_x-xf_z \]

具体的なZMPの計算

簡単な例

• x₁: かかとの位置
• x₂: つま先の位置


• f₁: かかとに作用する床反力
• f₂: つま先に作用する床反力


• p: ZMPの位置（未知）

ZMPまわりのモーメント（の水平成分）は０\[ \begin{align} f_{1}(p-x_{1})-f_{2}(x_{2}-p)&=0 \\ f_{1}p-f_{1}x_{1}-f_{2}f_{2}+f_{2}p&=0 \\ (f_1+f_2)p&=f_{1}x_{1}+f_{2}x_{2} \\ p&=\frac{f_{1}x_{1}+f_{2}x_{2}}{f_1+f_2} \end{align} \]

式の確認。全体重がつま先に集中している場合、かかとに作用している力f₁=0、つま先だけにf₂がかかる\[ p=\frac{0x_{1}+f_{2}x_{2}}{0+f_2}=x_2 \]ZMPはつま先に一致する

６軸力センサーによるZMPの計測

市販されている６軸力センサーの例（株式会社レプトリノ）［http://www.leptrino.co.jp/P_CFS.html］

６軸力センサーで計測される値: F_x, F_z（床からの垂直方向の力）, τ（Y軸回りのモーメント）

ZMPの計算\[ \begin{align} -\tau&=F_{z}p \\ p&=-\frac{\tau}{F_z} \end{align} \]（モーメント[Nm]/力[N]で）位置の次元の量になる。-τのマイナスは回転軸の方向

（実際には、６軸力センサーは足の裏に付いていない）

（６軸力センサーを取り付けてある）かかとの高さを考慮したZMPの計算\[ p=\frac{-\tau-aF_x}{F_z} \]

両足支持期のZMP

（上記は片足のZMP）

右足と左足それぞれについてZMPを計算して、それを力で配分することによって全体のZMPを計算する\[ p=\frac{f_R}{f_R+f_L}p_R+\frac{f_L}{f_R+f_L}p_L \]

ZMPと重心運動の関係

テーブル台車モデルで定式化（簡単なモデルで近似）

ZMPまわりに重力と台車の加速度が作り出すモーメントが０\[ \tau=(x-p)Mg-Z_{h}M\ddot{x}=0 \]（ZMPまわりのモーメントは０）

ZMP方程式の導出（pに関して解く）\[ \begin{align} (x-p)Mg-Z_{h}M\ddot{x}&=0 \\ xMg-pMg-Z_{h}M\ddot{x}&=0 \\ -pMg&=-xMg+Z_{h}M\ddot{x} \\ -p&=\frac{-xMg+Z_{h}M\ddot{x}}{Mg} \\ -p&=-x+\frac{Z_{h}\ddot{x}}{g} \\ p&=x-\frac{Z_{h}}{g}\ddot{x} \end{align} \]

ZMP方程式の意味

• 基本的にはxとpだけの式。\(\ddot{x}\)は台車の加速度。掛かっている係数\(\frac{Z_h}{g}\)は定数
  （ZMPは重心だけでなく、重心の加速度に影響を受ける）
• 台車が静止している場合、\(\ddot{x}=0\)なのでp=x。ZMPは重心の真下にある
• 台車が前に加速するとZMPはその分だけ後ろに移る
• 重心が高い場合、\(-\frac{Z_{h}}{g}\ddot{x}\)の項の効果をみると、小さな加速でZMPが大きく変化する
• 逆に、重心が低い場合、ZMPを動かすためには大きな加速が必要になる

ZMP方程式を実際に計算

• 重心の高さ Z_h=0.98m
• 台車の位置 x=0.4m
• ZMP p=0m
• 重力加速度 g=9.8m/s²

ZMP方程式を変形\[ \begin{align} p&=x-\frac{Z_{h}}{g}\ddot{x} \\ -\frac{Z_{h}}{g}\ddot{x}&=p-x \\ \ddot{x}&=-\frac{g}{Z_h}(p-x) \\ \ddot{x}&=\frac{g}{Z_h}(x-p) \end{align} \]

台車（重心）の加速度\[ \ddot{x}=\frac{9.8}{0.98}(0.4-0)=4\,\mathrm{m/s^2} \]

ZMP方程式の解法

ZMP方程式は重心の運動xからZMPの位置pを求める式。本当に行いたいのは、ZMPを与えてxを求めたい。求めたいのは未知数の重心軌道x

ZMPはあらかじめ決定しておく（一歩を何秒ぐらいで歩いてほしいか、一歩をどれくらいの距離で動いてほしいか）

歩行パターン生成問題

連続だったZMP方程式を離散化する\[ p_i=x_i-\frac{Z_h}{g}\ddot x_i \]Δtは、たとえば5ms間隔。ΔtごとのZMP位置を考えて、Δtごとの重心位置を計算したい。

\( \ddot{x} \)を置き換える。加速度を位置の差分で表現する\[ \ddot{x_i}=\frac{1}{\Delta t}(v_{i+1}-v_i)=\frac{1}{\Delta t}(\frac{x_{i+1}-x_i}{\Delta t}-\frac{x_{i}-x_{i-1}}{\Delta t})\]

速度の差分は加速度。さらに、速度は位置の差分から計算できる。

式を整理してまとめる\[ \ddot x_i=\frac{1}{\Delta t^2}(x_{i-1}-2x_{i}+x_{i+1}) \]（加速度の近似式）

\( \ddot x_i \)はxで表現できるので、これをZMP方程式に代入して、整理する。\[ \begin{align} p_i&=x_i-\frac{Z_h}{g}\frac{1}{\Delta t^2}(x_{i-1}-2x_{i}+x_{i+1}) \\ p_i&=\left(-\frac{Z_h}{g\Delta t^2}\right)x_{i-1}+\left(1+2\frac{Z_h}{g\Delta t^2}\right)x_i+\left(-\frac{Z_h}{g\Delta t^2}\right)x_{i+1} \end{align} \]

g, Z_h, Δtはすべて定数。得られた、離散化されたZMP方程式\[ p_i=ax_{i-1}+bx_{i}+cx_{i+1} \]（a, b, cはまとめられた定数）

知りたいのは重心の位置x、ZMPの位置pは与えられている。これを解くためには、未知数が３つで、既知の値がpだけなのでこれだけでは解けない。

方程式を並べる。たとえば、時刻0から時刻99まで\[ p_0=ax_0+bx_0+cx_1 \\ p_1=ax_0+bx_1+cx_2 \\ p_2=ax_1+bx_2+cx_3 \\ \vdots \\ p_{98}=ax_{97}+bx_{98}+cx_{99} \\ p_{99}=ax_{98}+bx_{99}+cx_{99} \]

これをベクトルと行列（マトリクス）で書き直す\[ \begin{bmatrix} p_0 \\ p_1 \\ p_2 \\ \vdots \\ p_{98} \\ p_{99} \end{bmatrix}=\begin{bmatrix} a+b & c & & & & \\ a & b & c & & & \\ & a & b & c & & \\ & & \ddots & \ddots & \ddots & \\ & & & a & b & c \\ & & & & a & b+c \end{bmatrix}\begin{bmatrix} x_0 \\ x_1 \\ x_3 \\ \vdots \\ x_{98} \\ x_{99} \end{bmatrix} \]

こう書いてしまえば線形代数の問題になる \[ \boldsymbol p=M\boldsymbol x \]（pは100次元のZMPベクトル、xは100次元の重心ベクトル、100x100の大きな係数行列M）これもZMP方程式（重心軌道とZMPの関係式）

ZMP方程式がこう書かれたのであれば、逆行列を求めれば与えられたZMPから重心軌道xが求められる\[ \boldsymbol x=M^{-1}\boldsymbol p \]（ZMPから重心軌道を得る）

計算デモプログラムの実行

産総研の梶田先生が公開されているコード「BasicGaitGen」を使わせていただきました。m(_o_)m

Octaveを起動して「tridiagonal_demo」を実行

$ octave --no-gui
GNU Octave, version 4.0.0
Copyright (C) 2015 John W. Eaton and others.
This is free software; see the source code for copying conditions.
There is ABSOLUTELY NO WARRANTY; not even for MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE.  For details, type 'warranty'.

Octave was configured for "x86_64-pc-linux-gnu".

Additional information about Octave is available at http://www.octave.org.

Please contribute if you find this software useful.
For more information, visit http://www.octave.org/get-involved.html

Read http://www.octave.org/bugs.html to learn how to submit bug reports.
For information about changes from previous versions, type 'news'.

>> tridiagonal_demo

与えている目標ZMPの軌跡

（処理ごとに「pause」しているので、プロンプトから何かキーを押下）

行列（三重対角行列とその逆行列）の可視化

>> figure; imagesc(M)
>> figure; imagesc(M^-1)

M	M-1

重心の計算

>> x = M^-1 * zmpx;
>> y = M^-1 * zmpy;

ZMPと重心（の床面投影点）をプロット

X軸	Y軸

重心の運動を足の関節角度に変換する

逆運動学

実時間で重心軌道を作り出す方法について

《積み残し》

予見制御(Preview control)

«積み残し»

歩行安定化制御技術について

《積み残し》

参考文献

• 梶田 秀司, 比留川 博久, 原田 研介, 横井 一仁: ヒューマノイドロボット, オーム社, 2005.

© 2019 Tadakazu Nagai