[發明專利]一種用于室內外場景的人為回環即時定位和建圖方法及系統

在審

• 申請號: 202011411092.4
• 申請日: 2020-12-03
• 公開（公告）號: CN112697158A
• 公開（公告）日: 2021-04-23
• 發明（設計）人: 朱文俊;謝亮亮;易陽;馮李航
• 申請（專利權）人: 南京工業大學
• 主分類號: G01C21/32
• 分類號: G01C21/32
• 代理公司: 南京燦爛知識產權代理有限公司 32356
• 代理人: 吳亞

1.一種用于室內外場景的人為回環即時定位和建圖方法，其特征在于，包括以下步驟：

步驟一，定義位姿關系SLAM初始因子圖；

步驟二，采集機器人定位數據，對第i幀定位數據輸入校正因子，通過EM算法估算校正因子；

步驟三，通過矩陣變換串聯機器人前后變換的位姿，優化校正因子；

步驟四，通過校正因子約束優化機器人位姿，消除累計誤差，得到全局一致性地圖。

2.根據權利要求1所述的一種用于室內外場景的人為回環即時定位和建圖方法，其特征在于：步驟一具體包括以下步驟：

定義機器人在地圖中的初始位姿及其約束關系的初始因子圖G₀＝{X,F}；其中X為機器人初始行駛軌跡上的位姿估計集合，F＝{R}，R表示測量值和觀測值之間的相對位姿。

3.根據權利要求1所述的一種用于室內外場景的人為回環即時定位和建圖方法，其特征在于：步驟二具體包括以下步驟：

(201)，采集機器人定位數據，在機器人定位數據的第i幀輸入用于修正地圖的校正因子；

(202)，實例化一組校正因子，將校正因子添加到初始因子圖中，得到矯正后因子圖G₁＝{X,F∪H}，H表示校正因子，H＝P_a,P_b,S_a,S_b,X_a,X_b,m，P_a,P_b表示繪制的兩條線段a,b的端點集，是實數集，表示的是正實數；S_a,S_b表示由兩個線段a,b分別選擇的觀測值集，S是觀測值；表示從X_1:t中得到的一組位姿X_a和X_b，X_a指線段a所選擇的機器人位姿，X_b是線段b選擇的機器人位姿，X_1:t指的是機器人從1到t時刻的位姿集合；m∈M，m表示當前工作的校正模式，M是校正模式的集合，校正模式包括共點，共線，垂直和平行。

4.根據權利要求3所述的一種用于室內外場景的人為回環即時定位和建圖方法，其特征在于：估算校正因子H具體包括以下幾個步驟：

步驟2001，輸入線段a,b的端點集P_a,P_b并選擇校正模式；

步驟2002，給定初始輸入線段端點和校正模式m，是線段a所選擇的初始輸入端點，是線段b所選擇的初始輸入端點，基于EM算法對輸入端點θ＝{P_a,P_b}做極大似然估計，確定輸入端點θ＝{P_a,P_b}，計算出校正因子H。

5.根據權利要求4所述的一種用于室內外場景的人為回環即時定位和建圖方法，其特征在于：

基于EM算法對輸入端點θ＝{P_a,P_b}做極大似然估計具體包括以下步驟：

將對數似然函數l最大化，

其中θ＝{P_a,P_b}是輸入有效線段的端點，θ^old是使得當前似然期望值達到最大化的參數值θ的前一次迭代值；s_i∈S是觀測值，s_i表示第i幀定位數據的觀測值，變量z_i為觀測值s_i的隱變量；p(z_i|s_i,θ^old)和p(z_i,s_i|θ)的表達式基于正態分布N(μ(θ),σ²)的誤差生成模型，σ是指定點時準確度的標準偏差；μ(θ)是特征的中心。