該系列主要為對前期工作進行梳理，以后所進行的工作也會部分在此記錄。

使用NDT(正態分布變換)進行點云建圖和定位

前言

定位模塊是自動駕駛最核心的模塊之一，定位又包括全局定位和局部定位，對于自動駕駛，其精度需要達到厘米級別。本文我們將討論全局定位，即確定無人車在全局下的位置。

傳統的AGV使用一類SLAM(simultaneous localization and mapping)的方法進行同時建圖和定位，但是該方法實現代價高，難度大，難以應用到自動駕駛領域。自動駕駛車輛行駛速度快，距離遠，環境復雜，使得SLAM的精度下降，同時遠距離的行駛將導致實時構建的地圖偏移過大。因此，如果在已有高精度的全局地圖地圖的情況下進行無人車的定位，將極大的簡化該問題。

因此，將問題分為獨立的兩部分：建圖Mapping和定位Matching。NDT是一種點云配準算法，可同時用于點云的建圖和定位。

概要

本文包含以下內容:NDT配準的原理

應用:NDT-Mapping

NDT-Matching

1. NDT配準的原理

以下是一張用于配準的target_map,即已經建好的點云地圖

以下是實時掃描到的一幀點云

以下是兩幅點云進行配準之后的結果,中間輸出的坐標軸為當前位置(x y z Y P R)

通過不斷的比對實時掃描到的點云和已經建好的全局點云地圖,我們就可以持續獲得我們當前的位置。ICP(迭代最近點)等配準算法通過對所有的點或者提取的特征點進行匹配配準以確定當前的位置,但是這樣就有一個問題:我們所處的環境是在不斷變化的,比如樹木的稀疏程度,或者環境中車輛及行人的移動,乃至固有的測量誤差,這些都會導致我們實時掃描到的點云與已建立的點云地圖有些許的差別,從而導致較大匹配誤差。

而NDT可以在很大程序上消除這種不確定性。NDT沒有計算兩個點云中點與點之間的差距,而是先將參考點云(即高精度地圖)轉換為多維變量的正態分布，如果變換參數能使得兩幅激光數據匹配的很好，那么變換點在參考系中的概率密度將會很大。因此，可以考慮用優化的方法,比如牛頓法,求出使得概率密度之和最大的變換參數，此時兩幅激光點云數據將匹配的最好。

可以這樣來做一個通俗的理解:NDT把我們所處的三維世界按照一定長度的立方體(比如30cm30cm30cm)進行了劃分,類似于一個魔方,每個立方體內并不是存儲一個或一些確切的點,而且存儲這個立方體被占據的概率密度。當接收到需要匹配的點云時,也按照這樣的劃分方式進行劃分,然后進行配準。

因此,NDT具有以下的特征:支持更大的地圖,更稠密的點云 --因為最終還是要劃分成voxel的形式

相比于ICP等基于點的匹配算法,速度更快

更加容忍環境的細微變化

2. 應用

得益于如今眾多的開源算法,我們不必重復造輪子了。

AutoWare是由日本名古屋大學和Tier IV主導的全棧開源自動駕駛系統,其core_percepetion模塊中對ndt_mapping和ndt_localization進行了很好的實現。本文將從Autoware的這兩個package入手,先梳理其架構和代碼,再對其進行修改,以適配本地環境,如TX2。

2.1 NDT_Mapping

以下是NDT_Mapping的結構圖(注:不包含imu和odom,對imu和odom信息的融合將在另一篇TODO文章里進行)NDT_Mapping結構

輸入點云處理

1.1 截取有效范圍

對于激光雷達,其過近的點由于落在車體上,過遠的點已經非常稀疏,因此都需要被過濾掉。

1.2 降采樣

// Apply voxelgrid filter pcl::VoxelGrid<:pointxyzi> voxel_grid_filter;

voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); // 室外可以稍微大些,如0.5-2.0;室內需要小一些 voxel_grid_filter.setInputCloud(scan_ptr);

voxel_grid_filter.filter(*filtered_scan_ptr); // filtered_scan_ptr接收降采樣后的點云

注:對于第一幀點云,需要直接加入map中,作為初始target_map點云

2. 設置NDT

ndt.setTransformationEpsilon(trans_eps); // 兩次變換之間允許的最大值,用于判斷是否收斂,作為迭代計算完成的閾值; =0.01ndt.setMaximumIterations(max_iter); // 最大迭代次數,超過則停止計算; =30ndt.setStepSize(step_size); // 牛頓法優化的最大步長; =0.1ndt.setResolution(ndt_res); // voxel的邊長大小,過小造成內存占用過高,過大會導致誤差偏大; =1.0ndt.setInputSource(filtered_scan_ptr); // 輸入source點云

注:ndt.setInputTarget()在更新global_map的時候進行,即直接將global_map輸入到ndt_target中

3. 進行ndt配準,計算變換矩陣

ndt.align(*output_cloud, init_guess);

output_cloud: 存儲source經過配準后的點云(由于source_pointcloud被極大的降采樣了,因此這個數據沒什么用)

pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer); // 對原始點云進行變換(framd_id: /velodyne->/map)

init_guess: ndt計算的初始估計位置,在不使用gnss/imu/odom的情況下,以上一幀車輛位置的變換矩陣作為init_guess 注:ndt對位置不敏感,通常在3m以內都可以迭代計算過去,但是ndt對角度比較敏感,因此初始初始的角度不能與實際相差過大(最好在±45°之內)

4. 每n米更新一次全局地圖

if (shift >= min_add_scan_shift)

{

map += *transformed_scan_ptr;

// update NDT ndt.setInputTarget(map_ptr);

// publish global_map // ... }

同時使用單獨的線程,按照一定的頻率進行地圖的保存。

最終效果:NDT_Mappinghttps://www.zhihu.com/video/1143966982750638080

備注：

每幀掃描到的點云中,落在地面上的點云大概占30%。一般來講,由于地面作為一個平面其相似性很強,因此這部分點云對定位的作用是不大的,可以考慮去除。但是有另一個因素需要考慮,即地面上的點云對抑制z軸漂移還是有很大作用的,因此,在NDT_Mapping中進行配準時保留地面,但在NDT_Matching中配準時使用去除地面的點云。

2.2 NDT_Matching

以下是NDT_Matching的結構圖(同樣不包含對imu和odom數據的融合,該數據融合部分會在另一篇中討論)NDT_Matching結構

可以看出NDT_Matching的邏輯還是很簡單清晰的,即不斷的接收實時掃描到的點云,以及異步更新target_map,并使用NDT算法不斷進行配準獲取當前位置。

此處接收的點云需要進行去地面處理,因此我們將新建一個節點進行點云去除地面的操作,NDT_Matching將訂閱該節點發布的去除地面后的點云。關于如何有效去除地面將在TODO該篇中討論。

此處的target_map是不斷更新的,即使用一個單獨的節點進行地圖加載的操作,該節點訂閱/current_pose,并按照一定的步長讀取/更新匹配用target_map。關于如何有效的進行target_map管理將在自動駕駛系列：點云地圖后處理和地圖動態加載該篇中討論。

由于NDT_Matching和NDT_Mapping極為相似,主要更新target_map上不同,因此NDT_Matching的代碼不在此處展開了。直接進行定位的視頻展示。NDT_Matchinghttps://www.zhihu.com/video/1143969149075226624

以下是我們最新改裝的小車https://www.zhihu.com/video/1143969296932868096

總結

以上是生活随笔為你收集整理的ndtmapping建图_自动驾驶系列：激光雷达建图和定位(NDT)的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。