[發明專利]一種基于相位相關法與因子圖的激光SLAM方法及其可讀存儲介質

在審

• 申請號: 202110689529.9
• 申請日: 2021-06-21
• 公開（公告）號: CN113379841A
• 公開（公告）日: 2021-09-10
• 發明（設計）人: 王冠
• 申請（專利權）人: 上海仙工智能科技有限公司
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/11;G06T5/00;G06F16/29;G01S17/89;G01C21/00
• 代理公司: 北京中濟緯天專利代理有限公司 11429
• 代理人: 季永康

1.一種基于相位相關法與因子圖的激光SLAM方法，其特征在于步驟包括：

S1獲取激光雷達采集的點云數據，判斷是否為初始幀，若判斷為初始幀，則執行初始化地圖的操作；

S2對點云數據做地面分割；

S3對點云數據做插值和濾波計算；

S4對擬合過的點云數據，分別向激光雷達坐標系的x，z兩個方向做投影；

S5對x，z兩個方向上構建平面特征像素圖，采用相位相關法對其頻域進行求解，得到相對定位信息；

S6判斷是否為關鍵幀，若否則返回步驟S1/若是則進入步驟S7；

S7：構建因子圖添加因子，添加絕對位姿因子，優化局部因子圖；

S8判斷是否有回環，若是則優化整體因子圖，消除建圖過程中的累計誤差/若否則更新地圖，然后返回步驟S1。

2.根據權利要求1所述的基于相位相關法與因子圖的激光SLAM方法，其特征在于，步驟S5中相位相關法求解步驟包括：

計算相鄰兩幀像素圖，滿足p_t(x，y)＝p_t-1(xcosθ+ysinθ，-xsinθ+ycosθ)，對兩邊進行傅里葉變換，獲取兩張平面特征像素圖的頻域：

F_t(ξ，η)＝F_t-1(ξcosθ+ηsinθ，-ξsinθ+ηcosθ)其中，ξ，η為離散頻率變量，用來確定x，y方向的頻率；將(ξ，η)轉為極坐標下為滿足

ξcosθ+ηsinθ＝ρcos(α-θ)，-ξsinθ+ηcosθ＝ρsin(α-θ)，

其中，ρ表示軸，α表示角，所以可以的到：

F_t(ρcosα，ρsinα)＝F_t-1(ρcos(α-θ)，ρcos(α-θ))

最終將二維正交坐標系中的平移關系，轉換為極坐標系下的旋轉關系：

F_t(ρ，α)＝F_t-1(ρ，α-θ)求出旋轉關系，之后由求出的旋轉變換還原到圖像p_t(x，y)，

p_t(x，y)＝p_t-1(x，y)+t＝p_t-1(x+x_t，y+y_t)

經過傅里葉變換之后可以得到：

此時兩張平面特征像素圖的互功率譜為：

式中F_t^*(ξ，η)是F_t(ξ，η)的共軛；

互功率譜的相位信息包含了兩張平面特征像素圖的相位差，求其傅里葉反變換得到一個二維沖擊函數δ(u-x_t，v-y_t)，此函數在(x_t，y_t)處取得峰值，其余位置函數值接近0，根據峰值以求出(x_t，y_t)。

3.根據權利要求1所述的基于相位相關法與因子圖的激光SLAM方法，其特征在于，步驟S6中關鍵幀選擇標準包括：平移一定的預設距離、旋轉一定的預設角度、已經經過一段預設時間，中的至少一種。

4.根據權利要求1所述的基于相位相關法與因子圖的激光SLAM方法，其特征在于，步驟S2中地面分割步驟包括：根據高度參數將地面分割出來，再在地面點云中，去除法向量和地面實際法向量區別超出閾值的點，然后對剩下的點進行平面參數擬合。