概要
こんなモデルを考える．
右足，左足を切り替えて歩行を実現していくのが，今回の目標である．
モーター等の入力はなく，粘性等のエネルギー損失もないので，初速・初期位置で動作が決定することに注意してほしい．
歩行イメージ
２本の足両方が着地し，安定状態になることはないとする．
片足をつけて，移動し，反対の足を着地しを繰り返す．
線形倒立振子モデルにおけるZMP方程式
次に数式モデルについて解説する．
足に重みはなく，胴体に質点があるモデル使う．
質点の質量を$m$とすると.
運動方程式は
\begin{eqnarray}
m \ddot{x}=f \cos\theta\\
m \ddot{y}=f \sin \theta -mg
\end{eqnarray}
足より伝わる力$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$とするのである．
\begin{eqnarray}
\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}
\end{eqnarray}
この式が，線形倒立振子モデルにおける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$とすると
\begin{eqnarray}
x_0 =A+B + x_z\\
vx_0 = (A-B) \sqrt{\frac{g}{y} } \\
\end{eqnarray}
となり，$A,B$を求められ，$x,vx$が計算できる．
速度$vx$が負になると，切り替え（歩行）に失敗したということになる．
\begin{eqnarray}
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}\\
\end{eqnarray}
である.代入し
\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$を与えた際，歩行が成立する条件は速度がずっと正であることより
\begin{eqnarray}
-\frac{x_{step}}{2} \sinh (wt) + \frac{v_0 }{w} \cosh (wt) > 0\\
w = \sqrt{\frac{g}{y}}\\
\end{eqnarray}
となることである．$-\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
このように初速を与えると
こんな感じで歩行を停止します．
最後に
次は，足に面積があるものの歩行解析をしてみようと思います．