[发明专利]一种结合GPS和雷达里程计的SLAM方法

摘要

一种结合GPS和雷达里程计的SLAM方法，包括如下步骤：1)采集差分GPS数据和来自激光雷达的点云数据；2)处理GPS数据获得位移(X,Y,Z)和姿态RPY角；3)匹配GPS数据和LiDAR的点云数据，通过时间戳对齐的方式实现数据匹配；4)结合步骤2)处理GPS得到的位姿数据和LiDAR的点云数据检验GPS数据的可靠性；5)使用雷达里程计算法LOAM获取(X,Y,Z)和RPY角；6)在GPS数据可靠的地方，使用GPS获取的位姿作为最终的位姿；在GPS数据不可靠的路段，利用该路段起点和终点的GPS位姿优化LOAM算法的位姿来获取最终的位姿；7)使用步骤6)输出的位姿转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图。本发明适用于大范围城市三维地图的构建。

主权项

1.一种结合GPS和雷达里程计的SLAM方法，其特征在于，所述SLAM方法包括如下步骤：1)数据采集使用差分GPS采集经纬高、RPY角、时间戳数据，RPY角包括Roll‑滚转角、Pitch‑俯仰角和Yaw‑偏航角；使用激光雷达LiDAR采集点云数据和时间戳；2)处理GPS数据获得位移(X,Y,Z)和姿态RPY角(X,Y,Z)为LiDAR起始位置到LiDAR当前位置的位移,RPY角表示的是LiDAR当前位置的姿态，其中，Z即高度差直接从差分GPS数据中获取LiDAR当前位置和初始位置的高度数据并求差得到，X，Y的值通过将LiDAR当前位置和初始位置的经纬度数据转换到UTM平面坐标系下并求差获得，RPY角直接从差分GPS数据中获取；(X,Y,Z)的计算如下：(X,Y,Z)＝(X末,Y末,Z末)‑(X初,Y初,Z初)其中，(X末,Y末,Z末)代表的是当前雷达的位置，(X初,Y初,Z初)代表的是初始雷达的位置；3)匹配GPS数据和LiDAR的点云数据通过时间戳对齐的方式实现数据匹配；GPS采集的数据的时间戳TimeGPS是周秒，激光雷达采集的数据的时间戳TimeLiDAR是距离最近的整点的秒数；此外，TimeGPS和TimeLiDAR存在一个18秒的闰秒差；为了实现时间戳格式的统一，对TimeGPS做预处理，记预处理后的GPS的时间戳为TimeGPSL:TimeGPSL＝TimeGPS％3600‑184)结合步骤2)处理后的GPS数据和LiDAR的点云数据检验GPS数据的可靠性对于LiDAR采集到的连续的两帧数据F1，F2，先用处理后的GPS数据将其转换到世界坐标系下，记转换后的点云为FW1，FW2，接着，使用LOAM算法的特征点提取方法提取FW1，FW2的角点和面点，记FW1角点和面点的数量为C2，S2；使用LOAM算法中的对应关系匹配方法在FW2的特征点中找F1的特征点的对应关系，记找到对应关系的特征点数量为C1，S1；计算找到对应关系的角点和面点的数量占比R1，R2:如果R1，R2，C2，S2都大于给定的阈值，那么认为GPS数据是可靠的；5)雷达里程计的方法获取RPY角和位移(X,Y,Z)利用激光雷达获取点云数据，使用高精度的雷达里程计LOAM算法获取RPY角和(X,Y,Z)；6)位姿融合在GPS数据可靠的地方，使用GPS获取的位姿作为系统最终的位姿；在GPS数据不可靠的路段，先将该路段起点的由GPS得到的位姿数据传递给LOAM算法作为初值，使用LOAM算法获得该路段每一帧点云的位姿T1，由RPY角和位移(X,Y,Z)信息计算得到的变换矩阵，然后，将上述位姿和该路段终点的由GPS得到的位姿数据作为输入，输入到ELCH算法中，获得每一帧点云位姿的累积误差T2，最后使用T2优化来自雷达里程计的位姿，记优化后的位姿为T3；T3＝T2*T17)获取最终的全局三维地图使用6)得到的融合后的位姿T4转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图；记点云中某个点在雷达坐标系下的坐标为P1，转换到世界坐标系下的坐标为P2；P2＝T4*P1。