CN109991636A - 基于gps、imu以及双目视觉的地图构建方法及*** - Google Patents

Abstract

本发明公开一种基于GPS、IMU以及双目视觉的地图构建方法及***，利用IMU以及双目视觉对其进行校正并在隧道等无GPS信号或信号弱的地方使用视觉导航。能够很好的解决视觉SLAM中的问题，并提高SLAM***的鲁棒性。采用了视觉信息和IMU信息融合的方法，将多种传感器数据进行融合，提供了新的算法结构，构建稳定有效的框架，对无人驾驶汽车或机器人进行稳定有效的自主定位与地图构建，实现GPS的定位信息进行校正及补充，应用到无人驾驶汽车和机器人***，进行大规模应用。

Description

基于GPS、IMU以及双目视觉的地图构建方法及***

技术领域：

本发明公开一种基于GPS、IMU以及双目视觉的地图构建方法及***，涉及到一种基于视觉特征和IMU信息的自主定位与地图构建***，属于机器人及控制技术领域。

背景技术：

近年来，互联网技术的迅速发展给汽车制造工业带来了革命性变化的机会。与此同时，汽车智能化技术正逐步得到广泛应用，这项技术简化了汽车的驾驶操作并提高了行驶安全性。而其中最典型也是最热门的未来应用就是无人驾驶汽车。在人工智能技术的加持下，无人驾驶高速发展，正在改变人类的出行方式，进而会大规模改变相关行业格局。

对于行驶在未知区域中的无人驾驶汽车而言，由于楼宇、树木遮挡等原因，汽车常常处于无信号或弱信号的状态，无法提供有效定位；在环境恶劣的地方，因天气等原因GPS或北斗导航***信号微弱，无法对无人驾驶汽车进行有效的定位。为此，无人驾驶汽车必须具有自主定位与地图构建的能力。通过实时的自主定位与地图构建，获取周围环境信息，确定无人驾驶汽车所处的位置，为路径规划提供重要的依据。

同时定位与地图构建(simultaneous localization an mapping,SLAM)技术是机器人领域一个重要的问题，SLAM问题可以描述为：配备传感器的机器人在空间中自由的移动，利用采集到的传感器信息，对自身位置进行定位，同时在自身定位的基础上增量式的构建地图，实现机器人的同时定位和制图。影响SLAM问题解决的两个重要因素分别为传感器的数据特征和观测数据相关性，如果能够高效的利用传感器数据并提高数据关联的准确性和鲁棒性，将能够影响到机器人的定位和地图的构建。

然而如今的无人驾驶技术大多仍处于辅助驾驶阶段，缺乏自主定位与地图构建的能力。同时较少的无人驾驶汽车采用单一传感器进行自主定位与地图构建，定位与地图构建精度较低，不能有效的将多种传感器进行融合，或者能够将多种传感器数据进行融合，但是不能对无人驾驶汽车进行稳定有效的自主定位与地图构建，并且不能大规模普及。

发明内容：

本发明提供一种基于GPS、IMU以及双目视觉的地图构建方法及***，利用IMU以及双目视觉对其进行校正并在隧道等无GPS信号或信号弱的地方使用视觉导航。能够很好的解决视觉SLAM中的问题，并提高SLAM***的鲁棒性。

本发明所述的一种基于GPS、IMU以及双目视觉的地图构建方法及***，采用的技术方案为：

本发明所述的一种基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，包括以下步骤：

步骤一、初始化无人驾驶汽车或机器人的位姿,确定无人驾驶汽车行驶或机器人运动的轨迹，校正IMU惯性测量单元、双目摄像头等数据保持时间一致性；

步骤二、进行IMU惯性测量单元和双目摄像头以及基于RTK解算的GNSS接收器的信息采集与提取，从而进行视觉特征跟踪；

步骤三、基于流型的IMU预积分，包括：IMU预积分计算、计算预计分雅可比和协方差矩阵、计算残差雅可比；

