Probabilistic Robotics - Chap.5 Robot Motion

5.1 Introduction

　モーションモデルはベイズフィルタの予測ステップにおいて肝となる状態遷移確率を構成する。本章では、ロボットの運動はある平面上での移動のみを取り上げる。

5.2 Preliminaries

5.2.1 Kinematic Configuration

　ロボットの姿勢は下記の式で表せるものとする。
${ \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} }$
ロボットの向きはよく、 ${bearing}$ や ${heading direction}$ と呼ばれる。また姿勢から向きを取ったものは ${位置}$ と呼ぶ。

5.2.2 Probabilistic Kinematics

　状態遷移確率は ${p(x_t | u_t, x_{t-1}}$ として表される。 ${x_t, x_{t-1}}$ はそれぞれ、時刻 ${t, t-1}$ でのロボット状態を、 ${u_t}$ は時刻 ${t}$ でのモーションコマンドを、それぞれ表すものとする。モーションモデルにおいて、 ${u_t}$ はオドメトリを指す場合もある。
　本章では、下記２種類の確率的モーションモデルを取り上げる。

速度モーションモデル

　 ${u_t = Velocity}$ なモデル。ただし速度を指令値通り制御することは、ノイズ等の都合上難しいことため、精度は良くないことが多い。

オドメトリモーションモデル

　 ${u_t = odometry}$ なモデル。速度モデルよりも精度が高いことが多い。
ただしオドメトリは動いた結果わかる情報である。そのため、動作の予測には使えない。そのため、速度モデルはモーションプランニングに、オドメトリモデルは状態推定に、それぞれ使うことが一般的である。以降ではそれぞれについて詳細を述べる。

5.3 Velocity Motion Model

　本モデルは、ロボットの速度 ${u_t}$ は並行運動 ${v_t}$ と回転運動 ${\omega_t}$ の２種類で構成されると過程する。なお ${v_t}$ と ${\omega_t}$ は、それぞれ１次元のスカラ値である。

5.3.1 Closed Form Calculation

　速度モデルは、下記のアルゴリズムで表現される。
${ 1: \ \ Algorithm motion\_model\_velocity(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \mu = \frac{1}{2} \frac{(x - x') cos\theta + (y - y') sin \theta}{(y - y') cos\theta - (x - x') sin \theta}\\ 3: \ \ \ \ \ x^{*} = \frac{x + x'}{2} + \mu (y - y') \\ 4: \ \ \ \ \ y^{*} = \frac{y + y'}{2} + \mu (x' - x) \\ 5: \ \ \ \ \ r^{*} = \sqrt{(x - x^{*})^2 + (y - y^{*})^2} \\ 6: \ \ \ \ \ \Delta \theta = atan2(y' - y^{*}, x' - x^{*}) - atan2(y - y^{*}, x - x^{*}) \\ 7: \ \ \ \ \ \hat{v} = \frac{\Delta \theta}{\Delta t} r^{*} \\ 8: \ \ \ \ \ \hat{\omega} = \frac{\Delta \theta}{\Delta t} \\ 9: \ \ \ \ \ \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega} \\ 10: \ \ \ \ return \ prob(v - \hat{v}, \alpha_1 v^2 + \alpha_2 \omega^2) \cdot prob(\omega - \hat{\omega}, \alpha_3 v^2 + \alpha_4 \omega^2) \cdot prob(\hat{\gamma}, \alpha_5 v^2 + \alpha_6 \omega^2) }$

ここで ${\alpha_1}$ から ${\alpha_6}$ は、ロボット固有の誤差パラメータでる。また ${prob(x, b^2)}$ は、モーションエラーをモデル化した関数を表す。この固有パラメータの決定方法について、特に本書には記載がないです。恐らく指令値と実測値の組み合わせを記録していき、その結果が良い感じに ${prob(x, b^2)}$ の結果とフィットするようにパラメータチューンが必要と思われます。

5.3.2 Sampling Algorithm

　5.3.1では計算で求めていましたが、本節ではパーティクルフィルタの様にサンプリングを介して解析に状態遷移確率を求める手法を紹介します。サンプル点から求める際の下記の手法を使う。
${ 1: \ \ Algorithm sample\_motion\_model\_velocity(u_t, x_{t-1}):\\ 2: \ \ \ \ \ \hat{v} = v + \boldsymbol{sample}(\alpha_1 v^2 + \alpha_2 \omega^2) \\ 3: \ \ \ \ \ \hat{\omega} = \omega + \boldsymbol{sample}(\alpha_3 v^2 + \alpha_4 \omega^2) \\ 4: \ \ \ \ \ \hat{\gamma} = \boldsymbol{sample}(\alpha_5 v^2 + \alpha_6 \omega^2) \\ 5: \ \ \ \ \ x^{'} = x - \frac{\hat{v}}{\omega} sin \theta + \frac{\hat{v}}{\omega} sin (\theta + \hat{\omega} \Delta t \\ 6: \ \ \ \ \ y^{'} = y + \frac{\hat{v}}{\omega} cos \theta - \frac{\hat{v}}{\omega} cos (\theta + \hat{\omega} \Delta t \\ 7: \ \ \ \ \ \theta^{'} = \theta + \hat{\omega} \Delta t + \hat{\gamma} \Delta t \\ 8: \ \ \ \ \ return \ x_t = (x^{'}, y^{'}, \theta^{'})^T\\ }$
２～４行目は、速度パラメータにノイズを加えます。その結果を使い、５～７行目で姿勢を計算しています。サンプリング手法を使う場合、運動方程式等の物理モデルを考慮せず、サンプル点の位置からのみ姿勢が求まるという利点がある。そのため実装の観点からすると、サンプリング手法を利用ことが楽である。

5.4 Odometry Motion Model

　オドメトリは、ドリフトやスリップによってエラーは生じるものの、速度に比べて計測誤差が小さい。また実際に動いた後でないと計測できないため、モーションプランニングには使えない。

5.4.1 Closed Form Calculation

　オドメトリをベースとしたモーションモデルは、今日の確率ロボティクスベースのシステムで中心的な役割を果たしている。
オドメトリは、ロボット内部の情報量である。そのため、実世界とリンクするためには座標変換が必要である。ただしドリフトやスリップが生じるため、変換行列は一定ではない。なおこの変換行列を求めることは、自己位置推定を行うことと等価である。
　オドメトリモデルは相対動作情報を使用する。相対動作情報とは、時刻 ${t-1}$ から ${t}$ の間で、どの程度動いたかを表す情報である。ロボットの時刻 ${t-1}$ と ${t}$ の時の姿勢のペアを ${u_t}$ とすると、
${ u_t = \begin{pmatrix} \overline{x}_{t-1} \\ \overline{x}_t \end{pmatrix} }$
となる。
　この動作情報から相対動作情報を取り出すと、回転・直進・回転の３ステップに分割できる。つまり ${( \begin{pmatrix} \delta_{rot1} \delta_{trans} \delta_{rot2} \end{pmatrix} }$
と表せる。この情報が分かれば、ある時刻 ${t-1}$ から ${t}$ の間の移動を再現できる。
確率的モーションモデルでは、上記３パラメータはノイズが加わっているものと仮定する。

以上を踏まえた、オドメトリモデルでの状態遷移確率を求めるアルゴリズムは下記の通りである。
${ 1: \ \ \ Algorithm \ motion\_model\_odometry(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \delta_{rot1} = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\ 3: \ \ \ \ \ \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\ 4: \ \ \ \ \ \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\ 5: \ \ \ \ \ \hat{\delta}_{rot1} = atan2(y^{'} - y, x^{'} - x) - {\theta} \\ 6: \ \ \ \ \ \hat{\delta}_{trans} = \sqrt{(x^{'} - x)^2 + (y^{'} - y)^2} \\ 7: \ \ \ \ \ \hat{\delta}_{rot2} = \theta^{'} - \theta - \delta_{rot1} \\ 8: \ \ \ \ \ p_1 = \boldsymbol{prob}(\delta_{rot1} - \hat{\delta}_{rot1}, \alpha_1 \hat{\delta}^2_{rot1} + \alpha_2 \hat{\delta}^2_{trans}) \\ 9: \ \ \ \ \ p_2 = \boldsymbol{prob}(\delta_{trans} - \hat{\delta}_{trans}, \alpha_3 \hat{\delta}^2_{trans} + \alpha_4 \hat{\delta}^2_{rot1} + + \alpha_4 \hat{\delta}^2_{rot2}) \\ 10: \ \ \ \ \ p_3 = \boldsymbol{prob}(\delta_{rot2} - \hat{\delta}_{rot2}, \alpha_1 \hat{\delta}^2_{rot2} + \alpha_2 \hat{\delta}^2_{trans}) \\ 11: \ \ \ \ \ return \ p_1 \cdot p_2 \cdot p_3 }$
なお ${\boldsymbol{prob}(a, b^2)}$ は、平均 ${0}$ で分散 ${b^2}$ の誤差分布における ${a}$ の発生確率を返す関数とする。また、 ${\alpha_1}$ から ${\alpha_4}$ は、ロボットの動作を特定するロボット固有のパラメータである。

上記アルゴリズムにおいて、各行が行っている内容は下記の通り

２～４行目：オドメトリ情報から相対動作情報 ${\begin{pmatrix} \delta_{rot1} \delta_{trans} \delta_{rot2} \end{pmatrix} }$ を抽出。
５～７行目：入力した姿勢 ${x_{t-1}}$ と ${x_t}$ から理想とする相対動作情報を算出。
８～１０行目：相対動作情報の各要素に対するエラーを確率的に計算
１１行目：相対動作情報の各要素に対するエラー発生確率をかけ合わせ、目的姿勢である ${x_t}$ からのずれを返す。

5.4.2 Sampling Algorithm

　サンプルベースでオドメトリモデルを求めるアルゴリズムは下記の通りである。 ${\boldsymbol{sample}(a,b^2}$ は、 ${a}$ の発生確率を返す、平均 ${0}$ で分散 ${b^2}$ な確率分布関数である。

${ 1: \ \ \ Algorithm \ motion\_model\_odometry(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \delta_{rot1} = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\ 3: \ \ \ \ \ \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\ 4: \ \ \ \ \ \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\ 5: \ \ \ \ \ \hat{\delta}_{rot1} = \delta_{rot1} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot1} + \alpha_2 \delta^2_{trans}) \\ 6: \ \ \ \ \ \hat{\delta}_{trans} = \delta_{trans} - \boldsymbol{sample}(\alpha_3 \delta^2_{trans} + \alpha_4 \delta^2_{rot1} + \alpha_4 \delta^2_{rot2}) \\ 7: \ \ \ \ \ \hat{\delta}_{rot2} = \delta_{rot2} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot2} + \alpha_2 \delta^2_{trans}) \\ 8: \ \ \ \ \ x^{'} = x + \hat{\delta}_{trans} cos(\theta + \hat{delta}_{rot1}) \\ 9: \ \ \ \ \ y^{'} = y + \hat{\delta}_{trans} sin(\theta + \hat{delta}_{rot1}) \\ 10: \ \ \ \ \ \theta^{'} = \theta + \hat{\delta}_{rot1} + \hat{\delta}_{rot2} \\ 11: \ \ \ \ \ return \ x_t = (x^{'}, y^{'}, \theta^{'})^T }$

5.5 Motion And Maps

　今まで扱っていた状態遷移確率では、周囲環境を考慮していなかった。そこで状態遷移確率のパラメータとして、新たに地図 ${m}$ を導入する。地図を知ることで、壁などの到達不可能な場所にいるのか、そうでない到達可能な場所にいるのかが判別できる。これにより、ロボットの姿勢をより精度良く求める事が可能となる。
　地図の導入により、状態遷移確率は ${p(x_t | u_t, x_{t-1}, m)}$ となる。地図を知ることによって、今まで議論してきた状態遷移確率では「遷移可能」としてきた場所に遷移出来なくなるので、
${ p(x_t | u_t, x_{t-1}) \neq p(x_t | u_t, x_{t-1}, m) }$
となる。よって新規なモデルとなることから、マップベースモーションモデルと呼ぶことにする。
　マープベースモデルを閉形式で計算することは難しい。一方で、都合の良いことに計算効率のよい近似方法がある。近似式は下記の通り。
${ p(x_t | u_t, x_{t-1}, m) = \eta \frac{p(x_t | u_t, x_{t-1}) p(x_t | m)}{p(x_t)} }$
ここで ${\eta}$ は正規化係数である。通常 ${p(x_t)}$ は一様分布で表される。なお周囲環境は動的に変化しないことから、 ${m}$ は定数として考える。もしも ${p(x_t | m) = 0}$ の時は、ロボットは地図上に存在する障害物上に存在することを意味する。
　なお上記近似には問題がある。モデルでは地図を考慮しているため、ロボットが存在できる姿勢は全て遷移対象となる。しかし、例えば壁の向こう側に一瞬で行くのは難しいが、そうした物理的に遷移が可能かは考慮していない。そのため、遷移先の姿勢への移動コスト加味するなどの工夫が必要である。