今回のシリーズでは、ロボットや自動運転車を制御する際にとても重要となる、ロボットや車の正確な位置を知る方法として、カルマンフィルタを用いたロボットの位置推定の方法を紹介しています。自己位置推定ロボットの自己位置推定(Localization)技術についての情報を紹介します。15:33前回の記事では、本シリーズで取り扱うカルマンフィルタを用いた自己位置推定の問題と線形動的システムについて説明しました。カルマンフィルタでロボットの位置推定をしてみよう!ロボットや自動運転車を制御するときに、ロボットや車の正確な位置を知ることはとても重要です。20:33今回は、この雑音が含まれている線形動的システムの自己位置推定問題を解くために、カルマンフィルタの基本的な式を紹介していきたいと思います。シリーズでは、上の図で表されるように黒線の目標軌跡から青線のように実際の軌跡がずれた場合について、カルマンフィルタを用いて赤線のようにロボットの位置を推定する問題を考えています。$$のように、入力や観測に雑音(wやv)が含まれているモデルを考えます。このように、状態方程式に雑音wが含まれていることから目標軌跡に対して実際の軌跡にズレが生じます。出力方程式にも雑音vが含まれているため、正確なロボットの位置を得ることもできません。このようなロボットの自己位置を推定する問題についてカルマンフィルタを用いた解法を考えていきます。カルマンフィルタの基本式カルマンフィルタを適用した位置推定を行うために、カルマンフィルタの基本的な式を紹介していきます。カルマンフィルタを行う際の定義として、時刻tでのロボットの推定状態を\(\hat{\boldsymbol{x}}_{t|t}\)と置きます。時刻tでの予測の不確かさを示す誤差共分散を\(\hat{\boldsymbol{P}}_{t|t}\)で表す事とします。カルマンフィルタには大きく予測ステップ更新ステップの2つのステップがあります。この2つのステップについて、基本式とその役割を紹介していきます。予測ステップ予測ステップは、1つ前の状態を用いて現在の状態を予測するステップです。時刻t-1での推定状態\(\hat{\boldsymbol{x}}_{t-1|t-1}\)と誤差共分散\(\hat{\boldsymbol{P}}_{t-1|t-1}\)を用いて、時刻tでの推定状態と誤差共分散を予測していきます。時刻t-1の推定状態\(\hat{\boldsymbol{x}}_{t-1|t-1}\)から予測した時刻tでのロボットの推定状態を\(\hat{\boldsymbol{x}}_{t|t-1}\)とします。$$となり、時刻t-1でのロボットの推定状態\(\hat{\boldsymbol{x}}_{t-1|t-1}\)から予測することが出来ます。上の式からも分かるように、この時は先の線形動的システムの状態方程式で考慮していた雑音wtによる影響を含んでいません。これは、未来に起こる雑音による誤差を実際に求めることは不可能なので、推定状態を予測する際はこの誤差を含めずに算出しています。この雑音wtを除くことによる影響は、誤差共分散\(\hat{\boldsymbol{P}}\)を求める際に考慮します。時刻tでの誤差共分散\(\hat{\boldsymbol{P}}_{t|t}\)を時刻t-1の状態から予測した誤差共分散\(\hat{\boldsymbol{P}}_{t|t-1}\)を求めていきます。$$のように、時刻t-1での誤差共分散\(\hat{\boldsymbol{P}}_{t-1|t-1}\)を用いて予測をしていきます。ここで、この誤差共分散\(\hat{\boldsymbol{P}}_{t|t-1}\)は、推定状態の不確かさを表しています。推定状態を予測する際に雑音による影響を無視していることから、推定状態の不確かさが増大し、この誤差共分散も雑音を示す共分散行列Qtの分だけ大きくなります。$$がカルマンフィルタの予測ステップにおける基本式です。更新ステップ更新ステップでは、予測した状態と観測の情報を用いて現在の状態を更新するステップです。時刻\(t-1\)での推定状態\(\hat{\boldsymbol{x}}_{t-1|t-1}\)と誤差共分散\(\hat{\boldsymbol{P}}_{t-1|t-1}\)から予測した推定状態\(\hat{\boldsymbol{x}}_{t|t-1}\)と誤差共分散\(\hat{\boldsymbol{P}}_{t|t-1}\)に加えて、観測による情報\(\boldsymbol{z}_t\)用いて、時刻\(t\)での推定状態\(\hat{\boldsymbol{x}}_{t|t}\)と誤差共分散\(\hat{\boldsymbol{P}}_{t|t}\)を更新していきます。$$を用いてロボットが予測した推定状態\(\hat{\boldsymbol{x}}_{t|t-1}\)に存在したとしたら得られるであろう観測情報との差を表しています。このとき、従来の出力方程式に含まれている雑音vによる影響は考慮せずに観測残差\(\tilde{\boldsymbol{y}}_t\)を求めています。観測残差\(\tilde{\boldsymbol{y}}_t\)を求める際に用いた出力方程式から取り除いた雑音vによる影響として観測残差の共分散Stを求めていきます。$$と算出することが出来ます。$$と算出することが出来ます。このカルマンゲインKtを用いて、時刻tでの推定状態\(\hat{\boldsymbol{x}}_{t|t}\)と誤差共分散\(\hat{\boldsymbol{P}}_{t|t}\)を求めていきます。算出したカルマンゲインKtを用いて、予測ステップで求めた推定状態\(\hat{\boldsymbol{x}}_{t|t-1}\)を更新します。$$のように、カルマンゲインKtと観測残差\(\tilde{\boldsymbol{y}}_t\)を用いて、予測ステップで求めた推定状態\(\hat{\boldsymbol{x}}_{t|t-1}\)を更新することで算出します。同様に、予測ステップで求めた誤差共分散\(\hat{\boldsymbol{P}}_{t|t-1}\)をカルマンゲインKtを用いて更新します。$$とカルマンゲインKtを用いて、予測ステップで求めた誤差共分散\(\hat{\boldsymbol{P}}_{t|t-1}\)を更新することで算出します。$$がカルマンフィルタの更新ステップにおける基本式になります。まとめ今回は、入力や出力に雑音が含まれている線形動的システムについて、ロボットの自己位置推定を行うためにカルマンフィルタの基本的な式を紹介しました。次回は、このカルマンフィルタの基本式を用いて実際にロボットの位置推定を行いたいと思います。19:27合わせて読みたいカルマンフィルタを使った自己位置推定についてのまとめ記事はこちら自己位置推定ロボットの自己位置推定(Localization)技術についての情報を紹介します。制御してみよう:簡単な制御モデルを求める実際に車やロボットの自動運転を行うために、車の現在位置と目標軌跡との誤差からハンドルの角度を算出し、車を目標軌跡に近づける方法を紹介します。記事を読む拡張カルマンフィルタとヤコビ行列:カルマンフィルタでロボットの位置推定をしてみよう(7)ロボットや自動運転車などの非線形動的システムを制御する際に重要となる、ロボットや車の正確な位置を得る方法として拡張カルマンフィルタを用いるために、非線形関数からヤコビ行列を算出する方法について紹介します。記事を読む拡張カルマンフィルタのシミュレーション結果:カルマンフィルタでロボットの位置推定をしてみよう(8)自動運転車や移動ロボットの非線形動的システムを制御するために重要となる現在位置を推定するために、拡張カルマンフィルタを用いた自己位置推定の方法を紹介します。実際にアルゴリズムを実装し、シミュレーションを用いて動作の確認をした結果も紹介します。記事を読む自動運転車を制御してみよう:実際に簡単な制御をしてみる車やロボットの自動運転を実現するために、車と目標軌跡との誤差から自動運転車を実際に制御する方法を紹介します。車やロボットの自動運転を実際に制御するために、車と目標軌跡との誤差からハンドル角度を算出して、自動運転車を実現するMATLABプログラムを紹介します。記事を読むカルマンフィルタでロボットの位置推定をしてみよう!不正確な情報の中でもロボットの位置を正確に推定するために、カルマンフィルタを用いたロボットの自己位置推定の方法を紹介します。記事を読むカルマンフィルタと線形動的システム:カルマンフィルタでロボットの位置推定をしてみよう(3)カルマンフィルタを用いたロボットの自己位置推定を行うために、取り扱う線形な動的システムについて説明します。記事を読む拡張カルマンフィルタって何?:カルマンフィルタでロボットの位置推定をしてみよう(5)自動運転車や移動ロボットなどの非線形動的システムの自己位置推定を行うために、拡張カルマンフィルタについて紹介します。記事を読むカルマンフィルタでロボットの自己位置推定をするプログラムの紹介ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法である、カルマンフィルタを用いたロボットの位置推定を行うためのMATLABプログラムを紹介します。記事を読むシミュレーションで動作確認:カルマンフィルタでロボットの位置推定をしてみよう(4)自動運転車や移動ロボットの現在位置を推定するために、カルマンフィルタを用いた自己位置推定の方法を紹介します。実際にアルゴリズムを実装し、シミュレーションを用いて動作の確認も行います。コメントをキャンセルメールアドレスが公開されることはありません。