一种基于激光反射强度的无人车实时定位方法与流程

本发明涉及一种无人车实时定位方法，尤其是涉及一种基于激光反射强度的无人车实时定位方法。

背景技术：

随着计算机技术和人工智能的发展，无人驾驶汽车(以下简称无人车)在军事、交通、工业生产、物流仓储、日常生活等方面展现出巨大的应用前景。在国防军事方面，无人车多用于执行危险场景下的军事任务，如军事救援和物资输送等。在交通安全方面，无人驾驶技术是促进智能交通系统发展的有效手段，基于人工智能的无人驾驶技术可以提高车辆行驶的主动安全性，能够有效减少驾驶员由于误操作导致的交通事故，从而提高交通行驶效率和安全性。在工业生产、物流仓储方面，无人车可以配合自动化生产线实现全自主无人生产，进一步推进工业生产的自动化和智能化，进而提高生产效率。另外，无人车的出现也将极大地方便人们的工作、旅游等日常生活。

无人车定位系统是自动驾驶系统中的一个重要组成部分，许多无人车算法(例如：行为决策、路径规划和车辆运动控制)都依赖于自身车辆的位姿信息。无人车定位系统根据不同的定位需求，分成三个层次：路网级别、介于路网和车道级别之间、车道级别。无人驾驶系统要求的定位系统是车道级别的，即要求的定位精度是厘米级。

可靠和精确的实时定位与导航是无人车安全行驶在城市环境下的基础。在早期的无人车中，高精度GPS融合航位推算算法通常被用作无人车定位系统，然而，在城市环境中，GPS信号会被高耸的建筑物遮挡，缺少足够的可视卫星，或存在信号多径反射，而航位推测传感器(加速度计和陀螺仪等)存在测量噪声，只通过航位推测算法来估计无人车的位姿，会导致定位系统累计误差越来越大，因此，基于GPS融合航位推测算法的定位系统无法保证无人车的长时间和长距离稳定运行。为了克服这个问题，基于高精度地图结合传感器检测的定位算法被提出并应用于无人驾驶系统中。

地图-感知相结合的定位算法主要为基于卡尔曼滤波器的特征层面定位。

基于卡尔曼滤波器(或用于非线性系统状态估计的扩展卡尔曼滤波器)的定位算法是地图-感知相结合定位算法中的一种经典解决方案，从车载传感器(相机、毫米波雷达或激光)提取的特征与高精度地图中的特征进行匹配，根据匹配结果，卡尔曼滤波器更新车辆位姿的均值和协方差。基于卡尔曼滤波器的定位方法具有结构简单，计算复杂度较低等优点。

例如，IBEO激光雷达被应用于高精度地图的构建和车辆定位，根据激光雷达返回的点云构建栅格地图，提取出静态物体作为地标与传感器数据实时匹配，从而计算出无人车的航向角误差和侧向误差，结合扩展卡尔曼滤波器估计无人车位姿。

尽管基于特征层面的定位算法具有结构简单，计算复杂度低等优势，但被应用于复杂环境下的无人车定位和估计非线性系统状态时，存在一些局限性。当无人车行驶在复杂环境下，车载传感器检测的特征点和地图数据关联是一个难点，其原因在于，特征点检测会存在误检和障碍物遮挡等情况，将错误的特征点与地图数据匹配，会导致定位算法失效。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种可以在复杂环境下实现实时精确定位的基于激光反射强度的无人车实时定位方法。

本发明的目的可以通过以下技术方案来实现：

一种基于激光反射强度的无人车实时定位方法，包括以下步骤：

S1，无人车行驶过程中，车载的激光雷达对城市道路两侧进行拍摄，获取多帧点云数据，并进行路沿点提取，将各帧点云数据中的路沿点转换到当前车辆坐标系下；

S2，在步骤S1得到的路沿点中，选取z轴坐标值位于设定范围内的路沿点，利用高斯反投影，获取其在GPS坐标系下的坐标，以无人车当前的GPS坐标点为原点，划分坐标空间得到栅格图，计算其中路沿点的激光反射强度的均值和方差，所述的z轴为竖直方向；

S3，采用滑动窗口搜索法，将栅格化的高精度地图与步骤S2得到的栅格图进行匹配，并计算位置偏差，位置偏差指的是步骤S2的栅格与最匹配的地图栅格之间的偏差，获得无人车在高精度地图上的位置，所述的高精度地图中包含路沿点的激光反射强度信息；

S4，利用卡尔曼滤波器预测车辆位姿。

所述的步骤S2中，统计落入到各栅格中的路沿点个数，选取路沿点个数大于设定数的栅格作为有效栅格，计算其中路沿点的激光反射强度的均值和方差，用于提供步骤S3中的匹配信息。

所述的步骤S3中，构建高精度地图的多个栅格图，利用高斯相似度分别将各栅格图与步骤S2得到的栅格图进行匹配，得到相似度最高的一对，并计算二者的位置偏差，其中，多个栅格图的原点不同。

所述的栅格图的栅格数为200x200，栅格的精度为0.25m。

所述的多个栅格图的x轴偏移步长为0.5m，y轴偏移步长2m，y轴正向为当前航向角方向，x轴为水平方向。

所述的步骤S1中，使用车载的惯性导航系统获取相邻两帧的位移变化量d_x、d_y和航向角变化量d_θ，若第k-1帧的路沿点Q_k-1坐标为(x_k-1,y_k-1)，则第k-1帧的路沿点转化到第k帧坐标系下的坐标为