步骤四、IMU初始化(视觉惯性联合初始化)，这是视觉和惯性数据融合的第一步，是一段松耦合过程，将视觉数据和快速的IMU数据相结合，包括：陀螺仪偏置标定(零偏)、尺度恢复和重力加速度预估计、加速度计偏置标定和尺度重力加速度优化；

步骤五、使用紧耦合优化模型进行优化，包括：视觉惯性相邻帧紧耦合优化、视觉惯性局部地图紧耦合优化；

步骤六、估计无人驾驶汽车或机器人轨迹和周围环境三维地图的相似程度，得出实时位姿；

步骤七、构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。

1、在步骤2)信息采集与提取中：

在每次双目摄像机采集到图像后，对图像进行ORB特征的提取，提取ORB特征后，将提取的结果存储；

对无人驾驶汽车或机器人内部的IMU数据进行实时采集，并送入基于流型的IMU预积分中进行处理。

2.在步骤3)基于流型的IMU预积分中：

所述的IMU预积分计算中，世界坐标系、IMU坐标系以及相机坐标系通常用W、B、C来表示，以一定的时间间隔Δt来采样，测量IMU的加速度a_B和角速度ω_B。IMU测量模型：

其中，表示坐标系B下IMU测量模型的角速度和加速度，_Bω_WB(t)∈R³表示在坐标系B下IMU中心B相对于世界坐标系W的瞬时角速度，R_WB表示IMU中心B相对于世界坐标系W的旋转矩阵，为R_WB的转置，_Wa(t)∈R³表示在W坐标下IMU的瞬时加速度，_Wg表示在W坐标系下重力矢量，b表示零偏，η表示高斯随机噪声，b^a、b^g分别表示加速度和重力矢量零偏，η^a、η^g分别表示加速度和重力矢量高斯随机噪声；

旋转R、平移p和速度v的导数可以表示为：

其中，_Wv表示在W坐标系下IMU的瞬时速度，表示B相对于W的瞬时角速度，R_WB表示IMU中心B相对于世界坐标系W的旋转矩阵，_Wp表示W坐标系下的瞬时平移，为其导数；

世界坐标系下的旋转R、平移p和速度v可由一般的积分公式求得,离散时间下采用欧拉积分可以将上面连续时间积分改写并联立可得：

Δt为一定的时间间隔，η^gd(t)、η^ad(t)表示离散时间下加速度和重力矢量高斯随机噪声，b^a、b^g分别表示加速度和重力矢量零偏，η^ad、η^gd分别表示加速度和重力矢量高斯随机噪声，为IMU测量模型的角速度和加速度，这时候的状态量是世界坐标系下的；

设现在有i和j两个相邻关键帧，需要已知i时刻对应的IMU状态量，求解离散时间下j时刻关键帧对应的IMU预积分值：

Δt为一定的时间间隔，Δt_ij为i、j时刻之间的时间差，g为重力矢量，分别表示随时间变化的重力矢量和加速度零偏，表示随时间变化的IMU测量模型角速度，表示随时间变化的IMU测量模型速度，R_j、R_i、v_j、v_i、p_j、p_i分别为j时刻和i时刻的旋转、速度和平移，R_k为随时间变化的旋转矩阵，为随时间变化的离散加速度和重力矢量高斯随机噪声；

为了避免重复积分采用IMU预积分方法计算i、j时刻的状态量的相对变化，也就是将参考坐标系由世界坐标系转换到IMU的i时刻坐标系下，计算得到IMU状态量在i和j时刻之间的相对量：

R_j、R_i、v_j、v_i、p_j、p_i分别为j时刻和i时刻的旋转、速度和平移，分

别为随时间变化的加速度和重力矢量零偏，表示随时间变化的IMU测量模型角速度，表示随时间变化的IMU测量模型速度，为随时间变化的离散加速度和重力矢量高斯随机噪声，ΔR_ij、Δv_ij、Δp_ij为i、j之间的旋转、速度和平移差，为ΔR_i的转置，Δt_ij为i、j时刻之间的时间差，Δt为一定的时间间隔，ΔR_ik、Δv_ik为i、k时刻的旋转和速度差；

