CN114061596A - 自动驾驶定位方法、系统、测试方法、设备及存储介质 - Google Patents

Abstract

本发明涉及自动驾驶定位技术领域，公开了一种自动驾驶定位方法、系统、测试方法、设备及存储介质，采取组合导航和激光SLAM输出的定位数据经扩展卡尔曼滤波处理后生成的融合数据，通过将实时激光定位和组合导航融合的定位结果反向验证输入数据，即利用该融合数据剔除组合导航和激光SLAM定位中的异常数据，形成实时闭环的定位系统，再通过测试方法测试自动驾驶定位系统，提供自动驾驶定位系统的新的测试验证方案。本发明能够有效提高定位精度，提升组合导航的可靠性。

Description

自动驾驶定位方法、系统、测试方法、设备及存储介质

技术领域

本发明涉及自动驾驶定位技术领域，特别涉及一种自动驾驶定位方法、系统、测试方法、设备及存储介质。

背景技术

汽车自动驾驶是近年来的一项前沿研究技术，需要在没有人为操纵的情况下完整、安全、有效地驾驶汽车。定位是自动驾驶的基础模块，自动驾驶系统能够持续安全可靠运行的一个关键前提是车辆的定位系统必须实时稳定地输出足够高精度的位置和与位置相关的信息，包括车辆的经度、纬度、速度、俯仰角、航向角、更新频率等。一旦这些信息无法及时精确地获取，车辆就无法确定自身位置，必须立即退出自动驾驶模式，由驾驶员接管车辆操控。

要实现汽车自动驾驶系统，必须采用高精定位导航技术，GNSS和IMU组合导航系统是自动驾驶系统中常用的定位方案，GNSS(全球导航卫星系统)可以提供精确的绝对定位，但是在局部区域卫星信号丢失或者微弱时会导致定位信息延迟而造成车辆失控。IMU(惯性导航)虽然可以不依托外在信息，无惧极端环境提供稳定的位置和速度信息，但是存在定位误差随时间积累、长时间内不能保证足够的导航精度的问题。GNSS和IMU融合形成的组合导航系统可以发挥两种导航系统的优势。但是，在复杂场景下，受到信号遮挡和多径效应的影响，目前的GNSS/IMU组合导航系统仍然无法实现长时间、长距离的稳定定位。

因此，针对现有技术，因无法在全部地区实现准确实时的定位，定位精度、系统健壮性等各方面也仍然具有很大的提升空间的情况下，本发明的目的在于针对自动驾驶定位缺乏系统的测试方法，导致组合导航可靠性低、定位精度低而提出的一种自动驾驶定位方法、系统、测试方法、设备及存储介质。

发明内容

为了解决现有技术存在的缺漏，本发明提供了一种自动驾驶定位方法、系统、测试方法、设备及存储介质。

本发明的技术内容如下：

一种自动驾驶定位方法，包括：

输入位置、速度、姿态角数据集合以及实时激光点云数据；

将获取的位置、速度、姿态角数据集合经扩展卡尔曼滤波法处理；

将获取的实时激光点云数据经激光SLAM算法处理，再经扩展卡尔曼滤波法处理；

经扩展卡尔曼滤波法处理后获得融合输出定位数据，利用融合输出定位数据剔除输入的异常数据，形成闭环处理。

进一步地，所述实时激光点云数据经激光SLAM算法处理生成位置、航向角数据。

进一步地，经扩展卡尔曼滤波法处理后生成融合输出的位置、速度、姿态角定位数据，使用该融合输出定位数据剔除输入数据中的异常数据，形成闭环处理。

进一步地，剔除异常数据的步骤包括：

步骤一：确定融合定位输出数据、输入位置轨迹和激光SLAM位置轨迹的起始点；

步骤二：采用线性插值法对步骤一中的融合定位输出数据、输入位置轨迹和激光SLAM位置轨迹进行间隔重采样；

步骤三：计算等间隔重采样点之间的距离，提取并剔除输入和激光SLAM输入的异常位置数据；

步骤四：基于融合输出的速度数据，采用曲线相似性分析方法，提取并剔除输入的异常速度数据；

