CN110888125A - 一种基于毫米波雷达的自动驾驶车辆定位方法 - Google Patents

Abstract

本发明公开了一种基于毫米波雷达的自动驾驶车辆定位方法，主要基于毫米波雷达、GPS等传感器反馈的车辆信息，结合改进后的粒子滤波算法实现车辆车道级定位。本发明将车辆ODOM中的车辆的线速度和角速度输入滤波算法的预测模型，将毫米波雷达预处理后的点云数据和已经构建的占据栅格地图进行数据匹配，计算新粒子权重。此外为提高定位算法的鲁棒性，本专利对传统算法进行改进，根据粒子的权重和的不同计算定位结果以及相应的置信度；在重采样阶段，在粒子群中加入部分根据GPS位置信息计算出的新粒子。本发明在能够保证车辆定位精度的的同时有效降低了整个定位***的成本，更利于自动驾驶技术的普及。

Description

一种基于毫米波雷达的自动驾驶车辆定位方法

技术领域

本发明属于自动驾驶领域，特别涉及一种基于毫米波雷达的自动驾驶车辆定位方法。

背景技术

高精度定位是自动驾驶领域的的重要研究课题，目前常用高精度组合导航、多线激 光雷达、Camera等传感器结合高精度地图实现车道级定位，主要采用卡尔曼滤波、粒 子滤波、SLAM等算法。其中常规的卡尔曼滤波算法需要价格昂贵的高精度RTK和IMU， 在高架桥、隧道等GPS信号不稳定地方，其定位精度较低；SLAM算法在车辆快速运 行的高速公路上鲁棒性较低，此外其需要高性能的计算平台。

相比Camera，毫米波雷达能够获取被测物体的位置信息；相比多线激光雷达，毫米波雷达具有价格低廉，安装方便等优势。基于Camera的定位方法目前主要是视觉里程 计，其需要IMU补偿修正图像帧之间的相对运动，该方法在夜间无法工作，鲁棒性较 低，适用于低速场景。基于多线激光雷达的定位方位目前主要是NDT，其需要借助外 部信息提供初始化位置，此外其计算算力较高，对高精度地图的建图精度也有较高的要 求。普通的粒子滤波算法直接对所有粒子加权求和，输出单一的定位数据；此外，在结 构化道路中，因环境信息的相似性，粒子群容易分散，算法的鲁棒性较低。

发明内容

发明目的：本发明针对现有技术存在的问题，提出了一种基于毫米波雷达的自动驾 驶车辆定位方法。

技术方案：为实现上述目的，本发明提供了一种基于毫米波雷达的自动驾驶车辆定 位方法，包括以下步骤：

步骤1：设置在车辆上的毫米波雷达、GPS和ODOM传感器实时采集车辆的信息；

步骤2：对毫米波雷达采集到的点云数据进行预处理生成局部栅格图；

步骤3：生成粒子群，将上一时刻粒子群中每个粒子的状态输入到恒定转率和速度模型中，计算粒子群中每个粒子当前时刻的状态；

步骤4：根据公式

计算每个粒子的权重；其中wⁱ表示第i个粒子的权重，n表示预处理后状态为占据状态的子栅格的总数，d_k表示栅格局部图中第k 个占据状态的子栅格在先验占据栅格地图中的最短距离，σ表示先验占据栅格地图的精 度，η表示毫米波雷达的调整系数；

步骤5：计算所有粒子的权重和，若粒子的总权重和小于最小权重和时候，车辆位姿状态为所有粒子状态的平均值；否则，根据所有粒子权重加权求和计算车辆的状态； 并根据粒子的权重计算当前定位结果的置信度；

步骤6：生成新的粒子群，根据新的粒子群重复步骤3～5。

进一步，所述步骤1中的毫米雷达有3个分别设置在车头和车尾，其中车头设置一个，车尾设置两个，三个毫米波雷达之间的夹角均为120度。

进一步，所述步骤2中队毫米波雷达采集到的点云数据进行预处理并生成局部栅格 图的方法包括以下步骤：

步骤201：剔除车辆正前后方安全距离内雷达散射截面积大于第一阈值的点云数据。

步骤201：将剩余的点云数据映射到一个平面空间中，将该平面空间平均分成多个栅格；

