確率ロボティクス第7回: センシングと推定（その2）

千葉工業大学 上田 隆一

This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.

今回の内容

• ベイズフィルタの実装
  • カルマンフィルタ（線形な場合）
  • カルマンフィルタ（非線形な場合。拡張カルマンフィルタと呼ばれる）
  • パーティクルフィルタ

前回のおさらい

• ベイズフィルタ
  • 次の2つの式で構成される
    • ロボットが動いたとき:
    • 情報が得られた時:
  • から次の計算の繰り返しでが求まる
  • 動きと情報を信念分布に変換
• どうやって実装するの？

カルマンフィルタ（線形な場合）

• 線形: 観測方程式が線形な形で書けること
  • • 時刻の添え字は省略
    • : 情報（ベクトルになっていることが必要）
    • : 定数項
    • : 雑音
    • : （といっしょに）位置でどんなセンサ値が得られるかをモデル化した行列
• 確率的な観測方程式の表記（がガウス分布に従うと仮定）
  • • : 雑音の共分散行列

線形なカルマンフィルタの適用例

次のような場合を想定

• 状態（位置・向き）:
  • 軸、軸はそれぞれ緯度、経度の方向とあわせる
• 観測:
  • GNSSから直接、緯度と経度をもらえる（誤差あり）
  • ジャイロから方角の情報をもらえる（誤差あり）

観測方程式（尤度関数）はこうなる

• • • : 1mあたりの緯度、経度
  • • いずれも原点調整の変数
  • : 誤差の大きさを測って共分散行列にしたもの
    • この場合は対角成分（分散）にだけ数値を置けばよさそう
• 右辺が尤度関数に（計測されたの尤もらしさ）

情報が得られたときの更新式

• • は、それぞれの平均値、共分散行列

• 第4回で出てきた式: と同じ形

• としてを求めてみましょう

  • ，，、，，、を代入

カルマンゲインの導入

• 前ページの最後の2つの式で、GNSS、ジャイロの値を分布に反映できた
  • ただしもっと簡単になるので計算続行
• 次の記号を導入
  • : カルマンゲイン（なんなのかは後で説明）
  • : 分布の中心で得られるであろうセンサデータ
• を整理（やってみましょう）
  • 
    
    
    

カルマンゲインの導入（続き）

• を整理（これもやってみましょう）
  • 
    
• まとめ
  • • 新たな推定姿勢 (実際の情報 元の推定姿勢で期待された情報)
      元の推定姿勢
  • • 得られた情報の分だけ共分散行列を小さく

カルマンゲイン: 観測が信念分布に与える影響力の強さ

カルマンゲインの変形

• は右辺にがあるので計算できない
  • を除去しましょう
    • を代入
    • ウッドベリーの公式を利用
• 答え
  • 
    
    
    
    

カルマンフィルタ

• 以上の結果と第6回の動きの式のまとめ
  • ロボットが移動したとき
    • （おさらい: 元の平均位置に変位の平均値を足す）
    • （おさらい: 変位の不確かさを足す）
  • ロボットが観測したとき
• 上記の更新式をカルマンフィルタと呼ぶ
  • ガウス分布の演算によるベイズフィルタの実装

拡張カルマンフィルタ

• 非線形な場合にどうカルマンフィルタを適用するか？
  • 第6回と同様、線形化する
• 例題: カメラで点ランドマークを観測（カメラの位置=ロボットの位置）
  • ランドマークまでの距離、ロボットからのランドマークの方角が計測可能
  • ランドマーク自身の方角も計測可能
  • ランドマークの位置、方角:

観測モデル・尤度関数

• 観測モデル:
  • 観測方程式: で得られるセンサ値のベクトルの平均値
    • 要素は（はセンサ値の空間のベクトルとなる）
  • : での雑音の共分散行列
    • 下準備としての近似: の中心での共分散行列に近似
      • は以後、単にと表記