步骤五：基于融合输出的姿态角数据，采用曲线相似性分析方法，提取并剔除输入和激光SLAM输入的异常角度数据。

本发明提供了一种自动驾驶定位系统，包括：组合导航设备模块、激光雷达模块、自动驾驶控制器模块及自动驾驶定位模块，

所述组合导航设备模块，用于向所述自动驾驶定位模块提供位置、速度、姿态角数据集合；

所述激光雷达模块，用于向所述自动驾驶定位模块提供实时激光点云数据；

所述自动驾驶定位模块，用于实现自动驾驶定位方法；

所述自动驾驶控制器模块，用于运行所述自动驾驶定位模块。

进一步地，所述自动驾驶定位模块采取基于组合导航和激光SLAM融合技术的自动驾驶定位程序。

本发明提供了一种测试方法，所述测试方法适用于自动驾驶定位系统，所述测试方法包括：

通过组合导航设备模块及自动驾驶定位功能模块的组合测试完成自动驾驶定位系统的测试。

进一步地，所述组合导航设备模块的测试步骤包括：

步骤一：测试组合导航设备在静止状态下的定位稳定性；

步骤二：测试组合导航设备在运动状态下的不同定位状态的变化；

步骤三：以高精度导航定位系统为基准，测试组合导航设备的定位精度；

步骤四：测试组合导航设备的固定解和非固定解的互相切换时间；

步骤五：测试组合导航设备重复多次冷启动的时间的变化；

步骤六：测试及分析组合导航设备的丢包率和丢包区间。

进一步地，所述自动驾驶定位功能模块的测试步骤包括：

步骤一：在GNSS信号优良、固定设施完备的场地中，布置固定的导航轨道，测试自动驾驶定位精度和定位数据更新频率；

步骤二：在GNSS信号遮挡区域测试自动驾驶定位精度和数据处理时间。

本发明还提供了一种自动驾驶定位设备，所述设备包括：存储器、处理器及存储在所述存储器上并在所述处理器上运行的自动驾驶定位程序，所述自动驾驶定位程序配置为实现如上述中任一项所述的自动驾驶定位方法的步骤。

本发明还提供了一种电子存储介质，所述存储介质中存储有至少一条指令或至少一段程序，所述至少一条指令或至少一段程序由处理器加载并执行以实现如上述任一项的自动驾驶定位方法。

本发明至少包括以下有益效果：

(1)本发明采用一种基于组合导航和激光SLAM相融合的实时闭环的自动驾驶定位方法，能够弥补卫星信号易受环境影响的缺陷，进一步地提升组合导航的可靠性，有效提高定位精度；

(2)本发明的自动驾驶定位系统采用的定位方法为闭环的处理方法，组合导航设备和激光SLAM输入的定位数据经扩展卡尔曼滤波处理后输出的结果数据，又会用于组合导航设备和激光SLAM输入定位数据的筛选，方向验证输入数据的有效性，同时采用基于深度学习的实时激光定位方法，输出具有高准确性和强鲁棒性的激光定位数据，使得融合定位的数据更加可靠；

(3)本发明提供的自动驾驶定位系统的功能级测试方法，该测试方法适用于任何包含组合导航设备的自动驾驶定位系统，本发明从组合导航设备测试、定位功能测试两大方面开展自动驾驶定位系统的测试，并且从静态定位数据稳定性、定位状态、定位精度、固定解切换时间、冷启动时间、丢包六个方面设置的一套组合导航设备测试方法，为自动驾驶定位系统的测试验证提供型的验证方案，该测试操作方便、测试结果可靠性较高。

附图说明

图1为本发明的整体结构流程结构示意图。

图2为本发明的自动驾驶定位系统流程结构示意图。

图3为本发明的实施例中的激光SLAM处理流程结构示意图。

图4为本发明的实施例中的融合定位数据剔除输入异常数据的流程结构示意图。

图5为本发明的组合导航设备测试流程结构示意图。

具体实施方式

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

结合图1-5所示，本发明提供了一种自动驾驶定位方法，包括：

输入组合导航设备的位置、速度、姿态角数据集合以及激光雷达的实时激光点云数据；

