[發明專利]一種結合GPS和雷達里程計的SLAM方法

有效

• 申請號: 201811306455.0
• 申請日: 2018-11-05
• 公開（公告）號: CN109507677B
• 公開（公告）日: 2020-08-18
• 發明（設計）人: 張劍華;林瑞豪;吳佳鑫;馮宇婷;徐浚哲;陳勝勇
• 申請（專利權）人: 浙江工業大學
• 主分類號: G01S17/86
• 分類號: G01S17/86;G01S17/89;G01S19/42;G01S19/53;G09B29/00
• 代理公司: 杭州斯可睿專利事務所有限公司 33241
• 代理人: 王利強

1.一種結合GPS和雷達里程計的SLAM方法，其特征在于，所述SLAM方法包括如下步驟：

1)數據采集

使用差分GPS采集經緯高、RPY角、時間戳數據，RPY角包括Roll-滾轉角、Pitch-俯仰角和Yaw-偏航角；使用激光雷達LiDAR采集點云數據和時間戳；

2)處理GPS數據獲得位移(X,Y,Z)和姿態RPY角

(X,Y,Z)為LiDAR起始位置到LiDAR當前位置的位移,RPY角表示的是LiDAR當前位置的姿態，其中，Z即高度差直接從差分GPS數據中獲取LiDAR當前位置和初始位置的高度數據并求差得到，X，Y的值通過將LiDAR當前位置和初始位置的經緯度數據轉換到UTM平面坐標系下并求差獲得，RPY角直接從差分GPS數據中獲取；(X,Y,Z)的計算如下：

(X,Y,Z)＝(X_末,Y_末,Z_末)-(X_初,Y_初,Z_初)

其中，(X_末,Y_末,Z_末)代表的是當前雷達的位置，(X_初,Y_初,Z_初)代表的是初始雷達的位置；

3)匹配GPS數據和LiDAR的點云數據

通過時間戳對齊的方式實現數據匹配；

GPS采集的數據的時間戳Time_GPS是周秒，激光雷達采集的數據的時間戳Time_LiDAR是距離最近的整點的秒數；此外，Time_GPS和Time_LiDAR存在一個18秒的閏秒差；為了實現時間戳格式的統一，對Time_GPS做預處理，記預處理后的GPS的時間戳為Time_GPSL:

Time_GPSL＝Time_GPS％3600-18

4)結合步驟2)處理后的GPS數據和LiDAR的點云數據檢驗GPS數據的可靠性

對于LiDAR采集到的連續的兩幀數據F₁，F₂，先用處理后的GPS數據將其轉換到世界坐標系下，記轉換后的點云為F_W1，F_W2，接著，使用LOAM算法的特征點提取方法提取F_W1，F_W2的角點和面點，記F_W1角點和面點的數量為C₂，S₂；使用LOAM算法中的對應關系匹配方法在F_W2的特征點中找F₁的特征點的對應關系，記找到對應關系的特征點數量為C₁，S₁；計算找到對應關系的角點和面點的數量占比R₁，R₂:

如果R₁，R₂，C₂，S₂都大于給定的閾值，那么認為GPS數據是可靠的；

5)雷達里程計的方法獲取RPY角和位移(X,Y,Z)

利用激光雷達獲取點云數據，使用高精度的雷達里程計LOAM算法獲取RPY角和(X,Y,Z)；

6)位姿融合

在GPS數據可靠的地方，使用GPS獲取的位姿作為系統最終的位姿；在GPS數據不可靠的路段，先將該路段起點的由GPS得到的位姿數據傳遞給LOAM算法作為初值，使用LOAM算法獲得該路段每一幀點云的位姿T₁，由RPY角和位移(X,Y,Z)信息計算得到的變換矩陣，然后，將上述位姿T₁和該路段終點的由GPS得到的位姿數據作為輸入，輸入到ELCH算法中，獲得每一幀點云位姿的累積誤差T₂，最后使用T₂優化來自雷達里程計的位姿，記優化后的位姿為T₃；

T₃＝T₂*T₁

7)獲取最終的全局三維地圖

使用6)得到的融合后的位姿T₃轉換激光雷達的點云數據到世界坐標系下，獲取最終的全局地圖；

記點云中某個點在雷達坐標系下的坐標為P₁，轉換到世界坐標系下的坐標為P₂；

P₂＝T₃*P₁。