• 尤度関数:
  • 線形化が必要に
    • をのような線形な式に近似

観測方程式の線形近似

• • 分布の中心での値 + 中心からのズレによる変化量
  • • 例題で具体的にがどうなるかは、あとから考えましょう
• 近似した式の整理

拡張カルマンフィルタの導出

• • （再掲）線形な場合のときの式:
    • p.7とほぼ同じなので以後省略

拡張カルマンフィルタ

第6回の移動の式と合わせると次のように

• ロボットが移動したとき
• ロボットが観測したとき

例題への適用: 観測方程式を立てる

• 得られるセンサ値を計算（詳細は省略）
• 観測方程式:
  

例題への適用: ヤコビ行列の算出

• （計算してみましょう）
  • 
    

例題への適用: の決定

（教科書には記述が抜けている模様）

• ごとに分散を測定or見積もって作成
• 例:
  • : 距離によって値を大きくする（例: ）
  • は定数

例（詳解確率ロボティクスから）

• 左: 観測なし。右: 観測あり。
• は無い
  • やは行列になるが問題ない
  • なんでそれでを推定できるか考えてみましょう

パーティクルフィルタ

• モンテカルロ法を利用したベイズフィルタ
• 移動ロボットではカルマンフィルタより用いられる
  • 第6回の例のように、壁にぶつかるなどのシミュレーションに適する
    （ただし、あまり真面目にやっている実装はない）
  • 雑音がガウス分布に従わない場合が多い
    • LiDARのデータ: 物体までの距離を遠く測ることは少ないが、人が横切って短く計測することは多い

パーティクル（再定義）

• 右図: おさらい
• 第6回のパーティクルに「重み」という変数を追加
  • 
    
    • 右図の矢印がそれぞれ重みを持つ
  • 集合:
• 真の姿勢が内にある確率の近似方法を定義
  • • : かっこの中がtrueなら1、falseなら0
• センサ情報を反映する前のパーティクルの集合:

センサ情報を反映するときのパーティクルの処理

• に情報を反映してに
• 尤度関数の値を重みにする
  • 姿勢はそのまま（）
• がモンテカルロ法で受け持つ確率に
  • の計算は省略して次の操作で辻褄を合わせることもある

ロボットが動いた時のパーティクルの処理

• に制御指令を反映してに
• 単に各パーティクルを動かすのではなく、リサンプリングする
  • 重みが偏っていくのを防ぎ、重みの大きいパーティクルを増殖
• 例
  • 左: リサンプリングなし（重みに尤度をかけていく）
  • 右: リサンプリングあり
    

リサンプリングの手続き（素朴なもの）

• 以下を繰り返してを作成
  • 1: から重みに比例する確率でひとつパーティクルを選択
  • 2:
  • 3:
• 上記の方法は計算量が大きく、選択に偏りが発生するので、実装ではもっとよい方法（系統サンプリング）が使用される

パーティクルフィルタのまとめ

• センサ情報の反映:
  • に対し
• 移動の反映:
  • に対し
    • （重みを選択される確率に）

これでベイズフィルタの近似になっている

パーティクルフィルタの応用例

（実例はYouTube等で）

• レーザースキャナを用いた移動ロボットでよく用いられる
• 尤度関数: 次のような「占有格子地図」との比較で設計（尤度場）
  • スキャンの先が障害物の記録された格子にヒットすると尤度が上がる
  • 実際にはグレーゾーンを設けるが、基本、この簡単なモデルがよく機能する
    • いくつか地図にない障害物があっても点数が大きく下がらない
      

まとめ

• ベイズフィルタの実装をみてきた
  • カルマンフィルタ、拡張カルマンフィルタ、パーティクルフィルタ
• カルマンフィルタはいまでも機械の制御によく用いられる
• 移動ロボットについてはパーティクルフィルタがいまでもよく用いられる
  • ただしもっと新しい方法もあるので流行は気にしておきましょう