[發明專利]一種基于激光和二維碼融合的即時定位與地圖構建方法

有效

• 申請號: 201811110672.2
• 申請日: 2018-09-21
• 公開（公告）號: CN109211251B
• 公開（公告）日: 2022-01-11
• 發明（設計）人: 肖烜;覃梓雨;王晨;劉康妮;劉俐伶
• 申請（專利權）人: 北京理工大學
• 主分類號: G01C21/32
• 分類號: G01C21/32;G01C21/34
• 代理公司: 北京理工大學專利中心 11120
• 代理人: 郭德忠;李愛英

1.一種基于激光和二維碼融合的即時定位與地圖構建方法，應用于無人移動平臺，其特征在于，所述無人移動平臺包括處理模塊、單目相機、激光雷達、里程計以及慣性測量單元；

所述方法包括以下步驟：

S1：所述單目相機采集地圖待構建區域上預先鋪設的二維碼，其中，所述二維碼包含表征二維碼在地圖待構建區域上的坐標的ID信息；

S2：所述處理模塊對二維碼進行解碼，得到二維碼在地圖待構建區域上的坐標，再根據二維碼的坐標得到無人移動平臺在世界坐標系下的初始全局坐標；

S3：將無人移動平臺在世界坐標系下的全局坐標、無人移動平臺的航向角、無人移動平臺的速度以及無人移動平臺的角速度作為聯邦卡爾曼濾波器的狀態向量；其中，所述全局坐標由步驟S2中的初始全局坐標以及里程計測量的無人移動平臺的行程與速度確定；

S4：根據所述狀態向量構建聯邦卡爾曼濾波器的狀態方程，根據里程計的觀測模型、慣性測量單元的觀測模型以及單目相機的觀測模型構建聯邦卡爾曼濾波器的觀測方程；

S5：根據聯邦卡爾曼濾波算法對所述狀態方程與觀測方程進行求解，得到所述狀態向量的最優解；

S6：構建粒子濾波即時定位與地圖構建算法的運動狀態轉移方程與運動觀測方程，其中，當無人移動平臺的速度小于設定閾值、無轉彎或者運動路徑上存在障礙物時，所述運動狀態轉移方程的控制向量為激光雷達對地圖待構建區域進行特征匹配得到的位姿，當無人移動平臺的速度不小于設定閾值、有轉彎或者運動路徑上不存在障礙物時，所述運動狀態轉移方程的控制向量為所述狀態向量的最優解中包含的位姿；其中，所述位姿為所述全局坐標與無人移動平臺的航向角；

S7：對所述運動狀態轉移方程與運動觀測方程進行迭代求解，得到地圖待構建區域的全局地圖，具體為：

S701：將所述運動觀測方程轉換為似然函數；

S702：將地圖待構建區域平均劃分多個柵格，采用激光雷達掃描地圖待構建區域的局部區域，將局部區域中存在障礙物的柵格設置為被占用，將局部區域中沒有障礙物的柵格設置為可通行，從而得到局部柵格地圖，其中，局部柵格地圖作為初始全局地圖；

S703：根據步驟S6中運動狀態轉移方程的控制向量生成建議分布函數以及步驟S2中無人移動平臺在世界坐標系下的初始全局坐標，預測下一時刻無人移動平臺可能出現的位置坐標；

S704：利用步驟S701的似然函數獲取步驟S703中下一時刻無人移動平臺可能出現的位置坐標的權重；

S705：將下一時刻無人移動平臺可能出現的位置坐標的加權和，作為下一時刻無人移動平臺最有可能出現的位置坐標，并將下一時刻無人移動平臺最有可能出現的位置坐標對應的局部地圖更新步驟S702中的初始全局地圖；

S706：將下一時刻無人移動平臺最有可能出現的位置坐標代替步驟S703中無人移動平臺在世界坐標系下的初始全局坐標，重新執行步驟S703～S705，以此類推，直到得到地圖待構建區域的全局地圖。

2.如權利要求1所述的一種基于激光和二維碼融合的即時定位與地圖構建方法，其特征在于，所述根據二維碼的坐標得到無人移動平臺在世界坐標系下的初始全局坐標，具體為：

通過計算機視覺的方法獲取二維碼中心點到單目相機鏡頭中心點的變換矩陣，單目相機鏡頭中心點到無人移動平臺中心點的變換矩陣；

根據二維碼在地圖待構建區域上的坐標以及兩個變換矩陣，得到無人移動平臺在世界坐標系下的初始全局坐標。