视觉惯性激光数据融合的模块化无人车定位方法及系统技术方案

本发明专利技术公开了一种视觉惯性激光数据融合的模块化无人车定位方法及系统，该方法包括以下步骤：1)采集模块通过单目相机、激光雷达及IMU获取无人驾驶汽车当前信息；2)根据步骤1)采集的无人驾驶汽车当前信息，位姿估计模块通过单目里程计和IMU预积分模型进行汽车的位姿估计，获得无人驾驶汽车的位姿信息；3)位姿优化模块根据步骤2)的位姿估计信息，建立多传感器融合的优化模型，并通过权重系数动态调节模块调节各传感器优化比重，增强了环境适应性，优化后得到车辆的最佳位姿，并将其转化至世界坐标系下，得到车辆的实时位姿。本发明专利技术能够满足在复杂环境下无人驾驶车辆定位的精确性和鲁棒性要求，适用于无人驾驶车辆的复杂环境下的定位。

【技术实现步骤摘要】
视觉惯性激光数据融合的模块化无人车定位方法及系统
本专利技术涉及智能汽车定位技术，尤其涉及一种视觉惯性激光数据融合的模块化无人车定位方法及系统。
技术介绍
随着社会经济和科学技术的飞速发展，人们出行的需求也越来越旺盛，汽车逐渐成为了人们生活中不可或缺的代步工具。经过多年发展，传统的汽车行业为了应对今后越来越复杂多变的需求，也兴起一股无人驾驶技术的研究热潮。诸如国内的有百度、华为、驭势等，国外的则有谷歌、特斯拉等科技公司和NVIDIA等芯片厂商都加入了无人驾驶技术的研发当中。目前国内各大高校也开始重视无人驾驶相关技术的开发。未来，汽车工业的转型，无人驾驶将成为主要的方向。基于各种传感器的定位技术是无人驾驶中的关键技术，利用传感器获取的车辆环境与车辆自身的各种信息，计算得到车辆在环境中的准确位置。单独一种或两种传感器的定位方案如IMU航迹推演、视觉惯性里程计、激光惯性里程计等，因为传感器自身的局限性难以在复杂环境(高速运动、弱光照环境等)下进行准确的定位，鲁棒性和准确性无法保证应用要求，多传感器的融合方案中如何合理利用各传感器的优势也是一大挑战。基于此背景，本文提出一种视觉惯性激光数据融合的模块化无人车定位方法及系统。
技术实现思路
本专利技术要解决的技术问题在于针对现有技术中的缺陷，提供一种视觉惯性激光数据融合的模块化无人车定位方法及系统。本专利技术解决其技术问题所采用的技术方案是：一种视觉惯性激光数据融合的模块化无人车定位方法，包括以下步骤：1)采集获取无人驾驶汽车当前信息，包括通过单目相机采集的图像信息，激光雷达采集的三维点云信息以及IMU采集的加速度及角速度数据；2)根据步骤1)采集的无人驾驶汽车当前信息，进行汽车的位姿估计，获得无人驾驶汽车的位姿信息；3)根据位姿估计的位姿信息建立多传感器融合的优化模型，根据优化模型得到车辆的最佳位姿，并将其转化至世界坐标系下，得到车辆的实时位姿；所述优化模型为基于传感器模型的IMU测量误差、激光测量误差、相机测量误差设定误差函数，根据误差函数联立建立多传感器融合的优化模型。按上述方案，所述步骤2)中获得无人驾驶汽车的位姿信息，具体如下：通过对极几何约束从本质矩阵中计算单目相机的帧间运动信息(xv，yv，zv，αv，βv，γv)，，(其中6个物理量分别表示欧式坐标系中在x，y，z三坐标轴中的平移与旋转量，下同),并通过路标点对比算法筛选单目图像的关键帧进行标记，对IMU的测量数据进行预积分处理，然后与单目相机进行联合初始化，恢复单目相机的尺度，并估计系统的速度、陀螺仪零偏及重力方向，得到无尺度的位姿变换(xvi，yvi，zvi，αvi，βvi，γvi)，即无人驾驶汽车的位姿信息。按上述方案，所述步骤2)中获得无人驾驶汽车的位姿信息，具体如下：2.1)通过对极几何约束从本质矩阵中计算单目相机的帧间运动信息(xv，yv，zv，αv，βv，γv)：利用ORB特征点算法提取图像中的ORB特征点，并通过采用FLANN算法将相邻两帧图像I和I+1的特征点匹配，利用对极几何约束，通过RANSAC算法来估计两幅图像的本质矩阵，对本质矩阵进行奇异值分解获得两帧图像之间的旋转矩阵R与平移向量t，并恢复为帧间相对位姿(xv，yv,zv,αv,βv,γv)；2.2)利用共视路标点算法筛选图像中的关键帧，并进行标记；2.3)利用IMU在i时刻的检测量其中是i时刻的IMU坐标系下的加速度，是i时刻的IMU坐标系下的角速度，建立IMU的预积分模型如下：其中是i时刻到j时刻的平移量，是i时刻到j时刻的速度量，是i时刻到j时刻的旋转量。通过IMU预积分得到的旋转矩阵R与平移向量t恢复单目图像中的旋转矩阵R与平移向量t中的未知尺度，得到无尺度的估计位姿，将其转换到世界坐标系下为(xvi,yvi,zvi,αvi,βvi,γvi)。按上述方案，所述步骤2.2)利用共视路标点算法筛选图像中的关键帧，具体为：将先验关键帧的A个共视匹配特征点存储为路标点集合，获取单目相机传入的最新图像I，检测到I中共有B个特征点，其中有W个与路标点匹配，判断是否满足下式：σWA和σWB分别为预先设置的阈值，如果满足则将I记录为新的关键帧，并将先验关键帧中的最旧帧删除。按上述方案，所述步骤3)中优化模型的误差函数如下：IMU测量误差函数具体为：其中，为通过估计的相对运动恢复的IMU位置，为通过估计的相对运动恢复的IMU速度，为通过估计的相对运动恢复的IMU旋转，为估计的加速度计与陀螺仪零偏；相机测量误差函数具体为：其中，为系统对路标点l在相机坐标系中坐标的预测值归一化处理的结果，为路标点为相机观察到的路标l在归一化平面的坐标，两者相减即为相机测量误差；激光测量误差函数具体为：其中，其中，和分别是相机与激光雷达之间的旋转矩阵和平移向量；和为两帧激光点云的匹配点；对坐标利用相机坐标系Ci和Cj间的旋转矩阵和平移向量从坐标系Ci变换至坐标系Cj，对应坐标按上述方案，所述步骤3)中根据优化模型得到车辆的最佳位姿，是通过建立位姿优化模型的最小二乘问题，构建残差函数进行最小二乘的迭代优化，利用L-M法迭代优化该最小二乘问题，得到车辆的最佳位姿，其中残差函数为：其中，是IMU的测量误差，是视觉测量误差，是激光雷达的扫描误差，P是残差项对应的协方差矩阵；σ为视觉测量误差的权重系数，τ为激光测量误差的权重系数。按上述方案，所述步骤3)中视觉测量误差的权重系数和激光测量误差的权重系数确定方法如下：根据环境信息调控单目相机与激光雷达在优化中所占的权重，权重系数根据单目相机的关键帧共视路标点数和激光雷达点云数据的匹配特征数确定，设定两帧图像关键帧的最佳共视路标点数为A及两帧激光点云数据的最佳匹配特征点数为B，在每次建立优化模型前系计算当前关键帧共视路标点数a与最佳共视路标点数A比值a/A，计算当前激光点云匹配特征点数b与最佳匹配特征点数B比值b/B，视觉测量误差的权重系数σ＝2×(a/A)/(b/B)，激光测量误差的权重系数τ＝2×[1-(a/A)/(b/B)]。一种视觉惯性激光数据融合的模块化无人车定位系统，包括：采集模块，用于采集获取无人驾驶汽车当前信息，包括通过单目相机采集的图像信息，激光雷达采集的三维点云信息以及IMU采集的加速度及角速度数据；位姿估计模块，用于根据无人驾驶汽车当前信息，进行汽车的位姿估计，获得无人驾驶汽车的位姿信息；位姿优化模块，用于根据位姿估计模块的位姿信息建立多传感器融合的优化模型，根据优化模型得到车辆的最佳位姿，并将其转化至世界坐标系下，得到车辆的实时位姿；所述优化模型为基于传感器模型的IMU测量误差、激光测量误差、相机测量误差设定误差函数，根据误差函数联立建立多传感器融合的优化模型。按上述方案，所述位本文档来自技高网...