步骤203：统计每个栅格中的点云数量，若小于第二阈值，标记该栅格为0，否则，标记该栅格为1；

步骤204：分别在每个栅格平均划分成多个子栅格，子栅格中的标记与所在的栅格的标记一致；从而得到局部栅格图。这样可以有效的解决毫米波雷达输出点云数量少， 不稳定、易跳变等数据采集不稳定的问题。

进一步，所述步骤4中栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离d_k的获得方法为：

步骤401：设最大距离阈值为d_max，以先验占据栅格地图中第l个占据子栅格为中心，计算横向、纵向为d_max范围内第m行n列的未占据子栅格到第l个占据子栅格 的距离，记为

步骤402：以先验占据栅格地图中第l+1个占据子栅格为中心，计算横向，纵向为d_max范围内第m行n列的未占据子栅格到第l+1个占据子栅格的距离，记为

步骤403：若

小于

先验占据栅格地图中m行n列的未占据子栅格的最小 距离记

否则仍是

步骤404：按照步骤401-403遍历先验占据栅格地图中所有占据子栅格，可获得在先验占据栅格地图中未占据态子栅格到先验占据栅格地图中占据态子栅格的最小距离，其中处于占据态子栅格的最小距离为0；

步骤405：将局部栅格图覆盖到先验占据栅格地图上，根据局部栅格图中占据子栅格在先验占据栅格地图上所以对应的子栅格的状态，得到先验占据栅格地图上对应的子栅格的最小距离，这个即为第k个雷达数据子栅格在先验占据栅格地图中的最短距离 d_k。这样能够有效提高整个定位方法的效率，在能够保证定位定位精度的同时有效降 低了对计算平台的要求。

进一步，所述步骤6中生成新的粒子群的方法为：每个粒子按照权重从大到小进行排序，其中排名前10％的粒子，每个复制3份；排名10％-20％的粒子，每个复制2份； 排名20％-60％的粒子，每个复制1份，此外根据GPS的位置信息加入总数10％的新粒 子；其中新加入的GPS粒子呈椭圆形，椭圆的长半径与道路垂直；加入的总粒子的10％ 的新粒子的位置计算公式为：

公式中

和

分别表示根据GPS生成的第j个粒子的横坐标和纵坐标，x_gps和y_gps为当前GPS输出车辆的横坐标和纵坐标，h_road为道路的朝向，

和

分别为第j 个粒子根据高斯分布生成的椭圆形的短半径和长半径，其中j表示根据GPS生成的新粒 子的标号。这样能够有效的防止粒子退化，提高定位的鲁棒性。

工作原理：本发明基于毫米波雷达、GPS等传感器反馈的车辆信息，结合改进后的粒子滤波算法实现车辆车道级定位。本发明将车辆ODOM中的车辆的线速度和角速度 输入滤波算法的预测模型，将毫米波雷达预处理后的点云数据和已经构建的占据栅格地 图进行数据匹配，计算新粒子权重。此外为提高定位算法的鲁棒性，本专利对传统算法 进行改进，根据粒子的权重和的不同计算定位结果以及相应的置信度；在重采样阶段， 在粒子群中加入部分根据GPS位置信息计算出的新粒子。

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

1、本发明采用的毫米波雷达和GPS均为低成本传感设备，具有明显价格优势，有利于自动驾驶技术普及。

2、本发明采用的定位算法算力较低，无需高昂计算平台。

3、对毫米波雷达采集的数据进行预处理可以有效的解决毫米波雷达输出点云数量 少，不稳定、易跳变等数据采集不稳定的问题。

4、本发明采用改进常规粒子滤波算法，在重采样阶段根据GPS的位置加入椭圆形粒子点，同时算法输出与定位结果相对应的置信度，方便其它模块使用。有效的增加了 鲁棒性。

5、在重采样阶段根据GPS信息增加新的粒子能够有效的防止粒子退化，提高定位的鲁棒性。

附图说明

图1为本发明中毫米波雷达在车辆上的设置示意图。

图2为本发明的工作流程图。

图3是本发明定位方法进行车辆定位效果图。

具体实施方式

下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基 于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有 其他实施例，都属于本发明保护的范围。

