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