一种车辆位姿获取方法及装置制造方法及图纸

本发明专利技术的技术方案涉及一种车辆位姿获取方法，包括：根据获取的IMU数据得到车辆当前的IMU估计位姿和对应的时间戳；结合IMU估计位姿和对应的时间戳，对获取的LiDAR扫描帧的激光点云进行运动畸变校正，得到预处理LiDAR扫描帧；从预处理LiDAR扫描帧中进行车辆位姿的特征点提取；根据车辆位姿的特征点，得到LiDAR的观测误差函数；根据IMU数据，得到IMU的误差函数；将LiDAR的观测误差函数和所述IMU的误差函数进行叠加计算，得到车辆位姿的误差函数；结合车辆位姿的特征点和所述IMU数据，令车辆位姿的误差函数达到最小值，从而得到车辆的位姿。本发明专利技术提供给的车辆位姿获取方法，在距离和方向上进行约束，具有更强的鲁棒性，并且约束位姿，提高获取的车辆位姿精确度。提高获取的车辆位姿精确度。提高获取的车辆位姿精确度。

【技术实现步骤摘要】
一种车辆位姿获取方法及装置


[0001]本专利技术涉及一种车辆位姿获取方法及装置。

技术介绍

[0002]激光SLAM(Simultaneous Localization And Mapping)系统相比于视觉SLAM系统在各种光照条件下更加可靠，但是仅仅依靠LiDAR进行位姿估计存在很多局限性，随着自动驾驶车行驶路程增加，激光里程计的误差将不断积累，难以满足自动驾驶车导航需求。而IMU能够输出高频的测量数据，为LiDAR数据扫描帧之间提供位姿估计值。因此，通过多传感器进行信息融合，可以有效弥补单一传感器存在的缺陷，降低激光里程计累计误差。
[0003]然而现有的车辆的位姿获取方法中，基于欧式距离最小的原则进行匹配，由于LiDAR数据存在大量的噪声，特征点匹配约束不足，容易导致里程计精度低。在此基础上，需要提出约束性更强、里程计精度更高的车辆位姿获取方法。

技术实现思路