组合导航的位置数据集合为P1＝{(x₁₀，y₁₀，z₁₀)，…，(x_1i，y_1i，z_1i)，…}，速度数据集合为V1＝{(v_x10，v_y10，v_z10)，…，(v_x1i，v_y1i，v_z1i)，…}，姿态角数据集合为A1＝{(α₁₀β₁₀，γ₁₀)，…，(α_1i，β₁i，γ_1i)，…}。

将获取组合导航设备的位置、速度、姿态角数据集合经扩展卡尔曼滤波法处理；

将获取激光雷达的实时激光点云数据经激光SLAM算法处理，再经扩展卡尔曼滤波法处理；

所述实时激光点云数据经激光SLAM算法处理生成位置、航向角数据。

激光点云数据经过激光SLAM算法处理后生成位置、航向角，激光SLAM算法使用深度学习模型来学习点云特征，输出更加准确和更强鲁棒性的定位结果，同时，将特征匹配和位姿计算的过程和实时制作点云特征地图结合在一起，减少了激光定位的计算时间，实时性更高，使得激光定位和组合导航融合的结果更加可靠。

激光SLAM算法的处理流程如图3所示，实时激光点云数据经过剔除噪声点和离群点、点云降采样、提取PointNet特征后，生成实时点云特征图，将之进行相邻点云特征匹配，计算粗略位姿，再基于粗略位姿提取局部点云特征地图，然后经过Flann特征匹配、Rahsac剔除错误点对处理后，得到正确的特征匹配点对，再基于此计算位姿变换关系，输出精确位姿，精确位姿包括位置集合P2＝{(x₂₀，y₂₀，z₂₀)，…，(x_2i，y_2i，z_2i)，…}，航向角集合A2＝{γ₂₀，…，γ₂₁，…}，同时，基于精确位姿，转换点云特征图，生成精确的点云特征图，并拼接出完整点云特征图，生成点云特征地图，已生成的点云特征地图在实时激光点云数据处理的过程中，可以与之结合直接进行提取局部点云特征地图，减少激光定位的计算时间，实时激光点云数据在进行激光SLAM算法处理的过程中，所生成的实时点云特征图可以直接进行Flann特征匹配、Ransac剔除错误点对处理，也可以直接转换点云特征图的位姿。

经扩展卡尔曼滤波法处理后获得融合输出定位数据，利用融合输出定位数据剔除输入的异常数据，形成闭环处理。

经扩展卡尔曼滤波法处理后生成融合输出的位置、速度、姿态角定位数据，使用该融合输出定位数据剔除组合导航和激光SLAM定位中的异常数据，形成闭环处理。

采用扩展卡尔曼滤波方法，以组合导航设备的输出数据为初始值，以组合导航和激光SLAM输出的定位数据为观测量，综合输出位置集合P0＝{(x₀₀，y₀₀，z₀₀)，…，(x_0i，y_0i，z_0i)，…}、速度集合V0＝{(v_x00、v_y00、x_z00)，…，(v_x0i，v_y0i，v_z0i)，…}、姿态角集合A0＝{(α₀₀，β₀₀，γ₀₀)，…，(α_0i，β_0i，γ_0i)，…}。

以融合定位结果剔除输入异常数据的流程如图4所示，

比较P1、P2和P0的轨迹相似性，通过曲线相似性分析方法剔除P1和P2中的异常值，步骤如下：

确认融合定位输出数据集P0的起始位置P₀₀＝(x₀₀，y₀₀，z₀₀)、基于组合导航定位数据P1和激光SLAM数据P2，计算组合导航位置轨迹和激光SLAM位置轨迹中距离P₀₀最短的位置点P₁₀＝(x₁₀，y₁₀，z₁₀)和P₂₀＝(x₂₀，y₂₀，z₂₀)，以P₁₀和P₂₀作为组合导航位置轨迹和激光SLAM位置轨迹的起始点；

采用线性插值的方法对P0、P1、P2进行等间隔重采样，以间隔d的采样点表示轨迹集合P0、P1、P2，分别以P0’、P1’和P2’表示，P0’＝{P₀₀，P₀₁，…，P_0j，…}，P1’＝{P₁₀，P₁₁，…，P_1j，…}，P2’＝{P₂₀，P₂₁，…，P_2j，…}，其中，P_0j＝P₀₀+j*d，P_1j＝P₁₀+j*d，P_2j＝P₂₀+j*d；

