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

# 概要

![１.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/0bdc6631-db2f-59fb-e26b-ea4a347fcb41.png)
こんなモデルを考える．
右足，左足を切り替えて歩行を実現していくのが，今回の目標である．
モーター等の入力はなく，粘性等のエネルギー損失もないので，初速・初期位置で動作が決定することに注意してほしい．


# 歩行イメージ
![２.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/a4511830-58db-1c5f-6cf1-ad07fe844484.png)
２本の足両方が着地し，安定状態になることはないとする．
片足をつけて，移動し，反対の足を着地しを繰り返す．

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

```math
\begin{eqnarray}
m \ddot{x}=f \cos\theta\\
m \ddot{y}=f \sin \theta -mg
\end{eqnarray}
```
足より伝わる力$f$は，足の接地方向と一致する．
なぜなら，方向がずれていた場合
![7.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/824cc065-b343-e8b3-9b47-2fc2dc2cd163.png)
といえる

以上より


```math
\frac{\sin \theta}{\cos \theta} = \frac{\ddot{y} + g}{\ddot{x}} = \frac{y}{x-x_z}\\
```
といえる．$\frac{y}{x-x_z}$は
![8.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/c7f440ad-2e7a-0cad-593e-fd54f8548c5f.png)
より出した．

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

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

```math
\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方程式の考察
![３.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/811710b1-6c63-9c53-b0ca-04988d1cb234.png)

ZMP方程式を簡略化した式

```math
\frac{y}{g} \ddot{x} = x -x_z
```
に従うと以上より考えられる．
$x_z$を中心として，発散する不安定なシステムである．
$x,\dot{x}$がある状態の時に着地足を切り替えることで，足の着地点$x_z$の更新が行われる．
その時，次の二つの動作が考えられる．
![４.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/8509166c-9810-8ef0-20cc-56c7a003b265.png)
では，この動作2になる条件を考えよう．一度，反時計回りに回りだしたら．戻らないのは想像がつく．よって，速度が一度でも負になれば，歩行に失敗したといえる．

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

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

```math
\frac{y}{g} \ddot{x} = x -x_z
```
には，解が存在する．

```math
x -x_z = A \exp( \sqrt{\frac{g}{y} }t) + B \exp( -\sqrt{\frac{g}{y} }t)
```
速度$vx$は

```math
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$とすると

```math
\begin{eqnarray}
x_0 =A+B + x_z\\
vx_0 = (A-B)   \sqrt{\frac{g}{y} } \\
\end{eqnarray}
```
となり，$A,B$を求められ，$x,vx$が計算できる．
速度$vx$が負になると，切り替え（歩行）に失敗したということになる．

```math
\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}
```
である.代入し

```math
\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}
```
さらに

```math
\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}
```
が得られる．

# とりあえず，シミュレーションしてみよう！
流れは
![6.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/ac780748-2a97-26db-b618-4525f7b0c2dd.png)


```python
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()
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/236d21d3-49e1-bef2-18af-b9ac87ee210a.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/d0c249f1-e087-e867-dcbd-bf83f85acf24.png)


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



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

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

```math
\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$はいくつになるか考えてみよう．


```math
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}$の歩行速度を持たせれば確実に歩行させることができる．

```python
v0 = 2*math.sqrt(g/y) / 2 + 0.001
```
このように初速を与えると
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/6e2815a3-fb15-4a60-0dd7-aa9a306f2aad.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/f8ed2168-3444-6f59-e568-9d738cb8c9d4.png)
こんな感じで，速度がほぼ0になります．



```python
v0 = 2*math.sqrt(g/y) / 2 
```
このように初速を与えると
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/f79471a3-aee8-5bd2-c8ab-2365d732fad9.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/9e0c3425-d9e0-3113-3c14-58d3c9e87100.png)
こんな感じで歩行を停止します．

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