[发明专利]利用半径搜索优化激光雷达定位的方法

说明书

本发明属于道路网络导航技术领域，提供一种利用半径搜索优化激光雷达定位的方法，利用多线程编程方案，在另外的线程中，以输入GPS坐标为中心，在半径为R，高度为H的范围内，利用方法F生成N个位姿参数，不停的尝试匹配，直到满足设定的匹配精度S后结束。本发明针对车辆所需要定位的位置，通过GPS粗略坐标，在数据库中查询当地的道路分布和车辆可达区域信息，在生成预测位姿的时候，过滤掉车辆不可达的位姿，可大大减少预测位姿的数量，从而提高匹配精度和速度。

技术领域

本发明涉及自动驾驶领域的无线电导航，具体涉及一种利用半径搜索优化激光雷达定位的方法。

背景技术

车辆定位是自动驾驶系统中非常重要的一个环节，用于确定汽车在高精度地图中的朝向和位置。车辆定位有多种方式，其中使用激光雷达点云进行匹配是比较常见的方式，NDT-正态分布变换（Normal Distributions Transform，NDT），以下称ndt，是一种常见的激光雷达点云匹配算法，该算法原理是将激光雷达产生的目标点云和高精度点云地图信息转换为多维变量的正态分布，通过计算分布概率，若两者匹配度较高，则认为扫描点云和地图位姿相匹配，ndt虽然有效率高，可离线计算的优势，但由于算法所限，对于同样的激光雷达点云，会在高精度地图中存在多个可能的匹配位姿点，为了提高定位的准确度，需要将激光雷达点云的粗略位姿作为输入传递给ndt，目前通用的做法中，此姿态由GPS定位给出，但由于GPS精度和漂移问题，会导致每次给出的预估姿态不尽相同，从而导致有可能长时间无法达到所需要的匹配精度，甚至出现车辆定位出现丢失的情况。

发明内容

发明目的：本发明的目的在于提供一种优化的方法，提高ndt预测位姿的准确度，从而保证ndt定位的准确性和稳定性。

技术方案：为解决上述技术问题，本发明提供的利用半径搜索优化激光雷达定位的方法，包括以下步骤：

步骤1，初始化，建立多个搜索线程。

步骤2，通过GPS获取的粗略坐标，在可达区域和道路数据库中查询当地的道路分布和车辆可达区域信息；

步骤3，结合车辆状态，过滤掉车辆不可达的位姿，在数据库中搜索最优的位姿点生成函数F，在线程运行时，F产生位姿点P，生成预测位姿；

步骤4，利用P作为初始位姿对激光雷达点云与既有的点云地图进行ndt匹配；

步骤5，判断达到设定精度S后搜索线程暂停，若未达到设定进度S则返回步骤3继续运行。

具体地，在步骤2中，根据车载GPS获取的车身基本位置，利用地图提供商，百度高德等或者自建地图服务查询得到车身周围的可能道路信息和可能的行驶区域。获取道路信息后，根据车载IMU或者其他传感器获取车身朝向信息，筛选出当前状态下车辆最可能行驶/停靠的道路和区域。

所述步骤3中查询结果产生后的自适应算法函数F的伪代码如下：

{

…

Results = QueryByGPS() //根据车辆GPS查询当前可能的道路列表

foreach (lane in Result) //循环当前的道路列表

{

A = Euler(lane) //获取道路朝向欧拉角

B = Euler(vehicle)//获取车辆朝向欧拉角

if (abs(A-B) N) //如果道路朝向角和车辆朝向角偏差小于N

List.Add(lane) //则将当前道路加入可选列表

}