移動ロボットの軌道追従制御1.1


移動ロボット

恐縮ですが、カレンダーの14日目に参入させて頂きました。

自律移動ロボットの軌道追従制御についてお話したいと思います。

自律移動ロボットの主な構成要素

*自己位置推定

*地図生成

*軌道計画

*軌道追従←今回はここ!!!!!(とても楽しいです!)


移動ロボットの軌道追従制御1.1

移動ロボットの軌道追従制御手法の一つとして, Kanayama等によって提案された軌道追従制御について説明する.

参考論文はこちら

A stable tracking control method for an autonomous mobile robot

Y. Kanayama ; Y. Kimura ; F. Miyazaki ; T. Noguchi

https://ieeexplore.ieee.org/abstract/document/126006

kana2.jpg

この軌道追従制御はリアプノフ関数を用いて, 漸近安定であることが保証されており, 目標軌道が時間関数で与えられることを前提としている. 上付き添え字$\bigcirc^{cmd}$は指令値, 添え字$\bigcirc^{res}$は応答値, 添え字$\bigcirc^{err}$は偏差である.

\begin{eqnarray}

\boldsymbol{P}^{err} &=&
\left[
\begin{array}{ccc}
x^{err}&y^{err}&\theta^{err}
\end{array}
\right]^T \nonumber \\
&=&
\left[
\begin{array}{ccc}
\cos \theta^{res}&\sin \theta^{res}&0 \\
-\sin \theta^{res}&\cos \theta^{res}&0 \\
0&0&1
\end{array}
\right]
(\boldsymbol{P}^{cmd}-\boldsymbol{P}^{res})
\end{eqnarray}\tag{1}

ここで, 移動ロボットは真横に移動出来ないので式(2)の拘束が課せられる.

\begin{eqnarray}

\dot {x}^{cmd}\sin \theta^{cmd}-\dot{y}^{cmd}\cos \theta^{cmd} = 0
\end{eqnarray}\tag{2}

式(1)と式(2)を用いることにより, $\boldsymbol{\dot{P}}^{err}$の成分は式(3)-(5)のように計算される.

\begin{eqnarray}

\dot{x}^{err}&=&(\dot {x}^{cmd}-\dot {x}^{res})\cos \theta^{res}+(\dot {y}^{cmd}-\dot {y}^{res})\sin \theta^{res} \\
&\ &-(x^{cmd}-x^{res})\dot{\theta}^{res}\sin\theta^{res}+(y^{cmd}-y^{res})\dot{\theta}^{res}\cos\theta^{res}\\
&=&y^{err}\omega^{res}-v^{res}+v^{cmd}\cos\theta^{err} \tag{3}\\

\dot{y}^{err}&=&-(\dot {x}^{cmd}-\dot {x}^{res})\sin \theta^{res}+(\dot {y}^{cmd}-\dot {y}^{res})\cos \theta^{res} \nonumber \\
&\ &-(x^{cmd}-x^{res})\dot{\theta}^{res}\cos\theta^{res}-(y^{cmd}-y^{res})\dot{\theta}^{res}\sin\theta^{res}\nonumber\\
&=&-x^{err}\omega^{res}+v^{cmd}\sin\theta^{err}\tag{4}\\

\dot{\theta}^{err}&=&\dot{\theta}^{cmd}-\dot{\theta}^{res}=\omega^{cmd}-\omega^{res}\tag{5}
\end{eqnarray}

以上をまとめると, 位置誤差は式(6)になる.

\begin{eqnarray}

\dot{\boldsymbol{P}}^{err} &=&
\left[
\begin{array}{ccc}
\dot x^{err}&\dot y^{err}&\dot \theta^{err}
\end{array}
\right]^T \nonumber \\
&=&
\left[
\begin{array}{ccc}
y^{err}\omega^{res}-v^{res}+v^{cmd}\cos\theta^{err} \\
-x^{err}\omega^{res}+v^{cmd}\sin\theta^{err}\\
\omega^{cmd}-\omega^{res}
\end{array}
\right]
\end{eqnarray}\tag{6}

ここで, $v^{cmd}$と$\omega^{cmd}$は指令軌道$\boldsymbol{P}^{cmd}=(x^{cmd}, y^{cmd}, \theta^{cmd})^T$の並進速度, 旋回速度である.

ここで, 式(7)の速度指令値を生成する.

\begin{eqnarray}

\left[
\begin{array}{c}
v^{cmd}_{t} \\
\omega^{cmd}_{t}
\end{array}
\right]
&=&
\left[
\begin{array}{c}
v^{cmd}\cos \theta^{err}+K_xx^{err}\\
\omega^{cmd}+v^{cmd}(K_yy^{err}+K_\theta\sin\theta^{err})\\
\end{array}
\right]
\end{eqnarray}\tag{7}

