計測自動制御学会東北支部 第 339 回研究集会 (2022.10.28)
資料番号 339-5

移動ロボットの自己位置推定と制御
Control and Localization for Mobile Robots○渋谷渓太 ，村松鋭一○ Keita Shibuya，Eiichi Muramatsu
山形大学

Yamagata University
キーワード : 自己位置推定 (Localization)，拡張カルマンフィルタ (Extended Kalman Filter)，観測方程式 (Observation equation)，ガウス分布 (Gaussian distribution)，

連絡先 : 〒 992-8510 米沢市城南 4-3-16 山形大学 工学部 機械システム工学科 村松研究室
村松鋭一，Tel.: (0238)26-3327，E-mail: muramatu@yz.yamagata-u.ac.jp

1.

研究の背景に大きな雑音や，自己推定に大きな変化が起き自動車の自動運転，移動ロボットの活用のためには移動体が移動空間において自己位置，姿勢を認識することは必須の技術となっている．また，安全な移動のためには周囲の物体の認識も重要である．

る場合において有効となる推定と制御の方法を考える．まずは基本的な自己位置推定の確認と共に他の移動体との衝突回避の制御が可能であるかを確認する．次に，センサ値の大きな変化と自己位置の大きな変化に対応できるような推定法を提案する．これは過去の自己位置推定の推定値を利用するものである．

2.

目的雑音を考慮した移動体の自己位置推定，周囲3.の環境認識のための高精度な推定方法の提案を目指す．移動ロボットは非線形システムである
ため，自己位置推定の方法として拡張カルマンフィルタが考えられる．これはある程度理想的な環境においては有効性を発揮するが，例えばセンサの値に一時的にエラーが生じる，障害物との衝突といった突発的な変化に対して誤差が移動ロボットのモデリング

本研究で扱う移動ロボットが移動する際の，速度の x, y 方向の成分，角速度はそれぞれ以下の(1) 式のように表される．
  ẋ
v cos θ   ẏ  =  v sin θ θ̇ω大きくなるなどの問題がある．
本研究では拡張カルマンフィルタを用いた自己位置推定と，推定値を用いた制御において，特–1–(1)各記号は以下のとおりとする．

f : 状態遷移関数






ut : 時刻 t における制御指令値
xtxt−1 µ̂t : ロボットが移動した後の分布の中心
 yt  =  yt−1 θtθt−1Σ̂t : ロボットが移動した後の共分散行列
 
Ft : f を µt−1 周りで xt−1 で偏微分した時のヤ
vt cos θt−1 vt sin θt−1  ∆t (ωt = 0)
コビ行列


ωt
+(2)
vt ωt−1 {sin (θt−1 + ωt ∆t) − sin θt−1 }
 vt ωt−1 {− cos (θt−1 + ωt ∆t) + cos θt−1 }  (ωt ̸= 0)


ωt ∆t
Mt : vω 空間の共分散行列′
シミュレーションにおいては (1),(2) 式を用い
A : 出力 u が指令値 u からずれた量に比例しtttてロボットの運動を表現する．ここで，(2) 式をて遷移後の姿勢 x をどれだけずらすかを決める

状態遷移関数 f とする．

行列4.2

観測後の信念分布の更新

ランドマーク mj のセンサ値 zj,t が得られると，ベイズの定理より

bt (x) = η −1 pj (zj,t |x)bˆt (x) = η −1 Lj (x|zj,t )bˆt (x)Fig. 1

移動ロボットの運動モデル(5)と分布に反映される．
ここで，ガウス分布で表された b̂t と尤度関数 Lj4.の式を代入すると拡張カルマンフィルタ

移動ロボットの自己位置推定には拡張カルマンフィルタを用いる．自己位置推定に関しては，予測ステップ（制御指令値による推定），更新ステップ（センサ値による推定）からなる．

1b(x) = η −1 exp{− [z − h(x)]T Q(x)−1 [z − h(x)]−21
T −1(x − µ̂) Σ̂ (x − µ̂)}2(6)また、ランドマークが観測された際のセンサ

4.1

移動後の分布の更新

ロボットが移動した後の分布の更新はそれぞれ以下の式で行われる．

µ̂t = f (µt−1 , ut )

値を観測方程式 h(x)( √)(x − mx )2 + (y − my )2h(x) =(7)tan−1 (my − y, mx − x) − θとする．観測後の分布の更新では，

h(x) ≈ h(µ̂) + H(x − µ̂)(3)(8)と線形近似する．

Σ̂t = Ft Σt−1 FtT + At Mt ATt(4)

ばらつきの共分散行列 Q(x) に関しても，()[lµ̂ σl ]2 0Qlµ̂ =(9)0σφ2–2–を Q(x) の代わりに用いる．これらの式の近似と表される．姿勢のずれに関しては、ランダムによって，式（6）はガウス分布とされる．

変数生成関数を用いて姿勢角に正規分布に従う計算をまとめると、ランドマークの観測後の標準偏差 3[deg] の変数を加える．
また，もう１つ大きな雑音としてロボットが信念 b の共分散行列 Σ，信念 b の中心 µ，はそある距離を移動したらある範囲内で突発的に姿れぞれ，

Σ = (H T Q−1 H + Σ̂−1 )−1(10)勢を変える雑音も加える．ロボットの姿勢に関しては一様分布で選択する．一様分布は以下の確率密度関数で表される．