从上面的公式可以看出，整个预积分公式和偏置，噪声都有关系，需要分别分析偏置和噪声对***的影响，噪声服从高斯正态分布，偏执服从随机游走模型；

在初始化使用的IMU预积分是没有偏置的一阶项，雅可比矩阵是在初始化完成之后进行优化之前才计算而来的，得到IMU准确的预积分值(真值)，和估计值从而得到IMU的残差；

***是服从高斯分布的，协方差矩阵是按照高斯分布计算得到，是个9*9的矩阵；

所述计算残差雅可比：

残差：

待估计的参数：

ΔR_ij、Δv_ij、Δpi_j为i、j时刻的旋转、速度和平移差，为器平均值， 为i时刻的加速度和重力矢量零偏，为其平均值，δba、δbg为其更新量，φ为旋转，Δt_ij帧间时间差，R_i、R_j、v_i、v_j、p_i、p_j表示i、j时刻的旋转矩阵、速度和平移，为其转置；

位移残差对应的雅可比如上，j时刻相应的雅可比也可如此推导，速度残差和旋转的雅可比公式与平移相似。至此IMU预积分完成。

3.在步骤4)IMU初始化(视觉惯性联合初始化)中：

利用两个联系关键帧的旋转量来估计陀螺仪的偏置，在一个常量b_g的基础上加上一个微小的改变，通过最小化旋转误差来计算出陀螺仪偏置的更新量：

其中，N是关键帧的个数，ΔR_i,i+1是两个连续关键帧预积分旋转测量量，J为协方差矩阵，分别为i+1时刻的B坐标系相对于W坐标系和i时刻的W坐标系相对于B坐标系的旋转矩阵；给以零初始值，上述函数通过G2O优化，求解最小目标函数对应的即为陀螺仪的初始偏置；

当求出零偏后将其代入预积分公式会重新计算一遍预积分值，使预积分数值更加准确；

要进行尺度恢复和重力加速度预估计，首先需要建立预估计状态向量X＝[s,g_W],其中，s是尺度，g_W是世界坐标系下的重力加速度；

这里使用了三个关键帧(用1、2、3来表示)联立视觉和IMU预积分数据构建一个AX＝B的最小二乘超定方程，至少需要四个关键帧，采用奇异值分解求最小二乘问题，速度较慢但是精度高；

I为单位矩阵，_WP_C为W坐标系下相机中心C的位移，R_WB、R_WC为W坐标系相对于B坐标系和C坐标系的旋转矩阵，Δt_nm为两帧之间的时间差(n,m＝1,2,3)；

由于尺度恢复和重力加速度预估计的计算过程没有考虑到加速度计偏置的影响，使得重力加速度和加速度计偏置难以区分，很可能导致***病态性问题，所以进行加速度计偏置标定和尺度重力加速度优化；

设惯性坐标系I的重力加速度方向g_W的归一化单位矢量θ为角度。然后计算世界坐标系W和惯性坐标系I之间的旋转矩阵：

然后把重力加速度表示成：

由于R_WI是绕Z轴的旋转，所以只受X,Y方向的影响：

包含加速度的偏置：

采用三个连续的关键帧来消除速度量：

其中[]_(:,1:2)表示矩阵的前两列，加速度偏置更新量δθ为微小角度变量，_WP_C为W坐标系下相机中心C的位移，_CP_B为IMU中心B在C坐标系下的平移，s是尺度，Δt_nm、Δp_nm、Δv_nm分别是两帧之间的时间、位移和速度差(n,m＝1,2,3)，R_WB、R_WC、R_WI为W坐标系相对于B坐标系、C坐标系和I坐标系的旋转矩阵，为I坐标系下g_I的归一化单位矢量，J为协方差矩阵；只优化重力加速度的方向，每次优化得到的数据都会被重新代入计算出最新的g_W继续优化，直达变量收敛。

在步骤5)使用紧耦合优化模型进行优化中：

