CN107015238A - 基于三维激光雷达的无人车自主定位方法 - Google Patents

Abstract

一种基于三维激光雷达的无人车定位算法，步骤1，从单帧的激光点云数据中提取出路沿特征点，根据车辆运动学模型，将多帧检测的路沿特征点坐标转换到当前车辆坐标系下；步骤2，利用Beam model方法提取出多帧的路沿特征点的轮廓；步骤3，将步骤2提取的路沿轮廓与高精度地图数据进行匹配，计算出当前车辆的侧向、纵向和航向角偏差作为观测值，输入到卡尔曼滤波器进行车辆位姿估计。能够保证车辆长时间，长距离的稳定定位。可适用于多种激光传感器，并且也能够获得较高的定位精度。能够有效去除异常点，使得地图匹配更精确，定位精度更高。

Description

基于三维激光雷达的无人车自主定位方法

技术领域

本发明涉及一种无人车自主定位方法。

背景技术

现有技术状况及其存在缺陷

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

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

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

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

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

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

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

发明内容

本发明需要保护的方法技术方案，表征为：

一种基于三维激光雷达的无人车定位算法，其特征在于，

步骤1，从单帧的激光点云数据中提取出路沿特征点，根据车辆运动学模型，将多帧检测的路沿特征点坐标转换到当前车辆坐标系下；

步骤2，利用Beam model方法提取出多帧的路沿特征点的轮廓(创新点)；

步骤3，将步骤2提取的路沿轮廓与高精度地图数据进行匹配，计算出当前车辆的侧向、纵向和航向角偏差作为观测值，输入到卡尔曼滤波器(该卡尔曼滤波器本身已为现有技术)进行车辆位姿估计。

步骤1，从单帧的激光点云数据中提取出路沿特征点，根据车辆运动学模型，将多帧检测的路沿特征点坐标转换到当前车辆坐标系下。

步骤(1.1)路沿特征点检测

具体为：

所述步骤1，利用车载的惯性导航系统获取相邻两帧车辆的位移和航向角变化量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)]

步骤2、多帧的路沿特征点的轮廓提取

首先通过高斯投影(现有技术)，得到局部坐标系下的路径点坐标(x_ini,j,y_ini,j)，j表示第j个路径点。

然后在无人车的每个路径点上设置多个beam model，每个beam model用beam区域来表示(创新点)，beam区域用下式表示：

其中Z_p为第p个beam区域。

在每个beam区域中，计算最近的路沿坐标：

其中r_i＝(x_i,y_i)其是第i个路沿坐标。

将每束Beam遇到的最近的路沿点提取出来，最终，提取出的路沿轮廓为C＝{d_p,p＝1,2,…,n}

步骤3、地图匹配

步骤3.1、将地图数据点集由M表示，采用最近点迭代算法(该算法本身已属于现有技术)的目的是最小化如下代价函数：

J＝∑dist(TC,M)

其中dist为欧式距离函数，

通过以下迭代方法解得：

步骤(3.1.1)在M中找到每个C_i的关联点，通过KD树寻找最近点实现。

步骤(3.1.2)利用奇异值分解方法计算变换矩阵T。

步骤(3.1.3)应用变换：C＝TC，然后计算代价J。

步骤(3.1.4)当代价的改变小于阈值τ时终止迭代。

在迭代过程结束之后，获得了C和M的变换矩阵T：

其中θ、m_x、m_y分别为地图匹配得到的航向角、x方向的偏移量和y方向的偏移量。

步骤3.2、定位

经过上述迭代算法后，获得了变换矩阵T，将t时刻的车辆位置表示为根据车辆运动学模型(现有技术)预测t+1时刻的车辆

位姿：

预测t+1时刻的的车辆位姿表示为：式中Δt为时间间隔。

第一个卡尔曼滤波器用于融合GPS测量值和预测值输出值，提供给第二个卡尔曼滤波器。

