物理シミュレーションによるオドメトリの検証

はじめに

前回は横滑り運動を考察しましたが、マイクロマウスのような２輪差動式の移動ロボットのシミュレーションについてもプログラムを作ることができました。そこで、自己位置推定のオドメトリ計算について

直線近似
円弧近似
横滑りを考慮した場合の直線近似
横滑りを考慮した場合の円弧近似

これら４つの場合について物理シミュレーションを用いてその性能を比較してみたいと思います。

また、横滑りについては物理方程式に基づいて、横滑り角の微分方程式を導いて、オドメトリの近似計算を考慮するための式の導出過程について丁寧に書き下してみました。

以後の実際の計算では物理シミュレーションにより疑似的にエンコーダ情報やジャイロ情報を作り出し、オドメトリを計算しそれぞれの離散化による誤差について見てみたいと思います。

はじめに
オドメトリによる自己位置推定の計算方法
横滑りを考慮したオドメトリの算出
- 運動を計算する各種方程式
- 横滑り角を用いた方程式の整理
  - 直線近似
  - 円弧近似
数値実験
おわりに
参考文献
付録：ソースコード

オドメトリによる自己位置推定の計算方法

以前の記事でオドメトリについてどの様なものかかきましたが、プログラムで使いやすいように式を整理してみたいと思います。

blog.rikei-tawamure.com

必要な情報

以前の記事にもありますように、オドメトリという言葉は距離計測から来ていますので、周期ごとに距離情報が得られることが必要です。もし周期ごとに速度や角速度の情報が何らかの手段で得られましたら、それらを活用することもできます。マイクロマウスのような移動ロボットがオドメトリによって自己位置推定するのには以下のようなセンサーからの情報が活用できるかと思います。

ホイールエンコーダからのタイヤの回転量情報
ジャイロからの角速度情報
加速度計からの加速度情報 ‐ 地磁気センサーからの磁北の方向

ここでは、エンコーダのみか、ジャイロが有ることを想定して話を展開します。

エンコーダの積算距離から推定できる状態量

オドメトリ計算を考えているとたまに記号の使い方がわからなくなるので、下図はロボットの位置の各順番の関係を整理して描いています。

本記事ではｋ番目の情報が「現在」として最新の情報という事で統一します。

各エンコーダの積算距離から次のように各種状態量が推定できます。

ロボットの現時点での角度は

$\begin{eqnarray} \psi_k=\frac{{l_R}_{(k)}-{l_L}_{(k)}}{W} \end{eqnarray}$

一つ過去の時点の速度は、まず各車輪の速度を数値微分で求め

$\begin{eqnarray} {v_R}_{(k-1)}=\frac{{l_R}_{(k)}-{l_R}_{(k-1)}}{h} \end{eqnarray}$

$\begin{eqnarray} {v_L}_{(k-1)}=\frac{{l_L}_{(k)}-{l_L}_{(k-1)}}{h} \end{eqnarray}$

続いて、各車輪の速度からロボットの速度を求めます。

$\begin{eqnarray} V_{k-1}=\frac{{v_R}_{(k-1)}+{v_L}_{(k-1)}}{2} \end{eqnarray}$

ロボットの角速度（ヨーレート） $r$ は以下のようになります。

$\begin{eqnarray} r_{k-1}=\frac{{v_R}_{(k-1)}-{v_L}_{(k-1)}}{W} \end{eqnarray}$

ここまで準備しますと、オドメトリ計算による位置の推定ができます。

オドメトリ計算

オドメトリ計算の手段には主に直線近似と円弧近似があります。

直線近似

水平方向の現在位置の推定は以下の式で行います。

$\displaystyle{ x_{k} =x_{k-1}+V_{k-1} h \cos \psi_{k-1} }$

垂直方向の現在位置の推定は以下の式で行います。

$\displaystyle{ y_{k} =y_{k-1}+V_{k-1} h \sin \psi_{k-1} }$

円弧近似

以下のように、 $t_{k-1}$ から $t_k$ の $h$ 秒間に変化したロボットの角度を $\Delta \psi_{k-1}$ とします。

$\begin{eqnarray} \Delta \psi_{k-1}= \psi_k - \psi_{k-1} \end{eqnarray}$

水平方向の現在位置の推定は以下の式で行います。

$\begin{eqnarray} x_k = x_{k-1}+\frac{2V_{k-1}} {r_{k-1} }\cos \left( \psi_{k-1} + \frac{\Delta \psi_{k-1}}{2} \right ) \sin \frac{\Delta \psi_{k-1}}{2} \end{eqnarray}$