视觉惯性相邻帧紧耦合优化存在两种优化方式：(1)出现地图更新：

首先构建整体优化状态向量，包含当前时刻j的旋转平移速度位移加速度计偏置和陀螺仪偏置整体误差方程包含视觉重投影误差E_proj和IMU测量误差E_IMU；

视觉重投影方程就是简单的针孔相机重投影方程，并没有什么特别其残差，雅可比计算和信息矩阵与一般视觉无异；

IMU测量误差方程包含两部分，分别是P,V,R和b_a,b_g偏置；其残差，残差雅可比矩阵和信息矩阵在IMU预积分过程已经结算完成，直接带入即可：

e_b＝b^j-b^j

(40)

e_R、e_p、e_v、e_b为旋转、平移、速度和偏置误差，R_WB、R_WC、R_WI为W坐标系相对于B坐标系、C坐标系和I坐标系的旋转矩阵，b为偏置；

(2)没出现地图更新：

如上所述，首先构建整体优化状态向量，包括当前时刻j+1和上一时刻j的旋转 平移速度位移加速度计偏置和陀螺仪偏置整体误差方程包含视觉重投影误差E_proj、IMU测量误差E_IMU和先验误差E_prior：

视觉和IMU的误差方程和前面相同，先验误差是j时刻的状态量；

e_R、e_p、e_v、e_b为旋转、平移、速度和偏置误差，为其转置，R_WB为W坐标系相对于B坐标系旋转矩阵，R_BW为B坐标系相对于W坐标系旋转矩阵，b为偏置，ρ为系数，_Wp_B、_Wv_B为IMU中心B在W坐标系下的平移和速度；

视觉惯性局部地图紧耦合优化时，需要优化的关键帧包含有视觉重投影误差和IMU测量误差。

在步骤7)的闭环检测中，构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度：

本发明所述的一种基于GPS、IMU以及双目视觉的地图构建***，该***包括车载运算中心、双目摄像头、IMU惯性测量单元、基于RTK解算的GNSS接收器电连接而成；

所述车载运算中心将ROS框架将双目摄像头、IMU惯性测量单元和基于RTK解算的GNSS接收器采集到的数据进行存储以及处理；

所述基于RTK解算的GNSS接收器接收GPS定位信息；

所述双目摄像机采集周围图像信息进行点云建模；

所述IMU惯性测量单元采集测量汽车三轴姿态角以及加速度；

首先初始化无人驾驶汽车或机器人的位姿,确定无人驾驶汽车行驶或机器人运动的轨迹，校正IMU惯性测量单元、双目摄像头等数据保持时间一致性；之后进行IMU惯性测量单元和双目摄像头以及基于RTK解算的GNSS接收器的信息采集与提取，送入车载运算中心；其次在车载运算中心中进行基于流型的IMU预积分、IMU初始化(视觉惯性联合初始化)、使用紧耦合优化模型优化计算；估计无人驾驶汽车或机器人轨迹和周围环境三维地图的相似程度，得出实时位姿；构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。

本发明的积极效果在于：

采用了视觉信息和IMU信息融合的方法，将多种传感器数据进行融合，提供了新的算法结构，构建稳定有效的框架，对无人驾驶汽车或机器人进行稳定有效的自主定位与地图构建，实现GPS的定位信息进行校正及补充，应用到无人驾驶汽车和机器人***，进行大规模应用。

附图说明：

图1是本发明的***结构图示；

图2、图3为实施例2在成都电子科技大学校园内测试时双目摄像头所拍摄图片。

具体实施方法：

下面将结合附图，对本发明的优选实施进行详细的描述。应当理解，优选实施仅为了说明本发明，而不是为了限制本发明的保护范围。

实施例1

本发明基于GPS、IMU以及双目视觉的自主定位与地图构建方法及***，包括以下步骤：

1、初始化无人驾驶汽车或机器人的位姿,确定无人驾驶汽车行驶或机器人运动的轨迹，校正IMU惯性测量单元、双目摄像头的数据保持时间一致性，保证其在同一局域网下；

