研究室ゼミでの論文紹介資料です。 Robust Vehicle Localization in Urban Environments Using Probabilistic Maps Jesse Levinson, Sebastian Thrun International Conference on Robotics and Automation (ICRA), 2010

1. Nagoya University
Parallel & Distributed Systems Lab.
論⽂紹介 (2016.9.7)
Robust Vehicle Localization in Urban Environments
Using Probabilistic Maps
橘川 雄樹
yuki@ertl.jp
名古屋⼤学 ⼤学院
情報科学研究科 情報システム学専攻
枝廣研究室 モビリティグループ
博⼠後期課程 1年
1

2. Nagoya University
Parallel & Distributed Systems Lab.
論⽂について
p タイトル
Robust Vehicle Localization in Urban Environments Using Probabilistic Maps
p 著者
Jesse Levinson, Sebastian Thrun @Stanford University（当時）
p 出典
International Conference on Robotics and Automation (ICRA), 2010
p 選定理由
Google Carの位置推定技術の調査
LIDARから得られる反射強度を⽤いた位置推定の調査
2

3. Nagoya University
Parallel & Distributed Systems Lab.
章構成
I. Introduction
II. Probabilistic Maps
a. Map Alignment Using GraphSLAM
b. Laser Calibration
c. Map Creation
III. Online Localization
a. Motion Update
b. Measurement Update
c. Most Likely Estimate
IV. Experimental Results
a. Quantitative Results
b. Autonomous Success
V. Conclusion
3

4. Nagoya University
Parallel & Distributed Systems Lab.
Introduction
4

5. Nagoya University
Parallel & Distributed Systems Lab.
Introduction
⾃動運転の歴史
n 2004 DARPA Grand Challenge (砂漠のコース) - 完⾛なし (最⾼11.78km)
n 2005 DARPA Grand Challenge - 5チーム完⾛
優勝: スタンフォード⼤学 Stanley (チームリーダー: Sebastian Thrun)
n 2007 DARPA Urban Challenge (市街地を想定したコース) - 6チーム完⾛
優勝: カーネギーメロン⼤学 Boss (チームリーダー: Chris Urmson)
準優勝: スタンフォード⼤学 Junior
5
Stanley Boss Junior

6. Nagoya University
Parallel & Distributed Systems Lab.
Introduction
背景
⾃動運転の位置推定はGPS/IMU以上の精度が求められる
Previous Work
Levinson, Jesse, Michael Montemerlo, and Sebastian Thrun. "Map-Based Precision
Vehicle Localization in Urban Environments." Robotics: Science and Systems. Vol. 4. 2007.
Ø GraphSLAMにより経路を算出、⾚外線反射地図の作成
Ø Particle Filterを⽤いた位置推定
Ø 環境の変化(移動物体など)に対応困難
地図データの情報不⾜のため (反射強度の平均値のみ保存)
本論⽂
Ø 確率分布を考慮した⾚外線反射地図の作成
Ø 様々な環境に対応可能な位置推定を提案
6

7. Nagoya University
Parallel & Distributed Systems Lab.
Probabilistic Maps
a. Map alignment Using GraphSLAM
b. Laser Calibration
c. Map Creation
7

8. Nagoya University
Parallel & Distributed Systems Lab.
Probabilistic Map
Probabilistic Map
LIDARから得られる⾚外線反射強度の平均・分散を各セルに保存
Probabilistic Map 作成⼿順
a. ループクロージングを⾏い、経路のアラインメント
b. LIDARの各ビームのキャリブレーション
c. キャリブレーションされた反射強度を、⾛⾏経路を基にして地図上に投影
8

