CN111060099B - 一种无人驾驶汽车实时定位方法 - Google Patents

Abstract

本发明公开了一种无人驾驶汽车实时定位方法，涉及人工智能领域,包括运动模型预测模块、点云配准模块与状态校正模块，所述系统的硬件部分包括上位机、惯性元件传感器、3D激光雷达。本发明所述的一种无人驾驶汽车实时定位方法，可以在室外的厂区、道路、城镇等场景中为无人车实时提供当前状态信息，并在场景地图中动态显示出来，能够在先验地图点云较稀疏时仍能保持好的定位效果，且不依赖GPS或RTK就能实现实时局部定位，当3D激光雷达和惯性元件传感器的输入信息有一个消失时仍能在一定精度范围内完成实时定位，保证了系统较强的鲁棒性，为无人车的安全提供了保障，具备一定的使用前景。

Description

一种无人驾驶汽车实时定位方法

技术领域

本发明涉及人工智能领域，具体为一种无人驾驶汽车实时定位方法。

背景技术

随着《中国制造2025》的稳步推进，国家各地方政策对人工智能产业的积极引导，以及学术和产业界传感与感知技术的突破，智能驾驶技术迎来了飞速发展的阶段。而L4和L5级别的自动驾驶作为智能驾驶技术的理想目标，是人工智能皇冠中各国和各厂商奋力竞争摘取的最璀璨的明珠，是无人驾驶市场领域高效的生产力，对提高我国高新产业发展，加快我国经济结构改革，奠定我国在未来新兴产业国际市场的地位具有十分重要的意义。无论是在景区、厂区，还是在码头、城市等无人驾驶应用场景，无人车定位技术是智能驾驶系统的核心基础功能。

然而，我国移动机器人产业发展较慢，无人驾驶产业更是起步较晚，近些年才得到重视，获得了较快发展，但相比于发展较早的美国还有一些距离需要追赶。本发明专利通过融合实时点云和惯性信息，在先验3D点云地图基础上，借助点云匹配和卡尔曼滤波器等技术，可以实现无人车的实时定位，为进一步的导航规划奠定了基础，在无人驾驶汽车的导航规划与控制时，无人车的状态估计是关键的基础工作，目前成熟通用的无人车实时定位技术方案较少，为此，我们提出了一种无人驾驶汽车实时定位方法。

发明内容

本发明要解决的技术问题是克服现有的缺陷，提供一种无人驾驶汽车实时定位方法，可以在室外的厂区、道路、城镇等场景中为无人车实时提供当前状态信息，并在场景地图中动态显示出来，所述状态信息主要包括无人车的位置坐标、姿态、速度。

为实现上述目的，本发明提供如下技术方案：一种无人驾驶汽车实时定位方法，包括运动模型预测模块、点云配准模块与状态校正模块，所述系统的硬件部分包括上位机、惯性元件传感器、3D激光雷达，所述运动模型预测模块根据当前时刻惯性元件传感器的观测数据和前一时刻的无人车状态，通过建立的运动模型预测当前时刻的无人车状态，所述点云配准模块以运动模型预测模型的预测结果作为猜测值，将先验点云地图与当前扫描得到的场景点云进行点云匹配，得到当前扫描点云和场景之间的坐标变换矩阵，利用此变换矩阵对匹配前的猜测值进行转换，输出为匹配后的点云和无人车的位姿信息，所述状态校正模块根据运动模型预测模块的结果和点云配准模块的输出结果，校正得到新的无人车状态高斯分布，取分布的均值作为最终的定位结果。

优选的，所述上位机为工控机型号为科拉德IPC-805A，所述惯性元件传感器型号为迈普时空M39，所述3D激光雷达型号为北科天绘R-Fans-16M。

优选的，以0时刻惯性元件传感器确定的坐标系作为世界坐标系，3D激光雷达坐标系也已和惯性元件传感器坐标系做好了标定，无人车在任意时刻的状态定义为：x＝[p_t，q_t，v_t]^T，所述p_t表示位置，q_t表示方向四元数，v_t表示速度，并且认为任意时刻的无人车车状态都服从高斯分布。

优选的，所述无人车状态预测方程为

x_t＝[p_t-1+Δt·v_t-1q_t-1·Δq_t，v_t-1+Δt·a_t-1]，所述Δt是到t-1时刻的时间间隔，a_t-1是t-1时刻的惯性元件传感器的加速度测量值，Δq_t是在Δt时间间隔内产生的旋转，用四元数表示为 是t时刻惯性元件传感器的绕X轴的角速度测量值。

优选的，所述运动模型预测模块中的运动模型采用里程计运动模型，表述为x_t＝f(μ_t，x_t-1)，为当前时刻惯性元件传感器的测量值，包括角速度信息和加速度信息，所述先验点云地图为在定位应用的场景中，利用3D激光雷达的采集数据和激光SLAM系统生成的该场景下的点云地图，定位应用中，对地图点云的稀疏性没有要求，所述点云配准方法采用的是正态分布换(NDT)法。