2、进行IMU惯性测量单元和双目摄像头以及基于RTK解算的GNSS接收器的信息采集与提取，从而进行视觉特征跟踪；

3、基于流型的IMU预积分，包括：IMU预积分计算、计算预计分雅可比和协方差矩阵、计算残差雅可比；

4、IMU初始化(视觉惯性联合初始化)，这是视觉和惯性数据融合的第一步，是一

段松耦合过程，将视觉数据和快速的IMU数据相结合，包括：陀螺仪偏置标定(零

偏)、尺度恢复和重力加速度预估计、加速度计偏置标定和尺度重力加速度优化；

5、使用紧耦合优化模型进行优化，包括：视觉惯性相邻帧紧耦合优化、视觉惯性局部地图紧耦合优化；

紧耦合需要把图像特征加入到特征向量中，得到最终的位姿信息。最后输出双目摄像头采集到的图像以及IMU、时间以及GPS坐标的文档。

6、估计无人驾驶汽车或机器人轨迹和周围环境三维地图的相似程度得出实时位姿；

7、构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。

首先通过将IMU估计得位姿序列和相机估计的位姿序列对齐可以估计出相机轨迹的真实尺度，而且IMU可以很好地预测出图像帧的位姿以及上一时刻特征点在下帧图像的位置，提高特征跟踪算法匹配速度和应对快速旋转的算法鲁棒性，最后IMU中加速度计提供的重力向量可以将估计的位置转为实际导航需要的世界坐标系中。同时，智能手机等移动终端对摄像头的大量需求大大降低了传感器的价格成本。综合以上，是一种低成本高性能的导航方案。

实施例2

如图1所示，基于GPS、IMU以及双目视觉的地图构建***，包括车载运算中心、双目摄像头、IMU惯性测量单元、基于RTK解算的GNSS接收器等。

硬件层面融合可以将双目摄像头和IMU集成在整个电路板上，全部信号传输都是在电路板板内完成，直接在控制视觉***的微处理器以及控制IMU的微处理器之间进行数据的交换，通过视觉处理把环境或是目标信息更细节的东西提升给相关算法之后，可以提升整个***的性能。也可以采用纯数据的交互和融合，通过数据总线进行传感器间数据的交换。双目摄像头、IMU惯性测量单元、基于RTK解算的GNSS接收器的数据输出端口通过数据总线将采集到的数据输入到车载运算中心中，在车载运算中心中进行数据的存储以及通过算法对双目摄像头、IMU惯性测量单元数据进行融合并对基于RTK解算的GNSS接收器得到的GPS信号进行校正。

所述车载运算中心为整个***的核心部件，使用ROS框架将双目摄像头、IMU惯性测量单元和基于RTK解算的GNSS接收器采集到的数据进行存储以及处理。所述基于RTK解算的GNSS接收器接收GPS定位信息；所述双目摄像机采集周围图像信息进行点云建模；所述IMU惯性测量单元采集测量汽车三轴姿态角以及加速度。

首先初始化无人驾驶汽车或机器人的位姿,确定无人驾驶汽车行驶或机器人运动的轨迹，校正IMU惯性测量单元、双目摄像头等数据保持时间一致性；之后进行IMU惯性测量单元和双目摄像头以及基于RTK解算的GNSS接收器的信息采集与提取，送入车载运算中心；其次在车载运算中心中进行基于流型的IMU预积分、IMU初始化(视觉惯性联合初始化)、使用紧耦合优化模型优化计算；估计无人驾驶汽车或机器人轨迹和周围环境三维地图的相似程度，得出实时位姿；构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。

图2、图3为在成都电子科技大学校园内测试时双目摄像头所拍摄图片，我们可以看出由于采用了双目摄像头，由于知道双目摄像头的焦距和二者之间的距离以及角度，在图中取一固定标志物，通过计算我们可以更加准确的判断标志物与观测点之间的距离，提高了检测精度。