本发明提供的车辆定位方法中，对于车辆参数的采集主要采用毫米波雷达、GPS和ODOM传感器。其中，毫米波雷达采用的型号为：ARS408-21；GPS采用的型号为： GN200B；ODOM是车辆通过can总线发出的线速度和角速度信息。结合考虑毫米波雷 达水平视场角为120°和便于检测两侧的道路边缘，如高度公路上护栏，城市道路路牙石、 绿化带等原因。如图1所示，本发明中采用三个毫米波雷达：车头一个，车尾两个，三 个雷达之间夹角成120°；通常情况车头车尾各一个毫米波雷达；在车辆较少的结构化道 路中只安装车头一个毫米波雷达。

本实施例公开了一种基于毫米波雷达的自动驾驶车辆定位方法，主要包括以下步骤：

步骤1：毫米波雷达采集到的点云数据进行预处理生成局部栅格图；其中预处理主要包括以下步骤：

步骤101：剔除车辆正前后方DIS_SAFT米内雷达散射截面积RCS(雷达散射截面 积Radar Cross-Section)大于雷达散射截面积阈值RCS_H的点云数据。其中，DIS_SAFT 表示与前方和后方车辆的安全距离，正常情况该值的选取与车辆速度相关，速度越大， 安全距离越大，速度越小，安全距离越小。其中，RCS_H的取值范围为20-40dBms。

步骤102：将剩余的点云数据映射到长为M_REC，宽为N_REC的平面空间中，该 平面空间中每个栅格标记为GRID_BIG，每个栅格的长为M_GRID，宽为N_GRID。其 中，M_REC表示当前需要感知的环境的局部矩形平面的长度，M_REC的优选范围为 60～80米。N_REC表示当前需要感知的环境的局部矩形平面的宽度，N_REC的优选范 围为10～30米。M_GRID表示栅格的长度，M_GRID的优选范围为0.75-1.25米。N_GRID 表示栅格的宽度，N_GRID的优选范围为0.25-0.5米。N_GRID为M_GRID的整数倍， 这样可以检测出道路两旁的绿化带、栏杆、栅栏等呈长条形的物体。M_REC为M_GRID 的整数倍，N_REC为N_GRID的整数倍。

步骤103：统计每个栅格GRID_BIG中的点云数量，若小于NUM_MAX，标记该栅 格为0，即表示该栅格为未占据状态；否则，标记该栅格为1，即表示该栅格为占据状 态。其中，NUM_MAX表示每个栅格GRID_BIG中含有的点云个数的阈值。NUM_MAX 的优选范围为3～5个。

步骤104：在栅格GRID_BIG中，划分子栅格标记为GRID_SML，若栅格GRID_BIG 为0，其中的子栅格GRID_SML全为0，即表示子栅格GRID_SM为未占据状态；若栅 格GRID_BIG为1，其中的子栅格GRID_SML全为1，即表示子栅格GRID_SM为占据 状态。从而得到局部栅格图，局部栅格图中子栅格的尺寸与先验占据栅格地图中栅格尺 寸一致。

步骤2：在先验占据栅格地图上以采集到的车辆所在的经纬度为圆心，GPS定位偏差λ为半径做圆；并在圆内根据高斯分布设置粒子群；根据车辆ODOM输出的线速度 和角速度，和上一时刻粒子群中每个粒子的状态，计算粒子群中每个粒子当前时刻的状 态。其中每个粒子的状态包括每个粒子的位置坐标，速度信息和航向信息；

其中，计算粒子群中每个粒子当前时刻的状态主要包括以下步骤：将车辆ODOM(里程计)输出的速度信息输入到CTRV(恒定转率和速度模型)运行模型中，CTRV 运行模型输出每个粒子的状态信息，CTRV运行模型为：

式中，

表示第i个粒子在t时刻的航向角，

表示第i个粒子在t-1时刻的航向角，

表示在t-1时刻IMU输出的车辆的航向角的角速度加入高斯噪声的值， Δt表示时间差，

表示第i个粒子在t时刻在UTM坐标系下的横坐标，

表示第i个 粒子在t-1时刻在UTM坐标系下的横坐标，v_t表示在t时刻车辆ODOM(里程计)输 出的车辆的速度加入高斯噪声后的值，

表示第i个粒子在t时刻在UTM坐标系下的纵 坐标，

表示第i个粒子在t-1时刻在UTM坐标系下的纵坐标。