与现有技术相比，本发明的有益效果是：

本发明在先验地图点云较稀疏时仍能保持好的定位效果，且不依赖GPS或RTK就能实现实时局部定位，当3D激光雷达和惯性元件传感器的输入信息有一个消失时仍能在一定精度范围内完成实时定位，保证了系统较强的鲁棒性，为无人车的安全提供了保障。

附图说明

图1为本发明中无人驾驶汽车实时定位方法流程图；

图2为本发明中运动模型预测模块流程图；

图3为本发明中点云配准模块流程图；

图4为本发明中状态校正模块流程图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

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

一种无人驾驶汽车实时定位方法，包括运动模型预测模块、点云配准模块与状态校正模块，系统的硬件部分包括上位机、惯性元件传感器、3D激光雷达，运动模型预测模块根据当前时刻惯性元件传感器的观测数据和前一时刻的无人车状态，通过建立的运动模型预测当前时刻的无人车状态，点云配准模块以运动模型预测模型的预测结果作为猜测值，将先验点云地图与当前扫描得到的场景点云进行点云匹配，得到当前扫描点云和场景之间的坐标变换矩阵，利用此变换矩阵对匹配前的猜测值进行转换，输出为匹配后的点云和无人车的位姿信息，状态校正模块根据运动模型预测模块的结果和点云配准模块的输出结果，校正得到新的无人车状态高斯分布，取分布的均值作为最终的定位结果。

本发明的系统框架流程如附图1所示，运动模型预测模块的功能是根据前一时刻的状态和当前的惯性元件传感器的测量值，通过运动模型预测出当前时刻的状态信息服从高斯分布，INS为观测无人车惯性状态的惯导系统即惯性元件传感器，其将观测得到的角速度信息、加速度信息传递给状态预测组件处理，状态预测组件以接收到的惯性状态信息更新运动模型，然后通过更新后的运动模型和前一时刻的状态做出状态预测，前一时刻的状态是指在前一时刻时状态校正模块中的校正结果，点云配准模块的功能是以运动模型预测模块的状态预测结果中的位姿信息为无人车位姿真值的猜测值，在先验点云地图上，以猜测值为初始位姿，对当前3D激光雷达扫描到的场景点云信息进行配准，配准完成后再根据配准结果得到3D激光雷达观测下的无人车状态服从的高斯分布，Range为3D激光雷达实时扫描得到的当前时刻场景的点云信息，Map为过去通过3D激光雷达采集得到场景点云信息生成的先验点云地图，利用的是Lego-Loam激光SLAM方法，状态校正模块以运动模型预测模块和点云配准模块的结果为输入，分别作为状态转移概率和测量概率，最终计算出无人车状态的后验高斯分布。

运动模型预测模块流程图如附图2所示，输入量INS为惯性元件传感器测量值，作为运动模型的控制量，表示为ctr＝[occ，gyr]^T其中acc和gyr分别代表惯性元件传感器中加速度计和陀螺仪的读数，即无人车加速度和角速度的测量值，输入量状态为上一时刻校正得到的无人车状态，表示为x_-1＝[p_t-1，q_t-1，v_t-1]^T其中p_y-1、q_t-1、v_t-1分别代表上一时刻无人车的位置、姿态和速度信息，首先对部分参数初始化，确定当前时刻的噪声协方差R_t和惯性元件的偏置参数b_t，R_t表示测量过程中环境和系统等不可测变量对测量结果造成的干扰所服从的高斯分布的协方差矩阵，由经验赋值；b_t表示所用惯性元件传感器在理想状态使用中的测量值均值和真值的固定偏差，由人工标定确定；该两个变量和所用设备及应用环境有关，故不能给出确切值，由于后续环节需要对状态分布的协方差参数∑进行求根操作，故应保证其为正定的，确保协方差正定的方法是对协方差进行求取特征值确定的对角阵D和特征向量V，然后限制D的对角元素值在一定阈值以上，该阈值取值为e＝10⁹，然后重新确定协方差的值：

∑＝VDV^T

然后对输入的前一时刻状态进行采样，采样数量为2n+1，n是状态的维数，本方案中状态维数为10，采样点x[t]的确认规则是：

x^[0]＝μ

式中，λ＝a²(n+k)-n，α和k为确定o点分布在均值多远的范围内的比例系数，然后通过运动模型x_t＝f(μ_t，x_t-1)进行采样点的状态转移，运动模型函数f表述为：

μ_t为当前时刻的惯性元件传感器测量值，Δq_t是在Δt时间间隔内产生的旋转，用四元数表示为:

是t时刻惯性元件传感器的绕X轴的角速度测量值，对该模型输入采样点后，得到新的预测态的均值和协方差，每个采样点有两个与之相关的权值，一个权值/>计算均值时使用，另一个权值/>计算高斯协方差时使用。权值的确定策略如下：

预测状态的高斯模型的均值和协方差如下：

