線形倒立振子モデルに基づく，歩行解析

16725 ワード
ロボット 制御工学 ロボット テキストリンク

概要


こんなモデルを考える．
右足，左足を切り替えて歩行を実現していくのが，今回の目標である．
モーター等の入力はなく，粘性等のエネルギー損失もないので，初速・初期位置で動作が決定することに注意してほしい．

歩行イメージ


２本の足両方が着地し，安定状態になることはないとする．
片足をつけて，移動し，反対の足を着地しを繰り返す．

線形倒立振子モデルにおけるZMP方程式

次に数式モデルについて解説する．
足に重みはなく，胴体に質点があるモデル使う．
質点の質量を$m$とすると.
運動方程式は

m \ddot{x}=f \cos\theta\\
m \ddot{y}=f \sin \theta -mg

足より伝わる力$f$は，足の接地方向と一致する．
なぜなら，方向がずれていた場合

といえる

以上より

\frac{\sin \theta}{\cos \theta} = \frac{\ddot{y} + g}{\ddot{x}} = \frac{y}{x-x_z}\\

といえる．$\frac{y}{x-x_z}$は

より出した．

ここでものすごい強引な仮定を導入する

y = const

つまり，$\ddot{y} = 0$とするのである．

\frac{\sin \theta}{\cos \theta} = \frac{ g}{\ddot{x}} = \frac{y}{x-x_z}\\
\therefore \frac{ g}{\ddot{x}} = \frac{y}{x-x_z}\\
\therefore x-x_z = \frac{y}{g} \ddot{x}

この式が，線形倒立振子モデルにおけるZMP方程式であり，運動方程式である．

ZMP方程式の考察

ZMP方程式を簡略化した式

\frac{y}{g} \ddot{x} = x -x_z

に従うと以上より考えられる．
$x_z$を中心として，発散する不安定なシステムである．
$x,\dot{x}$がある状態の時に着地足を切り替えることで，足の着地点$x_z$の更新が行われる．
その時，次の二つの動作が考えられる．

では，この動作2になる条件を考えよう．一度，反時計回りに回りだしたら．戻らないのは想像がつく．よって，速度が一度でも負になれば，歩行に失敗したといえる．

歩行条件

$\dot{x}$が正になるのが，動作2の条件である．
$\ddot{x}$は切り替え始めは負で速度が減っていき，山を越えると（頂点を超えると）正になり速度が増していく．そして，着地足を切り替える

では，速度$\dot{x}$が正になる条件を考えよう．
二階微分の微分方程式なので，

\frac{y}{g} \ddot{x} = x -x_z

には，解が存在する．

x -x_z = A \exp( \sqrt{\frac{g}{y} }t) + B \exp( -\sqrt{\frac{g}{y} }t)

速度$vx$は

vx= A   \sqrt{\frac{g}{y} }  \exp( \sqrt{\frac{g}{y} }t) - B   \sqrt{\frac{g}{y} }  \exp( -\sqrt{\frac{g}{y} }t)

となる．
初期状態を$x_0,vx_0$とし，着地足の場所を$x_z$とすると

x_0 =A+B + x_z\\
vx_0 = (A-B)   \sqrt{\frac{g}{y} } \\

となり，$A,B$を求められ，$x,vx$が計算できる．
速度$vx$が負になると，切り替え（歩行）に失敗したということになる．

A=\frac{x_0 - x_z  + \sqrt{\frac{y}{g} } vx_0 }{2}\\
B=\frac{x_0 - x_z  - \sqrt{\frac{y}{g} } vx_0 }{2}\\

である.代入し

\begin{align}
vx &=& 　 \sqrt{\frac{g}{y} }　\{ A \exp( \sqrt{\frac{g}{y} }t) - B   \exp( -\sqrt{\frac{g}{y} }t) \}\\
&=& \sqrt{\frac{g}{y} }　\{ (x_0 -x_z) \sinh(\sqrt{\frac{g}{y} }t) +  \sqrt{\frac{y}{g} } vx_0 \cosh(\sqrt{\frac{g}{y} }t) \}\\
\end{align}

さらに

\begin{align}
x &=&(x_0 -x_z) \cosh(\sqrt{\frac{g}{y} }t) +  \sqrt{\frac{y}{g} } vx_0 \sinh(\sqrt{\frac{g}{y} }t)  + x_z\\
\end{align}

が得られる．

とりあえず，シミュレーションしてみよう！

流れは

import numpy as np
import math
import matplotlib.pyplot as plt

x0 = 0
v0 = 5
xz = 1
x_switch = 1
g=9.81
y=1

t=0
t_now = 0
dt = 0.0001

#保存用
T=[]
x_save=[]
v_save=[]

while True:
    if t_now>10:
        break

    x=(x0 - xz)*np.cosh(math.sqrt(g/y) * t)+math.sqrt(y/g) *v0 * np.sinh(math.sqrt(g/y) * t) + xz

    v= math.sqrt(g/y) * ( (x0 - xz)*np.sinh(math.sqrt(g/y) * t)
                         +math.sqrt(y/g) *v0 * np.cosh(math.sqrt(g/y) * t) )

    x_save.append(x)
    v_save.append(v) 
    T.append(t_now)    

    if x - xz > x_switch:
        """
        足を切り替える．
        いろいろ更新
        """
        print(x,xz)
        xz = xz + 2
        x0 = x
        v0 = v
        t = 0

    plot_max = 100
    if x<-1 *plot_max or x>plot_max:
        break

    t= t+dt
    t_now = t_now + dt


plt.figure(0)

plt.title('x')
plt.grid(True)
plt.xlabel('t')
plt.ylabel('x')
#plt.ylim(0,20) 
plt.plot(T, x_save)
plt.show()

plt.figure(1)

plt.title('v')
plt.grid(True)
plt.xlabel('t')
plt.ylabel('v')
#plt.ylim(0,50) 
plt.plot(T, v_save)
plt.show()

こんな感じで一定歩行が実現できています．

次の目標は，切り替えタイミングと歩行失敗の関連付けです．

切り替えタイミングと歩行成功条件

歩行時の一歩の大きさ$x_{step}$と切り替え時の足の速度$v_0$を与えた際，歩行が成立する条件は速度がずっと正であることより

-\frac{x_{step}}{2} \sinh (wt) + \frac{v_0 }{w} \cosh (wt) > 0\\
w = \sqrt{\frac{g}{y}}\\

となることである．$-\frac{x_{step}}{2} \sinh (wt) + \frac{v_0 }{w} \cosh (wt)$のグラフを書くことで成功するか判断できる．

最後に歩行時の一歩の大きさ$x_{step}$のみを与えた際の，歩行が成立する切り替え時の足の速度$v_0$はいくつになるか考えてみよう．

v_0 > \frac{x_{step} w}{2} \tanh (wt)\\

と変形できるので，$ \frac{x_{step} w}{2} \tanh (wt) $で最小速度がわかりそうですが，経過時間$t$の最大値が不明のため，少々難しい．あきらめる．
$tanh$は-1~1なので，あまり考えず$\frac{x_{step} w}{2}$の歩行速度を持たせれば確実に歩行させることができる．

v0 = 2*math.sqrt(g/y) / 2 + 0.001

このように初速を与えると


こんな感じで，速度がほぼ0になります．

v0 = 2*math.sqrt(g/y) / 2

このように初速を与えると


こんな感じで歩行を停止します．

最後に

次は，足に面積があるものの歩行解析をしてみようと思います．