步骤3：根据公式

计算每个粒子的权重；其中wⁱ表示第i个粒子的权重，n表示预处理后状态为占据状态的子栅格的总数，d_k表示局部栅格图中第k 个占据状态的子栅格在先验占据栅格地图中的最短距离，σ表示先验占据栅格地图的精 度，η表示毫米波雷达的调整系数。其中，栅格局部图中第k个占据状态的子栅格在先 验占据栅格地图中的最短距离d_k的计算方法为：

步骤301：设最大距离阈值为d_max，以先验占据栅格地图中第l个占据子栅格为中心，计算横向、纵向为d_max范围内第m行n列的未占据子栅格到第l个占据子栅格 的距离，记为

步骤302：以先验占据栅格地图中第l+1个占据子栅格为中心，再次计算横向，纵向为d_max范围内第m行n列的未占据子栅格到第l+1个占据子栅格的距离，记为

步骤303：若

小于

先验占据栅格地图中m行n列的未占据子栅格的最小 距离记

否则仍是

步骤304：按照步骤301-303遍历先验占据栅格地图中所有占据子栅格，可获得在先验占据栅格地图中未占据态子栅格到先验占据栅格地图中占据态子栅格的最小距离，其中处于占据态子栅格的最小距离为0。其中d_max设置3-5米。

步骤305：将步骤1生成的局部栅格图覆盖到先验占据栅格地图上，根据局部栅格图中占据子栅格在先验占据栅格地图上所以对应的子栅格的状态，可以得到该子栅格的最小距离，即可知第k个雷达数据子栅格在先验占据栅格地图中的最短距离d_k。

步骤4：计算所有粒子的权重和，若粒子的总权重和小于最小权重和W_MIN时候，车辆位姿状态为所有粒子状态的平均值；否则，根据所有粒子权重加权求和计算车辆的 状态；并根据粒子的权重计算当前定位结果的置信度。计算公式分别如下：

其中X表示算法输出的车辆位姿状态向量，其中X中包括车辆的位置和航向角，B表示输出位姿状态的置信度，m为粒子总数，Xⁱ为第i个粒子的位姿状态，wⁱ为第i个 粒子的权重，w^sum为所有粒子的权重和，w^fit为预处理毫米波雷达后的所有子栅格和占 据栅格地图中栅格位置完全匹配(即步骤3中d_k＝0)时的单个粒子的权重，m为总粒 子数。最小权重和W_MIN的优选范围为1.5～2.5。

步骤5：在重采样阶段，生成新的粒子群，并根据新的粒子群重复步骤2～4。每个粒子按照权重从大到小进行排序，其中排名前10％的粒子，每个复制3份；排名10％-20％ 的粒子，每个复制2份；排名20％-60％的粒子，每个复制1份，此外根据GPS的位置 信息加入总数10％的新粒子组成新的粒子群。其中新加入的GPS粒子呈椭圆形，椭圆 的长半径与道路垂直；加入的总粒子的10％的新粒子的位置计算公式为：

公式中

和

分别表示根据GPS生成的第j个粒子的横坐标和纵坐标，x_gps和y_gps为当前GPS输出车辆的横坐标和纵坐标，h_road为道路的朝向，

和

分别为第j个粒 子根据高斯分布生成的椭圆形的短半径和长半径，其中j表示根据GPS生成的新粒子的 标号。

上述步骤中，步骤3中权重计算方法主要约束算法的横向定效果，在结构化道路上， 因道路结构的相似性，粒子群容易在纵向位置发散；根据步骤5中GPS位置信息加入 的椭圆形粒子主要为提高该算法的纵向定位效果，新加入呈椭圆形的粒群，椭圆的长 轴为横向，短轴为纵向，这两部分相结合能够提高算法的定位精度。步骤4中根据粒子 权重计算车辆的位姿状态，同时求出其置信度，可解决道路两侧出现道路边缘不完整而 导致的定位抖动现象，具有较强的鲁棒性，为其它模块提供丰富的数据借口。

采用本发明提供的定位方法进行车辆定位效果图如图3所示，图中白色点为粒子群， 灰色箭头为定位轨迹，灰色点为先验占据栅格地图中的占据子栅格，黑色点为局部栅格 图中的占据子栅格。本发明中部分数据结果如表1所示。

