Path Planningは、各点でのコスト関数の和がなるべく小さくなるように、かつ、終端の点がなるべく目標地点に近くなるように、経路を決める問題である。また、移動体の物理上の構造を考慮するという制約も付く(e.g.急激に方向転換をすることは不可能)。これを制御工学の言葉で言うと、
- 各点$x_i$でのコスト関数$\to$ステージコスト関数$L(x_i)$
- ホライズンの終端の点が目標点に近いかどうか$\to$終端コスト関数$\phi(x_f)$
- プラントの物理的構造を考慮する。
なので、ここでは、Path Planningを制御工学の最適制御問題として解いてみる。
プラント
Path Planningのアルゴリズムをいくつか見てみると、移動物体の速度が考慮されておらず、経路を決める際に打たれる点はそれぞれ等距離ずつ離れている。
ということで、プラントから速度と時間の概念を除き(あるいは、プラントの速度は一定であると仮定する)、 時間$t$ではなくて経路長$s$による状態方程式 を作る。
まずは、状態$x$を以下のように定義する。
x(s)=[x_1(s) ~ x_2(s) ~ \theta(s)]^\mathrm T
$x_1,~ x_2$はプラントの位置座標、$\theta$はプラントが今向いている角度である。
次に、状態方程式を示す。通常の、時間による状態方程式とは以下のような式だが、
\frac{d}{dt}x(t)=f(x(t),u(t))
今回は以下のような式とする。ただし、$s$は経路長である。
\frac{d}{ds}x(s)=f(x(s),u(s))
つまり、$f(x,u)$は、プラントが単位経路進むと状態$x$がどのくらい変わるのかを表した関数となり、具体的には
f(x(s),u(s))=[\cos(\theta(s)) ~ \sin(\theta(s)) ~ u(s)]^\mathrm{T}
とする。なお、$u(s)$は入力であり、プラントの向きを変える角速度に対応する。
という訳で、時間の次元が存在しない状態空間を仮定した。
問題設定
上の図のような環境を考える。
プラントの初期状態を$x_0=[0 ~ 0 ~ 0]^\mathrm T$(黄の+点)とし、目標状態を$x^{goal}=[15 ~ 0 ~ 0]^\mathrm T$(紫の*点)とする。
$x_0$から$x_f$の間にはいくつかの障害物(黒い円2つ)があり、これを避けながら$x_0\to x_f$と移動するための最適な入力$u(s)$を求める。
なお、離散時間系で解くこととし、時間刻みならぬ 距離刻み を$\Delta s=0.2$、ホライズンを$h=100$とした。
コスト関数
コスト関数には、ステージコスト関数$L(x(s))$と終端コスト関数$\phi(x(s_f))$の2種類がある。
ステージコスト関数
状態$(x_1,~ x_2,~ \theta)$がもしも障害物に入っていたら、ステージコスト関数$L(x)$を課すこととする。
その障害物の中心座標を$(x_1^o,~ x_2^o)$とすると、ステージコスト関数$L(x)$に対する状態$x$の微分を
\frac{dL(x)}{dx}=[G\frac{x_1^o-x_1}{d} ~ G\frac{x_2^o-x_2}{d} ~ 0]^\mathrm T
とする。なお、$d$は状態と障害物の中心の距離$|(x_1^o,~x_2^o)-(x_1,~x_2)|$、$G$は勾配の強さを決める定数$G=3$である。
これは、今の位置から障害物の中心に向かう単位ベクトルに$G$をかけたものである。つまり、コスト関数$L(x)$の形状は、障害物の中心が最も標高が高い円錐である。
なお、2つ以上の障害物が重なっていることはない。
終端コスト関数
終端コスト関数は以下のように定義される。
\phi(x)=\frac{W}{2}(x_1-x_1^{goal})^2+(x_2-x_2^{goal})
ただし、$W$は重みを表す定数であり、$W=0.1$である。また、終端コスト関数の状態に対する微分は
\frac{d\phi(x)}{dx}=[W(x_1-x_1^{goal}) ~ W(x_2-x_2^{goal}) ~ 0]^\mathrm T
である。ここから分かるように、角度はコストの対象としないこととした。つまり、プラントが目標座標$(x_1^{goal},~ x_2^{goal})$に到達すれば、その時の角度$\theta$は何でもOKということ。
実験結果
入力$u(s)$の初期値を$u(s)=0$としたので、最初は以下のようなPath(青線)となった。
障害物にモロ当たってる。。
ここで、$u(s)$を最適化すると・・・
障害物を避けながら、うまく目標地点まで到達できた。
振り返り
- ちょうどホライズンの終端で目標地点に到着するようなPathになっているが、もうちょい近道できると思う。このような少し回り道なPathが作られてしまった原因としては、状態$x$と目標地点$x^{goal}$の距離のコストが終端コスト関数でしか与えられていなかったためであると考えられる。なるべく短い道のりで、かつ、障害物を避けるようなPath Planningをしたいのであれば、状態$x$と目標地点$x^{goal}$の距離のコストをステージコスト関数にも載せるべきだと考えられる。
- 今回は入力$u(s)$に制約条件を付けなかったが、制約条件を付けてやってみても面白く実用的かもしれない。
- これと似たPath Planningの手法としてDWAというものがあるが、DWAとこのアルゴリズムの長短がそれぞれ何なのかを検討した方がいいね。
- このアルゴリズムを、実際にプラントを動かしながら逐次行っていくことによって、モデル予測制御のようなことができるのでは？
勾配法の実装
今回は最適化を勾配法で実施したが、この実装を示す。
... 工事中 ...