13.3K Views
October 09, 22
スライド概要
■ドローンやロボットを自作することを通じて制御や関連技術の生涯勉強情報を提供■工学博士■防大航空宇宙→筑波大博士■陸自→対戦車誘導弾等の装備品開発→高専教員→大学教員■ロボットランサー優勝→マイクロマウスニューテクノロジー賞受賞■指導者としてつくばチャレンジバンナム賞→飛行ロボコンマルチコプタ部門1位等々■北海道函館出身
第14回ロボット⼯学 カルマンフィルタ
ルドルフ・カルマン 1930年ハンガリのブタペスト⽣まれのア メリカの学者(2016年没) 1960年、⼤学院の時にカルマンフィルタ を考案する。 アポロ宇宙船の軌道推定に使⽤され,その実 ⽤性が⾼く評価される。 Wikipediaより転載 現代制御理論の⽣みの親。
ロボットも宇宙船もまっすぐ⾏きたいと 思っていても、太陽光による熱による推 進装置の歪みや燃料噴射の不均⼀、路⾯ の摩擦の局所的な違いによって予定通り には進めない
左行け ロボットも宇宙船もこういう軌跡を描 いて動いて欲しいという要求があり、制 御する必要がある。 そのため、位置を観測する必要がある。 しかし、観測にも観測の誤差が必ずあ り本当の位置はわからない。 右行け
動きが思う様にいかないこと は、いつどのくらいになるか は確率的な現象。 観測した時に含まれている誤 差もいつどのくらいになるか は確率的な現象。
システムノイズ 動きが思う様にいかないこと は、いつどのくらいになるか は確率的な現象。 観測ノイズ 観測した時に含まれている誤 差もいつどのくらいになるか は確率的な現象。
速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 𝑥! = 𝑥!"# + 𝑣 𝐿! = 𝑥! 時間 𝑡! = 𝑡!"# + 1 時間は1秒ごとに増えていくとします
速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 𝑥! = 𝑥!"# + 𝑣 𝐿! = 𝑥! 時間 ノイズがなければ観測⽅程式からセン サーで測った距離がロボットの位置を表 していることになるが、実際はノイズが あるのでそうはならない。 𝑡! = 𝑡!"# + 1 時間は1秒ごとに増えていくとします
速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 ノイズがなければ観測⽅程式からセンサ で測った距離がロボットの位置を表して いることになるが、実際はノイズがある のでそうはならない。 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 𝐿! = 𝑥! + 𝑤% 𝑤! :システムノイズ 𝑤" :観測ノイズ
速度 𝑣 𝑚/𝑠 距離測定 𝐿! 状態⽅程式 位置 𝑥! 観測⽅程式 出発点の壁 ノイズがなければ観測⽅程式からセンサ で測った距離がロボットの位置を表して いることになるが、実際はノイズがある のでそうはならない。 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 𝐿! = 𝑥! + 𝑤% 𝑤! :システムノイズ 𝑤" :観測ノイズ 実際に得られるデータは 観測(距離測定)𝐿# のみ
システムノイズがなけ ればこの位置にロボッ トがいたはず(理想) 状態⽅程式 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 観測⽅程式 𝐿! = 𝑥! + 𝑤% 𝑤! 𝑤" ロボットの位置 :システムノイズ :観測ノイズ
状態⽅程式 𝑥! = 𝑥!"# + 𝑣 + 𝑤$ 観測⽅程式 𝐿! = 𝑥! + 𝑤% システムノイズがなけ ればこの位置にロボッ トがいたはず(理想) ロボットの位置 𝑤! 𝑤" :システムノイズ :観測ノイズ
理想からの変動を取り出して ノイズの影響を強調した 状態⽅程式 𝑥)! = 𝑥)!"# + 𝑤$ 観測⽅程式 𝐿* ! = 𝑥)! + 𝑤% 𝑤! :システムノイズ 𝑤":観測ノイズ ここで ロボットの位置の変化分 上50秒間 下500秒間 𝑥)! = 𝑥! − 𝑛𝑣 𝐿* ! = 𝐿! − 𝑛𝑣
理想からの変動を取り出して ノイズの影響を強調した 状態⽅程式 𝑥)! = 𝑥)!"# + 𝑤$ 実際にセンサから得られる情報はオレ ンジの情報なので、そこから⻘⾊のロ ボットの位置情報を推定する。 カルマンフィルタ ロボットの位置の変化分 上50秒間 下500秒間 観測⽅程式 𝐿* ! = 𝑥)! + 𝑤% 𝑤! :システムノイズ 𝑤":観測ノイズ ここで 𝑥)! = 𝑥! − 𝑛𝑣 𝐿* ! = 𝐿! − 𝑛𝑣
ノイズの性質 ノイズの値はランダムに現れるも のとみなします。 そしてたくさんのノイズの値を集 められたとすると、その平均は0と 考えます。 平均が0で同じになっても、値の ばらつき⽅には違いが出てきます。 ばらつき⽅を⽰す値を分散σ2と ⾔います。その平⽅根が標準偏差σ です。 ノイズを10万回シミュレーション で⽣成して、それぞれの値の回数を 数えてヒストグラムを作りました。 平均0の ノイズ σ=1 σ=3 ノイズのヒストグラム 分散の⼤きさによって釣鐘型 の分布の裾野が広がる
線形化 話を簡単にしたいので、理想からの変動だけを⾒ていきます こちらで話を進めます 状態⽅程式 状態⽅程式 観測⽅程式 観測⽅程式 𝑥# = 𝑥#$% + 𝑣 + 𝑤! 𝐿# = 𝑥# + 𝑤" 𝑥'# = 𝑥'#$% + 𝑤! 𝐿( # = 𝑥'# + 𝑤" 右の2式で戻せる 𝑥'# = 𝑥# − 𝑛𝑣 𝐿( # = 𝐿# − 𝑛𝑣
カルマンフィルタの考え⽅ 1. ⾃分(ロボット)はどこにい るのか?システムノイズは0 (最も確率が⾼いノイズの 値)で状態⽅程式で予測する 2. センサからの観測情報を使っ て、予想を補正する。 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する
カルマンフィルタの考え⽅ 1. ⾃分(ロボット)はどこにい るのか?システムノイズは0 (最も確率が⾼いノイズの 値)で状態⽅程式で予測する 2. センサからの観測情報を使っ て、予測を補正する。 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する 𝑥̅# = 𝑥$#$% 最新の予測 ⼀つ前の推定
カルマンフィルタの考え⽅ 1. ⾃分(ロボット)はどこにい るのか?システムノイズは0 (最も確率が⾼いノイズの 値)で状態⽅程式で予測する 2. センサからの観測情報を使っ て、予測を補正する。 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する 𝑥̅# = 𝑥$#$% 最新の予測 ⼀つ前の推定 𝑥$# = 𝑥̅# + 𝐾(𝐿( # − 𝑥̅# ) イノベーション カルマンゲイン
カルマンフィルタの考え⽅ 𝑥̅# = 𝑥$#$% 1. ⾃分(ロボット)はどこにい るのか?システムノイズは0 (最も確率が⾼いノイズの 値)で状態⽅程式で予測する 最新の予測 2. センサからの観測情報を使っ て、予測を補正する。 最新の推定 3. 補正の際、推定のばらつきが ⼩さいと考えれば予想を重視 し、観測ノイズが⼩さいと考 えれば観測をより重視する ⼀つ前の推定 カルマンゲイン 𝑥$# = 𝑥̅# + 𝐾# (𝐿( # − 𝑥̅# ) イノベーション カルマンゲインに関しては以下の式から計算して 使います。 𝐾! = 𝜎(" ! # 𝜎(" ! # + 𝜎$ # 𝜎!! " # :位置のばらつきの予測値(あとで説明) 𝜎$ # :観測ノイズの分散
位置推定式の意味についての補⾜ 𝑥-! = 𝑥̅! + 𝐾! (𝐿* ! − 𝑥̅! ) 𝐾# = 𝜎)! # (1) $ $ 𝜎)! # + 𝜎" $ (2) (1)式に(2)式を代⼊して整理すると次式になる 𝑥-! = 𝜎% & & 𝜎3$ ! + 𝜎% 𝑥 ̅ + ! & 観測が信⽤できるということは、ば らつき𝜎! " が⼩さいので推定値に対し てこの項の影響(予測の影響)は⼩ さくなる 𝜎3$ ! & & 𝜎3$ ! + 𝜎% *! 𝐿 &
位置推定のばらつきの予測と推定 カルマンフィルタは位置の推定と同時に位置推定のばらつき(分 散)も予測して推定します。 位置推定のばらつき(確からしさ)は最初わからない事が多いの ですが、適当な値から始めてもこの推定によって確からしい値に 段々収束し、通常ばらつきが⼩さくなり位置推定精度が上がってい きます。 状態⽅程式からこ の様になります 位置の推定のばらつきの(分散)の予測 $ $ 𝜎)! # = 𝜎,! #%& + 𝜎! $ 位置の推定のばらつきの(分散)の推定 $ 𝜎,! # = 1 − 𝐾# 𝜎)! # $ 𝜎! # :システムノイズの分散 カルマンゲインを 使って予測から推 定へ更新してます
カルマンフィルタ位置推定のながれ 1. 2. 3. 4. 5. 6. 位置を予測 位置の分散を予測 カルマンゲインを計算 カルマンゲインと位置の予測とイノベーションから位置を推定 カルマンゲインと位置の分散の予測から位置の分散を推定 1にもどり繰り返す ※初期の位置と分散、システムノイズの分散、観測の分散は あらかじめ与えます
課題 配布したエクセルファイルには上の図で⽰される500秒間のロボットの位置の観測 データが含まれている。 このデータからカルマンフィルタを⽤いて、ロボットの位置を推定しなさい。 ロボットの初期位置 0 位置推定の初期分散 100 観測の分散 9 システムノイズの分散 1
計算は配布したエクセルシートを⽤いて⾏いなさい。提出はファイル名を 「番号⽒名カルマンフィルタ」にして提出しなさい。 推定した結果は、下図に⽰す様に、観測データと重ねてグラフにして⽰しなさい