本发明涉及计算机视觉技术，尤其是一个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-18
4)结合步骤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*t1
7)获取最终的全局三维地图
使用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＝time1对应的gps数据就是和它相匹配的数据；
4)结合lidar的点云数据匹配的步骤2)处理后的gps数据检验gps数据的可靠性
对于lidar采集到的连续的两帧(lidar自转一圈采集到的数据)数据f1，f2，先用处理后的gps数据将其转换到世界坐标系下，记转换后的点云为fw1，fw2，然后提取fw1，fw2的特征点，接着在fw2中寻找fw1的特征点的对应关系，如果fw1，fw2有足够多的特征点且有足够多的fw1的特征点找到了对应关系则认为gps数据可靠，否则，gps数据不可靠；
使用loam(lidarodometryandmappinginreal-time)算法的特征点提取方法提取fw1，fw2的角点和面点，记fw1角点和面点的数量为c2，s2；使用loam算法中的对应关系匹配方法在fw2的特征点中找f1的特征点的对应关系，记找到对应关系的特征点数量为c1，s1。计算找到对应关系的角点和面点的数量占比r1，r2：
如果r1，r2，c2，s2都大于给定的阈值，那么认为gps数据是可靠的，c2，s2足够大说明fw1，fw2有足够多的特征点，r1，r2足够大则说明足够多的fw1的特征点找到了对应关系；
5)雷达里程计的方法获取rpy角和位移(x,y,z)
使用雷达里程计loam获取rpy角和位移(x,y,z)；
仅将激光雷达的点云数据作为输入，使用高精度的雷达里程计loam算法获取rpy角和(x,y,z)；
6)位姿融合
在gps数据可靠的地方，使用gps获取的位姿作为系统最终的位姿；在gps数据不可靠的路段，优化步骤5)得到的位姿作为最终位姿；
在gps数据不可靠的路段，先将该路段起点的由gps得到的位姿数据传递给loam算法作为初值，使用loam算法获得该路段每一帧点云的位姿t1(由rpy角和位移(x,y,z)信息计算得到的变换矩阵)，然后，将上述位姿和该路段终点的由gps得到的位姿数据作为输入，输入到elch(anexplicitloopclosingtechniquefor6dslam)算法中，获得每一帧点云位姿的累积误差t2，最后使用t2优化来自雷达里程计的位姿，记优化后的位姿为t3；
t3＝t2*t1
7)获取最终的全局三维地图
使用步骤6)得到的融合后的位姿t4转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图；
记点云中某个点在雷达坐标系下的坐标为p1，转换到世界坐标系下的坐标为p2；
p2＝t4*p1。