クロソイド曲線対応オドメトリ


オドメトリ(odmetry)は,独立二輪型車両ロボットは常に円弧/直進運動しかしないという仮定のもとで,相対的な移動量を求める自己位置推定法であった.

本稿では仮定を少しゆるめた自己位置推定法を提案する.

概要

厳密な表記

以下のようにモデル化される独立二輪型車両ロボットを考える.
 \begin{bmatrix} \dot{x}\\\dot{y}\\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v \cos\theta\\ v \sin\theta\\ \omega \end{bmatrix}
ただし,弧状連結な平面空間M\subset\mathbb{R}^2に対し,x,y,\theta\in M\times S^1,\displaystyleは状態量であり,ロボットの位置,ロボットの姿勢角を意味している.また,v,\omega\in \mathbb{R}は制御入力であり,ロボットの併進速度,ロボットの角速度を意味している.M\displaystyleは複数個の障害物が配置された平面空間とイメージできる.

時間の分割\{t_i|t_1=0,t_\infty = \infty,t_i<t_{i+1},i \in \mathbb{N}に対し,v\displaystyle[t_i,t_{i+1})\displaystyleの時間で一定の値v(t_i)\displaystyleをとるとする.\omega\displaystyleの時間で\omega(t_i)+k(t-t_i),k\in\mathbb{R}\displaystyleをとるとする.k\displaystyleは角加速度と思えばよい.さらに,時刻0におけるロボットの初期位置・姿勢角x(0),y(0),\theta(0)\displaystyleは既知であるとする.

k=0\displaystyleの時はオドメトリそのものであるから,本稿ではk\neq 0\displaystyleとして議論を進める.

区間[t_i,t),(t\in[t_i,t_{i+1}))において\dot{x},\dot{y},\dot{\theta}\displaystyleは連続であるからリーマン積分可能である.t\in [t_i,t_{i+1})\displaystyleに対し,ロボットの位置,姿勢角は以下のように求まる.

(1)    \begin{align*} \begin{bmatrix} x(t)\\y(t)\\ \theta(t) \end{bmatrix} =& \begin{bmatrix} x(0)\\y(0)\\ \theta(0) \end{bmatrix} + \int_{t_i}^{t} \begin{bmatrix} v(t_i) \cos\theta(s)\\ v(t_i) \sin\theta(s)\\ \omega(s) \end{bmatrix} ds\\ &+ \sum_{j=0}^{j=i-1} \int_{t_j}^{t_{j+1}} \begin{bmatrix} v(t_j) \cos\theta(s)\\ v(t_j) \sin\theta(s)\\ \omega(s) \end{bmatrix} ds\\ =& \begin{bmatrix} x(t_i)\\y(t_i)\\ \theta(t_i) \end{bmatrix} + \int_{t_i}^{t} \begin{bmatrix} v(t_i) \cos\theta(s)\\ v(t_i) \sin\theta(s)\\ \omega(s) \end{bmatrix} ds \end{align*}

第2式右辺第2項である積分式を解析的に解くことを考えよう.\theta(t)=\theta(t_i)+\int_{t_i}^{t}\omega(s)ds=\theta(t_i)+(t-t_i)\omega(t_i)+\frac{k}{2}(t-t_i)^2\displaystyleに注意する.\theta(t)\displaystyleに平方完成をかけて,

(2)    \begin{align*} &\theta(t_i)+(t-t_i)\omega(t_i)+\frac{k}{2}(t-t_i)^2\\ &=\left( \sqrt{\frac{k}{2}}\left( t-t_i+\frac{\omega(t_i)}{k}\right) \right)^2-\frac{\omega(t_i)^2-2k\theta(t_i)}{2k}\\ &=:X(t)^2+Y \end{align*}

また,以下の通り,フレネル積分S,C:\mathbb{R}\to \mathbb{R}\displaystyleを定義すると,これらは\sin(x^2),\cos(x^2)\displaystyleの原始関数のひとつとなっている.

(3)    \begin{align*} S(x)&:=\sum_{n=0}^{\infty}\frac{(-1)^nx^{4n+3}}{(2n+1)!(4n+3)}\\ C(x)&:=\sum_{n=0}^{\infty}\frac{(-1)^nx^{4n+1}}{(2n)! (4n+1)} \end{align*}

ゆえに,

(4)    \begin{align*} &\int_{t_i}^{t}v(t_i) \cos\theta(s) ds\\ & =v(t_i)\int_{t_i}^{t} \cos(X(s)^2+Y) ds\\ & =\sqrt{\frac{2}{k}}v(t_i)\left( \cos Y\left[ C\left( \sqrt{\frac{k}{2}}(s-t_i+\frac{\omega(t_i)}{k})\right) \right]_{t_i}^t \\ & - \sin Y\left[ S\left( \sqrt{\frac{k}{2}}(s-t_i+\frac{\omega(t_i)}{k})\right) \right]_{t_i}^t \right) \end{align*}

(5)    \begin{align*} &\int_{t_i}^{t}v(t_i) \sin\theta(s) ds\\ &=v(t_i)\int_{t_i}^{t} \sin(X(s)^2+Y) ds\\ & =\sqrt{\frac{2}{k}}v(t_i)\left( \cos Y\left[ S\left( \sqrt{\frac{k}{2}}(s-t_i+\frac{\omega(t_i)}{k})\right) \right]_{t_i}^t\\ & + \sin Y\left[ C\left( \sqrt{\frac{k}{2}}(s-t_i+\frac{\omega(t_i)}{k})\right) \right]_{t_i}^t \right)\\ &\int_{t_i}^{t}\omega(s)ds=(t-t_i)\omega(t_i)+\frac{k}{2}(t-t_i)^2 \end{align*}

以上のように,フレネル積分C,S\displaystyleの値がわかれば,かろうじて解けることが分かる.

考察

計算にかかるコストは定数オーダーとはいえ,かなり複雑な数式になった.オドメトリと違って,直接\omega,v,k(=\dot{\omega})を陽に求めなければならない.これに対しては,2階の厳密微分器を使うなどして対応すれば良いと思われる.C,S\displaystyleはあらかじめオフラインで計算しておき,テーブルを用意しておけば良い.C,S\displaystyleに周期性があるかどうかすら分からないので,巨大なk\displaystyleの値に対してテーブルが用意しきれるのかという問題は残る.

参考までに,C,S\displaystyleに対し,級数の演算を50次で打ち切るグラフは以下のようになる.
C2s2graph

\lim_{x\to \infty}\sin (x)\displaystyleは存在せず,\pm 1\displaystyleに振動する.ゆえに,C(x),S(x)\displaystyleは,微小な時間で微分値が1と-1を行き来する関数で,かつ,その周期はxが大きくなるにつれて,短くなる関数であることが分かる.なお,フレネル積分は引数を無限大に近付けると,\frac{1}{2}\sqrt{\frac{\pi}{2}}\displaystyleに収束することが知られている.

クロソイド曲線を求める時にもフレネル積分がネックになることとあわせて考えると,計測と制御が表裏一体の問題になっていることが分かり,大変面白い.なお,フレネル積分はガンマ関数を使って,以下のように一般化されることが知られている.

(6)    \begin{align*} S(\infty)=\int_0^\infty\sin(x^2)\ dx \\ \int_0^\infty\sin(x^a)\ dx = \frac{\Gamma\left(\frac{1}{a}\right)\sin(\frac{\pi}{2a})}{a}\end{align*}

どうもガンマ関数が本質に隠れている臭いがする.

実装

厳密微分器がないと無理じゃないかな?

参考文献

外部リンク