以上所述仅为本发明的优选实施例，并不用于限制本发明。显然，本领域的技术人员，可以对本发明进行各种改动和变形而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变形属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变形在内。

Claims (7)

1.一种基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，包括以下步骤：

步骤一、初始化无人驾驶汽车或机器人的位姿,确定无人驾驶汽车行驶或机器人运动的轨迹，校正IMU惯性测量单元、双目摄像头等数据保持时间一致性；

步骤二、进行IMU惯性测量单元和双目摄像头以及基于RTK解算的GNSS接收器的信息采集与提取，从而进行视觉特征跟踪；

步骤三、基于流型的IMU预积分，包括：IMU预积分计算、计算预计分雅可比和协方差矩阵、计算残差雅可比；

步骤四、IMU初始化(视觉惯性联合初始化)，这是视觉和惯性数据融合的第一步，是一段松耦合过程，将视觉数据和快速的IMU数据相结合，包括：陀螺仪偏置标定(零偏)、尺度恢复和重力加速度预估计、加速度计偏置标定和尺度重力加速度优化；

步骤五、使用紧耦合优化模型进行优化，包括：视觉惯性相邻帧紧耦合优化、视觉惯性局部地图紧耦合优化；

步骤六、估计无人驾驶汽车或机器人轨迹和周围环境三维地图的相似程度，得出实时位姿；

步骤七、构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。

2.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤2)信息采集与提取中：

在每次双目摄像机采集到图像后，对图像进行ORB特征的提取，提取ORB特征后，将提取的结果存储；

对无人驾驶汽车或机器人内部的IMU数据进行实时采集，并送入基于流型的IMU预积分中进行处理。

3.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法及***，其特征在于，在步骤3)基于流型的IMU预积分中：

所述的IMU预积分计算中，世界坐标系、IMU坐标系以及相机坐标系通常用W、B、C来表示，以一定的时间间隔Δt来采样，测量IMU的加速度a_B和角速度ω_B。IMU测量模型：

其中，表示坐标系B下IMU测量模型的角速度和加速度，_Bω_WB(t)∈R³表示在坐标系B下IMU中心B相对于世界坐标系W的瞬时角速度，R_WB表示IMU中心B相对于世界坐标系W的旋转矩阵，为R_WB的转置，_Wa(t)∈R³表示在W坐标下IMU的瞬时加速度，_Wg表示在W坐标系下重力矢量，b表示零偏，η表示高斯随机噪声，b^a、b^g分别表示加速度和重力矢量零偏，η^a、η^g分别表示加速度和重力矢量高斯随机噪声；

旋转R、平移p和速度v的导数可以表示为：

其中，_Wv表示在W坐标系下IMU的瞬时速度，表示B相对于W的瞬时角速度，R_WB表示IMU中心B相对于世界坐标系W的旋转矩阵，_Wp表示W坐标系下的瞬时平移，为其导数；

世界坐标系下的旋转R、平移p和速度v可由一般的积分公式求得,离散时间下采用欧拉积分可以将上面连续时间积分改写并联立可得：

Δt为一定的时间间隔，η^gd(t)、η^ad(t)表示离散时间下加速度和重力矢量高斯随机噪声，b^a、b^g分别表示加速度和重力矢量零偏，η^ad、η^gd分别表示加速度和重力矢量高斯随机噪声，为IMU测量模型的角速度和加速度，这时候的状态量是世界坐标系下的；

设现在有i和j两个相邻关键帧，需要已知i时刻对应的IMU状态量，求解离散时间下j时刻关键帧对应的IMU预积分值：

Δt为一定的时间间隔，Δt_ij为i、j时刻之间的时间差，g为重力矢量，分别表示随时间变化的重力矢量和加速度零偏，表示随时间变化的IMU测量模型角速度，表示随时间变化的IMU测量模型速度，R_j、R_i、v_j、v_i、p_j、p_i分别为j时刻和i时刻的旋转、速度和平移，R_k为随时间变化的旋转矩阵，为随时间变化的离散加速度和重力矢量高斯随机噪声；