Tµ = ΣH Q−1(z − h(µ̂)) + µ̂(11){と表される．ここで，ΣH T Q−1 をカルマンゲイ

p(x|a, b) =ン K とし整理すると式 (8)，(9) は

Σ = (I − KH)Σ̂(12)µ = K(z − h(µ̂)) + µ̂(13)と表すことができる．これらの式を用いて観測後の分布は更新される．

1/(b − a) (a <=x<= b)0(otherwise)(15)

また，ある領域 X ⊂ χ 内から点 x ∈ X を選
ぶときの確率密度関数は{
η −1 (x ∈ X)
p(x|X) =0(x ∈/ X)∫(16)′

各記号は以下のとおりとする．

(ここで，η =Lj : 尤度関数

また，姿勢が変わるまでの時間も一様分布に従x : ロボットの x 座標 y : ロボットの y 座標

うランダムデータから選択する．

θ : ロボットの姿勢角x′ ∈X 1dx )センサに加える雑音に関しては，距離の計測

mx : ランドマークの x 座標 my : ランドマーに対しては距離に比例する標準偏差，方角に対クの y 座標

しては一定の標準偏差でガウス分布に従う雑音µ̂t : b̂t の平均値を混ぜる．

H : µ̂ まわりで h を x で偏微分したもの (x − µ̂

また、センサにはランドマークを見間違えてに比例する値)しまう雑音も取り込む．偽のランドマークの位

I : 単位行列

置を一様分布より抽出することにより表現する．

5.

雑音の性質

本研究で想定する雑音について説明する．

6.

推定と制御

6.1

ロボットと自己位置推定

本報告でのシミュレーションでは移動ロボットの運動，ロボットの取得するセンサ値に雑音を加え，周囲の環境に障害物を加えた．
ロボットの運動に関しては，道に落ちている小石を雑音と見立てある距離を移動したら姿勢角を変える雑音を取り込んだ．小石を踏むまでの距離は指数分布に従うと仮定した．指数分布

黒の線がロボットの軌跡、赤色の円が移動ロ
ボット，青色の線分が推定姿勢，青色の楕円が推定範囲，括弧内の数値がそれぞれ x, y 座標，
姿勢角 θ の推定値，星マークはランドマークを表す．ロボットがランドマークを検知した際にピンクの線が描かれる．の確率密度関数はp(x|λ) = λe−λx(x ≥ 0)(14)–3–

Fig. 2

自己位置推定と制御のシミュレータ

Fig. 46.2

推定値を用いた制御

衝突回避の制御

通常の制御から，位置の姿勢が大きく変化した

Fig3 は推定値を用いた制御を加えた場合のシ状態から元の状態へ復帰する制御に切り替えるミュレーションである．姿勢角の推定値に応じ場合，まずはその大きな変化を検討する必要がて角速度を変更することにより姿勢を修正した．

ある．本研究では過去の自己位置推定値を用い目標とする方向に進むことができており，自己た現在位置の推定値を比較し，それらの差が大き位置推定と制御の基本的機能が確認できた．

な場合に状態を元へ復帰させる制御を提案する．

Fig5 が制御を加えた場合のシミュレーションである．移動ロボットの過去の推定値を呼び出すことにより本来存在すべき位置を予測し，それと現在の推定値の差を求め，復帰させることが可能であることを確認した．

Fig. 3

6.3

自己位置推定と制御

他の移動体を考慮した制御

以下の Fig4 は移動体の接近に対して衝突を回避した制御の例である．
（黒の円が他の移動体を表す）ロボットの推定値とランドマークの位置より相対距離，相対角度を算出し制御を加えることにより衝突を回避できることを確認した．

6.4

突発的な姿勢の変化に対する制御

以上の基本的な機能の確認のもと，本研究では突発的な事象によってロボットの位置と姿勢が大きく変化した場合の制御方法について検討する．

–4–

Fig. 5

自己位置の大きな変化からの復帰

6.5

センサ値の大きな変化への対応

センサから送られる信号の受信環境が悪い場
合，センサ情報に基づく推定値の更新により，推定誤差が大きくなってしまう．これに対する対応として状態推定値の更新において状態方程式に基づく推定値をセンサに基づく推定値の比率を調整する方法を考えた．
つぎのように観測後の更新の式に新たに変数

Fig. 7a を追加した．
µ = K(z − h(µ̂))a + µ̂

修正を加えた場合(17)

Fig7 は推定に対して修正を加えた場合のシ状態方程式に基づく推定値センサ値による推定ミュレーションである．推定値のずれに応じて値をそれぞれ算出し，これらの差が大きくなっ推定に用いるセンサ値の割合を調整することにた場合に変数 a の値を小さくすることによりセより修正を加えた．修正を加えた場合のほうがンサ値の比率を小さくした．ここで，z はセンFig6 よりも良好な推定ができていることが確認

サによる観測値，h(µ̂) はそれに対する推定値，できる．

µ̂ は状態方程式に基づく推定値である

7.

まとめ

小さな雑音に対する制御，他の移動体に対する制御と基本的な制御に関しては，推定値を用いることにより実装できた．
自己位置の突発的な変化に関しては，過去の推定値も用いる方法を提案し，有効性を確認した．また，状態方程式に基づく推定値とセンサ値の利用の比率を調整する方法方法を提案した．

Fig. 6

修正を加えていない場合

今後は関数を用いてより合理的な a の値を考察する．

8.

参考文献とその参照

参考文献
1) 上 田 隆 一:

“詳 解 確 率 ロ ボ ティク ス”,

61/151, 講談社 (2019)2) 大塚敏之: “非線形最適制御入門”, コロナ社 (2011)–5–