[發明專利]一種基于車載慣性的激光雷達與視覺組合導航算法

有效

• 申請號: 201911193672.8
• 申請日: 2019-11-28
• 公開（公告）號: CN110849362B
• 公開（公告）日: 2022-01-04
• 發明（設計）人: 陳友生
• 申請（專利權）人: 湖南率為控制科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/32;G01S17/87
• 代理公司: 北京慕達星云知識產權代理事務所(特殊普通合伙) 11465
• 代理人: 曹鵬飛

1.一種基于車載慣性的激光雷達與視覺組合導航方法，其特征在于，包括如下具體步驟：

步驟1：利用張正友標定法對車載的深度相機進行標定，獲得標定相機；

步驟2：對車載的慣導系統進行初對準，獲得對準矩陣；

步驟3：采用車載的激光雷達進行角點掃描，獲得雷達信息，根據所述對準矩陣獲得所述慣導系統的慣導信息，對所述慣導信息進行慣導解算獲得慣導定位信息，對所述雷達信息和所述慣導定位信息進行匹配定位和卡爾曼濾波，獲得激光雷達與慣導濾波參數，實現所述激光雷達與所述慣導系統的組合導航定位，獲得激光雷達組合導航定位信息；

步驟4：利用所述標定相機采集載體車行駛過程中的視頻，對所述視頻進行處理后與所述慣導系統的所述慣導定位信息進行深度融合，獲得深度視覺與慣導濾波參數，實現對所述慣導定位信息的校正與定位，獲得視覺組合導航定位信息；

步驟5：將深度視覺與慣導濾波參數、激光雷達與慣導濾波參數通過卡爾曼濾波算法進行互補濾波，獲得互補濾波參數，對所述激光雷達組合導航定位信息進行優化；

步驟6：根據所述互補濾波參數得到的慣性、所述激光雷達組合導航定位信息與所述視覺組合導航定位信息對所述慣導系統進行反饋校正，抑制所述慣導系統的發散，獲得精準定位結果，同時根據所述精準定位結果繪制所述載體車行駛過程的軌跡點云圖并保存；

步驟7：根據優化后的參數對所述慣導系統獲得的所述慣導定位信息進行校正后，進入所述步驟3，繼續進行所述慣導解算，為下一時刻定位做準備。

2.根據權利要求1所述的一種基于車載慣性的激光雷達與視覺組合導航方法，其特征在于，所述步驟1中對所述深度相機進行標定的過程為：

步驟11：打印一張間距已知的棋盤格紙張，并固定貼覆于平板上；

步驟12：利用所述深度相機采集所述棋盤格紙張若干圖片；

步驟13：對所述圖片檢測特征點；

步驟14：根據所述特征點采用解析解估算方法計算出內部參數和外部參數；

步驟15：根據極大似然估計策略對所述內部參數和所述外部參數進行優化，獲得優化參數。

3.根據權利要求2所述的一種基于車載慣性的激光雷達與視覺組合導航方法，其特征在于，所述步驟2中對所述慣導系統進行初對準的具體步驟為：

步驟21：根據所述載體車所在位置的緯度L、重力加速度g、地球自轉角速度w_ie和載體車運動引起的相對地球旋轉角度w_eg，計算出捷聯慣導矩陣

步驟22：根據所述載體車的陀螺儀輸出與加速度計輸出的比力值f^b，計算出所述載體車的平臺誤差角并構造矩陣

步驟23：根據所述矩陣對所述捷聯慣導矩陣進行修正，完成初對準獲得對準矩陣

4.根據權利要求3所述的一種基于車載慣性的激光雷達與視覺組合導航方法，其特征在于，所述步驟3中實現所述激光雷達與所述慣導系統組合導航定位的過程為：

步驟31：采用所述激光雷達進行角點掃描，并計算所述載體車的粗估計位置，確定匹配序列x₁,x₂,…，x_n；

步驟32：根據所述慣導系統的所述對準矩陣通過慣導解算獲得慣導解算軌跡，從而確定匹配搜索區域和慣導匹配序列；

步驟33：令所述匹配序列x₁,x₂,…，x_n根據所述慣導匹配序列y₁,y₂,…,y_n在數字地圖中的所述匹配搜索區域內進行遍歷搜索，采用MAD算法獲得最小距離度量；

步驟34：根據所述慣導解算軌跡獲得慣導位置信息，將所述慣導位置信息和所述最小距離度量的差采用卡爾曼濾波進行優化，獲得激光雷達與慣導濾波參數，以及所述激光雷達與所述慣導系統的所述激光雷達組合導航定位信息。

5.根據權利要求4所述的一種基于車載慣性的激光雷達與視覺組合導航方法，其特征在于，所述步驟4中通過所述深度相機實現對所述慣導系統的慣導定位信息進行校正與定位的過程為：

步驟41：通過標定后的所述深度相機獲得所述載體車行駛過程中的視頻；

步驟42：根據所述深度相機的所述優化參數對所述視頻進行關鍵幀圖片提取，獲得所述關鍵幀圖片的Kinect信息，并求出平移估計矩陣T和旋轉估計矩陣R；

步驟43：根據所述平移估計矩陣T求出所述載體車的視覺測量位置信息，根據所述旋轉估計矩陣R求出所述載體車的視覺測量姿態信息；

步驟44：所述加速度計通過所述初對準，經過所述慣導解算獲得慣導速度信息，所述陀螺儀通過所述初對準，經過所述慣導解算獲得所述慣導位置信息；對所述視覺測量位置信息、所述視覺測量姿態信息、所述慣導速度信息和所述慣導位置信息進行深度融合，獲得融合信息；

步驟45：對所述融合信息進行卡爾曼濾波，獲得深度視覺與慣導濾波參數，對所述慣導定位信息進行校正與定位，獲得所述視覺組合導航定位信息。