为了避免重复积分采用IMU预积分方法计算i、j时刻的状态量的相对变化，也就是将参考坐标系由世界坐标系转换到IMU的i时刻坐标系下，计算得到IMU状态量在i和j时刻之间的相对量：

R_j、R_i、v_j、v_i、p_j、p_i分别为j时刻和i时刻的旋转、速度和平移，分别为随时间变化的加速度和重力矢量零偏，表示随时间变化的IMU测量模型角速度，表示随时间变化的IMU测量模型速度，为随时间变化的离散加速度和重力矢量高斯随机噪声，ΔR_ij、Δv_ij、Δp_ij为i、j之间的旋转、速度和平移差，为ΔR_i的转置，Δt_ij为i、j时刻之间的时间差，Δt为一定的时间间隔，ΔR_ik、Δv_ik为i、k时刻的旋转和速度差；

从上面的公式可以看出，整个预积分公式和偏置，噪声都有关系，需要分别分析偏置和噪声对***的影响，噪声服从高斯正态分布，偏执服从随机游走模型；

在初始化使用的IMU预积分是没有偏置的一阶项，雅可比矩阵是在初始化完成之后进行优化之前才计算而来的，得到IMU准确的预积分值(真值)，和估计值从而得到IMU的残差；

***是服从高斯分布的，协方差矩阵是按照高斯分布计算得到，是个9*9的矩阵；

所述计算残差雅可比：

残差：

待估计的参数：

ΔR_ij、Δv_ij、Δp_ij为i、j时刻的旋转、速度和平移差，为器平均值， 为i时刻的加速度和重力矢量零偏，为其平均值，δb^a、δb^g为其更新量，φ为旋转，Δt_ij帧间时间差，R_i、R_j、v_i、v_j、p_i、p_j表示i、j时刻的旋转矩阵、速度和平移，为其转置；

位移残差对应的雅可比如上，j时刻相应的雅可比也可如此推导，速度残差和旋转的雅可比公式与平移相似。至此IMU预积分完成。

4.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤4)IMU初始化(视觉惯性联合初始化)中：

利用两个联系关键帧的旋转量来估计陀螺仪的偏置，在一个常量b_g的基础上加上一个微小的改变，通过最小化旋转误差来计算出陀螺仪偏置的更新量：

其中，N是关键帧的个数，ΔR_i,i+1是两个连续关键帧预积分旋转测量量，J为协方差矩阵，分别为i+1时刻的B坐标系相对于W坐标系和i时刻的W坐标系相对于B坐标系的旋转矩阵；给以零初始值，上述函数通过G2O优化，求解最小目标函数对应的即为陀螺仪的初始偏置；

当求出零偏后将其代入预积分公式会重新计算一遍预积分值，使预积分数值更加准确；

要进行尺度恢复和重力加速度预估计，首先需要建立预估计状态向量X＝[s,g_W],其中，s是尺度，g_W是世界坐标系下的重力加速度；

这里使用了三个关键帧(用1、2、3来表示)联立视觉和IMU预积分数据构建一个AX＝B的最小二乘超定方程，至少需要四个关键帧，采用奇异值分解求最小二乘问题，速度较慢但是精度高；

I为单位矩阵，_WP_C为W坐标系下相机中心C的位移，R_WB、R_WC为W坐标系相对于B坐标系和C坐标系的旋转矩阵，Δt_nm为两帧之间的时间差(n,m＝1,2,3)；

由于尺度恢复和重力加速度预估计的计算过程没有考虑到加速度计偏置的影响，使得重力加速度和加速度计偏置难以区分，很可能导致***病态性问题，所以进行加速度计偏置标定和尺度重力加速度优化；

设惯性坐标系I的重力加速度方向g_W的归一化单位矢量θ为角度。然后计算世界坐标系W和惯性坐标系I之间的旋转矩阵：

然后把重力加速度表示成：

由于R_WI是绕Z轴的旋转，所以只受X,Y方向的影响：