所述的步骤S4中，当前t时刻无人车的位姿为则t+1时刻的无人车位姿计算式为

其中，为预测的无人车位姿，d_θ为航向角变化量，Δt为时间间隔，v^t为t时刻的车速。

与现有技术相比，本发明具有以下优点：

(1)路沿作为城市道路中用于分隔机动车道和非机动车道，是非常重要的一种环境特征，相比车道线，路沿具有空间上的差异(不是颜色)，因此路沿的检测准确率高，可以满足大部分城市环境下的定位需求。

(2)采用反射强度特征与地图匹配可以弥补路沿特征在纵向方向上定位精度低的缺点。

(3)基于车辆的运动学模型，将多帧路沿坐标变换到同一坐标系下，获得密集的反射强度信息，统计栅格内的路沿点个数，有效剔除误判的路沿点，获取有效栅格，减少计算量。

(4)结合高精度地图，使用区域搜索概率匹配算法，可以实时精确估计车辆的位姿。

(5)栅格的个数和尺寸影响了定位算法的计算复杂度，通过实验的方法选取了200x200大小，精度0.25m的栅格图，可以满足定位精度和计算时间的要求。

附图说明

图1为本发明方法的流程图；

图2为本实施例方法得到的单帧路沿；

图3为本实施例方法得到的多帧路沿；

图4为本实施例系统架构图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例

一种基于激光反射强度的无人车实时定位方法，该方法首先从单帧的激光点云数据中提取出激光反射强度特征，根据车辆运动学模型，将多帧检测的路沿特征点坐标转换到当前无人车坐标系下，然后，利用区域概率匹配搜索算法讲反射强度分布与高精度地图中的反射强度分布匹配，计算出当前无人车的侧向、纵向和航向角偏差作为观测值，输入到卡尔曼滤波器进行位姿估计。本实施例兼具创新性实用性，可以在复杂环境下实现实时精确定位，能够有效提高无人车行驶的安全性。

具体步骤如下：

1.路沿特征点检测

采用Velodyne HDL-32E激光雷达作为环境感知传感器。采用基于点云数据空间特征的路沿检测算法，首先，在大部分城市环境中，路沿的高度是有统一标准的，一般高出路面10-15cm，其次，在笛卡尔坐标系中，路沿的z轴高度变化剧烈。根据这些空间特征，对单帧点云数据进行路沿提取，其检测结果如图2所示。

由于单帧检测的路沿特征点的密度随着检测距离的增加而减小，为了获得一个对路沿的完整描述，需要将多帧检测的路沿转换到同一坐标系下。由于传感器坐标系是随着无人车的运动而改变的，因此，利用车载的惯性导航系统获取相邻两帧车辆的位移和航向角变化量d_x、d_y和d_θ，单帧的路沿特征点表示为Q_k，其中Q_k＝(x_k,y_k)为第k帧的路沿坐标，因此，在当前车辆坐标系下的多帧路沿为：

R＝[Q_k f(Q_k-1) f²(Q_k-2) ... fⁿ(Q_k-n)]

其结果如图3所示。

2.激光点云反射强度图生成

首先对每帧点云的高度信息取z轴小于-1.8m的点云，通过惯导数据保存20帧地面点云的局部坐标数据，利用高斯反投影，将其投影到GPS坐标。

以当前时刻的经纬度和航向角为原点创建200x200的栅格图，探测距离为-25～25m，每个栅格图的精度约等于0.25m。然后将上述转换后的点云GPS坐标投影到栅格图中，计算落入每个栅格的反射强度的均值、方差和点云个数。通过遍历生成的栅格图，计算出落入栅格点的个数大于60的所有栅格坐标，为后面的栅格图匹配提供信息。

3.基于滑动窗口的栅格图匹配

地图匹配的目的是估计出检测到的路沿与高精度地图数据的偏差。滑动窗口搜索法是一种用于估计两个栅格图位置关系的算法。在当前车辆的GPS坐标为原点，航向角为y轴正方向，从地图中取出多个200x200的栅格图(每个栅格图的原点偏移量分别为左右-0.5m～0.5m，前后偏移-2.0m～2.0m，即在左右-0.5～0.5和前后-2.0～2.0内的搜索区域内寻找匹配度最高的位置)。

在每个从地图中取出的栅格图中找出与第2步中相同坐标的所有栅格，每个栅格包括了反射强度的均值和方差。利用高斯相似度公式计算得到两个栅格图直接的相似度，并利用加权算法得到匹配后的位置偏差。

4.定位

经过路沿匹配算法和滑动窗口匹配算法后，获得变换矩阵T，其中θ、t_x、t_y分别为地图匹配得到的航向角、x方向的偏移量和y方向的偏移量，在第二个卡尔曼滤波器中，这三个量作为卡尔曼滤波的观测值。采用两个卡尔曼滤波器对噪声滤波并估计出一个相对精确的位置信息。将t时刻的车辆位置表示为根据车辆运动学预测t+1时刻的车辆位姿：

其中为预测的车辆位姿，Δt为时间间隔。

第一个卡尔曼滤波器用于融合GPS测量值和预测值。第二个卡尔曼滤波器用于融合变换矩阵T和第一个卡尔曼滤波器的输出值。