计算P0上各点分别和P1、P2对应点的距离DT1＝{dist(P₀₀，P₁0)，dist(P₀₁，P₁₁)，…，dist(P_0j，P_1j)，…}、DT2＝{dist(P₀₀，P₂₀)，dist(P₀₁，P₂₁)，…，dist(P_0j，P_2j)，…}。设置确定距离阈值T_D，记录DT1和DT2中超过T_D的距离值所对应的P1’和P2’中的点，P1和P2中在这些点位置附近的点即为组合导航和激光SLAM输入的异常定位数据，剔除异常数据；

基于融合输出的速度数据，采用曲线相似性分析方法，比较V1和V0随时间的变化趋势，剔除组合导航输入速度V1中的异常速度数据；

基于融合输出的姿态角数据，采用曲线相似性分析方法，比较A1、A2与A0随时间的变化趋势，剔除组合导航输入姿态角A1和激光SLAM输入的姿态角A2中的异常角度数据。

本发明提供了一种自动驾驶定位系统，包括：组合导航设备模块、激光雷达模块、自动驾驶控制器模块及自动驾驶定位模块，

所述组合导航设备模块，用于向所述自动驾驶定位模块提供位置、速度、姿态角数据集合；

所述激光雷达模块，用于向所述自动驾驶定位模块提供实时激光点云数据；

所述自动驾驶定位模块，用于实现自动驾驶定位方法；

所述自动驾驶控制器模块，用于运行所述自动驾驶定位模块。

进一步地，所述自动驾驶定位模块采取基于组合导航和激光SLAM融合技术的自动驾驶定位程序。

本发明提供了一种测试方法，所述测试方法适用于自动驾驶定位系统，所述测试方法包括：

通过组合导航设备模块及自动驾驶定位功能模块的组合测试完成自动驾驶定位系统的测试。

组合导航设备模块的测试包括：

1、静态定位数据稳定性测试：测试组合导航设备在静止状态下的定位稳定性，统计静止状态下一段时间内位置数据的分布情况。

在车辆上安装好组合导航设备，将车辆静止停放到一空旷地带，待组合导航设备能接收到稳定的卫星信号，设备状态稳定后，记录一段时间ΔT内的接收数据，统计位置数据x、y的分布情况，计算x、y的极差、方差、标准差和协方差，通过下式计算，分析位置数据的离散程度。

x、y的极差：R_x＝x_max-x_min，R_y＝y_max-y_min，其中，x_max、x_min、y_max、y_min分别为x的最大值、x的最小值、y的最大值、y的最小值。

x的方差、标准差：

y的方差、标准差：

其中，

为x的平均值，

为y的平均值，n为数据记录数；

x、y的协方差：

2、定位状态测试：测试组合导航设备在运动状态下的不同定位状态的变化。

选定测试场地和运行轨迹，将安装有组合导航的同一车辆以不同的车速沿着指定轨迹行驶，设备接收数据，统计固定解、浮点解、伪距解、单点解等不同定位状态下的运行轨迹，计算不同车速下不同定位状态的轨迹所占比例，分析在运动状态下组合导航设备定位的稳定性。

3、定位精度测试：以高精度导航定位系统为基准，对比分析在相同轨道线上的组合导航设备和基准定位系统的位置误差，测试组合导航设备的定位精度。

在空旷的测试场地中布置固定的导航轨道，安装有待测组合导航设备的车辆沿导航轨道线行驶，记录接收的定位数据集合{x,y}；拆除待测组合导航设备，在同一车辆的相同位置安装高精度导航定位系统，按照同样的方式记录接收的基准定位数据集合{x0,y0}，形成基准轨迹曲线R0。计算{x,y}中每个定位点到R0的距离，形成集合{d}，即为定位误差，设定误差统计区间的范围，计算落入每个区间的数量，统计d的分布情况。计算{d}中的最大值d_max，最小值d_min和平均值

