基于地图标记的多模式融合定位方法及系统与流程

本发明涉及自动驾驶定位，更为具体地说是指基于地图标记的多模式融合定位方法及系统。

背景技术：

1、在自动驾驶领域，考虑到运营场景的复杂性，通常基于多种车载传感器，例如激光雷达、imu、rtk、轮速计等，进行多传感器融合定位，以满足不同场景下的定位需求。在特征单一或者缺失的场景下，激光定位容易产生误匹配，影响定位输出，对于存在建筑物、树木遮挡的场景，rtk定位精度可能下降。因此，结合运营场景选择对应的定位模式尤为重要，需要保障定位的稳定性，实现多场景连续定位。

2、在定位模式选择和切换过程中，需要解决两个问题：如何选择定位模式以及如何实现定位切换。为了实现多场景定位，需要在系统中实现并集成多种定位方案，以满足不同场景的定位需求；结合运营场景对运营路线进行划分，在不同场景下选择合适的定位模式进行定位。考虑定位切换问题，需要保障切换过程中定位输出的连续性，不能产生异常跳变，防止对规划和控制产生干扰，导致车辆出现异常行为。

3、授权公告号为cn 120141445a的中国发明专利公开了一种自动驾驶多传感器融合定位系统及方法，系统包括融合定位模块、激光雷达点云定位模块、组合惯导定位模块、里程计模块、前视摄像头模块、地图模块。通过综合判断组合惯导模块定位状态、激光雷达点云定位模块状态、前视摄像头模块识别车道线数据、里程计模块数据、地图模块数据，自动切换至适应当前运行场景的定位模式状态；定位模式状态包括：状态①、纯组合惯导定位；状态②、纯激光雷达点云定位；状态③、组合惯导定位为主定位系统且激光雷达点云定位做校准系统；状态④、组合惯导定位且车道线校准；状态⑤、激光雷达点云定位且车道线校准。然而，该专利的定位是根据实际运行实时切换，可能会受到实际场景影响而不够稳定，若受到环境干扰或者单次检测异常导致错误切换将会降低运营的稳定性。

技术实现思路

1、为解决以上问题，本发明提供一种基于地图标记的多模式融合定位方法及系统。

2、本发明采用如下技术方案：

3、基于地图标记的多模式融合定位方法，包括以下步骤：

4、s10、获取车辆的点云数据、加速度、角速度及车速信息；

5、s20、基于加速度和角速度信息对当前激光雷达点云进行畸变校正，积分得到两帧点云之间的位置和姿态改变量，进行帧间点云匹配，获得里程计估计结果，得到关键帧的定位信息；记录组合导航解状态，基于固定解状态下的位置和姿态信息为关键帧添加全局约束，基于优化后的关键帧信息将关键帧点云拼接得到点云地图；

6、s30、根据建图时记录的组合导航解状态信息，对运营路线进行初步划分和标记，对于较长时间出现其他解状态的路段，标记为激光定位路段，其余路段标记为融合定位路段；在后续的运营过程中监控定位状态，判断是否需要修改地图标记；

7、s40、在定位过程中实时读取地图标记，切换至对应的定位模式；在激光定位中基于线性运动估计得到匹配初始值，基于ndt算法将当前帧点云和点云地图进行匹配，得到激光定位结果；在组合导航定位中，基于rtk和imu输出utm坐标和车辆当前姿态估计结果；在融合定位中，基于航迹推算结果进行车辆运动估计，基于激光定位和组合导航定位结果进行观测更新，得到融合定位结果；

8、s50、在定位模式切换的过程中，对比切换前后定位的位置和姿态信息差异，当位置和姿态差异都在设置阈值范围内，基于平滑系数输出定位信息；当位置或者姿态差异超过阈值时，停车并上报异常；

9、s60、实时监控不同定位模式的定位状态，并将定位状态实时上报到云平台用于后台监控和分析；

10、s70、根据运营过程上报的定位状态优化运营路线标记。

11、一种优选的实施方案，上述步骤s60中不同定位模式的定位状态根据以下进行划分：激光定位基于匹配值、迭代次数、匹配时间划分可用、警告、错误三种状态，组合导航定位中根据解状态分为固定解、浮点解和其他解状态；融合定位根据观测数据源，将状态分为激光和组合导航观测、激光观测、组合导航观测、航迹推算四种状态。

12、一种优选的实施方案，上述步骤s70的运营路线标记优化方法如下：当激光定位状态长时间为错误状态时，该路段标记为组合导航定位路段；当组合导航状态长时间为其他解状态时，该路段标记为激光定位路段；当激光状态大部分为可用或者警告状态，组合导航存在部分其他解状态时，该路段标记为融合定位路段。

13、本发明还提供一种基于地图标记的多模式融合定位系统，用于实现以上多模式融合定位方法，包括：