ただし, $K_x, K_y, K_\theta$は正の定数である.


式(7)の速度指令値を用いることで, $\boldsymbol{{P}}^{err}=0$となり, 軌道追従が可能となる.

詳細は次回話したいと思います.(リアプノフ関数を用いて, 漸近安定を示す流れです.)

*次回はこちら

以下は$sin$波を指令値として与えたシミュレーションです.

簡易なプログラム(2019/02/02訂正)なので見れば内容も分かると思いますが, Python初心者なので更に良い書き方あれば教えてください.

*Gifの生成の仕方がわかれば,,,教えてください.

*初投稿なので修正点あればご教授お願い致します.(コメントありがとうございます!まだまだ募集中です!)

kana1.jpg

* 訂正後(定義の見直し、yield、saveの利用 *コメント参照)2019/02/02


MR_kana.py

#------------------------------------------------

# Trajectory Tracking Python
#
# Author:MeRT
#
#------------------------------------------------
import math

def trajectory(r, x_res=0.0, y_res=0.0, th_res=0.0, dT=0.01, K_x=10.0, K_y=25.0, K_th=20.0):
for t in r:
time = dT * t

# trajectory-command
t_sin = math.sin(0.1 * time)
t_cos = math.cos(0.1 * time)
x_cmd = 0.1 * time
y_cmd = 0.1 * t_sin
th_cmd = math.atan(0.1 * t_cos)
v_cmd = 0.1 / math.cos(math.atan(0.1 * t_cos))
w_cmd = (-0.01 * t_sin) / (1 + 0.01 * t_cos * t_cos)

# error-value
x_err = (x_cmd - x_res) * math.cos(th_res) + (y_cmd - y_res) * math.sin(th_res)
y_err = -(x_cmd - x_res) * math.sin(th_res) + (y_cmd - y_res) * math.cos(th_res)
th_err = th_cmd - th_res

# control
v_tra = v_cmd * math.cos(th_err) + K_x * x_err
w_tra = w_cmd + v_cmd * (K_y * y_err + K_th * math.sin(th_err))

# position-vel
dx_res = v_tra * math.cos(th_res)
dy_res = v_tra * math.sin(th_res)
dth_res = w_tra

# position
x_res += dx_res * dT
y_res += dy_res * dT
th_res += dth_res * dT

# result
yield time, x_cmd, y_cmd, th_cmd, x_err, y_err, th_err, v_tra, w_tra, x_res, y_res, th_res

def save(filename, data):
with open(filename, "w") as f:
for values in data:
print(*(f"{value:.7f}" for value in values), sep=", ", file=f)

save("MR_tra.dat", trajectory(range(5000)))



  • 訂正前(Cのノリで書いたもの)


MR_kana.py

#------------------------------------------------

#Trajectory Tracking Python
#
#Author:MeRT
#
#------------------------------------------------
import math
Time=0.0
#res
x_res=0.0
y_res=0.0
th_res=0.0

#commnd
x_cmd=0.0
y_cmd=0.0
th_cmd=0.0
v_cmd=0.0
w_cmd=0.0

#err
x_err=0.0
y_err=0.0
th_err=0.0

#tracking
v_tra=0.0
w_tra=0.0

#define
dT = 0.01
K_x = 10.0
K_y = 25.0
K_th = 20.0

#
with open("MR_tra.dat", "a") as f:
for t in range(5000):
#trajectory-command
x_cmd=0.1*dT*t
y_cmd=0.1*math.sin(0.1*dT*t)
th_cmd=math.atan(0.1*math.cos(0.1*dT*t))
v_cmd=0.1/math.cos(math.atan(0.1*math.cos(0.1*dT*t)))
w_cmd=(-0.01*math.sin(0.1*dT*t))/(1+0.01*math.cos(0.1*dT*t)*math.cos(0.1*dT*t))

#error-value
x_err=(x_cmd-x_res)*math.cos(th_res)+(y_cmd-y_res)*math.sin(th_res)
y_err=-(x_cmd-x_res)*math.sin(th_res)+(y_cmd-y_res)*math.cos(th_res)
th_err=th_cmd-th_res

#control
v_tra=v_cmd*math.cos(th_err)+K_x*x_err
w_tra=w_cmd+v_cmd*(K_y*y_err+K_th*math.sin(th_err))

#position-vel
dx=v_tra*math.cos(th_res)
dy=v_tra*math.sin(th_res)
dth=w_tra

#position
x_res+=dx*dT
y_res+=dy*dT
th_res+=dth*dT
#Time
Time=t*dT

result = "{:.7f}, {:.7f}, {:.7f}, {:.7f}, {:.7f}, {:.7f}, {:.7f}, {:.7f}, {:.7f}\n".format(Time, x_cmd, y_cmd, th_cmd, x_res, y_res, th_res, v_tra, w_tra)
f.write(result)