4、固定解切换时间测试：测试组合导航设备的固定解和非固定解的互相切换时间。

安装有待测组合导航设备的车辆以不同速度沿着指定轨迹行驶，记录接收的数据内容，记录固定解到非固定解、非固定解到固定解的时间点，并计算时间差。

5、冷启动时间测试：测试组合导航设备重复多次冷启动的时间的变化。

选择待测组合导航设备定位信号优良的场地，采用拔电方式关闭设备电源，等待一段时间(如，5分钟)，给设备通电并接收数据，记录从通电到第一次观测有固定解的时间T。断电，再重复多次相同操作，记录时间T，计算T的最大值、最小值、平均值、标准差。

6、丢包测试：测试及分析组合导航设备的丢包率和丢包区间，包括数据对比测试和数据完整性测试。

数据对比测试：安装有待测组合导航设备的车辆以不同速度沿着指定轨迹行驶，记录设备通过通讯接口发送的数据内容，对比设备内部接收记录的数据内容，分析计算不同速度时的丢包率，查看行驶轨迹中丢包的区间；

数据完整性测试：根据组合导航设备通过通讯接口发送的数据内容，统计时间连续性，检查数据自身的完整性，计算不同速度时的丢包率，查看行驶轨迹中丢包的区间。

自动驾驶定位功能模块的测试包括：

(1)选择GNSS(Global Navigation Satellite System)信号优良、固定设施较完备的场地中，布置固定的导航轨道，测试自动驾驶定位精度和定位数据更新频率。

开启车辆自动驾驶定位模块，以不同的速度沿导航轨道线行驶，记录接收的定位数据内容和时间，计算不同速度下的数据更新频率f；然后在同一车辆上安装高精度导航定位系统，按照同样的方式记录接收的数据内容，作为基准定位数据，参照组合导航设备的定位精度测试方法，计算自动驾驶定位数据和基准数据的误差，统计误差分布情况，计算误差的最大值、最小值和平均值。

(2)在隧道等GNSS信号遮挡区域测试自动驾驶定位精度和数据处理时间。

在隧道中设置控制点，使用高精度测量仪器，如全站仪等，测量控制点的精确位置坐标(X0、Y0)，开启车辆自动驾驶定位模块，驾驶车辆以不同的速度通过隧道，途中经过控制点，记录自动驾驶系统输出的控制点位置数据(X、Y)和时间，计算(X0、Y0)和(X、Y)的误差，分析误差随时间的变化，统计误差分布情况，并计算误差的最大值、最小值和平均值。

在自动驾驶定位模块开始接收传感器数据和输出定位数据处加上时间戳，计算输入输出数据的时间差ΔPT，记录Δτ时间内的ΔPT值，统计ΔPT的分布情况。

本发明还提供了一种自动驾驶定位设备，所述设备包括：存储器、处理器及存储在所述存储器上并在所述处理器上运行的自动驾驶定位程序，所述自动驾驶定位程序配置为实现如上述中任一项所述的自动驾驶定位方法的步骤。

本发明还提供了一种电子存储介质，所述存储介质中存储有至少一条指令或至少一段程序，所述至少一条指令或至少一段程序由处理器加载并执行以实现如上述任一项的自动驾驶定位方法。

本发明的组合导航采用的是GNSS和IMU组合导航。

本发明采用基于组合导航和激光SLAM相融合的实时闭环的自动驾驶定位系统，能够弥补卫星信号易受环境影响的缺陷，进一步提升组合导航的可靠性，有效提高定位精度，自动驾驶定位系统中的组合导航设备和激光SLAM输入的定位数据经扩展卡尔曼滤波处理后输出的结果数据，又会用于组合导航设备和激光SLAM输入定位数据的筛选，反向验证输入的定位数据的有效性，同时，采用基于深度学习的实时激光定位方法，输出具有高准确性和强鲁棒性的激光定位数据，使得融合定位的结果更加可靠，优化定位系统综合性能。

同时提供一种针对自动驾驶定位缺乏系统的测试方法，建立一套适用于各种组合导航设备的自动驾驶定位系统，与现有自动驾驶系统采取的系统级测试方法不同，本发明从组合导航设备测试、定位功能测试两大方面开展自动驾驶定位系统的测试，对自动驾驶定位系统进行相对全面的测试，测试操作简单，可靠性较高。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种自动驾驶定位方法，其特征在于：包括：

