CN110988894B - 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法 - Google Patents

一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法 Download PDF

Info

Publication number
CN110988894B
CN110988894B CN201911360717.6A CN201911360717A CN110988894B CN 110988894 B CN110988894 B CN 110988894B CN 201911360717 A CN201911360717 A CN 201911360717A CN 110988894 B CN110988894 B CN 110988894B
Authority
CN
China
Prior art keywords
state
unmanned vehicle
point cloud
time
real
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911360717.6A
Other languages
English (en)
Other versions
CN110988894A (zh
Inventor
张祖锋
杨迪海
闵文芳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Changjia Fengxing Suzhou Intelligent Technology Co ltd
Original Assignee
Changjia Fengxing Suzhou Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Changjia Fengxing Suzhou Intelligent Technology Co ltd filed Critical Changjia Fengxing Suzhou Intelligent Technology Co ltd
Priority to CN201911360717.6A priority Critical patent/CN110988894B/zh
Publication of CN110988894A publication Critical patent/CN110988894A/zh
Application granted granted Critical
Publication of CN110988894B publication Critical patent/CN110988894B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Abstract

本发明公开了面向港口环境的多源数据融合的无人驾驶汽车实时定位方法，包括以下步骤，(1)通过惯性传感器观测的无人车惯性信息，计算出当前时刻的预测状态，(2)将先验点云地图与当前场景点云进行点云匹配，并根据预测状态的数值匹配出配准结果，包括无人车的位置信息和点云；(3)根据预测状态数据和配准结果，校正得到无人车状态的分布，并取其分布的均值作为定位结果。在先验地图点云较稀疏时，能保持好的定位效果，且不依赖GPS或RTK就能实现实时局部定位，保证了无人驾驶汽车的定位性能高，为无人车的安全提供了保障。

Description

一种面向港口环境的多源数据融合的无人驾驶汽车实时定位 方法
技术领域
本发明属于人工智能技术领域，尤其是涉及一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法。
背景技术
近年来，全球范围内掀起了自动驾驶技术研究的热潮。自动驾驶技术经过十多年发展，逐渐从实验室研究，到封闭环境技术竞赛，再到固定场景示范应用，虽然取得了长足的进展，但距离实际应用仍有加大距离。车企普遍采用的由低级辅助驾驶到高级辅助驾驶，再到限定场景自动驾驶，最后到达全自动驾驶的路途并不十分顺利。
L4级别(超高度自动驾驶)和L5级别(全自动驾驶)的自动驾驶座位智能驾驶技术的理想目标，是人工智能皇冠中各国和厂商奋力竞争摘取的最璀璨的明珠，是无人驾驶市场领域高效的生产力，奠定我国在未来新兴产业国际市场的领导地位具有十分重要的意义。
在面向港口环境的无人驾驶应用场景中，有大量的集装箱卡车且一般行驶速度较低，在场景中还有大量金属箱体和其他金属障碍物。无人车定位是自动驾驶系统的核心基础功能，同时也是限制自动驾驶技术发展的瓶颈技术之一，在港口的运行环境下，无人车依靠单一传感器难以达到理想的定位效果。高精度卫星导航接收机、惯性导航设备、激光雷达、视觉等传感器可以实现绝对或相对定位，但是在港口场景中，上述的装置接收的信号弱或无法接收到信号时会影响无人车的实时定位，从而影响无人车的安全运行。
因此，亟需一种能够解决上述技术问题，通过融合实时点云和惯导信息，可以实现无人车精准、稳定、实时定位，为进一步的导航规划奠定了基础。
发明内容
本发明的目的是提供一种结构简单、融合实时点云和惯导信息的多源数据、确保在传感器失灵的情况下，依然能够完成无人车的实时定位、无人车安全性高的面向港口环境的多源数据融合的无人驾驶汽车实时定位方法。
本发明的技术方案如下：
一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法，包括以下步骤：
(1)预测无人车的当前时刻状态：通过惯性传感器观测无人车惯性信息，根据前一时刻的状态和当前时刻的惯性传感器的测量值，得出服从高斯分布的当前时刻的预测状态
Figure GDA0003520152810000021
的均值μ′和协方差∑′；
(2)点云配准：将所述步骤(1)中得到的无人车的预测状态
Figure GDA0003520152810000022
的均值μ′和协方差∑′作为预测值，对先验点云地图与当前场景点云进行点云匹配，得到当前场景点云和场景之间的坐标变换矩阵，利用该变换矩阵对匹配前的预测值进行转换，输出匹配后配准结果，所述配准结果包括点云和无人车的位置信息；
(3)状态校正：根据所述步骤(1)的预测状态数据和步骤(2)的配准结果，校正得到校准后的无人车状态。
在上述技术方案中，所述步骤(1)中预测出服从高斯分布的当前时刻的状态信息的步骤如下：
(1-1)对惯性传感器的滤波参数初始化，确定在当前时刻t的噪声协方差Rt和惯性元件的偏置参数bt，计算出当前时刻t的协方差值∑，由如下公式计算；
∑＝VDVT；
其中，D为对角阵，V为特征向量，Rt为惯性传感器在测量过程中环境和系统不可测变量对测量结果造成的干扰所服从的高斯分布的协方差矩阵；
(1-2)对前一时刻状态进行采样，计算出预测状态
Figure GDA0003520152810000023
的采样点χ[i]的分布，由如下公式计算：
χ[0]＝μ
Figure GDA0003520152810000024
Figure GDA0003520152810000025
λ＝α2(n+κ)-n
其中，设定采样数量为2n+1，n是状态的维数，设定状态维数为10，μ、α、κ为确定σ点分布在均值范围内的比例系数；
(1-3)对所述步骤(1-2)中的采样点χ[i]根据预设的运动模型函数进行状态转移，得到新的采样点为y[i]，由如下公式计算：
y[i]＝f(χ[i],ctrt)；
ctr＝[acc,gyr]T；
xt-1＝[pt-1,qt-1,vt-1]T
[pt,qt,vt]T＝ft＝[pt-1,qt-1,vt-1]T·[1,Δq,1]·[vt-1dt,0,acct·Δt]T；
Figure GDA0003520152810000031
其中，y[i]＝f(χ[i],ctrt)为运动模型函数，ctrt为当前时刻的惯性元件传感器测量值，acc为无人车加速度值，gyr为无人车角速度值，xt-1为上一时刻无人车状态，pt-1为上一时刻无人车的位置，qt-1为上一时刻无人车的姿态，vt-1为上一时刻无人车的速度，pt为当前时刻t无人车的位置，qt当前时刻t无人车的姿态，vt为当前时刻t无人车的速度，Δqt为无人车在当前时刻与上一时刻的时间间隔Δt内产生的旋转，
Figure GDA0003520152810000032
分别为无人车在当前时刻t的x、y、z方向上的角速度值；vt-1dt是速度在时间上的积分；Δq为上一时刻和当前时刻无人车的姿态变化；
(1-4)对所述步骤(1-3)中得到采样点y[i]进行加权统计，计算得出预测状态
Figure GDA0003520152810000033
的均值μ′和协方差∑′，由如下公式计算：
Figure GDA0003520152810000034
Figure GDA0003520152810000035
Figure GDA0003520152810000036
Figure GDA0003520152810000037
Figure GDA0003520152810000038
其中，每个采样点有2个权值，
Figure GDA0003520152810000039
为用于计算均值的权值，
Figure GDA00035201528100000310
为用于计算协方差的权值，β为确定σ点分布在均值范围内的比例系数；y′是y[i]的导数。
在上述技术方案中，所述步骤(2)中点云匹配的步骤如下：
(2-1)激光雷达扫描获取实时场景点云Range和先验场景点云Map，设置先验场景点云Map为源点云，实时场景点云点云Range为目标点云；
(2-2)将所述步骤(1-4)中得到的预测状态
Figure GDA00035201528100000311
的均值μ′和协方差∑′作为初始的预测值，在点云库中将预测状态
Figure GDA00035201528100000312
转换而获取点云转换矩阵；
(2-3)根据所述步骤(2-2)中的点云转换矩阵，在预测状态
Figure GDA00035201528100000313
的均值μ′基础上计算出配准状态，得到配准的无人车的位置信息和点云。
在上述技术方案中，所述步骤(2-2)中采用正态分布转换法将预测状态转换而得到点云转换矩阵。
在上述技术方案中，所述步骤(3)中的校准步骤如下：
(3-1)以根据所述步骤(1-4)得到的预测状态
Figure GDA0003520152810000041
为输入值，与所述步骤(1)中确保协方差正定相同的方法来计算采样点，通过观测模型映射提取所述采样点的位置和姿态信息
Figure GDA0003520152810000042
(3-2)对所述步骤(2-3)中提取的采样点进行加权统计，计算出观测状态Zt的采样均值
Figure GDA0003520152810000043
状态协方差∑t，由如下公式计算：
Figure GDA0003520152810000044
Figure GDA0003520152810000045
(3-3)根据所述步骤(1-2)中预测状态
Figure GDA0003520152810000046
及所述步骤(3-2)中的观测状态St，计算出互协方差
Figure GDA0003520152810000047
由以下公式计算：
Figure GDA0003520152810000048
(3-4)将所述步骤(3-3)中得到的互协方差
Figure GDA0003520152810000049
计算卡尔曼增益Kt，由如下公式计算：
Figure GDA00035201528100000410
(3-5)根据所述步骤(3-4)中的卡尔曼增益Kt，计算得到最终的校正状态均值μt和状态协方差∑t，由如下公式计算：
Figure GDA00035201528100000411
Figure GDA00035201528100000412
其中，zt为点云配准后的状态均值，St是观测状态Zt的残差。
在上述技术方案中，所述步骤(2)中配准结果的状态维度为6。
本发明的另一个目的是提供一种用于无人驾驶汽车的实时定位装置，包括上位机、惯性传感器、激光雷达，所述上位机与惯性传感器、激光雷达通讯连接；
所述上位机用于接收并处理所述惯性传感器和激光雷达的数据，将处理得到的结果返回当前时刻的无人车状态；
所述惯性传感器用于为上位机实时提供惯性信息，所述惯性信息包括当前时刻的角速度和线性角速度；
所述激光雷达用于为上位机实时提供视角内当前场景的3D点云信息。
本发明的另一个目的是提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行命令，当处理器执行所述计算机执行指令时，实现所述的无人驾驶汽车实时定位方法。
本发明具有的优点和积极效果是：
1.针对在港口场景下，当无人驾驶汽车扫描得到的先验地图点云较稀疏时，能保持好的定位效果，且不依赖GPS或RTK就能实现实时局部定位，保证了无人驾驶汽车的定位性能高，为无人车的安全提供了保障。
2.在车载的激光雷达和惯性传感器的输入信息中消失时，依旧能在一定精度范围内完成对无人驾驶汽车的实时定位，保证了较强的鲁棒性。
附图说明
图1是本发明的无人驾驶汽车实时定位方法的流程图；
图2是本发明中预测无人车的当前时刻状态的流程图；
图3是本发明中点云配准的流程图；
图4是本发明中状态校正的流程图。
具体实施方式
以下结合具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明，决不限制本发明的保护范围。
实施例1
如图所示，INS为惯性传感器的测量值；以0时刻惯性传感器确定的坐标系作为世界坐标系，激光雷达得到的点云坐标系与惯性传感器的坐标系做好标定。
本发明的面向港口环境的多源数据融合的无人驾驶汽车实时定位方法，具体包括以下步骤：
(1)预测无人车的当前时刻状态：通过惯性传感器观测无人车惯性信息，根据前一时刻的状态和当前时刻的惯性传感器(型号为迈普时空M39)的测量值，预测出服从高斯分布的当前时刻的状态信息；
(1-1)对惯性传感器的滤波参数初始化，确定在当前时刻t的噪声协方差Rt和惯性元件的偏置参数bt，计算出当前时刻t的协方差值∑，由如下公式计算；
∑＝VDVT；
其中，D为对角阵，V为特征向量，Rt为惯性传感器在测量过程中环境和系统不可测变量对测量结果造成的干扰所服从的高斯分布的协方差矩阵，由于上述Rt与bt为所述惯性传感器及应用环境有关，因此，需要根据实际环境而定义具体的数值。
在此应当说明，由于后续的计算处理需要对状态分布的协方差值∑进行求根，因此要保证协方差值∑为正定的；并且根据对角阵D和特征向量V来计算协方差值∑，并且限制D的对角元素值在预设阈值e以上，且该阈值为e＝10-9：
(1-2)对前一时刻状态进行采样，计算出预测状态
Figure GDA0003520152810000061
的采样点χ[i]的分布，由如下公式计算：
χ[0]＝μ
Figure GDA0003520152810000062
Figure GDA0003520152810000063
λ＝α2(n+κ)-n
其中，设定采样数量为2n+1，n是状态的维数，设定状态维数为10，μ、α、κ为确定σ点分布在均值范围内的比例系数；
(1-3)对所述步骤(1-2)中的采样点χ[i]根据预设的运动模型函数进行状态转移，得到新的采样点为y[i]，由如下公式计算：
y[i]＝f(χ[i],ctrt)；
ctr＝[acc,gyr]T；
xt-1＝[pt-1,qt-1,vt-1]T
[pt,qt,vt]T＝ft＝[pt-1,qt-1,vt-1]T·[1,Δq,1]·[vt-1dt,0,acct·Δt]T；
Figure GDA0003520152810000064
其中，y[i]＝f(χ[i],ctrt)为运动模型函数，ctrt为当前时刻的惯性元件传感器测量值，acc为无人车加速度值，gyr为无人车角速度值，xt-1为上一时刻无人车状态，pt-1为上一时刻无人车的位置，qt-1为上一时刻无人车的姿态，vt-1为上一时刻无人车的速度，pt为当前时刻t无人车的位置，qt当前时刻t无人车的姿态，vt为当前时刻t无人车的速度，Δqt为无人车在当前时刻与上一时刻的时间间隔Δt内产生的旋转，
Figure GDA0003520152810000065
分别为无人车在当前时刻t的x、y、z方向上的角速度值；vt-1dt是速度在时间上的积分；Δq为上一时刻和当前时刻无人车的姿态变化；
(1-4)对所述步骤(1-3)中得到采样点y[i]进行加权统计，计算得出预测状态
Figure GDA0003520152810000066
的均值μ′和协方差∑′，由如下公式计算：
Figure GDA0003520152810000067
Figure GDA0003520152810000071
Figure GDA0003520152810000072
Figure GDA0003520152810000073
Figure GDA0003520152810000074
其中，每个采样点有2个权值，
Figure GDA0003520152810000075
为用于计算均值的权值，
Figure GDA0003520152810000076
为用于计算协方差的权值，β为确定σ点分布在均值范围内的比例系数；y′是y[i]的导数；
(2)点云配准：将所述步骤(1)中得到的无人车的预测状态
Figure GDA0003520152810000077
的均值μ′和协方差∑′作为预测值，对先验点云地图与当前场景点云进行点云匹配，得到当前场景点云和场景之间的坐标变换矩阵，利用该变换矩阵对匹配前的预测值进行转换，从而输出匹配后的点云和无人车的位置信息；
(2-1)激光雷达扫描获取实时场景点云Range和先验场景点云Map，设置先验场景点云Map为源点云，实时场景点云Range为目标点云；
(2-2)将所述步骤(1-4)中得到的预测状态
Figure GDA0003520152810000078
的均值μ′和协方差∑′作为初始的预测值，通过正态分布转换法(NDT)，在点云库(在点云库中有NDT算法模块)中将预测状态
Figure GDA0003520152810000079
转换而获取点云转换矩阵；
(2-3)根据所述步骤(2-2的点云转换矩阵，在预测状态
Figure GDA00035201528100000710
的均值μ′基础上计算出配准状态，得到配准结果(包括无人车的位置、姿态数据)。
进一步地说，在所述步骤(2-3)中，得到配准状态后，将配准状态显示公布出来而得到最后的配准结果，其中，配准结果的状态维度为6。
(3)状态校正：根据所述预测状态数据和配准结果，校正得到校准后的无人车状态；
(3-1)以根据所述步骤(1-4)得到的预测状态
Figure GDA00035201528100000711
为输入值，与所述步骤(1)中确保协方差正定相同的方法来计算采样点，通过观测模型映射提取所述采样点的位置和姿态信息
Figure GDA00035201528100000712
(3-2)对所述步骤(2-3)中提取的采样点进行加权统计，计算出观测状态Zt的采样均值
Figure GDA00035201528100000713
状态协方差∑t，由如下公式计算：
Figure GDA00035201528100000714
Figure GDA00035201528100000715
(3-3)根据所述步骤(1-2)中预测状态
Figure GDA0003520152810000081
及所述步骤(3-2)中的观测状态Zt，计算出互协方差
Figure GDA0003520152810000082
由以下公式计算：
Figure GDA0003520152810000083
(3-4)将所述步骤(3-3)中得到的互协方差
Figure GDA0003520152810000084
计算卡尔曼增益Kt，由如下公式计算：
Figure GDA0003520152810000085
(3-5)根据所述步骤(3-4)中的卡尔曼增益Kt，计算得到最终的校正状态均值μt和校正状态协方差∑t，由如下公式计算：
Figure GDA0003520152810000086
Figure GDA0003520152810000087
其中，zt为点云配准后的状态均值，St是观测状态Zt的残差。
(4)在得到的所述步骤(3-5)校正状态均值μt和校正状态协方差∑t后，返回配准点云。
实施例2
在实施例1的基础上，基于无人驾驶汽车实时定位方法的实时定位装置，包括上位机、惯性传感器、激光雷达，所述上位机与惯性传感器、激光雷达通讯连接。
所述上位机用于接收并处理所述惯性传感器和激光雷达的数据，将处理得到的结果返回当前时刻的无人车状态。
进一步地说，所述上位机为工控机，型号为科拉德IPC-805A。
所述惯性传感器用于为上位机实时提供惯性信息，所述惯性信息包括当前时刻的角速度和线性角速度。
所述激光雷达用于为上位机实时提供视角内当前场景的3D点云信息。
进一步地说，所述激光雷达采用3D激光雷达，3D激光雷达的型号为北科天绘R-Fans-16M。
进一步地说，实施例1中的Map为通过3D激光雷达采集得到的场景点云信息，利用Lego-Loam激光SLAM方法得到场景点云信息。
实施例3
在一些可能的实施方式中，本申请提供的无人驾驶汽车实时定位方法的各个方面还可以实现为一种程序产品的形式，其包括程序代码，当程序产品在计算机设备上运行时，程序代码用于使用计算机设备执行本说明书上描述的根据本申请的实施方式的无人驾驶汽车实时定位方法中的步骤。
程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光线、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件或者上述的任意核实的组合。
计算机可读存储介质中存储有计算机执行命令，当处理器执行计算机执行指令时，实现如本申请的实施方式的无人驾驶汽车实时定位方法。
为了易于说明，实施例中使用了诸如“上”、“下”、“左”、“右”等空间相对术语，用于说明图中示出的一个元件或特征相对于另一个元件或特征的关系。应该理解的是，除了图中示出的方位之外，空间术语意在于包括装置在使用或操作中的不同方位。例如，如果图中的装置被倒置，被叙述为位于其他元件或特征“下”的元件将定位在其他元件或特征“上”。因此，示例性术语“下”可以包含上和下方位两者。装置可以以其他方式定位(旋转90度或位于其他方位)，这里所用的空间相对说明可相应地解释。
而且，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个与另一个具有相同名称的部件区分开来，而不一定要求或者暗示这些部件之间存在任何这种实际的关系或者顺序。
以上对本发明做了示例性的描述，应该说明的是，在不脱离本发明的核心的情况下，任何简单的变形、修改或者其他本领域技术人员能够不花费创造性劳动的等同替换均落入本发明的保护范围。

Claims (8)

1.一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法，其特征在于，包括以下步骤：
(1)预测无人车的当前时刻状态：通过惯性传感器观测无人车惯性信息，根据前一时刻的状态和当前时刻的惯性传感器的测量值，得出服从高斯分布的当前时刻的预测状态
Figure FDA0003520152800000011
的均值μ′和协方差∑′；
(2)点云配准：将所述步骤(1)中得到的无人车的预测状态
Figure FDA0003520152800000012
的均值μ′和协方差∑′作为预测值，对先验点云地图与当前场景点云进行点云匹配，得到当前场景点云和场景之间的坐标变换矩阵，利用该变换矩阵对匹配前的预测值进行转换，输出匹配后配准结果，所述配准结果包括点云和无人车的位置信息；
(3)状态校正：根据所述步骤(1)的预测状态数据和步骤(2)的配准结果，校正得到校准后的无人车状态。
2.根据权利要求1所述的无人驾驶汽车实时定位方法，其特征在于，所述步骤(1)中预测出服从高斯分布的当前时刻的状态信息的步骤如下：
(1-1)对惯性传感器的滤波参数初始化，确定在当前时刻t的噪声协方差Rt和惯性元件的偏置参数bt，计算出当前时刻t的协方差值∑，由如下公式计算；
∑＝VDVT；
其中，D为对角阵，V为特征向量，Rt为惯性传感器在测量过程中环境和系统不可测变量对测量结果造成的干扰所服从的高斯分布的协方差矩阵；
(1-2)对前一时刻状态进行采样，计算出预测状态
Figure FDA0003520152800000013
的采样点χ[i]的分布，由如下公式计算：
χ[0]＝μ
Figure FDA0003520152800000014
Figure FDA0003520152800000015
λ＝α2(n+κ)-n
其中，设定采样数量为2n+1，n是状态的维数，设定状态维数为10，μ、α、κ为确定σ点分布在均值范围内的比例系数；
(1-3)对所述步骤(1-2)中的采样点χ[i]根据预设的运动模型函数进行状态转移，得到新的采样点为y[i]，由如下公式计算：
y[i]＝f(χ[i]，ctrt)；
ctr＝[acc，gyr]T；
xt-1＝[pt-1，qt-1，vt-1]T
[pt，qt，vt]T＝ft＝[pt-1，qt-1，vt-1]T·[1，Δq，1]·[vt-1dt，0，acct·Δt]T；
Figure FDA0003520152800000021
其中，y[i]＝f(χ[i]，ctrt)为运动模型函数，ctrt为当前时刻的惯性元件传感器测量值，acc为无人车加速度值，gyr为无人车角速度值，xt-1为上一时刻无人车状态，pt-1为上一时刻无人车的位置，qt-1为上一时刻无人车的姿态，vt-1为上一时刻无人车的速度，pt为当前时刻t无人车的位置，qt当前时刻t无人车的姿态，vt为当前时刻t无人车的速度，Δqt为无人车在当前时刻与上一时刻的时间间隔Δt内产生的旋转，
Figure FDA0003520152800000022
分别为无人车在当前时刻t的x、y、z方向上的角速度值；vt-1dt是速度在时间上的积分；Δq为上一时刻和当前时刻无人车的姿态变化；
(1-4)对所述步骤(1-3)中得到采样点y[i]进行加权统计，计算得出预测状态
Figure FDA00035201528000000212
的均值μ′和协方差∑′，由如下公式计算：
Figure FDA0003520152800000023
Figure FDA0003520152800000024
Figure FDA0003520152800000025
Figure FDA0003520152800000026
Figure FDA0003520152800000027
其中，每个采样点有2个权值，
Figure FDA0003520152800000028
为用于计算均值的权值，
Figure FDA0003520152800000029
为用于计算协方差的权值，β为确定σ点分布在均值范围内的比例系数；y′是y[i]的导数。
3.根据权利要求2所述的无人驾驶汽车实时定位方法，其特征在于，所述步骤(2)中点云匹配的步骤如下：
(2-1)激光雷达扫描获取实时场景点云Range和先验场景点云Map，设置先验场景点云Map为源点云，实时场景点云点云Range为目标点云；
(2-2)将所述步骤(1-4)中得到的预测状态
Figure FDA00035201528000000210
的均值μ′和协方差∑′作为初始的预测值，在点云库中将预测状态
Figure FDA00035201528000000211
转换而获取点云转换矩阵；
(2-3)根据所述步骤(2-2)中的点云转换矩阵，在预测状态
Figure FDA0003520152800000031
的均值μ′基础上计算出配准状态，得到配准的无人车的位置信息和点云。
4.根据权利要求3所述的无人驾驶汽车实时定位方法，其特征在于：所述步骤(2-2)中采用正态分布转换法将预测状态转换而得到点云转换矩阵。
5.根据权利要求4所述的无人驾驶汽车实时定位方法，其特征在于，所述步骤(3)中的校准步骤如下：
(3-1)以根据所述步骤(1-4)得到的预测状态
Figure FDA0003520152800000032
为输入值，与所述步骤(1)中确保协方差正定相同的方法来计算采样点，通过观测模型映射提取所述采样点的位置和姿态信息
Figure FDA0003520152800000033
(3-2)对所述步骤(2-3)中提取的采样点进行加权统计，计算出观测状态Zt的采样均值
Figure FDA0003520152800000034
状态协方差∑t，由如下公式计算：
Figure FDA0003520152800000035
Figure FDA0003520152800000036
(3-3)根据所述步骤(1-2)中预测状态
Figure FDA0003520152800000037
及所述步骤(3-2)中的观测状态Zt，计算出互协方差
Figure FDA0003520152800000038
由以下公式计算：
Figure FDA0003520152800000039
(3-4)将所述步骤(3-3)中得到的互协方差
Figure FDA00035201528000000310
计算卡尔曼增益Kt，由如下公式计算：
Figure FDA00035201528000000311
(3-5)根据所述步骤(3-4)中的卡尔曼增益Kt，计算得到最终的校正状态均值μt和状态协方差∑t，由如下公式计算：
Figure FDA00035201528000000312
Figure FDA00035201528000000313
其中，zt为点云配准后的状态均值，St是观测状态Zt的残差。
6.根据权利要求5所述的无人驾驶汽车实时定位方法，其特征在于：所述步骤(2)中配准结果的状态维度为6。
7.一种用于无人驾驶汽车实时定位装置，其特征在于：包括上位机、惯性传感器、激光雷达，所述上位机与惯性传感器、激光雷达通讯连接；
所述上位机用于接收并处理所述惯性传感器和激光雷达的数据，将处理得到的结果返回当前时刻的无人车状态；
所述惯性传感器用于为上位机实时提供惯性信息，所述惯性信息包括当前时刻的角速度和线性角速度；
所述激光雷达用于为上位机实时提供视角内当前场景的3D点云信息。
8.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质中存储有计算机执行命令，当处理器执行所述计算机执行指令时，实现如权利要求1-6任一项所述的无人驾驶汽车实时定位方法。
CN201911360717.6A 2019-12-25 2019-12-25 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法 Active CN110988894B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911360717.6A CN110988894B (zh) 2019-12-25 2019-12-25 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911360717.6A CN110988894B (zh) 2019-12-25 2019-12-25 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法

Publications (2)

Publication Number Publication Date
CN110988894A CN110988894A (zh) 2020-04-10
CN110988894B true CN110988894B (zh) 2022-04-08

Family

ID=70077019

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911360717.6A Active CN110988894B (zh) 2019-12-25 2019-12-25 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法

Country Status (1)

Country Link
CN (1) CN110988894B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111735451B (zh) * 2020-04-16 2022-06-07 中国北方车辆研究所 一种基于多源先验信息的点云匹配高精度定位方法
CN112835085B (zh) * 2020-07-09 2022-04-12 北京京东乾石科技有限公司 确定车辆位置的方法和装置
CN112835086B (zh) * 2020-07-09 2022-01-28 北京京东乾石科技有限公司 确定车辆位置的方法和装置
CN111812658B (zh) * 2020-07-09 2021-11-02 北京京东乾石科技有限公司 位置确定方法、装置、系统和计算机可读存储介质
CN111968179B (zh) * 2020-08-13 2022-07-19 厦门大学 地下停车场自动驾驶车辆定位方法
CN112782733B (zh) * 2021-01-28 2023-08-01 北京斯年智驾科技有限公司 高精度定位方法、装置、系统、电子装置和存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN110501712A (zh) * 2019-09-05 2019-11-26 北京百度网讯科技有限公司 用于确定位置姿态数据的方法、装置、设备和介质

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN110501712A (zh) * 2019-09-05 2019-11-26 北京百度网讯科技有限公司 用于确定位置姿态数据的方法、装置、设备和介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于GNSS/SINS/双目视觉里程计的车载导航系统分析与设计;冯黎等;《汽车技术》;20191031(第10期);第37-41页 *

Also Published As

Publication number Publication date
CN110988894A (zh) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110988894B (zh) 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法
CN110645974B (zh) 一种融合多传感器的移动机器人室内地图构建方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN111060099B (zh) 一种无人驾驶汽车实时定位方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN112882053B (zh) 一种主动标定激光雷达和编码器外参的方法
Demim et al. Cooperative SLAM for multiple UGVs navigation using SVSF filter
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN111189454A (zh) 基于秩卡尔曼滤波的无人车slam导航方法
Quan et al. AGV localization based on odometry and LiDAR
CN116719037A (zh) 一种用于智能割草机器人的环境感知方法及系统
CN116642482A (zh) 基于固态激光雷达和惯性导航的定位方法、设备和介质
CN108680162B (zh) 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法
CN114067210A (zh) 一种基于单目视觉导引的移动机器人智能抓取方法
KR101907611B1 (ko) 자율 주행체의 위치 정보를 획득하는 방법
Bikmaev et al. Improving the accuracy of supporting mobile objects with the use of the algorithm of complex processing of signals with a monocular camera and LiDAR
CN113029173A (zh) 车辆导航方法及装置
Weyers et al. Improving occupancy grid FastSLAM by integrating navigation sensors
Eraghi et al. Improved Unscented Kalman Filter Algorithm to Increase the SLAM Accuracy
US20240134033A1 (en) Method for determining a movement state of a rigid body
Fortes et al. Implementation of the Particle Filter in an embedded system for the localization of a differential mobile robot
Akkaya et al. An ANN based NARX GPS/DR system for mobile robot positioning and obstacle avoidance
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
Yan et al. Research on robot positioning technology based on inertial system and vision system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant