Page 1
定位與地圖建構課程模組
智慧聯網應用無人載具人才培育先期計畫
台北科技大學機械系許志明
109. 06. 09
Chapter III
SLAM離線測試與分析
Chapter IV
實際場域地圖建置
NTUT校園地圖建置
NTUT無人工廠地圖建置
SLAM精度評估
預期達成目標:在實際場域中建立地圖並且學會如何評估結果的好壞
利用SLAM核心演算法執行bag file 做離線測試
利用ROS工具以及Matlab離線分析錄製bag file的成果
預期達成目標:用離線錄已製好的bag檔案測試SLAM演算法的可行性並運用工具分析bag
file
規劃時數:3hr
規劃時數:8hr
校園街景圖 校園街景點雲圖
校內無人工廠地圖
SLAM 模組課程
Page 2
內 容
同步定位與地圖構建概念
同步定位與地圖構建模組課程大綱
3D同步定位與地圖構建技術
同步定位與地圖構建離線分析
實際場域同步定位與地圖構建測試
2
Page 3
同步定位與地圖構建概念
3
同步定位與地圖構建
(Simultaneous Localization And Mapping,SLAM)
從未知環境的未知地點出發,在運動過程中通過重複觀測
到的地圖特徵定位自身位置和姿態
根據自身位置增量式的構建地圖,從而達到同時定位和地
圖構建的目的
Page 4
同步定位與地圖構建概念
4
同步定位與地圖構建
(Simultaneous Localization And Mapping,SLAM)
是一個雞生蛋蛋生雞的問題
精確的定位需要用到一個無偏差的地圖
但這樣的地圖又需要精確的位置估測來繪製
將兩方面的演算合在一個循環中;不同演算中相互疊代的
反饋以增進雙方的演算連續解的提升。
Page 6
學習課程大綱
6
Chapter ISLAM相關背景知識
機率網格地圖
掃描匹配技術
最佳化方法
迴環偵測
預期達成目標:學習者可以了解
SLAM的核心數學理論
SLAM 模組課程
規劃時數:4hr
高斯濾波
非參數濾波
預期達成目標:學習者可以了解SLAM的核心數
學理論
Chapter II 核心SLAM演算法
Gmapping演算法流程
粒子濾波
改善提議分佈
選擇性重新採樣
預期達成目標:熟悉Gmapping演算法流程
規劃時數:2hr
Hector演算法流程
機率網格地圖
地圖存取
掃描匹配
預期達成目標:熟悉Hector演算法流程
Google cartographer演算法流程
子地圖
閉環
掃描匹配
預期達成目標:熟悉
Cartographer演算法流程
規劃時數:2hr
規劃時數:2hr
3D SLAM 技術
ICP SLAM NDT SLAM
Feature-based SLAM
預期達成目標:熟悉3D SLAM
技術演算法流程
規劃時數:3hr
Page 7
學習課程大綱
7
Chapter III
SLAM離線測試與分析
Chapter IV
實際場域地圖建置
NTUT校園地圖建置
NTUT無人工廠地圖建置
SLAM精度評估
預期達成目標:在實際場域中建立地圖並且學會如何評估結果的好壞
利用SLAM核心演算法執行bag file 做離線測試
利用ROS工具以及Matlab離線分析錄製bag file的成果
預期達成目標:用離線錄已製好的bag檔案測試SLAM演算法的可行性並運用工具分析bag
file
規劃時數:3hr
規劃時數:8hr
校園街景圖 校園街景點雲圖
校內無人工廠地圖
SLAM 模組課程
Page 8
學習課程目標
8
以無人載具定位感知技術之實際應用場域為學習標的
專案式導向學習(Project-Based Learing)方法
藉由使用以ROS為基礎的主流SLAM 開源程式範例
展開相關主題理論和核心技術之學習
以實用場域導向教學,重點式地進行布局以補足人才
培育之缺口,以利後續發展實際產業鏈結。
Page 9
3D同步定位與地圖構建技術
智慧聯網應用無人載具人才培育先期計畫
9
Page 10
10
Iterative Closest Point SLAM (迭代最近點)傳統使用的點雲配對法,速度較慢,精度普通
Normal Distributions Transform SLAM(正態分佈變換)近期推出的定位演算法,速度較ICP快且精度更高
Feature-based SLAM (基於特徵方法)LOAM(光達掃描邊緣與平面定位),KITTI Benchmark 排名第一演算法
LeGO-LOAM(影像化計算邊緣與平面定位)速度快,可達到與LOAM相當的精度
3D同步定位與地圖構建技術
Page 11
迭代最近點(ICP)
11
ICP演算法:最小化兩筆點雲資料的差異
藉由最小化兩筆點雲資料之誤差計算出轉移矩陣(R,T)
透過Cauchy Robust調整權重
(R,T)𝒙𝒙 = 𝒙𝒙𝟏𝟏,𝒙𝒙𝟐𝟐,⋯ ,𝒙𝒙𝒌𝒌
𝒑𝒑 = 𝒑𝒑𝟏𝟏,𝒑𝒑𝟐𝟐,⋯ ,𝒑𝒑𝒌𝒌
Page 13
迭代最近點(ICP)
13
𝐸𝐸(𝑹𝑹,𝑻𝑻) = ⁄1 𝑁𝑁𝑃𝑃�𝑖𝑖=1
𝑁𝑁𝑃𝑃
𝑤𝑤𝑖𝑖 𝑥𝑥𝑖𝑖 − 𝑹𝑹𝑝𝑝𝑖𝑖 − 𝑻𝑻 2
𝑅𝑅:旋轉矩陣 𝑻𝑻:平移矩陣 NP:參考點雲數量 𝑤𝑤𝑖𝑖:權重值(標準ICP的權重值為1)
Cauchy Robust的加權式ICP :
計算誤差的中位數:𝐾𝐾𝑟𝑟𝑟𝑟𝑟𝑟 = median 𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒 × 𝐾𝐾
其中𝐾𝐾 = 8.1776調整權重植𝑤𝑤𝑖𝑖
𝑤𝑤𝑖𝑖 = ⁄1 (1 + ( ⁄𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒 𝐾𝐾𝑟𝑟𝑟𝑟𝑟𝑟))2
標準ICP 加權式ICP
Page 14
迭代最近點(ICP)
14
當前掃描 參考掃描
點雲濾波 點雲濾波
應用當前姿態轉換
點雲匹配,建立對應關係
最小化誤差計算
檢查轉換是否收斂
定位結果輸出
否
是
-60 -40 -20 0 20 40 60-50
0
50
100
-60 -40 -20 0 20 40 60-50
0
50
100
Page 17
正態分佈變換(NDT)
17
Biber, P., and W. Strasser. "The Normal Distributions Transform: A New Approach to Laser Scan Matching." Intelligent Robots and Systems Proceedings. 2003.
將參考點雲所佔的空間劃分成指定大小(CellSize)的網格或體素(Voxel)計算每個網格的多維正態分佈參數
多維正態分佈參數
Page 18
正態分佈變換(NDT)
NDT流程
18
𝑠𝑠𝑠𝑠𝑒𝑒𝑒𝑒𝑒𝑒 𝑝𝑝 = �𝑖𝑖
exp(−(𝑥𝑥𝑖𝑖′ − 𝑝𝑝𝑖𝑖)𝑇𝑇(Σ𝑖𝑖)−1(𝑥𝑥𝑖𝑖′ − 𝑝𝑝𝑖𝑖)
2)
𝑥𝑥𝑖𝑖′ = 𝑅𝑅𝑥𝑥𝑖𝑖 + 𝑡𝑡
Page 19
19
-60 -40 -20 0 20 40 60-50
0
50
100
參考點雲
正態分佈變換(NDT)
Page 20
20
欲匹配的點雲
正態分佈變換(NDT)
Page 21
21
-60 -40 -20 0 20 40 60-50
0
50
100
匹配結果
正態分佈變換(NDT)
Page 22
以特徵為基礎SLAM
LeGO-LOAM流程
22
地面分割
特徵提取位姿估計地圖建構
點雲轉換 點雲分群
Lidar Odometry And Mapping (LOAM)
Lightweight and Ground Optimized LOAM (LeGO-LOAM)
Page 24
以特徵為基礎SLAM (LeGO-LOAM)
地面分割
24
a. 地面分割前的點雲
b. 地面分割後的點雲,地面點雲以紅色標示
Page 25
以特徵為基礎SLAM (LeGO-LOAM)
點雲分群
25
θHorizontal scan NHorizontal scan N+1
Segmented Points
Outlier points Ground points
Raw point cloud
Point cloud group < 30 < 10θ 。
Outlier
Segmented
Page 26
以特徵為基礎SLAM (LeGO-LOAM)
特徵提取
26
Roughness of point
C > edge Threshold = edge (green)
C < plane Threshold = edge (pink)
Edge
Plane
Page 27
以特徵為基礎SLAM (LeGO-LOAM)
位姿估計
27
yawθpitchθ
rollθ
xt
yt
zt
Page 28
以特徵為基礎SLAM (LeGO-LOAM)
地圖建構
28
異常點
地面點雲
分割點雲 位姿估計 地圖建構
回環檢測
回環檢測(loop closure detection)當來到曾經造訪之掃描環境的特徵時,可以重新校正目前的位姿,以降低SLAM累積誤差
Page 29
Adaptive LeGO-LOAM
演算法流程
29
地面分割
自適應特徵提取
位姿估計地圖建構
點雲轉換自適應
點雲取樣和分群
Page 30
ALeGO-LOAM
自適應點雲分群
30
短距離點雲群自適應點雲取樣與分群
相同物體位於不同距離使用不同門檻值
異常點異常點點雲群點雲分群
相同物體位於不同距離使用相同門檻值
遠距離點雲群中距離點雲群
Page 31
ALeGO-LOAM
自適應點雲取樣和分群
31
= 即時運算點雲數量的極限值rN= 取樣點雲數pcS
原始點雲
地面點
自適應特徵提取
異常點自適應點雲取樣與分類
min pc rN S N< <
分類點雲pcS是
否
minN = 特徵提取所需最小點雲數量
Page 32
ALeGO-LOAM 自適應特徵提取
32
Roughness of point
遠距離特徵
短距離邊緣門檻值seC C>
短距離平面門檻值spC C<
中距離邊緣門檻值meC C>
中距離平面門檻值mpC C<
遠距離邊緣門檻值feC C>
遠距離平面門檻值fpC C<
近距離特徵 中距離特徵
邊緣門檻值eC C> 平面門檻值pC C<
特徵提取誤判情形
自適應特徵提取
Page 33
ALeGO-LOAM
33
自適應點特徵提取
min
min
e e
p p
F F
F F
N N
N N
>
>
特徵點雲
是
否
分類點雲
Lidar Odometry
pcS
eF pF
= 平面特徵篩選數pFN= 邊緣特徵篩選數
eFN
= 平面特徵定位所需下限值
= 邊緣特徵定位所需下限值mineFN
minpFN自適應特徵提取
Page 34
ALeGO-LOAM
34
3D街景圖 Lidar點雲圖
實車測試結果
Page 35
ALeGO-LOAM
35
街景圖
Lidar點雲圖
Page 36
ALeGO-LOAM
36
實車測試結果
Page 37
同步定位與地圖構建離線分析
智慧聯網應用無人載具人才培育先期計畫
37
Page 38
目錄
資料集介紹
演算法於資料集上的效能評估
38
Page 39
資料集介紹
KITTI 資料集錄製設備
39
1. InertialNavigation System (GPS/IMU): OXTS RT 3003
2. Laserscanner: Velodyne HDL-64E
3. Color cameras, 1.4 Megapixels:
Point Grey Flea 2 (FL2-14S3C-C)
Page 40
資料集介紹
KITTI 資料使用流程
40
KITTI資料庫
相機0
光達
IMU
相機1
相機2
相機3
感測器座標
ROSBAGKitti2Bag
ToolRos topic
Topic name
/image_raw0
/points_raw
/IMU_raw
/image_raw1
/image_raw2
/image_raw3
transform
Page 41
資料集介紹
KITTI Benchmark Vision Odometry
41
場景 00 矮房市區
kitti data 00
Page 42
資料集介紹
KITTI Benchmark Vision Odometry
42
場景 01 高速公路
kitti data 01
Page 43
資料集介紹
KITTI Benchmark Vision Odometry
43
場景 05 矮房市區
kitti data 05
Page 44
資料集介紹
KITTI Benchmark Vision Odometry
44
場景 07 矮房市區經過樹林
kitti data 07
Page 45
演算法於資料集上的效能評估
KITTI 00場景
45
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAM
MAE = Mean absolute error
MAE and run time
MAE Time
NDT-Mapping 3.11m 3.99s
Lego-Loam 2.52m 0.145s
ALego-Loam 1.88m 0.098s
起點
Page 46
演算法於資料集上的效能評估
KITTI 00場景
46
LeGO-LOAMNDT-MappingALeGO-LOAM
ALeGO-LOAM LeGO-LOAM NDT-Mapping
Page 47
演算法於資料集上的效能評估
KITTI 00場景
47
LeGO-LOAM
LeeGo-LOAMLeGO-LOAM
(X) Fixed points cloud sampling(X)Fixed Feature Extraction
Runtime 0.15s
ALeGo-LOAM(O)Adaptive Feature Extraction
Runtime 0.098s
(O) Adaptive points cloud sampling
Page 48
演算法於資料集上的效能評估
KITTI 00場景-建圖情形
48
Page 49
演算法於資料集上的效能評估
KITTI 01場景
49
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAM
MAE and run time
MAE Time
NDT-Mapping 43.9m 1.48s
Lego-Loam 359.8m 0.045s
ALego-Loam 55.2m 0.091s
MAE = Mean absolute error
起點
Page 50
演算法於資料集上的效能評估
KITTI 01場景
50
LeGO-LOAMNDT-MappingALeGO-LOAM
ALeGO-LOAM LeGO-LOAM NDT-Mapping
MAE Time
NDT-Mapping 43.9m 1.48s
Lego-Loam 359.8m 0.045s
ALego-Loam 55.2m 0.091s
Page 51
演算法於資料集上的效能評估
KITTI 01場景
51
Edge過少導致 發生偏移(綠色點)
足夠的Edge提供給 定位(綠色點)
, ,x y yawt t θ , ,x y yawt t θ
Page 52
演算法於資料集上的效能評估
KITTI 01場景-原始演算法與自適應比較(影片)
52
Page 53
演算法於資料集上的效能評估
KITTI 01場景建圖情形
53
Page 54
演算法於資料集上的效能評估
KITTI 05場景
54
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAMMAE and run time
MAE Time
NDT-Mapping 1.72m 1.52s
Lego-Loam 0.60m 0.096s
ALego-Loam 0.34m 0.098s
MAE = Mean absolute error
起點
Page 55
演算法於資料集上的效能評估
KITTI 05場景
55
LeGO-LOAMNDT-MappingALeGO-LOAM
Page 56
演算法於資料集上的效能評估
KITTI 05場景建圖情形
56
Page 57
演算法於資料集上的效能評估
KITTI 07場景
57
Ground Truth
LeGO-LOAM
NDT-Mapping
ALeGO-LOAM
MAE and run time
MAE Time
NDT-Mapping 1.14m 1.14s
Lego-Loam 0.62m 0.070s
ALego-Loam 0.33m 0.096s
MAE = Mean absolute error
起點
Page 58
演算法於資料集上的效能評估
KITTI 07場景
58
ALeGO-LOAM LeGO-LOAM NDT-Mapping
LeGO-LOAMNDT-MappingALeGO-LOAM
Page 59
演算法於資料集上的效能評估
KITTI 07場景建圖情形
59
Page 60
演算法於資料集上的效能評估
演算法分別於 KITTI場景的效能
60
00 01 05 07
NDT-Mapping 3.99s 1.48s 1.52s 1.14s
LeGO-Loam 0.145s 0.045s 0.096s 0.070s
ALeGO-Loam 0.098s 0.091s 0.098s 0.096s
Tabel 1. MAE on the four sequence(m)
Tabel 2. Runtime average of odometry on the four sequence
Real-Time OdometryLower than 0.1s
00 01 05 07
NDT-Mapping 3.11 43.9 1.72 1.14
Lego-Loam 2.52 359.8 0.60 0.62
ALeGO-Loam 1.88 55.2 0.34 0.33
Page 61
實際場域同步定位與地圖構建測試
智慧聯網應用無人載具人才培育先期計畫
61
Page 62
實際場域同步定位與地圖構建測試
實際場域介紹
演算法於實際場域上的效能評估
62
Page 63
實際場域介紹
室內場域-校園智能工廠
63
Page 64
實際場域介紹
室內場域-校園智能工廠-實驗設備介紹
64
nanoPi M4
RPLidar A1
Page 65
實際場域介紹
室內場域-校園智能工廠
65
實驗一 實驗二
沿磁軌行走軌跡
Page 66
實際場域介紹
室外場域-大佳河濱公園-實驗設備介紹
66
實驗場域
RTK(NovAtel OEM617D)Velodyne VLP-16
X-sens MTI-10
Logitech Camera
實驗設備
Page 67
實際場域介紹
室外場域-大佳河濱公園
67
實驗一
實驗二
Page 68
演算法於實際場域上的效能評估
68
校園智能工廠-實驗一標地物尺寸分析
Page 69
演算法於實際場域上的效能評估
69
校園智能工廠-實驗一軌跡定向分析
Page 70
演算法於實際場域上的效能評估
70
校園智能工廠-實驗二標地物尺寸分析
Page 71
演算法於實際場域上的效能評估
71
校園智能工廠-實驗二軌跡定向分析
Page 72
演算法於實際場域上的效能評估
72
Ground TruthLeGO-LOAMNDT-MappingALeGO-LOAM
MAE(m)
Time(s)
NDT-Mapping 0.29 1.1Lego-Loam 0.31 0.02ALego-Loam 0.23 0.07
時速約 20km/hr
大佳河濱公園-實驗一
Page 73
演算法於實際場域上的效能評估
73
大佳河濱公園-實驗一建圖情形
Page 74
演算法於實際場域上的效能評估
74
大佳河濱公園-實驗二
Ground TruthLeGO-LOAMNDT-MappingALeGO-LOAM
MAE(m)
Time(s)
NDT-Mapping 0.52 1.1sLego-Loam 0.33 0.02sALego-Loam 0.28 0.07s
時速約 50km/hr
Page 75
演算法於實際場域上的效能評估
75
大佳河濱公園-實驗二建圖情形
Page 76
演算法於實際場域上的效能評估
76
大佳河濱公園-實驗二建圖情形
點雲地圖物件與實際物件之量測值比對圖
Page 77
77
謝謝 !敬請指教 !
[email protected]
定位與地圖建構課程模組
台北科技大學機械系許志明