第二个卡尔曼滤波器用于融合变换矩阵T和第一个卡尔曼滤波器的输出值，得到最终的无人车位姿。

与现有技术相比，本发明的优点

1.利用三维激光雷达传感器进行环境感知，由于激光雷达是主动探测式传感器，因此，在任何光照条件下都能有效感知环境信息，进行车辆定位。

2.采用高精度地图作为先验知识，使得定位算法没有累计误差，能够保证车辆长时间，长距离的稳定定位。

3.本发明方法采用多帧点云数据技术进行环境检测，可适用于多种激光传感器，并且也能够获得较高的定位精度。

4.使用了beam model算法，能够有效去除异常点，使得地图匹配更精确，定位精度更高。

附图说明

图1激光原始点云

图2单帧路沿

图3多帧路沿

图4 Beam model

图5路沿轮廓

图6地图匹配

图7本发明流程图。

图8是第2步骤流程图。

具体实施方式

基于本发明技术方案，给出实施例和仿真实验。

实施例1

具体步骤如下：

步骤1，从单帧的激光点云数据中提取出路沿特征点，根据车辆运动学模型，将多帧检测的路沿特征点坐标转换到当前车辆坐标系下。

步骤(1.1)路沿特征点检测

本实施例采用的是Velodyne HDL-32E激光雷达作为环境感知传感器，其体积更小，更轻，结构坚固，有32个激光发射和接收装置(市售产品，该设备本身已属于现有技术)。HDL032E具有360度水平视角和41.3度的垂直视角，最大检测距离为70m，精度为2cm。在本实施例中，Velodyne HDL-32E激光雷达安装于车辆顶部。如图1所示，其原始点云坐标为三维极坐标。

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

步骤(1.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、多帧的路沿特征点的轮廓提取

从图3中可以看出，由于路沿检测算法的局限性，在多帧路沿中存在一些异常点，这些异常点通常位于路沿轮廓外侧。Beam model是由同一个起始点发射一系列虚拟beams的方法。在这里，beam model被用于提取路沿轮廓。

由于无人车的轨迹点是由GPS坐标表示的，因此，首先通过高斯投影(现有技术)，得到局部坐标系下的路径点坐标(x_ini,j,y_ini,j)，j表示第j个路径点。

然后在无人车的每个路径点上设置多个beam model，每个beam model用beam区域来表示(创新点)，beam区域用下式表示：

其中Z_p为第p个beam区域。

在每个beam区域中，计算最近的路沿坐标：

其中r_i＝(x_i,y_i)其是第i个路沿坐标。

将每束Beam遇到的最近的路沿点提取出来，最终，提取出的路沿轮廓为C＝{d_p,p＝1,2,…,n}，得到图5所示的结果。图4蓝色的线即为发射的Beam(已有文字指示)。

具体的算法流程如图8所示。

第2步骤的原理是，本发明采用Beam model是用来提取路沿轮廓的，这个算法有一个参数，就是发射虚拟Beam的原点，这个原点在本发明算法里就是车子行驶的轨迹点，由于GPS返回的车子的轨迹点是GPS全局坐标，所以用“高斯投影”的方法转换到以当前车子为原点的笛卡尔坐标，就是(x_ini,j,y_ini,j)所示，后面Zp表示每个虚拟Beam的区域，是一个关于坐标x,y的区域，x,y的关系满足Zp右边的不等式。最后就是计算出在每个beam区域中，距离原点最近的路沿点，把它提取出来，得到路沿轮廓C。

步骤3、地图匹配

步骤3.1、地图匹配的目的是估计出检测到的路沿与高精度地图数据的偏差。最近点迭代是一种用于估计两个点云集变换关系的算法。这里将地图数据点集由M表示，本发明采用最近点迭代算法(该算法本身已属于现有技术)的目的是最小化如下代价函数：

J＝∑dist(TC,M)

其中dist为欧式距离函数，

优化上述式子，通过以下迭代方法解得：

步骤(3.1.1)在M中找到每个C_i的关联点，通过KD树寻找最近点实现。

步骤(3.1.2)利用奇异值分解方法计算变换矩阵T。

步骤(3.1.3)应用变换：C＝TC，然后计算代价J。

步骤(3.1.4)当代价的改变小于阈值τ时终止迭代。

在迭代过程结束之后，获得了C和M的变换矩阵T：

其中θ、m_x、m_y分别为地图匹配得到的航向角、x方向的偏移量和y方向的偏移量。

步骤3.2、定位

经过上述迭代算法后，获得了变换矩阵T，采用了两个卡尔曼滤波器对噪声滤波并估计出一个相对精确的位置信息。将t时刻的车辆位置表示为

根据车辆运动学(现有技术)预测t+1时刻的车辆位姿：

预测t+1时刻的的车辆位姿表示为：式中Δt为时间间隔。

第一个卡尔曼滤波器用于融合GPS测量值和预测值输出值，提供给第二个卡尔曼滤波器。

第二个卡尔曼滤波器用于融合变换矩阵T和第一个卡尔曼滤波器的输出值，得到最终的无人车位姿。

Claims (2)

1.一种基于三维激光雷达的无人车定位算法，其特征在于，

步骤1，从单帧的激光点云数据中提取出路沿特征点，根据车辆运动学模型，将多帧检测的路沿特征点坐标转换到当前车辆坐标系下；

步骤2，利用Beam model方法提取出多帧的路沿特征点的轮廓；

步骤3，将步骤2提取的路沿轮廓与高精度地图数据进行匹配，计算出当前车辆的侧向、纵向和航向角偏差作为观测值，输入到卡尔曼滤波器进行车辆位姿估计。

步骤1，从单帧的激光点云数据中提取出路沿特征点，根据车辆运动学模型，将多帧检测的路沿特征点坐标转换到当前车辆坐标系下。

2.如权利要求1所述的方法，其特征在于，具体为：

所述步骤1，利用车载的惯性导航系统获取相邻两帧车辆的位移和航向角变化量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)]

