自動運転車

カルマンフィルタでロボットの位置推定をしてみよう!

ロボットや自動運転車を制御するときに、ロボットや車の正確な位置を知ることはとても重要です

 

しかし、ロボットを制御するための信号や外部の情報を得るセンサの信号にはノイズが含まれているため、正確な現在位置を得るための情報を得ることが難しくなります。

 

そこで、不正確な情報の中でもロボットの位置を正確に推定するために、カルマンフィルタ(Kalman Filter)という手法を用います。

 

今回からのシリーズでは、このカルマンフィルタを用いたロボットの位置推定の方法を紹介していきたいと思います。

 

カルマンフィルタを用いた位置推定

今回のシリーズで取り扱うカルマンフィルタによるロボットの位置推定問題を説明します。

 

位置推定の必要性

 

ロボットは図のように黒線で表される目標軌跡に沿って、点[0,0]から点[10,5]に向かって直進するように指示を受けています。

 

しかし、入力信号に対するノイズの影響により、実際のロボットの移動軌跡は青線のように目標軌跡(黒線)からズレが生じます。

 

そこで、センサ(GPSなど)を用いてロボットの実際の位置を測定することでロボットの現在位置情報を修正します。

 

このセンサでの観測値が正確であれば、ロボットの現在位置を正確に把握することが出来ます。

 

カルマンフィルタによる位置推定

ただ、残念ながらセンサの信号自体にも雑音(ノイズ、誤差)が含まれているため、完全に正確なロボットの位置を得ることはできません。

 

この様に、ロボットへの入力信号やセンサでの測定データにノイズが入っている場合について、カルマンフィルタを用いて雑音を加味した上でのロボットの自己位置推定を行います。

 

 

今回のシリーズでは、上の図で表されるように目標軌跡(黒線)から実際の軌跡(青線)がずれた場合についても、赤線のようにロボットの現在位置が推定できるようカルマンフィルタを用いた自己位置推定の方法を紹介していきます。

 

取り扱う線形動的システム

今回のシリーズでは、カルマンフィルタを用いて位置推定する対象として線形動的システムを取り扱います。

 

状態空間モデル

まず、取り扱う線形動的システムを状態空間モデルで表します。

 

時刻tでのロボットの状態ベクトルをxt、入力ベクトルをutとします。

 

ここで、状態ベクトルxtには各軸(X軸とY軸)の位置情報

$$ \boldsymbol{x} = \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} $$

が含まれており、入力ベクトルutには各軸についての移動距離

$$ \boldsymbol{u} = \begin{bmatrix} u_1 \\ u_2 \end{bmatrix} $$

が含まれています。

 

また、時刻tでのロボットの観測ベクトルをztと表します。

 

この観測ベクトルztは、センサを用いて得たロボットの位置情報

$$ \boldsymbol{z} = \begin{bmatrix} z_1 \\ z_2 \end{bmatrix} $$

で構成されています。

 

このシステムの状態空間モデルは

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{x}_t = \boldsymbol{F}_t \boldsymbol{x}_{t-1} + \boldsymbol{B}_t \boldsymbol{u}_t \\ \boldsymbol{z}_t = \boldsymbol{H}_t \boldsymbol{x}_{t} \end{array} \right. \end{eqnarray} $$

と状態方程式と出力方程式を用いて表すことが出来ます。

 

雑音を含んだ状態空間モデル

ただ、実際にはロボットの入力信号や観測値には雑音(誤差)が生じるため、

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{x}_t = \boldsymbol{F}_t \boldsymbol{x}_{t-1} + \boldsymbol{B}_t \boldsymbol{u}_t + \boldsymbol{w}_t \\ \boldsymbol{z}_t = \boldsymbol{H}_t \boldsymbol{x}_{t} + \boldsymbol{v}_t \end{array} \right. \end{eqnarray} $$

と雑音としてwvを用いた状態空間モデルで表します。

 

ここで、入力信号に対する雑音wtは共分散行列Qtという入力に対する不確かさを表した行列を用いて

$$ \boldsymbol{w}_t \sim N \left( 0,\boldsymbol{Q}_t \right) $$

と求められます。

 

また、センサに対する雑音vtは共分散行列Rtという観測に対する不確かさを示した行列を用いて

$$ \boldsymbol{v}_t \sim N \left( 0,\boldsymbol{R}_t \right) $$

と求めることが出来ます。

 

まとめ

今回は、このシリーズで取り扱うカルマンフィルタを用いた自己位置推定の問題と線形動的システムについて説明しました。

 

次回は、この線形動的システムの自己位置推定問題を解くために、カルマンフィルタの基本的な式を紹介していきたいと思います。

カルマンフィルタの基本式:カルマンフィルタでロボットの位置推定をしてみよう(2) 今回のシリーズでは、ロボットや自動運転車を制御する際にとても重要となる、ロボットや車の正確な位置を知る方法として、カルマンフィル...

COMMENT

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です