点云配准模块的流程图如附图3所示，输入量为3D激光雷达的实时扫描场景点云Range、先验的场景点云地图Map，并分别进行设置源点云、设置目标点云操作，即以Range为源点云在Map上进行配准，然后输入运动模型预测模块的预测结果作为点云配准的初始猜测值，然后进行点云配准，点云配准采用的是正态分布转换(NDT)法，在点云库(PCL)中有集成好的NDT算法模块，使用中可以在设置好参数后直接调用，然后获取点云转换矩阵，即实现源点云和目标点云的转换关系，然后根据转换矩阵在预测结果的基础上计算出配准状态，最后发布配准点云显示出来，得到最后的配准结果，即无人车的位置、姿态在点云配准后的状态值。

状态校正模块的流程图如附图4所示，首先以运动模型预测模块的结果预测状态为输入，采取和运动模型预测模块同样操作的确保协方差正定、计算采样点，然后通过观测模型映射提取采样点的位置和姿态信息提取采样点的位置和姿态信息的原因是因为预测状态的输入维度为10，包括无人车的位置、姿态、速度，而配准结果的状态维度为6，包括无人车的位置、姿态，故为了统一需要进行提取操作，然后对提取的采样点加权统计计算采样均值/>采样协方差得到观测状态s_t，所用方法和运动模型观测模块的预测状态均值、预测状态协方差相统一，然后确定预测状态/>和前一步得到的观测状态/>的互协

方差，公式如下：

然后计算卡尔曼增益K_t，公式如下：

最后根据上述计算结果，得到最终的校正状态均值和对应的协方差：

式中z_t为点云配准后的状态均值；至此，得到对当前时刻无人车状态分布的均值和协方差的预测值。

综上所述是本发明的优选实施方式，并不限于本发明，对于本领域的技术人员来说，本发明可以有多种变形。凡在本发明的精神和原则之内，所作的任何修改、等同转换、改进，均赢包含在本发明的保护范围之内。

Claims (5)

1.一种无人驾驶汽车实时定位系统，其特征在于：所述定位系统的硬件部分包括上位机、惯性元件传感器、3D激光雷达；所述系统的软件部分包括运动模型预测模块、点云配准模块与状态校正模块；所述运动模型预测模块根据当前时刻惯性元件传感器的观测数据和前一时刻的无人车状态，通过建立的运动模型预测当前时刻的无人车状态，所述点云配准模块以运动模型预测模型的预测结果作为猜测值，将先验点云地图与当前扫描得到的场景点云进行点云匹配，得到当前扫描点云和场景之间的坐标变换矩阵，利用此变换矩阵对匹配前的猜测值进行转换，输出为匹配后的点云和无人车的位姿信息，所述状态校正模块根据运动模型预测模块的结果和点云配准模块的输出结果，校正得到新的无人车状态高斯分布，取分布的均值作为最终的定位结果。

2.根据权利要求1所述的一种无人驾驶汽车实时定位方法，其特征在于：所述上位机为工控机型号为科拉德IPC-805A，所述惯性元件传感器型号为迈普时空M39，所述3D激光雷达型号为北科天绘R-Fans-16M。

3.根据权利要求1所述的一种无人驾驶汽车实时定位方法，其特征在于：以0时刻惯性元件传感器确定的坐标系作为世界坐标系，3D激光雷达坐标系也已和惯性元件传感器坐标系做好了标定，无人车在t时刻的状态定义为x_t＝[p_t，q_t，v_t]^T，所述p_t表示t时刻位置，q_t表示t时刻方向四元数，v_t表示t时刻速度，并且认为任意时刻的无人车状态都服从高斯分布。

4.根据权利要求1所述的一种无人驾驶汽车实时定位方法，其特征在于：所述无人车状态预测方程为：

x_t＝[p_t-1+Δt·v_t-1q_t-1·Δq_t，v_t-1+Δt·a_t-1]，p_t-1表示t-1时刻位置，q_t-1表示t-1时刻方向四元数，v_t-1表示t-1时刻速度所述Δt是t-1时刻到t时刻的时间间隔，a_t-1是t-1时刻的惯性元件传感器的加速度测量Δq_t值，是在Δt时间间隔内产生的旋转，用四元数表示为：

是t时刻惯性元件传感器的绕X轴的角速度测量值。

5.根据权利要求1所述的一种无人驾驶汽车实时定位方法，其特征在于：所述运动模型预测模块中的运动模型采用里程计运动模型，表述为x_t＝f(μ_t,x_t-1)，x_t-1与x_t分别为t-1和t时刻的无人车状态，μ_t为当前时刻惯性元件传感器的测量值，包括角速度信息和加速度信息；所述先验点云地图为在定位应用的场景中，利用3D激光雷达的采集数据和激光SLAM系统生成的该场景下的点云地图，定位应用中，对地图点云的稀疏性没有要求；所述点云配准模块中采用的是正态分布换法NDT进行点云匹配。