今回のシリーズでは、自動運転車や移動ロボットを制御するためにとても重要な正確な位置を知る方法として、カルマンフィルタを用いたロボットの位置推定の方法を紹介しています。自己位置推定ロボットの自己位置推定(Localization)技術についての情報を紹介します。15:33これまでの記事では、線形動的システムに対してカルマンフィルタを用いて自己位置推定を行いました。21:46しかし、世の中のシステムには非線形な動的システムが多く存在します。今回からの記事では、この様な非線形動的システムの自己位置推定を行うために、拡張カルマンフィルタを用いる手法を紹介していきます。まとめ線形動的システムとカルマンフィルタ前回までの記事で取り扱っていた線形動的システムとカルマンフィルタの基本式についておさらいします。線形動的システム線形動的システムは、その名の通り線形で動的なシステムです。$$のように線形な式で表すことが出来ます。上の式中に含まれるwとvは、入力や観測の雑音を表しています。詳しくは、こちらの記事を参考にしてください。カルマンフィルタでロボットの位置推定をしてみよう!ロボットや自動運転車を制御するときに、ロボットや車の正確な位置を知ることはとても重要です。20:33カルマンフィルタの基本式この線形な動的システムについてロボットの自己位置推定を行うために、カルマンフィルタを用います。予測ステップ更新ステップの2つのステップがあります。$$の2式で表されます。$$の5式で表されます。詳しくは、こちらの記事を参考にしてください。17:17このカルマンフィルタを用いた移動ロボットの自己位置推定は、線形システムには有効ですが、非線形なシステムには適応することが出来ません。そこで、このカルマンフィルタを少し拡張した、拡張カルマンフィルタを用いた非線形動的システムの自己位置推定を行いたいと思います。非線形動的システムまず、非線形な動的システムの状態空間モデルについて紹介します。基本的な状態空間モデルの構成は線形動的システムの場合と同じで、状態方程式と出力方程式で表されます。$$のようになり、非線形の関数であるfやhを用いて構成されています。このように関数fやhのような非線形関数を含んでいるので、このような状態空間モデルで表されるシステムを非線形システムとなります。線形動的システムの所で紹介した状態空間モデルの2式は、この関数fやhが線形な場合と同様の式を表しています。$$のように非線形関数fやh内に含まれる場合もあります。この様な非線形な状態空間モデルで表される非線形動的システムの自己位置推定を行うために、拡張カルマンフィルタを用いていきます。拡張カルマンフィルタ拡張カルマンフィルタは、カルマンフィルタを非線形システムに対応するように拡張したものです。拡張カルマンフィルタの基本的な形はカルマンフィルタと同じなので、カルマンフィルタが理解できていれば、拡張カルマンフィルタの理解もし易いと思います。拡張カルマンフィルタは、カルマンフィルタの場合と同様に予測ステップ更新ステップの2つのステップで構成されています。予測ステップ予測ステップでは、1つ前の状態から現在の状態を予測します。この予測ステップでは、推定状態xの予測\(\hat{\boldsymbol{x}}\)とその推定状態の確からしさを示す誤差共分散Pの予測\(\hat{\boldsymbol{P}}\)を行います。$$と求めることが出来ます。この推定状態を予測する際には実際には含まれるであろう雑音wによる影響は無視しています。この雑音wによる影響は誤差共分散Pを算出する際に考慮していきます。先程述べたように、雑音wによる影響を考慮して誤差共分散Pを予測していきます。$$と求めることが出来ます。$$のように、状態方程式で用いている非線形関数fのヤコビアンで定義した行列です。$$が拡張カルマンフィルタの予測ステップにおける基本式です。更新ステップ更新ステップでは、予測ステップで予測した状態と観測による情報から現在の状態を更新します。この更新ステップでは、時刻t-1の状態から予測した推定状態xと誤差共分散Pに、観測により得られる情報zを加えて現在の時刻tでの推定状態を求めていきます。観測残差の算出まず、実際に観測により得られた情報zと今の推定状態なら得られるであろう情報との差を示す観測残差\(\tilde{\boldsymbol{y}}\)を求めます。$$観測残差の共分散を算出そして、観測残差\(\tilde{\boldsymbol{y}}\)に対する誤差共分散\(\boldsymbol{S}\)を求めます。$$と、出力方程式で用いられている非線形関数hのヤコビアンで定義した行列です。カルマンゲインの最適化そして、求めた値(行列)を基に、カルマンゲインKの最適化を行います。最後に、最適化したカルマンゲインKを用いて推定状態xを更新します。$$誤差共分散の更新そして、誤差共分散Pも併せて更新します。$$が拡張カルマンフィルタの更新ステップにおける基本式になります。まとめ今回は、非線形動的システムの自己位置推定を行うために、拡張カルマンフィルタについて紹介しました。拡張カルマンフィルタの基本的な式はカルマンフィルタと同じなので、理解するのはそこまで難しくはないと思います。次回からは、実際にこの拡張カルマンフィルタを用いて移動ロボットの自己位置推定を行いたいと思います。22:19スポンサーリンクレクタングル(大)広告レクタングル(大)広告シェアするツイートフォローする関連コンテンツtajima関連記事シミュレーションで動作確認:カルマンフィルタでロボットの位置推定をしてみよう(4)自動運転車や移動ロボットの現在位置を推定するために、カルマンフィルタを用いた自己位置推定の方法を紹介します。実際にアルゴリズムを実装し、シミュレーションを用いて動作の確認も行います。記事を読むカルマンフィルタでロボットの位置推定をしてみよう!不正確な情報の中でもロボットの位置を正確に推定するために、カルマンフィルタを用いたロボットの自己位置推定の方法を紹介します。車やロボットの自動運転を実際に制御するために、車と目標軌跡との誤差からハンドル角度を算出して、自動運転車を実現するMATLABプログラムを紹介します。記事を読むカルマンフィルタでロボットの自己位置推定をするプログラムの紹介ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法である、カルマンフィルタを用いたロボットの位置推定を行うためのMATLABプログラムを紹介します。記事を読む自動運転車を制御してみよう:簡単な制御モデルを求める実際に車やロボットの自動運転を行うために、車の現在位置と目標軌跡との誤差からハンドルの角度を算出し、車を目標軌跡に近づける方法を紹介します。記事を読むカルマンフィルタの基本式:カルマンフィルタでロボットの位置推定をしてみよう(2)入力や出力に雑音が含まれている線形動的システムについて、ロボットの自己位置推定を行うためにカルマンフィルタの基本的な式を紹介します。記事を読む非線形動的システムの状態空間モデル:カルマンフィルタでロボットの位置推定をしてみよう(6)自動運転車や移動ロボットなどの自己位置推定を拡張カルマンフィルタで行うために、実際に非線形動的システムを状態空間モデルで表す方法を紹介します。記事を読む拡張カルマンフィルタとヤコビ行列:カルマンフィルタでロボットの位置推定をしてみよう(7)ロボットや自動運転車などの非線形動的システムを制御する際に重要となる、ロボットや車の正確な位置を得る方法として拡張カルマンフィルタを用いるために、非線形関数からヤコビ行列を算出する方法について紹介します。記事を読む拡張カルマンフィルタのシミュレーション結果:カルマンフィルタでロボットの位置推定をしてみよう(8)自動運転車や移動ロボットの非線形動的システムを制御するために重要となる現在位置を推定するために、拡張カルマンフィルタを用いた自己位置推定の方法を紹介します。実際にアルゴリズムを実装し、シミュレーションを用いて動作の確認をした結果も紹介します。記事を読む自動運転車を制御してみよう本シリーズは、実際に車やロボットの自動運転を行うために必要な制御工学の情報を紹介しています。本記事では、取り扱う自動運転車の問題と自動制御の全体像を紹介します。コメントをキャンセルメールアドレスが公開されることはありません。