一种结合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)输出的位姿转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图。本发明专利技术适用于大范围城市三维地图的构建。

【技术实现步骤摘要】
一种结合GPS和雷达里程计的SLAM方法
本专利技术涉及计算机视觉技术，尤其是一个SLAM(simultaneouslocalizationandmapping，同时定位和建图)方法。
技术介绍
SLAM技术是指机器人在一个陌生的环境中，能够同时构建周围环境的地图并定位出自己在该地图中位置的技术。SALM技术由很多的应用，如自动驾驶、机器人的定位和导航等。基于视觉里程计或是雷达里程计的SLAM技术，由于累积误差的存在，无法实现大范围的城市三维地图的构建。基于GPS的SLAM技术虽然没有累积误差，但是在城市地区，由于建筑物遮挡、信号干扰等原因，部分地方无法获得可靠的GPS数据，因而也无法实现大范围的城市三维地图的构建。
技术实现思路
为了实现大范围的城市三维地图的构建,本专利技术提出了一个结合雷达里程计和GPS的SLAM方法。本专利技术解决其技术问题所采用的技术方案是：一种结合GPS和雷达里程计的SLAM(simultaneouslocalizationandmapping，同时定位和建图)方法，所述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采集到的连续的两帧(LiDAR自转一圈采集到的数据)数据F1，F2，先用处理后的GPS数据将其转换到世界坐标系下，记转换后的点云为FW1，FW2，接着，使用LOAM(LidarOdometryandMappinginReal-time)算法的特征点提取方法提取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(AnExplicitLoopClosingTechniquefor6DSLAM)算法中，获得每一帧点云位姿的累积误差T2，最后使用T2优化来自雷达里程计的位姿，记优化后的位姿为T3；T3＝T2*T17)获取最终的全局三维地图使用6)得到的融合后的位姿T4转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图；记点云中某个点在雷达坐标系下的坐标为P1，转换到世界坐标系下的坐标为P2；P2＝T4*P1。本专利技术的技术构思为：在GPS数据可靠的时候，通过GPS获取激光雷达的位姿；在GPS数据不可靠的时候，通过雷达里程计获取GPS的位姿，并利用GPS的位姿对雷达里程计的位姿进行优化。进而实现适用与大范围城市地图构建的SLAM系统。特别地，首先采集GPS数据和点云数据，并对GPS数据进行必要的处理获取我们需要的位姿(位移和姿态)。然后，匹配GPS数据和激光雷达的点云数据。接下来，结合点云数据和来自GPS的数据判定GPS数据的可靠性。接着，通过雷达里程计获得位姿。最后，根据GPS数据可靠性的判定结果对来自GPS的位姿和雷达里程计的位姿进行位姿融合并使用融合后的位姿转换点云得到全局地图。本专利技术的有益效果主要表现在：有效地融合了GPS和雷达里程计，进而实现了一个能够实现大范围城市地图构建的SLAM系统。具体实施方式下面对本专利技术作进一步描述。一种结合GPS和雷达里程计的SLAM(simultaneouslocalizationandmapping，同时定位和建图)方法，包括如下步骤：1)数据采集固定XW-GI5651(差分GPS移动端)和VLP-16LiDAR(一款激光雷达)的相对位置，通过XW-GI5651向VLP-16LiDAR输出GPRMC数据，实现两者在硬件上的时间戳同步，然后采集数据；使用差分GPS采集经度、纬度、高度、RPY角(Roll-滚转角、Pitch-俯仰角和Yaw-偏航角，表示的是激光雷达的姿态)、时间戳(采集数据的时间)；使用激光雷达LiDAR采集点云数据和时间戳。2)处理GPS数据获得位移(X,Y,Z)和姿态RPY角对GPS返回的数据进行处理，得到需要的位移和姿态。(X,Y,Z)为LiDAR起始位置到LiDAR当前位置的位移,RPY角表示的是LiDAR当前位置的姿态；X，Y的值通过将LiDAR当前位置和初始位置的经纬度数据转换到UTM平面坐标系下并求差获得；Z即高度差直接从差分GPS数据中获取LiDAR当前位置和初始位置的高度数据并求差得到，(X,Y,Z)的计算如下：(X,Y,Z)＝(X末,Y末,Z末)-(X初,Y初,Z初)其中，(X末,Y末,Z末)代表的是当前雷达的位置，(X初,Y初,Z初)代表的是初始雷达的位置；RPY角直接从差分GPS数据中获取；3)匹配GPS数据和LiDAR的点云数据我们通过时间戳对齐的方式实现数据匹配GPS采集的数据的时间戳TimeGPS是周秒，激光雷达采集的数据的时间戳TimeLiDAR是距离最近的整点的秒数；此外，TimeGPS和TimeLiDAR存在一个18秒的闰秒差；为了实现时间戳格式的统一，对TimeGPS做预处理，记预处理后的GPS的时间戳为TimeGPSL：TimeGPSL＝TimeGPS％3600-18对于TimeLiDAR＝Time1时刻采集到的点云数据，TimeGPSL＝Ti本文档来自技高网...

【技术保护点】
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:...

【技术特征摘要】
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数据的可靠...

【专利技术属性】
技术研发人员：张剑华，林瑞豪，吴佳鑫，冯宇婷，徐浚哲，陈胜勇，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：浙江,33