垂直方向の現在位置の推定は以下の式で行います。

$\begin{eqnarray} y_k = y_{k-1}+\frac{2V_{k-1}} {r_{k-1} }\sin \left( \psi_{k-1} + \frac{\Delta \psi_{k-1}}{2} \right ) \sin \frac{\Delta \psi_{k-1}}{2} \end{eqnarray}$

ただし角速度が０となるときは上記の式は発散してしまいます。その際は直線状に走行することになるので以下の式で計算します。

$\displaystyle{ x_k=x_{k-1}+V_{k-1} h \cos \psi_{k-1} }$

$\displaystyle{ y_k=y_{k-1}+V_{k-1} h \sin \psi_{k-1} }$

ジャイロの活用

ここまでで、ヨーレート $r_k$ が登場していますが、ジャイロがあればエンコーダ距離情報からの数値微分ではなく直接ヨーレートが得られるためそちらを使用するのも良いです。また、ヨーレートを数値積分することでロボットの角度 $\psi_k$ が求まります。数値積分については台形積分を紹介します。

$\displaystyle{ \psi_k =\psi_{k-1} + \frac{h ( r_k + r_{k-1})}{2} }$

積分については誤差の蓄積もあるので注意が必要。定期的にリセットしなければなりません。

横滑りを考慮したオドメトリの算出

前回、マイクロマウスの横滑り運動を取り扱いました。

blog.rikei-tawamure.com

運動を計算する各種方程式

以下に挙げるのはその運動を計算する式になります。

ロボットのローカル座標に関する微分方程式

$\begin{eqnarray} \dot u &=& \frac{F_x}{m} + rv\\ \dot v &=&\frac{F_y}{m} - ru\\ \dot r &=&\frac{N}{I_{zz}}\\ \end{eqnarray}$

ロボットのワールド座標系の姿勢に関する微分方程式

$\begin{eqnarray} \dot X &=& u \cos \Psi - v \sin \Psi\\ \dot Y &=& u \sin \Psi + v \cos \Psi\\ \dot \Psi &=& r \end{eqnarray}$

横滑り角 $\beta$ について。

$\begin{eqnarray} \beta=\tan^{-1}\frac{v}{u} \end{eqnarray}$

力やモーメントに関する計算式

$\begin{eqnarray} F_x&=&F_x (t)\\ F_y&=&-K \beta\\ N&=&N(t) \end{eqnarray}$

横方向力は横滑り角 $\beta$ に比例します。比例係数はコーナリングパワー $K$ と呼ばれています。

横滑り角を用いた方程式の整理

ロボットは一定の速度 $V$ で走行しているとします。するとロボットのローカル座標における前後方向速度 $u$ と横方向速度 $v$ は次のように表すことができます。

$\begin{eqnarray} u&=& V \cos \beta\\ v&=& V \sin \beta\\ \end{eqnarray}$

上式をそれぞれ時間で微分すると

$\begin{eqnarray} \dot u&=& - V \dot \beta \sin \beta\\ \dot v&=& V \dot \beta \cos \beta\\ \end{eqnarray}$

ここで $\beta$ が微小だとすると

$\begin{eqnarray} u&\approx& V \\ v&\approx& V \beta\\ \end{eqnarray}$

$\begin{eqnarray} \dot u&\approx& - V \dot \beta \beta\\ \dot v&\approx& V \dot \beta\\ \end{eqnarray}$

これらを運動を表す方程式に代入して整理しオドメトリ計算に必要な式だけの残すと、 $\beta$ の微分方程式を含めて以下の式が得られます。

$\begin{eqnarray} \dot \beta &=& - \frac{K}{mV} \beta -r\\ \dot \Psi &=& r\\ \dot X &=& V(\cos \psi - \beta \sin \psi)\\ \dot Y &=& V(\sin \psi + \beta \cos \psi)\\ \end{eqnarray}$

更に最後の２式は三角関数の合成を使うと、次のようになります。

$\begin{eqnarray} \dot X &=& V\sqrt{1+\beta^2} \cos( \psi + \alpha)\\ \dot Y &=& V \sqrt{1+\beta^2}\sin ( \psi + \alpha )\\ \alpha&=&\arctan \beta \end{eqnarray}$

ここでも $\beta$ が微小だと考えると、上の３式は以下のように近似できます。

$\begin{eqnarray} \dot X &=& V\cos( \psi + \alpha)\\ \dot Y &=& V \sin ( \psi + \alpha )\\ \alpha&=& \beta \end{eqnarray}$

最終的にオドメトリ計算に必要な式は以下のようになります。

