今回のシリーズでは、ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法として、カルマンフィルタを用いたロボットの位置推定の方法を紹介しています。カルマンフィルタでロボットの位置推定をしてみよう!ロボットや自動運転車を制御するときに、ロボットや車の正確な位置を知ることはとても重要です。20:33前回の記事では、拡張カルマンフィルタによる自己位置推定を行うために、非線形関数のヤコビ行列を算出する方法を紹介しました。00:04今回の記事では、実際に非線形動的システムの移動ロボットについて拡張カルマンフィルタを用いた自己位置推定をシミュレーション上で行いたいと思います。今回行うシミュレーションの条件について説明します。対向2輪型の移動ロボットは位置[0,0]を始点として、X軸方向から徐々に円を描くように左に曲がる軌跡(黒線)を目標として動作します。入力uに対する雑音w(ノイズ、誤差)の影響により、実際の移動ロボットの軌跡(青線)は目標軌跡(黒線)に対してズレが発生します。そこで、センサを使って地図上に存在するマーカー[10,5]との距離と角度を求めることで、現在の位置を修正します。このセンサの出力信号zにも雑音v(ノイズ、誤差)が含まれているため、正確な距離や角度を得られず、ロボットの正確な位置を知ることができません。この様に、非線形システムで表されるロボットへの入力信号uやセンサでの測定データzにノイズ(wやv)が入っている場合について、拡張カルマンフィルタを用いて雑音(wやv)を加味した上でのロボットの自己位置推定を行います。今回の拡張カルマンフィルタによる非線形システムで与えられた移動ロボットの自己位置推定問題のアルゴリズムは、シミュレーションの初期条件を設定カルマンフィルタの予測ステップカルマンフィルタの更新ステップ以降2と3を繰り返すという流れになります。基本的なアルゴリズムの流れは、カルマンフィルタによる自己位置推定を行った際と同じです。拡張カルマンフィルタによる自己位置推定のシミュレーションを行うために、まず必要となる様々な初期条件を定義します。$$について、それぞれの式を関数(function)として用意します。この様に関数を用意しておくことで、何度も計算することになる状態方程式と出力方程式の取り扱いが容易になります。実際にMATLABで用意した関数は以下の通りです。ロボットの状態ベクトル(位置情報(X軸とY軸)と進行方向)について、目標となる指令の状態をxDes、雑音による影響を受けた実際の状態をxAct、そして拡張カルマンフィルタにより推定された状態をxEstと定義します。移動ロボットの初期位置は[0,0]で進行方向は0°(X軸方向)なので、これら3つの位置情報は[0,0,0]で初期化します。推定値xEstの確からしさを示す誤差共分散Pについて、時刻t=0での推定値[0,0,0]は確かなので、誤差共分散Pを単位行列Iで初期化しますこれらの初期設定をMATLABで記すと以下のようになります。Matrix予測ステップ予測ステップでは、1つ前の時刻t-1で推定したロボットの状態xEstと入力信号utから、時刻tでのロボットの状態xEstの推定(予測)を行います。ロボットの状態の予測は先程作成した関数StateEquation()を用いて算出します。この時、状態方程式の非線形関数に含まれてる入力に対する雑音wの影響は考慮せず、雑音はないものとして予測します。その代わりとして、共分散Qを用いて、推定した状態xEstの確からしさを示す誤差共分散Pを更新します。誤差共分散Pを算出するために、非線形関数fのヤコビ行列Ftを用います。この誤差共分散Pを求める計算は何度も行うため、ヤコビ行列Ftを求める関数も用意しておきます。実際にMATLABで用意したヤコビ行列Ftを求める関数は以下の通りです。$$のようになります。これら予測ステップでの計算をMATLABで記すと以下のようになります。Covariance更新ステップ更新ステップでは、実際の観測値zと予測ステップで推定した状態xEstならば得られるはずの観測値との差を求め、カルマンゲインKを求めます。求めたカルマンゲインKを用いてロボットの推定状態xEstと誤差共分散Pを更新します。移動ロボットが予測ステップで推定した状態xEstに存在すると考えた時に、得られるはずの観測値は出力方程式より求められるため、先に作成した関数OutputEquation()を用いて算出します。先程の予測ステップの場合と同様に、この時に出力方程式に含まれる雑音vによる影響は考慮をしません。その代わり、観測値の確からしさを示す誤差共分散Rを、推定した位置xEstの確からしさを示す誤差共分散Pと共に用いて、カルマンゲインKを算出します。この際に必要となる非線形関数hのヤコビ行列Htを求めるための関数を用意しておきます。実際にMATLABで用意したヤコビ行列Htを求める関数は以下の通りです。-1];endそして、求めたカルマンゲインKと観測値yを基にして、ロボットの推定位置xEstと誤差共分散Pを更新します。更新ステップでの計算をMATLABで記すと以下のようになります。Covarianceシミュレーション結果この予測ステップと更新ステップを順に繰り返すことで、拡張カルマンフィルタを用いた移動ロボットの自己位置推定を実現することが出来ます。今回紹介したアルゴリズムを用いて、実際にシミュレーションを行った結果をX-Y平面のグラフに示します。https://tajimarobotics.com/wp-content/uploads/2018/06/EKF.mp4シミュレーション結果から、目標の軌跡(黒線)から実際のロボットの軌跡(青線)がずれた場合でも、拡張カルマンフィルタを用いた非線形システムの自己位置推定により移動ロボットの現在位置が上手く推定(赤線)できていることが分かります。まとめ今回は、拡張カルマンフィルタを用いて非線形動的システムで表される移動ロボットの自己位置推定のアルゴリズムを実装し、実際にシミュレーションを用いて動作の確認を行いました。今回のシリーズで紹介した方法を用いることで、非線形動的システムで表される移動ロボットの位置を拡張カルマンフィルタを用いて推定することが出来ます。システムの状態空間モデル:カルマンフィルタでロボットの位置推定をしてみよう(6)自動運転車や移動ロボットなどの自己位置推定を拡張カルマンフィルタで行うために、実際に非線形動的システムを状態空間モデルで表す方法を紹介します。記事を読む拡張カルマンフィルタとヤコビ行列:カルマンフィルタでロボットの位置推定をしてみよう(7)ロボットや自動運転車などの非線形動的システムを制御する際に重要となる、ロボットや車の正確な位置を得る方法として拡張カルマンフィルタを用いるために、非線形関数からヤコビ行列を算出する方法について紹介します。記事を読む自動運転車を制御してみよう:実際に簡単な制御をしてみる車やロボットの自動運転を実現するために、車と目標軌跡との誤差から自動運転車を実際に制御する方法を紹介します。車やロボットの自動運転を実際に制御するために、車と目標軌跡との誤差からハンドル角度を算出して、自動運転車を実現するMATLABプログラムを紹介します。記事を読むカルマンフィルタと線形動的システム:カルマンフィルタでロボットの位置推定をしてみよう(3)カルマンフィルタを用いたロボットの自己位置推定を行うために、取り扱う線形な動的システムについて説明します。記事を読むカルマンフィルタの基本式:カルマンフィルタでロボットの位置推定をしてみよう(2)入力や出力に雑音が含まれている線形動的システムについて、ロボットの自己位置推定を行うためにカルマンフィルタの基本的な式を紹介します。記事を読むカルマンフィルタでロボットの位置推定をしてみよう!不正確な情報の中でもロボットの位置を正確に推定するために、カルマンフィルタを用いたロボットの自己位置推定の方法を紹介します。記事を読むシミュレーションで動作確認:カルマンフィルタでロボットの位置推定をしてみよう(4)自動運転車や移動ロボットの現在位置を推定するために、カルマンフィルタを用いた自己位置推定の方法を紹介します。実際にアルゴリズムを実装し、シミュレーションを用いて動作の確認も行います。記事を読むカルマンフィルタでロボットの自己位置推定をするプログラムの紹介ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法である、カルマンフィルタを用いたロボットの位置推定を行うためのMATLABプログラムを紹介します。記事を読む自動運転車を制御してみよう:簡単な制御モデルを求める実際に車やロボットの自動運転を行うために、車の現在位置と目標軌跡との誤差からハンドルの角度を算出し、車を目標軌跡に近づける方法を紹介します。コメントをキャンセルメールアドレスが公開されることはありません。