14、数据采集模块，基于激光雷达采集车辆周围点云数据，基于组合导航设备采集车辆的加速度、角速度数据，基于车速传感器采集车辆的当前速度；

15、建图模块，基于激光雷达和imu构建点云地图关键帧，使用组合导航进行全局约束，优化关键帧的位置和姿态，基于关键帧点云和位姿信息，拼接得到点云地图；

16、激光定位模块，基于建图模块构建的点云地图，将当前车辆扫描得到的激光点云数据和地图数据进行匹配，得到在地图坐标系下的定位信息，进一步转换到utm坐标系下输出定位结果；

17、组合导航定位模块，基于imu和rtk进行组合导航定位，输出utm坐标系下的定位信息；

18、融合定位模块，基于航迹推算进行车辆运动估计，将激光定位、组合导航定位输出作为观测信息，输出融合定位结果；

19、定位状态监控和上报模块，实时监控激光定位、组合导航定位、融合定位状态，将状态信息实时上报给云平台用于后台监控，进一步优化运营路线标记；

20、定位模式管理模块，实时读取地图标记信息，根据地图标记切换定位模式，读取不同定位模式的输出信息，输出定位结果供以上其余模块使用。

21、一种优选的实施方案，上述建图模块基于imu数据去除当前帧点云数据的运动畸变，基于imu测量得到的加速度和角速度信息，估计两帧激光点云之间的位置和姿态改变量，并进行点云的帧间匹配，得到里程计估计结果；将组合导航固定解状态下位置和姿态信息作为全局约束，基于图优化对关键帧的位置和姿态进行优化。

22、一种优选的实施方案，上述激光定位模块是基于ndt算法将当前车辆扫描得到的激光点云数据和地图数据进行匹配，得到当前点云在地图坐标系下的定位结果，结合地图起始点在utm坐标系下的初始偏置，将地图坐标系下的定位信息转换到utm坐标系下，获得全局定位信息。

23、进一步地，基于激光线性运动估计推算当前帧激光定位的初始状态，作为ndt算法匹配的初始值，进行当前帧点云和点云地图的匹配，计算激光定位的匹配值、迭代次数、匹配时间用于分析激光定位状态。

24、一种优选的实施方案，上述组合导航定位模块，基于车端和基站接收的卫星信号，进行差分运算，得到位置坐标，imu实现当前车辆姿态估计，将信息融合后输出utm坐标和车辆姿态。

25、一种优选的实施方案，上述融合定位模块，基于imu和车速数据进行航迹推算，对车辆的运动进行高频估计，将激光定位和组合导航定位输出的位置和姿态信息作为观测信息，根据定位状态赋予不同的融合权重，修正车辆运动估计结果，得到融合定位输出。

26、一种优选的实施方案，上述定位状态监控和上报模块根据激光定位匹配值、迭代次数、匹配时间，计算当前激光定位状态，分为可用、警告、错误三种状态；组合导航状态分为固定解、浮点解和其他解状态；融合定位状态分为激光和组合导航观测、激光观测、组合导航观测、航迹推算四种状态；根据定位状态优化运营路线标记：当激光定位状态长时间为错误状态时，该路段标记为组合导航定位路段；当组合导航状态长时间为其他解状态时，该路段标记为激光定位路段；当激光状态大部分为可用或者警告状态，组合导航存在部分其他解状态时，该路段标记为融合定位路段；其余情况根据不同定位模式的状态分布和比例确定地图标记。

27、一种优选的实施方案，上述定位模式管理模块，实时读取地图标记并切换至对应的定位模式，将切换前后定位的位置和姿态信息进行对比，根据位置和姿态差异执行对应的策略，具体如下：当定位模式切换前后的位置和姿态差异都在设置阈值范围内，设置平滑系数，平滑定位输出；当位置或者姿态差异超过阈值时，停车并上报异常，防止输出跳变信息影响车辆规划控制。

28、由上述对本发明的描述可知，和现有技术相比，本发明具有如下优点：

29、1、本发明集成多模式定位方案，包含激光定位、组合导航定位、融合定位，基于建图时的组合导航解状态初步划分并标记运营路线，持续监控并上报运营过程中激光定位、组合导航定位、融合定位情况，进一步优化运营路线标记，基于地图标记实现定位模式切换，实现多场景连续定位，提高定位稳定性。

30、2、本发明基于激光雷达和组合导航进行融合建图，在建图过程中使用组合导航固定解下的定位信息进行全局约束，构建全局点云地图。记录地图起始点在utm坐标系下的初始偏置，将激光定位的局部坐标转换到utm坐标系下，获得车辆的全局定位信息，使得在激光定位、组合导航定位、融合定位三种定位模式下，都能获取全局坐标系下的定位信息，且保持全局一致性，保证定位模式切换过程的连续性。