$\begin{eqnarray} \dot \beta &=& - \frac{K}{mV} \beta -r\\ \dot \Psi &=& r\\ \dot X &=& V\cos( \psi + \beta)\\ \dot Y &=& V \sin ( \psi + \beta )\\ \end{eqnarray}$

横滑り角の微分方程式は小島さんの方程式とほとんど同じものだと思います。以上の微分方程式を数値的に解いていくことを考えると、これらについても、直線近似と円弧近似の二つ考えてみたいと思います。

直線近似

オイラー法で解くことを考えると、一つ前のそれぞれの値を使い以下のようになると思います。

$\begin{eqnarray} \beta_k &=&\beta_{k-1} -h( \frac{K}{mV} \beta_{k-1} +r_{k-1})\\ X_k &=& X_{k-1}+hV_{k-1}\cos( \psi_{k-1} + \beta_{k-1})\\ Y_k &=& Y_{k-1}+hV_{k-1} \sin ( \psi_{k-1} + \beta_{k-1} )\\ \end{eqnarray}$

$\psi$ については、上記で述べたようにエンコーダから求めたり、ジャイロの数値積分から求めます。

円弧近似

制御周期間において円弧上に走る理論的根拠が無いのですが、実験的にこちらの計算も試してみようと思います。

導出の過程は「オドメトリによる移動ロボットの自己位置推定」の記事を参考にしてもらうことにして、結論だけを記述したいと思います。

便宜的に $\Delta \psi_{k-1}$ を以下のように定義します。

$\begin{eqnarray} \Delta \psi_{k-1} = (\psi_k + \beta_k)-(\psi_{k-1} + \beta_{k-1}) \end{eqnarray}$

同様にヨーレートも速度ベクトルのヨーレートの推定なので以下のように決めます。

$\begin{eqnarray} \omega_{k-1} = \frac{\Delta \psi_{k-1}}{h} \end{eqnarray}$

以上から、オドメトリの計算式は以下になります。

$\begin{eqnarray} \beta_k &=&\beta_{k-1} -h( \frac{K}{mV} \beta_{k-1} +r_{k-1})\\ X_k &=& X_{k-1}+\frac{2V_{k-1}} {\omega_{k-1} }\cos \left( \psi_{k-1} + \beta_{k-1}+ \frac{\Delta \psi_{k-1}}{2} \right ) \sin \frac{\Delta \psi_{k-1}}{2}\\ Y_k &=& Y_{k-1}+\frac{2V_{k-1}} {\omega_{k-1} }\sin \left( \psi_{k-1} + \beta_{k-1}+ \frac{\Delta \psi_{k-1}}{2} \right ) \sin \frac{\Delta \psi_{k-1}}{2}\\ \end{eqnarray}$

ただし角速度が０となるときは上記の式は発散してしまいます。その際は直線状に走行することになるので以下の式で計算します。

数値実験

以上の横滑りを考慮しないオドメトリについて直線近似と円弧近似、横滑りを考慮したオドメトリについて直線近似と円弧近似についての４パターンについてルンゲ・クッタ法を用いた物理シミュレーションを真値として比較した結果を以下に示したいと思います。

物理シミュレーションは刻み幅1e-5秒、オドメトリは各状態量のサンプリングは1e-3秒で行っています。

コーナリングパワー $K$ が５、１０、２０の場合についてそれぞれの計算を行ってみました。

コーナリングパワーが２０の場合の各方法の比較

赤い点が付いた曲線がオドメトリ計算の結果です。

"横滑りを考慮しない場合

"横滑りを考慮した場合

横滑り考慮した直線近似 — 横滑りを考慮した直線近似（左）と円弧近似（右）の比較

横滑り考慮した円弧近似 — 横滑りを考慮した直線近似（左）と円弧近似（右）の比較

コーナリングパワーが１０の場合の各方法の比較

"横滑りを考慮しない場合

"横滑りを考慮した場合

コーナリングパワーが５の場合の各方法の比較

"横滑りを考慮しない場合

"横滑りを考慮した場合

結果について

横滑りを考慮しない場合のオドメトリ計算は基本的にずれが大きく実際の自己位置推定には使えないと思います。横滑りを考慮した場合は円弧近似のほうが若干良い結果になっていると思います。タイヤのグリップが下回ると、あまり有意な差がみられないような気がしますが、良いタイヤを使った場合の自己位置推定には良さそうです。

おわりに

今回の計算では、数値計算誤差のみを取り扱ったことになります。物理方程式にもとづいて横滑りを考慮したほうが確実に自己位置推定の精度はあがり、更には単なるオイラー法である直線近似よりは円弧近似のほうが場合によっては性能向上が見込まれるのかなと言ったところが今日の結論です。改めて小島さんが立てた式の正しさを確認し感激しているところです。

