激光定位精度簡單測試

目的

　　利用LEGO-LOAM這種SLAM方法建立環境地圖，然後用NDT（OpenMP版）方法進行定位，通過與GPS數據對比計算激光定位的精度。

1　準備工作

　　測試在工廠環境中進行，其中既有開闊地帶，也有樹蔭遮擋的狹窄走廊，既有結構化特徵好的區域，也有樹木較多的非結構化區域。選擇百度Apollo無人車作爲試驗平臺，其上搭載了衛星定位傳感器和16線激光雷達。用差分衛星定位的結果作爲基準測試激光雷達定位精度，爲此提前用激光跟蹤儀對衛星定位精度進行測試，其最大位置誤差是4cm。

2　激光建圖

　　用Velodyne 16線激光雷達以10Hz頻率採集環境點雲數據，同時採集衛星定位數據，衛星輸出頻率也爲10Hz，保存爲rosbag，隨後利用LEGO-LOAM建立地圖。打開LEGO-LOAM的閉環檢測設置項（extern const bool loopClosureEnableFlag = true）。mapOptmization.cpp文件中的濾波器分辨率都設置爲0.1，關鍵幀間隔設置爲0.2（在saveKeyFramesAndFactor函數中）。LEGO-LOAM建立的地圖座標系原點就是開始時刻激光雷達的位置，但是LEGO-LOAM建立的地圖座標系不是Z軸向上，所以要轉換一下。
　　我沒有辦法測量建圖的精度，所以只能看最終的定位精度。

3　衛星數據座標轉換

　　衛星定位傳感器的輸出是WGS84座標系中的數據，包括經緯度、高度和偏航角等等，爲了對比，要與激光地圖統一到一個座標系。方法是：先將數據轉化爲地心地固座標系：WGS84$\rightarrow$ECEF，然後再將地心地固座標系轉化爲局部切平面中的座標系：ECEF$\rightarrow$Ground。ECEF和Ground座標系都是直角座標系。最後這一步將衛星座標系的原點設置爲開始時刻的位置，角度則爲0°。

4　定位結果

　　定位效果如下圖所示，顯示在Rviz中，其中有三個座標系：全局地圖座標系map、激光定位的座標系velodyne、衛星定位的座標系M2。開始時刻三個座標系是重合的。

　　無人車開始移動，velodyne和M2離開了map，如下圖所示。velodyne和M2這兩個座標系重合度還比較高。

　　一段距離後，velodyne和M2逐漸分離，如下圖所示。圖中網格的邊長是1米。

　　最後控制無人車回到起點（地圖的原點）附近，形成一個環形路徑。其目的是利於SLAM實現閉環，消除累積誤差。
　　全程計算兩個座標系velodyne和M2在水平面投影的歐式距離，如下圖所示。無人車總行駛距離500米左右，激光定位與衛星定位的最大距離誤差接近1.3米，平均誤差0.5米。角度誤差沒有計算，但是目測較小。340秒（0.1秒一個數據）以後的一段時間，誤差變化劇烈，此時無人車進入樹蔭中，結構化特徵不明顯。計算誤差時激光雷達和衛星的時間戳沒有嚴格對齊，但是差別不大（最大10ms）。
　　這次試驗初步的結論是，NDT激光定位的角度精度較高，對於位置精度來說，環境結構化程度越好（直線平面等特徵多）則精度越高，在樹木較多的地方精度不好。