robust vehicle localization in urban environments using probabilistic maps

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

Upload: kitsukawa-yuki

Post on 17-Jan-2017

134 views

Category:

Technology


0 download

TRANSCRIPT

Page 1: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

論⽂紹介 (2016.9.7)

Robust Vehicle Localization in Urban Environments

Using Probabilistic Maps

橘川 雄樹[email protected]

名古屋⼤学 ⼤学院情報科学研究科 情報システム学専攻

枝廣研究室 モビリティグループ博⼠後期課程 1年

1

Page 2: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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

Page 3: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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

Page 4: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Introduction

4

Page 5: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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

Page 6: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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

Page 7: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Probabilistic Mapsa. Map alignment Using GraphSLAM

b. Laser Calibration

c. Map Creation

7

Page 8: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Probabilistic Map

Probabilistic Map

LIDARから得られる⾚外線反射強度の平均・分散を各セルに保存

Probabilistic Map 作成⼿順

a. ループクロージングを⾏い、経路のアラインメント

b. LIDARの各ビームのキャリブレーション

c. キャリブレーションされた反射強度を、⾛⾏経路を基にして地図上に投影

8

Page 9: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Map Alignment Using GraphSLAM

GraphSLAM - グラフ構造を利⽤して、地図のループクロージング

GraphSLAMのアルゴリズム

1. ノードの⽣成

2. 拘束の⽣成

3. 拘束の最適化

9

ランドマーク

ノードx1

拘束

ノードx3

ノードx2

拘束

拘束

拘束

ri∑i

ランドマーク

オドメトリ

ロボット

全ての拘束に対するノード位置の同時確率を最⼤化

𝑝 𝒙 =$𝑝(𝒙|𝑐()*+,

(-.

どれだけノードを動かせば拘束を満たすか

本実装では、スキャンデータに対し、ICP(Iterative Closest Point)を⽤いて、拘束を⽣成

Page 10: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Laser Calibration

10

レーザー固有の反射特性による、ビーム間の反射強度の計測値の違いを調整

キャリブレーション前

キャリブレーション後

1. ⾛⾏軌跡からスキャンデータを地図上にマッピング2. 各セル内に属する点の反射強度の平均を計算3. 各ビームの計測値と平均値を対応づけ→ 64(ビーム数)×256(反射強度)の変換表を作成

ビームID 0…63

キャリブレーションができていないと…

ü 反射強度の平均がセルに当たったビームに依存ü 反射強度の分散が実際よりも⼤きくなってしまう

各セル内にマッピングされたスキャンの反射強度の平均値を計算

Page 11: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Map Creation

Probabilistic Mapの作成

1. ⾛⾏軌跡から、キャリブレーションされたスキャンデータをxy平⾯にマッピング

2. 各セル(15cm×15cm)に属するスキャンデータの反射強度の平均と分散を計算

Probabilistic Mapの利点

Ø 影や光に影響されない

Ø 複数のスキャンの平均を取ることにより、移動物体の軌跡が薄くなる(移動物体のデータを消す必要がない)

11

反射強度の平均 反射強度の分散

移動物体(⾞両)の軌跡は消滅 移動物体(⾞両)の通過部分

セルに路⾯と⽩線の境界が含まれているのため分散⼤

Page 12: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Online Localizationa. Motion Update

b. Measurement Update

c. Most Likely Estimate

12

Page 13: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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

Page 14: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

(補⾜)Histogram Filter

Histogram Filter

Ø 空間を等間隔に分割し、それぞれの領域での存在確率を計算

Ø 領域の数が増えると、計算時間増加

14

x

⾞両がいる確率

x

⾞両がいる確率

x

⾞両がいる確率

移動後の予想

センサデータを⽤いた推定

上2つの合成

参考http://www.slideshare.net/minoruchikamune/car-20140829

Page 15: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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 𝑥, 𝑦 = 𝜂 56𝑃 𝑖, 𝑗 5 exp −12 𝑖 − 𝑥 ? 𝑗 − 𝑥 ?/𝜎?

(,C

15

(補⾜)正規分布(ガウス分布)

平均: 𝜇, 分散: 𝜎?の時

𝑓 𝑥 =12𝜋𝜎?� exp −

𝑥 − 𝜇 ?

2𝜎?

移動後(x,y)にいる確率 (i,j)にいる確率 (i,j)→(x,y)に移動する確率

Page 16: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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)

Page 17: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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以上の誤差) 提案⼿法による位置推定

Page 18: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Experimental Resultsa. Quantitative Results

b. Autonomous Success

18

Page 19: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Experimental Results

評価環境

Ø C⾔語で実装

Ø ⼀般的なラップトップを使⽤

Ø 地図データのサイズは10MB/Mile

19

センサ 製品名 周波数

GPS/IMU Applanix LV-420 200 Hz

LIDAR Velodyne HDL-64E 10 Hz

実験⾞両⾚外線反射地図(セル: 15cm×15cm)

Page 20: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & 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)よりも⼩さい

Page 21: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Autonomous Success

Ø 本⼿法 - 実環境における完全⾃動運転が可能に

• マンハッタンでのデモンストレーション

• キャンパス周辺 4マイル の経路 (狭い道路、窮屈な交差点、交通量が多い…)

Ø Previous Work - 精度が⼗分でないため、交通量が少なく、幅が広い道路に限定

21

キャンパス周辺のコース

Page 22: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Conclusion

22

Page 23: Robust Vehicle Localization in Urban Environments Using Probabilistic Maps

Nagoya UniversityParallel & Distributed Systems Lab.

Conclusion

まとめ

Ø 確率モデル(平均・分散)を⽤いた⾚外線反射地図により、様々な環境を表現可能に

Ø GPS/IMUから得られる位置の縦/横⽅向の誤差を1/10に削減

Ø 本位置推定⼿法により、狭い道路や、交通量が多い実環境での完全⾃動運転を実現

今後の課題

Ø ⾼さ情報の活⽤例: レイトレーシングによる、移動物体の検出・除去

23