【技术保护点】
1.一种视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，包括以下步骤：/n1)采集获取无人驾驶汽车当前信息，包括通过单目相机采集的图像信息，激光雷达采集的三维点云信息以及IMU采集的加速度及角速度数据；/n2)根据步骤1)采集的无人驾驶汽车当前信息，进行汽车的位姿估计，获得无人驾驶汽车的位姿信息；/n3)根据位姿估计的位姿信息建立多传感器融合的优化模型，根据优化模型得到车辆的最佳位姿，并将其转化至世界坐标系下，得到车辆的实时位姿；所述优化模型为基于传感器模型的IMU测量误差、激光测量误差、相机测量误差设定误差函数，根据误差函数联立建立多传感器融合的优化模型。/n

【技术特征摘要】
1.一种视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，包括以下步骤：
1)采集获取无人驾驶汽车当前信息，包括通过单目相机采集的图像信息，激光雷达采集的三维点云信息以及IMU采集的加速度及角速度数据；
2)根据步骤1)采集的无人驾驶汽车当前信息，进行汽车的位姿估计，获得无人驾驶汽车的位姿信息；
3)根据位姿估计的位姿信息建立多传感器融合的优化模型，根据优化模型得到车辆的最佳位姿，并将其转化至世界坐标系下，得到车辆的实时位姿；所述优化模型为基于传感器模型的IMU测量误差、激光测量误差、相机测量误差设定误差函数，根据误差函数联立建立多传感器融合的优化模型。