步骤2、首先通过高斯投影(现有技术)，得到局部坐标系下的路径点坐标(x_ini,j,y_ini,j)，j表示第j个路径点。

然后在无人车的每个路径点上设置多个beam model，每个beam model用beam区域来表示(创新点)，beam区域用下式表示：

其中Z_p为第p个beam区域。

在每个beam区域中，计算最近的路沿坐标：

其中r_i＝(x_i,y_i)其是第i个路沿坐标。

将每束Beam遇到的最近的路沿点提取出来，最终，提取出的路沿轮廓为C＝{d_p,p＝1,2,…,n}

步骤3、地图匹配

步骤3.1、将地图数据点集由M表示，采用最近点迭代算法最小化如下代价函数：

J＝∑dist(TC,M)

其中dist为欧式距离函数，

通过以下迭代方法解得：

步骤(3.1.1)在M中找到每个C_i的关联点，通过KD树寻找最近点实现。

步骤(3.1.2)利用奇异值分解方法计算变换矩阵T。

步骤(3.1.3)应用变换：C＝TC，然后计算代价J。

步骤(3.1.4)当代价的改变小于阈值τ时终止迭代。

在迭代过程结束之后，获得了C和M的变换矩阵T：

其中θ、m_x、m_y分别为地图匹配得到的航向角、x方向的偏移量和y方向的偏移量。

步骤3.2、定位

将t时刻的车辆位置表示为根据车辆运动学模型(现有技术)预测t+1时刻的车辆位姿：

预测t+1时刻的的车辆位姿表示为：式中Δt为时间间隔。

第一个卡尔曼滤波器用于融合GPS测量值和预测值输出值，提供给第二个卡尔曼滤波器。

第二个卡尔曼滤波器用于融合变换矩阵T和第一个卡尔曼滤波器的输出值，得到最终的无人车位姿。