[发明专利]一种融合边缘信息的定位方法

说明书

本发明公开了一种融合边缘信息的定位方法，属于道路领域。S1、边缘检测：S2、边缘约束：S3、定位算法：本发明直接将边缘检测的结果与定位算法联合，提高边缘处定位精度的同时，有效避免左右摇摆问题，同时还可以减小规划难度。本发明将边缘约束直接纳入定位算法中，直接改善定位结果；边缘约束是在每次迭代过程中通过查找最近邻点的方式得到。

技术领域

本发明涉及道路领域，具体是一种融合边缘信息的定位方法。

背景技术

定位是无人驾驶的核心，是确定车辆与场景间坐标关系的核心模块。如何保证定位的平滑与高精度，是定位所要解决的关键问题。目前各种传感器融合算法层出不穷，无人车的定位问题早已解决，但在一些局部场景特定应用下，目前的定位问题仍有问题。

在平整的地面上，无人车的定位可以看成是二维定位问题，主要是求解平面坐标(X,Y)与偏向角Yaw的过程，在实际解求过程中，认为三个值相互独立，误差在各向独立分布。

在无人车沿边作业时(比如无人清扫车的沿边作业)，在垂直边缘的方向、也是垂直车辆的方向(车辆Y方向)上要求极为严格，些许偏差就可能导致车辆撞向边缘。

在室外园区场景中边缘多为马路崖(一般高在15-20cm)，在定位过程中这类特征相对较弱而被忽略，但实际上在沿此边作业时，此边的特征性应被加强用于定位的约束中。

目前的技术中，不会将边缘信息与定位算法融合，而是在规划过程考虑边缘信息，这样会导致沿边时不平滑，容易出现左右摇摆的情况。

发明内容

对于现有的产生的问题，本发明的目的在于提供一种融合边缘信息的定位方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种融合边缘信息的定位方法，对每帧点云其步骤如下：

S1、边缘检测：

通过地图获取待检测的边缘，此边缘为地图中的要素，其坐标系也是地图坐标系；

将地图中边缘通过定位(里程计推算得到)转换到激光雷达坐标，提取出距离转换边一定距离内的所有点(正负0.5m内)；

边缘处沿高度呈现Z字型折弯。根据此特征探测边缘点，并将点通过标定参数转换到车辆坐标系下；

对于直线边缘采用直线方程拟合：Ax+By+C＝0

对于曲线边缘采用曲线方程拟合：AX2+BXY+CY2+DX+EY+F＝0

最终无论是直接还是曲线都将其转换为折线表达。

S2、边缘约束：

地图中的边缘线Lmap与点云中提取出的边缘Llidar之间的坐标转换关系即为定位参数T。

Lmap与Llidar是由连续的拆线构成的，将其离散化采样(采样间隔0.1m)，构成连续的点组成的两个点集Pmap、Plidar。

假设Xmap、Xlidar为点集Pmap、Plidar中对应的最近点，则其满足约束：

RXlidar+t＝Xmap，其中T＝(R,t)

S3、定位算法：

考虑将边缘约束添加到定位算法中，以迭代最近点(IterativeClosestPoints,ICP)为例进行说明。

ICP算法是常用的点云配准算法，将当前帧点云与点云地图进行配准，可以求解车辆的位置与姿态，因此也是定位中常用的定位算法之一。