输入位置、速度、姿态角数据集合以及实时激光点云数据；

将获取的位置、速度、姿态角数据集合经扩展卡尔曼滤波法处理；

将获取的实时激光点云数据经激光SLAM算法处理，再经扩展卡尔曼滤波法处理；

经扩展卡尔曼滤波法处理后获得融合输出定位数据，利用融合输出定位数据剔除输入的异常数据，形成闭环处理。

2.根据权利要求1所述的一种自动驾驶定位方法，其特征在于：所述实时激光点云数据经激光SLAM算法处理生成位置、航向角数据。

3.根据权利要求1所述的一种自动驾驶定位方法，其特征在于：所述定位方法包括：经扩展卡尔曼滤波法处理后生成融合输出的位置、速度、姿态角定位数据，使用该融合输出定位数据剔除输入数据中的异常数据，形成闭环处理。

4.根据权利要求3所述的一种自动驾驶定位方法，其特征在于：所述融合输出定位数据剔除输入的异常数据的步骤包括：

步骤一：确定融合定位输出数据、输入位置轨迹和激光SLAM位置轨迹的起始点；

步骤二：采用线性插值法对步骤一中的融合定位输出数据、输入位置轨迹和激光SLAM位置轨迹进行间隔重采样；

步骤三：计算等间隔重采样点之间的距离，提取并剔除输入和激光SLAM输入的异常位置数据；

步骤四：基于融合输出的速度数据，采用曲线相似性分析方法，提取并剔除输入的异常速度数据；

步骤五：基于融合输出的姿态角数据，采用曲线相似性分析方法，提取并剔除输入和激光SLAM输入的异常角度数据。

5.一种自动驾驶定位系统，其特征在于：包括：组合导航设备模块、激光雷达模块、自动驾驶控制器模块及自动驾驶定位模块，

所述组合导航设备模块，用于向所述自动驾驶定位模块提供位置、速度、姿态角数据集合；

所述激光雷达模块，用于向所述自动驾驶定位模块提供实时激光点云数据；

所述自动驾驶定位模块，用于实现自动驾驶定位方法；

所述自动驾驶控制器模块，用于运行所述自动驾驶定位模块。

6.根据权利要求5所述的一种自动驾驶定位系统，其特征在于：所述自动驾驶定位模块采取基于组合导航和激光SLAM融合技术的自动驾驶定位程序，以实现自动驾驶定位系统的闭环处理。

7.一种测试方法，其特征在于：所述测试方法适用于自动驾驶定位系统，所述测试方法包括：

通过组合导航设备模块及自动驾驶定位功能模块的组合测试完成自动驾驶定位系统的测试。

8.根据权利要求7所述的一种测试方法，其特征在于：

所述组合导航设备模块的测试步骤包括：

测试组合导航设备在静止状态下的定位稳定性；

测试组合导航设备在运动状态下的不同定位状态的变化；

以高精度导航定位系统为基准，测试组合导航设备的定位精度；

测试组合导航设备的固定解和非固定解的互相切换时间；

测试组合导航设备重复多次冷启动的时间的变化；

测试及分析组合导航设备的丢包率和丢包区间。

所述自动驾驶定位功能模块的测试步骤包括：

在GNSS信号优良、固定设施完备的场地中，布置固定的导航轨道，测试自动驾驶定位精度和定位数据更新频率；

在GNSS信号遮挡区域测试自动驾驶定位精度和数据处理时间。

9.一种自动驾驶定位设备，其特征在于：所述设备包括：存储器、处理器及存储在所述存储器上并在所述处理器上运行的自动驾驶定位程序，所述自动驾驶定位程序配置为实现如上述权利要求1-4中任一项所述的自动驾驶定位方法的步骤。

10.一种电子存储介质，其特征在于：所述存储介质中存储有至少一条指令或至少一段程序，所述至少一条指令或至少一段程序由处理器加载并执行以实现如上述权利要求1-4任一项的自动驾驶定位方法。

Images