包含加速度的偏置：

采用三个连续的关键帧来消除速度量：

其中[]_(:,1:2)表示矩阵的前两列，加速度偏置更新量δθ为微小角度变量，_WP_C为W坐标系下相机中心C的位移，_CP_B为IMU中心B在C坐标系下的平移，s是尺度，Δt_nm、Δp_nm、Δv_nm分别是两帧之间的时间、位移和速度差(n,m＝1,2,3)，R_WB、R_WC、R_WI为W坐标系相对于B坐标系、C坐标系和I坐标系的旋转矩阵，为I坐标系下g_I的归一化单位矢量，J为协方差矩阵；只优化重力加速度的方向，每次优化得到的数据都会被重新代入计算出最新的g_W继续优化，直达变量收敛。

5.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤5)使用紧耦合优化模型进行优化中：

视觉惯性相邻帧紧耦合优化存在两种优化方式：(1)出现地图更新：

首先构建整体优化状态向量，包含当前时刻j的旋转平移速度位移加速度计偏置和陀螺仪偏置整体误差方程包含视觉重投影误差E_proj和IMU测量误差E_IMU；

视觉重投影方程就是简单的针孔相机重投影方程，并没有什么特别其残差，雅可比计算和信息矩阵与一般视觉无异；

IMU测量误差方程包含两部分，分别是P,V,R和b_a,b_g偏置；其残差，残差雅可比矩阵和信息矩阵在IMU预积分过程已经结算完成，直接带入即可：

e_b＝b^j-b^j (40)

e_R、e_p、e_v、e_b为旋转、平移、速度和偏置误差，R_WB、R_WC、R_WI为W坐标系相对于B坐标系、C坐标系和I坐标系的旋转矩阵，b为偏置；

(2)没出现地图更新：

如上所述，首先构建整体优化状态向量，包括当前时刻j+1和上一时刻j的旋转 平移速度位移加速度计偏置和陀螺仪偏置整体误差方程包含视觉重投影误差E_proj、IMU测量误差E_IMU和先验误差E_prior：

视觉和IMU的误差方程和前面相同，先验误差是j时刻的状态量；

e_R、e_p、e_v、e_b为旋转、平移、速度和偏置误差，为其转置，R_WB为W坐标系相对于B坐标系旋转矩阵，R_BW为B坐标系相对于W坐标系旋转矩阵，b为偏置，ρ为系数，_Wp_B、_Wv_B为IMU中心B在W坐标系下的平移和速度；

视觉惯性局部地图紧耦合优化时，需要优化的关键帧包含有视觉重投影误差和IMU测量误差。

6.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤7)的闭环检测中，构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。

7.一种基于GPS、IMU以及双目视觉的地图构建***，其特征在于：

该***包括车载运算中心、双目摄像头、IMU惯性测量单元、基于RTK解算的GNSS接收器；

所述车载运算中心将ROS框架将双目摄像头、IMU惯性测量单元和基于RTK解算的GNSS接收器采集到的数据进行存储以及处理；

所述基于RTK解算的GNSS接收器接收GPS定位信息；

所述双目摄像机采集周围图像信息进行点云建模；

所述IMU惯性测量单元采集测量汽车三轴姿态角以及加速度；

硬件层面融合可以将双目摄像头和IMU集成在整个电路板上，全部信号传输都是在电路板板内完成，直接在控制视觉***的微处理器以及控制IMU的微处理器之间进行数据的交换，通过视觉处理把环境或是目标信息更细节的东西提升给相关算法之后，可以提升整个***的性能。也可以采用纯数据的交互和融合，通过数据总线进行传感器间数据的交换。双目摄像头、IMU惯性测量单元、基于RTK解算的GNSS接收器的数据输出端口通过数据总线将采集到的数据输入到车载运算中心中，在车载运算中心中进行数据的存储以及通过算法对双目摄像头、IMU惯性测量单元数据进行融合并对基于RTK解算的GNSS接收器得到的GPS信号进行校正。