表1：

从表1中不难看出，本发明提供的定位方法的定位精度为车道级别，满足自动驾驶定位精度。本发明提供的定位方法不受天气、环境的影响，运行于隧道、桥洞等被遮挡 地方，鲁棒性强。

Claims (5)

1.一种基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：包括以下步骤：

步骤1：设置在车辆上的毫米波雷达、GPS和ODOM传感器实时采集车辆的信息；

步骤2：对毫米波雷达采集到的点云数据进行预处理生成局部栅格图；

步骤3：生成粒子群，将上一时刻粒子群中每个粒子的状态输入到恒定转率和速度模型中，计算粒子群中每个粒子当前时刻的状态；

步骤4：根据公式

计算每个粒子的权重；其中wⁱ表示第i个粒子的权重，n表示预处理后状态为占据状态的子栅格的总数，d_k表示栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离，σ表示先验占据栅格地图的精度，η表示毫米波雷达的调整系数；

步骤5：计算所有粒子的权重和，若粒子的总权重和小于最小权重和时候，车辆位姿状态为所有粒子状态的平均值；否则，根据所有粒子权重加权求和计算车辆的状态；并根据粒子的权重计算当前定位结果的置信度；

步骤6：生成新的粒子群，根据新的粒子群重复步骤3～5。

2.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤1中的毫米雷达有3个分别设置在车头和车尾，其中车头设置一个，车尾设置两个，三个毫米波雷达之间的夹角均为120度。

3.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤2中队毫米波雷达采集到的点云数据进行预处理并生成局部栅格图的方法包括以下步骤：

步骤201：剔除车辆正前后方安全距离内雷达散射截面积大于第一阈值的点云数据。

步骤201：将剩余的点云数据映射到一个平面空间中，将该平面空间平均分成多个栅格；

步骤203：统计每个栅格中的点云数量，若小于第二阈值，标记该栅格为0，否则，标记该栅格为1；

步骤204：分别在每个栅格平均划分成多个子栅格，子栅格中的标记与所在的栅格的标记一致；从而得到局部栅格图。

4.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤4中栅格局部图中第k个占据状态的子栅格在先验占据栅格地图中的最短距离d_k的获得方法为：

步骤401：设最大距离阈值为d_max，以先验占据栅格地图中第l个占据子栅格为中心，计算横向、纵向为d_max范围内第m行n列的未占据子栅格到第l个占据子栅格的距离，记为

步骤402：以先验占据栅格地图中第l+1个占据子栅格为中心，计算横向，纵向为d_max范围内第m行n列的未占据子栅格到第l+1个占据子栅格的距离，记为

步骤403：若

小于

先验占据栅格地图中m行n列的未占据子栅格的最小距离记

否则仍是

步骤404：按照步骤401-403遍历先验占据栅格地图中所有占据子栅格，可获得在先验占据栅格地图中未占据态子栅格到先验占据栅格地图中占据态子栅格的最小距离，其中处于占据态子栅格的最小距离为0；

步骤405：将局部栅格图覆盖到先验占据栅格地图上，根据局部栅格图中占据子栅格在先验占据栅格地图上所以对应的子栅格的状态，得到先验占据栅格地图上对应的子栅格的最小距离，这个即为第k个雷达数据子栅格在先验占据栅格地图中的最短距离d_k。

5.根据权利要求1所述的基于毫米波雷达的自动驾驶车辆定位方法，其特征在于：所述步骤6中生成新的粒子群的方法为：每个粒子按照权重从大到小进行排序，其中排名前10％的粒子，每个复制3份；排名10％-20％的粒子，每个复制2份；排名20％-60％的粒子，每个复制1份，此外根据GPS的位置信息加入总数10％的新粒子形成新的粒子群；其中新加入的GPS粒子呈椭圆形，椭圆的长半径与道路垂直；加入的总粒子的10％的新粒子的位置计算公式为：

公式中

和

分别表示根据GPS生成的第j个粒子的横坐标和纵坐标，x_gps和y_gps为当前GPS输出车辆的横坐标和纵坐标，h_road为道路的朝向，

和

分别为第j个粒子根据高斯分布生成的椭圆形的短半径和长半径，其中j表示根据GPS生成的新粒子的标号。

Images