9. Nagoya University
Parallel & Distributed Systems Lab.
Map Alignment Using GraphSLAM
GraphSLAM - グラフ構造を利⽤して、地図のループクロージング
GraphSLAMのアルゴリズム
1. ノードの⽣成
2. 拘束の⽣成
3. 拘束の最適化
9
ランドマーク
ノードx1
拘束
ノードx3
ノードx2
拘束
拘束
拘束
ri∑i
ランドマーク
オドメトリ
ロボット
全ての拘束に対するノード位置の
同時確率を最⼤化
𝑝 𝒙 = $ 𝑝(𝒙|𝑐()
*+,
(-.
どれだけノードを動かせば拘束を満たすか
本実装では、スキャンデータに対し、
ICP(Iterative Closest Point)を⽤いて、拘束を⽣成

10. Nagoya University
Parallel & Distributed Systems Lab.
Laser Calibration
10
レーザー固有の反射特性による、ビーム間の反射強度の計測値の違いを調整
キャリブレーション前
キャリブレーション後
1. ⾛⾏軌跡からスキャンデータを地図上にマッピング
2. 各セル内に属する点の反射強度の平均を計算
3. 各ビームの計測値と平均値を対応づけ
→ 64(ビーム数)×256(反射強度)の変換表を作成
ビームID 0…63
キャリブレーションができていないと…
ü 反射強度の平均がセルに当たったビームに依存
ü 反射強度の分散が実際よりも⼤きくなってしまう
各セル内にマッピング
されたスキャンの反射強度
の平均値を計算

11. Nagoya University
Parallel & Distributed Systems Lab.
Map Creation
Probabilistic Mapの作成
1. ⾛⾏軌跡から、キャリブレーションされたスキャンデータをxy平⾯にマッピング
2. 各セル(15cm×15cm)に属するスキャンデータの反射強度の平均と分散を計算
Probabilistic Mapの利点
Ø 影や光に影響されない
Ø 複数のスキャンの平均を取ることにより、移動物体の軌跡が薄くなる
(移動物体のデータを消す必要がない)
11
反射強度の平均 反射強度の分散
移動物体(⾞両)の軌跡は消滅 移動物体(⾞両)の通過部分
セルに路⾯と⽩線の境界が
含まれているのため分散⼤

12. Nagoya University
Parallel & Distributed Systems Lab.
Online Localization
a. Motion Update
b. Measurement Update
c. Most Likely Estimate
12

13. Nagoya University
Parallel & Distributed Systems Lab.
p 1次元空間（x座標）上のどこかに⾞両が存在
p センサデータを⽤い、x座標上の位置を推定し、確率で表現
p ⾞を1だけ右に動かすが、実際に1動いたとは限らない (Motion Update)
p 新たなセンサデータから、位置を推定 (Measurement Update)
p 新たなセンサデータから得られる存在確率(B)と、⾏動後の存在確率(A)を合成し、より精度の⾼い位置を算出
x
x
x
移動 移動
⾞両が
いる確率
⾞両が
いる確率
⾞両が
いる確率
x
･･･ A
･･･ B
ここにいる確率が⾼い これはセンサーのノイズ?
移動すると⼭はなだらかに
→ 位置はあいまいに
新たなセンサデータから、
ここにいる確率が⾼い
x
⾞両が
いる確率
より⾼いピーク
確率の合成により、ノイズは除去
参考
http://www.slideshare.net/minoruchikamune/car-20140829
（補⾜）Markov Localization (1次元)
13

14. Nagoya University
Parallel & Distributed Systems Lab.
（補⾜）Histogram Filter
Histogram Filter
Ø 空間を等間隔に分割し、それぞれの領域での存在確率を計算
Ø 領域の数が増えると、計算時間増加
14
x
⾞両が
いる確率
x
⾞両が
いる確率
x
⾞両が
いる確率
移動後の予想
センサデータを⽤いた推定
上2つの合成
参考
http://www.slideshare.net/minoruchikamune/car-20140829

15. Nagoya University
Parallel & Distributed Systems Lab.
Motion Update
Motion Updateによって、位置推定の精度は低下
GPS/IMUシステムの2つの座標系
1. Global座標(GPS座標)
GPSから得られるグローバルな座標、値が⾶び⾶びになる
2. Smooth座標(IMU座標)
IMUのデータを蓄積して得られるなめらかな座標、誤差が蓄積する
Global-Smoothのオフセットはガウスノイズ付きランダムウォークでモデル化
Motionによって、各セルに⾞両が存在する確率 (位置(i,j)から位置(x,y)への移動)
𝑃0 𝑥, 𝑦 = 𝜂 5 6 𝑃 𝑖, 𝑗 5 exp −
1
2
𝑖 − 𝑥 ? 𝑗 − 𝑥 ?/𝜎?
(,C
15
（補⾜）正規分布(ガウス分布)
平均: 𝜇, 分散: 𝜎?の時
𝑓 𝑥 =
1
2𝜋𝜎?
exp −
𝑥 − 𝜇 ?
2𝜎?
移動後(x,y)にいる確率 (i,j)にいる確率 (i,j)→(x,y)に移動する確率

16. Nagoya University
Parallel & Distributed Systems Lab.
Measurement Update
【求めたい確率】センサデータ z, 地図データ mの時、Global-Smoothのオフセット(x,y)の確率
𝑃 𝑥, 𝑦 𝑧, 𝑚 = 𝜂 5 𝑃 𝑧 𝑥, 𝑦, 𝑚 5 𝑃 𝑥, 𝑦
【P(x,y)の計算】GPS/IMUのデータ(平均 0, 分散 𝜎IJK
?
)と事後確率の積から、P(x,y)を推定
𝑃 𝑥, 𝑦 = 𝜂 5 exp
𝑥? + 𝑦?
−2𝜎IJK
? 5 𝑃0(𝑥, 𝑦)
【P(z|x,y,m)の計算】オフセット(x,y)かつ地図データがmの時に、センサデータがzとなる確率
𝑃 𝑧 𝑥, 𝑦, 𝑚 = $ exp
− 𝑚M NOP,QOR
− 𝑧S N,Q
?
2 𝑚S NOP,QOR
+ 𝑧S N,Q
?
T
(,C
𝑚M , 𝑚S: 地図の反射強度の平均,標準偏差 𝑚M , 𝑧S: センサデータの反射強度の平均,標準偏差
例: 𝑚S .WX,Y.Z
- GPSの位置から0.45m東、1.2m北の地図のセルにおける反射強度の標準偏差
(A)に(B),(C)を代⼊
𝑃 𝑥, 𝑦 𝑧, 𝑚 = 𝜂 5 $ exp
− 𝑚M NOP,QOR
− 𝑧S N,Q
?
2 𝑚S NOP,QOR
+ 𝑧S N,Q
?
T
(,C
5 exp
𝑥? + 𝑦?
−2𝜎IJK
? 5 𝑃0(𝑥, 𝑦)
16
- (A)
- (B)
- (C)

17. Nagoya University
Parallel & Distributed Systems Lab.
Most Likely Estimate
確率分布(P(x,y|z,m))が求められたので、具体的なx,yを計算
1. 確率の最⼤値 max],^ 𝑃(𝑥, 𝑦) を選択 – x,yが⾶び⾶びになることがあるため
2. 確率分布の重⼼を計算 – x,yが滑らかに変化
𝑥 =
∑ 𝑃(𝑥, 𝑦)T 5 𝑥],^
∑ 𝑃(𝑥, 𝑦)T
],^
, 𝑦 =
∑ 𝑃(𝑥, 𝑦)T 5 𝑦],^
∑ 𝑃(𝑥, 𝑦)T
],^
17
GPSによる位置推定(1m以上の誤差) 提案⼿法による位置推定