タイヤの滑りなどの影響やジャイロのノイズなどを考慮した計算をさらに加えてみたいとこです。

思った通りの結果が得られた感じでしたが、円弧近似が割と良い結果になったのがそれなりの時間をかけたのでその甲斐があったと思っております。

では、また！

参考文献

マイクロマウスに結構参考になることがたくさん書かれています。３式改はこの本を読み返して作りました。恩師が代表でまとめた本です。

付録：ソースコード

Jupyter labで実行しています。

blog.rikei-tawamure.com

%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
from tqdm.notebook import tqdm as tqdm

'''
def rk4(func, t, h, y, *x)
ルンゲ・クッタ法を一回分計算する関数
    引数リスト
    func:導関数
    t：現在時刻を表す変数
    h：刻み幅
    y：出力変数（求めたい値）
    *x:引数の数が可変する事に対応する、その他の必要変数
※この関数では時刻は更新されないため、これとは別に時間更新をする必要があります。
'''
def rk4(func, t, h, y, *x):
    k1=h*func(t, y, *x)
    k2=h*func(t+0.5*h, y+0.5*k1, *x)
    k3=h*func(t+0.5*h, y+0.5*k2, *x) 
    k4=h*func(t+h, y+k3, *x)
    y=y+(k1 + 2*k2 + 2*k3 + k4)/6
    return y

'''
導関数の書き方
def func(t, y, *state):
    func:自分で好きな関数名をつけられます
    t:時刻変数(変数の文字はtで無くても良い) 
    y:出力変数(変数の文字はyで無くても良い)
    *state:その他の必要変数(引数の数は可変可能))
#関数サンプル
def vdot(t, y, *state):
    s1=state[0]
    s2=state[1]
    return t+y+s1+s2
    
'''
#以下ロボットの位置と速度を計算するためルンゲクッタソルバに渡す導関数

def uDot(t, u, m, fx ,r, v):
    return fx/m+r*v

def vDot(t, v, m, fy, r, u):
    return fy/m-r*u

def rDot(t, r, Izz, N):
    return N/Izz

def xDot(t, x, u, v, psi):
    return u*np.cos(psi) -v*np.sin(psi)

def yDot(t, y, u, v , psi):
    return u*np.sin(psi) +v*np.cos(psi)

def psiDot(t, psi, r):
    return r

#初期化
U=np.empty(0)
V=np.empty(0)
R=np.empty(0)
X1=np.empty(0)
Y1=np.empty(0)
X2=np.empty(0)
Y2=np.empty(0)
X3=np.empty(0)
Y3=np.empty(0)
X4=np.empty(0)
Y4=np.empty(0)
X5=np.empty(0)
Y5=np.empty(0)
Psi=np.empty(0)
T=np.empty(0)

u=1
v=0
r=0
x=0
y=0
psi=0
t=0
Fx=0
Fy=0
Izz=1
N=0
s=0 #積分器
beta=0

#質量
m=0.1

#
#コーナリングパワー
###################
K=20

#刻み幅
h=1e-5

#制御周期
Tc=1e-3

#旋回パラメータ
psiref=np.pi
omegadot=385 #330
psi24=np.pi/4
TW1=0.01
TW24=np.sqrt(2*psi24/omegadot)
TW3=np.sqrt(omegadot/2/psi24)*(psiref-2*psi24)/omegadot
TIME=int((TW1*2+TW24*2+TW3)/h)

#制御を1msでするためのカウンタ
cnt=0