2.根据权利要求1所述的视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，所述步骤2)中获得无人驾驶汽车的位姿信息，具体如下：
通过对极几何约束从本质矩阵中计算单目相机的帧间运动信息(xv，yv，zv,αv，βv，γv)，并通过路标点对比算法筛选单目图像的关键帧进行标记，对IMU的测量数据进行预积分处理，然后与单目相机进行联合初始化，恢复单目相机的尺度，并估计系统的速度、陀螺仪零偏及重力方向，得到无尺度的位姿变换(xvi，yvi，zvi,αvi,βvi,γvi)，即无人驾驶汽车的位姿信息。


3.根据权利要求1所述的视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，所述步骤2)中获得无人驾驶汽车的位姿信息，具体如下：
2.1)通过对极几何约束从本质矩阵中计算单目相机的帧间运动信息(xv,yv,zv,αv,βv,γv)：
利用ORB特征点算法提取图像中的ORB特征点，并通过采用FLANN算法将相邻两帧图像I和I+1的特征点匹配，利用对极几何约束，通过RANSAC算法来估计两幅图像的本质矩阵，对本质矩阵进行奇异值分解获得两帧图像之间的旋转矩阵R与平移向量t，并恢复为帧间相对位姿(xv,yv,zv,αv,βv,γv)；
2.2)利用共视路标点算法筛选图像中的关键帧，并进行标记；
2.3)利用IMU在i时刻的检测量其中是i时刻的IMU坐标系下的加速度，是i时刻的IMU坐标系下的角速度，建立IMU的预积分模型如下：









其中，是i时刻到j时刻的平移量，是i时刻到j时刻的速度量，是i时刻到j时刻的旋转量；通过IMU预积分得到的旋转矩阵R与平移向量t恢复单目图像中的旋转矩阵R与平移向量t中的未知尺度，得到无尺度的估计位姿，将其转换到世界坐标系下为(xvi,yvi,zvi,αvi,βvi,αvi)。


4.根据权利要求3所述的视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，所述步骤2.2)利用共视路标点算法筛选图像中的关键帧，具体为：
将先验关键帧的A个共视匹配特征点存储为路标点集合，获取单目相机传入的最新图像I，检测到I中共有B个特征点，其中有W个与路标点匹配，判断是否满足下式：



σWA和σWB分别为预先设置的阈值，如果满足则将I记录为新的关键帧，并将先验关键帧中的最旧帧删除。


5.根据权利要求1所述的视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，所述步骤3)中优化模型的误差函数如下：
IMU测量误差函数具体为：



其中，R旋转矩阵，p表示平移向量，v表示速度，其对应时刻、所在坐标系、转换关系有各自的上下标表示，w表示世界坐标系，bi表示i时刻车体坐标系；为通过估计的相对运动恢复的IMU位置，为通过估计的相对运动恢复的IMU速度，gw表示重力加速度；为四元数，表示通过估计的相对运动所恢复的IMU旋转；分别表示平移量、速度量和旋转量的IMU预积分，为估计的加速度计与陀螺仪零偏；
相机测量误差函数具体为：



其中，为系统对路标点l在相机坐标系中坐标的预测值归一化处理的结果，为路标点为相机观察到的路标l在归一化平面的坐标，两者相减即为相机测量误差；
激光测量误差函数具体为：



其中



其中，和分别是相机与激光雷达之间的旋转矩阵和平移向量；和为两帧激光点云的匹配点；
对坐标利用相机坐标系Ci和Cj间的旋转矩阵和平移向量从坐标系Ci变换至坐标系Cj...

【专利技术属性】
技术研发人员：李其仲，童祺堃，黄妙华，
申请(专利权)人：武汉理工大学，
类型：发明
国别省市：湖北;42