18. Nagoya University
Parallel & Distributed Systems Lab.
Experimental Results
a. Quantitative Results
b. Autonomous Success
18

19. Nagoya University
Parallel & Distributed Systems Lab.
Experimental Results
評価環境
Ø C⾔語で実装
Ø ⼀般的なラップトップを使⽤
Ø 地図データのサイズは10MB/Mile
19
センサ 製品名 周波数
GPS/IMU Applanix LV-420 200 Hz
LIDAR Velodyne HDL-64E 10 Hz
実験⾞両⾚外線反射地図(セル: 15cm×15cm)

20. Nagoya University
Parallel & Distributed Systems Lab.
Quantitative Results
GraphSLAMを⽤いて算出された経路(Ground Truth)と、提案⼿法で算出された経路を⽐較
20
横⽅向の推定誤差 縦⽅向の推定誤差
位置推定後の誤差位置推定前(GPS/IMU)の誤差
RMS: 66cm
RMS: 9cm
RMS: 87cm
RMS: 12cm
RMS(Root Mean Square): ⼆乗平均平⽅根
ホイールエンコーダー or GPSのバイアス?
セルのサイズ(15cm)よりも⼩さい

21. Nagoya University
Parallel & Distributed Systems Lab.
Autonomous Success
Ø 本⼿法 - 実環境における完全⾃動運転が可能に
• マンハッタンでのデモンストレーション
• キャンパス周辺 4マイル の経路 (狭い道路、窮屈な交差点、交通量が多い…)
Ø Previous Work - 精度が⼗分でないため、交通量が少なく、幅が広い道路に限定
21
キャンパス周辺のコース

22. Nagoya University
Parallel & Distributed Systems Lab.
Conclusion
22

23. Nagoya University
Parallel & Distributed Systems Lab.
Conclusion
まとめ
Ø 確率モデル（平均・分散）を⽤いた⾚外線反射地図により、様々な環境を表現可能に
Ø GPS/IMUから得られる位置の縦/横⽅向の誤差を1/10に削減
Ø 本位置推定⼿法により、狭い道路や、交通量が多い実環境での完全⾃動運転を実現
今後の課題
Ø ⾼さ情報の活⽤
例: レイトレーシングによる、移動物体の検出・除去
23