#求解ループ
for n in tqdm(range(TIME)):
    
    #外力の計算（制御）
    Fy=-K*beta
    if t<TW1:
        N=0.0
        X1=np.append(X1, x)
        Y1=np.append(Y1, y)
    elif t<TW1+TW24:
        N=omegadot
        X2=np.append(X2, x)
        Y2=np.append(Y2, y)
    elif t<TW1+TW24+TW3:
        N=0
        X3=np.append(X3, x)
        Y3=np.append(Y3, y)
    elif t<TW1+TW24*2+TW3:
        N=-omegadot
        X4=np.append(X4, x)
        Y4=np.append(Y4, y)
    else:
        N=0.0
        X5=np.append(X5, x)
        Y5=np.append(Y5, y)
        
    #データのサンプリングと制御
    if cnt==int(Tc/h):
        cnt=0
        err=1-np.sqrt(u**2+v**2)
        s=s+err
        Fx=K*beta**2+25.0*err+0.06*s
        #Fx=100.0*err+0.01*s
        #Fx=0 #制御西にするときはコメントをはずす
    cnt=cnt+1
    
    
    U=np.append(U, u)
    V=np.append(V, v)
    R=np.append(R, r)
    Psi=np.append(Psi, psi)
    T=np.append(T, t)
    uold=u
    vold=v
    rold=r
    xold=x
    yold=y
    psiold=psi
    
    #数値積分（ルンゲクッタ関数呼び出し）
    u=rk4(uDot, t, h, uold, m, Fx, rold, vold)
    v=rk4(vDot, t, h, vold, m, Fy, rold, uold)
    r=rk4(rDot, t, h, rold, Izz, N)
    x=rk4(xDot, t, h, xold, uold, vold, psiold)
    y=rk4(yDot, t, h, yold, uold, vold, psiold)
    psi=rk4(psiDot, t, h, psiold, rold)
    
    t=t+h
    beta=np.arctan2(v, u)



#odometry test

dt=1e-3
eps=1e-8
smpling=int(dt/h)

u=U[0::smpling]
v=V[0::smpling]
psi=Psi[0::smpling]
r=R[0::smpling]

x=0
y=0
beta=0


Xo=np.array([0])
Yo=np.array([0])
Beta=np.array([0])
Psio=np.array([0])

#オドメトリの選択
#swの値で計算を切り替える
#0:直線近似
#1:円弧近似
#2:横滑り考慮した直線近似
#3:横滑り考慮した円弧近似
sw=3

firstflag=1
for us,vs,psis,rs in zip(u, v, psi, r):
    

    if firstflag==1:
        firstflag=0
        dus=us
        dvs=vs
        dpsis=psis
        drs=rs
        dbeta=beta
        continue
        
    #オドメトリの計算方法で場合分け
    #直線近似
    if sw==0:
        dVs=np.sqrt(dus**2+dvs**2)
        x=x+dt*dVs*np.cos(dpsis)
        y=y+dt*dVs*np.sin(dpsis)
    #円弧近似
    elif sw==1:
        deltapsi=psis-dpsis
        dVs=np.sqrt(dus**2+dvs**2)
        if np.sqrt(drs**2)<eps:
            x=x+dt*dVs*np.cos(dpsis)
            y=y+dt*dVs*np.sin(dpsis)
        else:
            x=x+2*dVs/drs*np.cos(dpsis+ deltapsi/2)*np.sin(deltapsi/2)
            y=y+2*dVs/drs*np.sin(dpsis+ deltapsi/2)*np.sin(deltapsi/2)

    #横滑り考慮した直線近似
    elif sw==2:
        dVs=np.sqrt(dus**2+dvs**2)
        x=x+dt*dVs*np.cos(dpsis + dbeta)
        y=y+dt*dVs*np.sin(dpsis + dbeta)
        beta=dbeta-dt*(K*dbeta/m/dVs + drs)
    #横滑り考慮した円弧近似
    elif sw==3:
        dVs=np.sqrt(dus**2+dvs**2)
        beta=dbeta-dt*(K*dbeta/m/dVs + drs)
        deltapsi=(psis+beta)-(dpsis+dbeta)
        omega=deltapsi/dt
        if np.sqrt(omega**2)<eps:
            x=x+dt*dVs*np.cos(dpsis+dbeta)
            y=y+dt*dVs*np.sin(dpsis+dbeta)
        else:
            x=x+2*dVs/omega*np.cos(dpsis+dbeta+ deltapsi/2)*np.sin(deltapsi/2)
            y=y+2*dVs/omega*np.sin(dpsis+dbeta+ deltapsi/2)*np.sin(deltapsi/2)

    dus=us
    dvs=vs
    dpsis=psis
    drs=rs
    dbeta=beta

    
    
    Xo=np.append(Xo, x)
    Yo=np.append(Yo, y)
    Beta=np.append(Beta, beta)

plt.figure(figsize=(10,10))

plt.xlim(0, 0.11)
plt.ylim(0, 0.11)
plt.xticks( np.arange(0.0, 0.11, 0.01) )
plt.yticks( np.arange(0.0, 0.11, 0.01) )
plt.grid()




plt.plot(X1,Y1, lw=5)
plt.plot(X2,Y2, lw=5)
plt.plot(X3,Y3, lw=5)
plt.plot(X4,Y4, lw=5)
plt.plot(X5,Y5, lw=5)
plt.xlabel('x[m]')
plt.ylabel('y[m]')
plt.plot(Xo, Yo, '.-', c='red')
plt.show()