[发明专利]视觉惯性激光数据融合的模块化无人车定位方法及系统

权利要求书

1.一种视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，包括以下步骤：

1)采集获取无人驾驶汽车当前信息，包括通过单目相机采集的图像信息，激光雷达采集的三维点云信息以及IMU采集的加速度及角速度数据；

2)根据步骤1)采集的无人驾驶汽车当前信息，进行汽车的位姿估计，获得无人驾驶汽车的位姿信息；

3)根据位姿估计的位姿信息建立多传感器融合的优化模型，根据优化模型得到车辆的最佳位姿，并将其转化至世界坐标系下，得到车辆的实时位姿；所述优化模型为基于传感器模型的IMU测量误差、激光测量误差、相机测量误差设定误差函数，根据误差函数联立建立多传感器融合的优化模型；

其中，优化模型是通过建立位姿优化模型的最小二乘问题，构建残差函数进行最小二乘的迭代优化，利用L-M法迭代优化该最小二乘问题，得到车辆的最佳位姿，其中残差函数为：

其中，是IMU的测量误差，是视觉测量误差，是激光雷达的扫描误差，P是残差项对应的协方差矩阵；σ为视觉测量误差的权重系数，τ为激光测量误差的权重系数；

视觉测量误差的权重系数和激光测量误差的权重系数确定方法如下：

根据环境信息调控单目相机与激光雷达在优化中所占的权重，权重系数根据单目相机的关键帧共视路标点数和激光雷达点云数据的匹配特征数确定，设定两帧图像关键帧的最佳共视路标点数为A及两帧激光点云数据的最佳匹配特征点数为B，在每次建立优化模型前系计算当前关键帧共视路标点数a与最佳共视路标点数A比值a/A，计算当前激光点云匹配特征点数b与最佳匹配特征点数B比值b/B，视觉测量误差的权重系数σ＝2×(a/A)/(b/B)，激光测量误差的权重系数τ＝2×[1-(a/A)/(b/B)]。

2.根据权利要求1所述的视觉惯性激光数据融合的模块化无人车定位方法，其特征在于，所述步骤2)中获得无人驾驶汽车的位姿信息，具体如下：

通过对极几何约束从本质矩阵中计算单目相机的帧间运动信息(x_v,y_v,z_v,α_v,β_v,γ_v)，并通过路标点对比算法筛选单目图像的关键帧进行标记，对IMU的测量数据进行预积分处理，然后与单目相机进行联合初始化，恢复单目相机的尺度，并估计系统的速度、陀螺仪零偏及重力方向，得到无尺度的位姿变换(x_vi,y_vi,z_vi,α_vi,β_vi,γ_vi)，即无人驾驶汽车的位姿信息。