[發明專利]一種GPS激光融合SLAM中坐標系動態對齊方法

在審

• 申請號: 202110706466.3
• 申請日: 2021-06-24
• 公開（公告）號: CN113343061A
• 公開（公告）日: 2021-09-03
• 發明（設計）人: 袁國斌;柏林;劉彪;舒海燕;宿凱;沈創蕓;祝濤劍;雷宜輝
• 申請（專利權）人: 廣州高新興機器人有限公司
• 主分類號: G06F16/909
• 分類號: G06F16/909;G06F16/29;G06F9/448;G06K9/62;G06F16/2455;G06F16/9035
• 代理公司: 廣州國鵬知識產權代理事務所(普通合伙) 44511
• 代理人: 葛紅

1.一種GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，包括步驟：

S1、獲取GPS數據并進行第一處理，將處理后的GPS數據并保存；

S2、獲取3D激光點云數據并進行第二處理，獲得激光里程計數據并保存；

S3、同步處理后的GPS數據和激光里程計數據；

S4、篩選數據點對；

S5、基于所述數據點對計算航向角，從而實現GPS激光融合SLAM中坐標系的動態對齊。

2.根據權利要求1所述的GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，在步驟S1中，所述獲取GPS數據并進行第一處理，將處理后的GPS數據并保存具體包括，通過第一回調函數接收GPS數據，把第一幀設為GPS坐標系的原點，后續GPS數據轉換到以GPS坐標系下的相對位置，并保存轉換后的數據至緩存。

3.根據權利要求2所述的GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，接收的GPS數據包括經度，緯度，高度，dop值以及時間戳。

4.根據權利要求2所述的GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，在步驟S2中，所述獲取3D激光點云數據并進行第二處理，獲得激光里程計數據并保存具體包括，通過第二回調函數接收3D激光點云數據，經由激光里程計算法計算機器人位置在機器人坐標系下運動軌跡并保存至緩存。

5.根據權利要求4所述的GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，在步驟S3中，所述同步處理具體為，加載GPS緩存數據和激光里程計緩存數據，根據每個數據的時間戳對齊GPS緩存數據值和激光里程計緩存數據值，并保存對齊好的數據點對。

6.根據權利要求5所述的GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，在步驟S4中，根據數據點對的兩點之間運動距離閾值，篩選數據點對。

7.根據權利要求6所述的GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，所述運動距離閾值大于0.3m。

8.根據權利要求6所述的GPS激光融合SLAM中坐標系動態對齊方法，其特征在于，所述航向角的具體計算步驟如下：

a)計算兩組點p_i＝[x_i y_i]^Tq_i＝[a_i b_i]^T的質心位置p、q：

兩組點分別去質心坐標：

p_i＝p_i-p，q_i＝q_i-p

b)基于SVD計算旋轉矩陣：

c)計算航向角：

Yaw＝atan2(R(1,0),R(0,0))

其中，x_iy_i——機器人位置i在GPS坐標系下的水平表示；

a_ib_i——機器人位置i在機器人坐標系下的水平表示。

9.一種計算機可讀存儲介質，其上存儲有計算機程序，其特征在于，該程序被處理器執行時實現權利要求1-8任一所述的GPS激光融合SLAM中坐標系動態對齊方法的步驟。

10.一種計算機設備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，其特征在于，所述處理器執行所述程序時實現權利要求1-8任一所述的GPS激光融合SLAM中坐標系動態對齊方法的步驟。