[0004]本专利技术提供一种车辆位姿获取方法及装置，旨在至少解决现有技术中存在的技术问题之一。
[0005]第一方面，本专利技术的技术方案涉及一种车辆位姿获取方法，包括：
[0006]获取车辆的IMU数据和LiDAR扫描帧；
[0007]根据所述IMU数据得到车辆当前的IMU估计位姿和对应的时间戳；
[0008]结合所述IMU估计位姿和对应的时间戳，对所述LiDAR扫描帧的激光点云进行运动畸变校正，得到预处理LiDAR扫描帧；
[0009]从所述预处理LiDAR扫描帧中进行车辆位姿的特征点提取；
[0010]根据所述车辆位姿的特征点，得到LiDAR的观测误差函数；
[0011]根据所述IMU数据，得到IMU的误差函数；
[0012]将所述LiDAR的观测误差函数和所述IMU的误差函数进行叠加计算，得到车辆位姿的误差函数；
[0013]结合所述车辆位姿的特征点和所述IMU数据，令所述车辆位姿的误差函数达到最小值，从而得到车辆的位姿。
[0014]第二方面，本专利技术的技术方案涉及一种车辆位姿获取装置，包括：
[0015]车辆参数获取模块，用于获取车辆的IMU数据和LiDAR扫描帧；
[0016]车辆估计位姿获取模块，用于根据所述IMU数据得到车辆当前的IMU估计位姿和对应的时间戳；
[0017]LiDAR扫描帧预处理模块，用于结合所述IMU估计位姿和对应的时间戳，对所述LiDAR扫描帧的激光点云进行运动畸变校正，得到预处理LiDAR扫描帧；
[0018]特征点获取模块，用于从所述预处理LiDAR扫描帧中进行车辆位姿的特征点提取；
[0019]LiDAR观测误差计算模块，用于根据所述车辆位姿的特征点，得到LiDAR的观测误
差函数；
[0020]IMU误差计算模块，用于根据所述IMU数据，得到IMU的误差函数；
[0021]车辆位姿误差计算模块，用于将所述LiDAR的观测误差函数和所述IMU的误差函数进行叠加计算，得到车辆位姿的误差函数；
[0022]车辆位姿计算模块，用于结合所述车辆位姿的特征点和所述IMU数据，令所述车辆位姿的误差函数达到最小值，从而得到车辆的位姿。
[0023]本专利技术的有益效果如下：在车辆位姿获取的过程中，特征点匹配方面，每种类别特征点在欧式距离最小范围内搜索最近点对应关系，同时进行特征点法向量和主向量方向上一致性检查，使其在距离和方向上的约束面，具有更强的鲁棒性；里程计方面，通过IMU状态估计，校正LiDAR数据产生的运动畸变，并使用IMU数据构建系统优化函数，约束位姿，提高获取的车辆位姿的精确度。
附图说明
[0024]图1是本专利技术一种车辆位姿获取方法的流程示意图；
[0025]图2是本专利技术一种车辆位姿获取装置的示意图。
具体实施方式
[0026]以下将结合实施例和附图对本专利技术的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本专利技术的目的、方案和效果。
[0027]需要说明的是，如无特殊说明，当某一特征被称为“固定”、“连接”在另一个特征，它可以直接固定、连接在另一个特征上，也可以间接地固定、连接在另一个特征上。本文所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。此外，除非另有定义，本文所使用的所有的技术和科学术语与本
的技术人员通常理解的含义相同。本文说明书中所使用的术语只是为了描述具体的实施例，而不是为了限制本专利技术。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的组合。
[0028]应当理解，尽管在本公开可能采用术语第一、第二、第三等来描述各种元件，但这些元件不应限于这些术语。这些术语仅用来将同一类型的元件彼此区分开。例如，在不脱离本公开范围的情况下，第一元件也可以被称为第二元件，类似地，第二元件也可以被称为第一元件。本文所提供的任何以及所有实例或示例性语言(“例如”、“如”等)的使用仅意图更好地说明本专利技术的实施例，并且除非另外要求，否则不会对本专利技术的范围施加限制。
[0029]参照图1，本专利技术实施例提供一种车辆位姿获取方法，包括：
[0030]步骤S11：获取车辆的IMU数据和LiDAR扫描帧。
[0031]具体的，车辆上装载有IMU(InertialMeasurementUnit，惯性测量单元)和LiDAR (LaserRadar，激光雷达)。IMU采集车辆的惯性测量数据，记为IMU数据；LiDAR采集车辆的雷达数据，所述雷达数据由多个激光点构成，每一帧LiDAR扫描帧内的多个激光点记为激光点云。
[0032]步骤S12：根据所述IMU数据得到车辆当前的IMU估计位姿和对应的时间戳。
[0033]步骤S13：结合所述IMU估计位姿和对应的时间戳，对所述LiDAR扫描帧的激光点云
进行运动畸变校正，得到预处理LiDAR扫描帧。
[0034]首先获取IMU估计位姿，包括：
[0035]S21:根据获取的IMU数据中的角速度ω、加速度α、白噪声η、偏置b得到IMU角度测量值和IMU速度测量值其中IMU角度测量值的具体表达式为：的具体表达式为：的具体表达式为：为IMU角度测量值,ω
(t)
为车辆t时刻的角速度，b
(t)
为车辆t时刻的偏置，η
(t)
为车辆 t时刻的白噪声；IMU速度测量值的具体表达式为：的具体表达式为：为 IMU速度测量值，为世界坐标系到车辆坐标系的旋转矩阵，g为重力加速度，α
(t)
为车辆t 时刻的加速度，b
(t)
为车辆t时刻的偏置，η
(t)
为车辆t时刻的白噪声；
[0036]S22：结合所述IMU角度测量值和IMU速度测量值得到车辆在t+Δt时刻的运动状态信息X；其中，所述运动状态信息X的具体表达式为X＝[R
T
，P
T
本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种车辆位姿获取方法，其特征在于，包括：获取车辆的IMU数据和LiDAR扫描帧；根据所述IMU数据得到车辆当前的IMU估计位姿和对应的时间戳；结合所述IMU估计位姿和对应的时间戳，对所述LiDAR扫描帧的激光点云进行运动畸变校正，得到预处理LiDAR扫描帧；从所述预处理LiDAR扫描帧中进行车辆位姿的特征点提取；根据所述车辆位姿的特征点，得到LiDAR的观测误差函数；根据所述IMU数据，得到IMU的误差函数；将所述LiDAR的观测误差函数和所述IMU的误差函数进行叠加计算，得到车辆位姿的误差函数；结合所述车辆位姿的特征点和所述IMU数据，令所述车辆位姿的误差函数达到最小值，从而得到车辆的位姿。2.如权利要求1所述的一种车辆位姿获取方法，其特征在于，所述根据所述IMU数据得到车辆当前的IMU估计位姿包括：根据获取的IMU数据中的角速度ω、加速度α、白噪声η、偏置b得到IMU角度测量值和IMU速度测量值其中IMU角度测量值的具体表达式为：的具体表达式为：为IMU角度测量值,ω
(t)
为车辆t时刻的角速度，b
(t)
为车辆t时刻的偏置，η
(t)
为车辆t时刻的白噪声；IMU速度测量值的具体表达式为：的具体表达式为：为IMU速度测量值，为世界坐标系到车辆坐标系的旋转矩阵，g为重力加速度，α
(t)
为车辆t时刻的加速度，b
(t)
为车辆t时刻的偏置，η
(t)
为车辆t时刻的白噪声；结合所述IMU角度测量值和IMU速度测量值得到车辆在t+Δt时刻的运动状态信息X；其中，所述运动状态信息X的具体表达式为X＝[R
T
,P
T
,v
T
,b
T
]
T
；车辆在t+Δt时刻的速度v
(t+Δt)
的具体表达式为v
(t)
为车辆在t时刻的速度，R
(t)
为车辆在t时刻的旋转矩阵；车辆在t+Δt时刻的位置P
(t+Δt)
的具体表达式为P
(t)
为车辆在t时刻的位置，v
(t)
为车辆在t时刻的速度；车辆在t+Δt时刻的旋转矩阵R
(t+Δt)
的具体表达式为的具体表达式为R
(t)
为车辆t时刻的旋转矩阵。3.如权利要求2所述的一种车辆位姿获取方法，其特征在于，所述结合所述估计位姿，对所述LiDAR扫描帧的激光点云进行运动畸变校正，得到预处理LiDAR扫描帧，包括：根据所述IMU估计位姿和时间戳，计算每个激光点产生的运动畸变量；将所述LiDAR扫描帧的激光点云转移至起始点坐标系；令所述LiDAR扫描帧每个激光点的原始坐标减去所述运动畸变量，得到预处理LiDAR扫描帧。4.如权利要求3所述的一种车辆位姿获取方法，其特征在于，所述计算每个激光点产生的运动畸变量包括：
计算车辆在i时刻和j时刻之间相对运动的旋转矩阵变化量ΔR
ij
，所述旋转矩阵变化量ΔR
ij
的具体表达式为ΔR
ij
＝(R
i
)
T
R
j
,R
i
为车辆在i时刻的旋转矩阵，R
j
为车辆在j时刻的旋转角度；计算车辆在i时刻和j时刻之间相对运动的位置变化量ΔP
ij
，所述位置变化量ΔP
ij
的具体表达式为R
i
为车辆在i时刻的旋转矩阵，P
j
为车辆在i时刻的位置，P
i
为车辆在j时刻的位置，v
i
为车辆在i时刻的速度，Δt
ij
为i时刻和j时刻的差值，g为重力加速度；计算车辆在i时刻和j时刻之间相对运动的速度变化量Δv
ij
，所述速度变化量Δv
ij
的具体表达式为Δv
ij
＝(R
i
)
T
(v
j
‑
v
i
‑
gΔt
ij
),R
i
为车辆在i时刻的旋转矩阵,v
j
为车辆在j时刻的速度，v
i
为车辆在i时刻的速度，g为重力加速度，Δt
ij
为i时刻和j时刻的差值。5.如权利要求4所述的一种车辆位姿获取方法，其特征在于，所述从所述预处理LiDAR扫描帧中进行车辆位姿的特征点提取，包括：对所述预处理LiDAR扫描帧中获取预处理的激光点云，并对所述预处理的激光点云进行双阈值地面滤波处理，得到滤波后的激光点云；将所述滤波后的激光点云分为地面点和非地面点；根据所述地面点和非地面点得到车辆位姿的多类别特征点。6.如权利要求5所述的一种车辆位姿获取方法，其特征在于，所述将所述滤波后的激光点云分为地面点和非地面点，包括：将所述滤波后的激光点云投影到水平参考面，并得到所述水平参考面的网格g
i
中最小高度平均值和相邻网格的最...

【专利技术属性】
技术研发人员：李晓欢，李春海，苏昭宇，陈倩，
申请(专利权)人：广西综合交通大数据研究院，
类型：发明
国别省市：