[发明专利]一种GPS激光融合SLAM中坐标系动态对齐方法

权利要求书

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中坐标系动态对齐方法的步骤。