一种基于滑动窗口优化的RGBD和IMU混合跟踪注册方法

一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
技术领域
1.本发明属于增强现实领域，适用于室内工业机械产品装配场景。具体涉及一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法。


背景技术：

2.在现如今的制造业，复杂机械产品的装配一直是生产过程的关键问题。据统计，在现代制造中装配工作量占整个产品研制工作量的20％-70％，平均为45％，装配时间占整个制造时间的40％～60％。目前复杂机械产品装配过程还是手工装配为主，这种装配方式具有装配周期长，装配效率低等问题，直接影响着产品的开发成本。
3.今年来，随着科学技术水平的不断提高，通过增强现实的辅助装配技术在一定程度上解决了手工装配过程上存在的问题。该方法借助计算机生成与装配相关虚拟装配辅助信息，并将其叠加到真实的装配作业环境中，给装配人员提供了一种虚实融合的增强现实环境，使产品的装配过程更为简洁、直观，从而有效的降低了产品的开发成本。
4.其中，跟踪注册技术是指将计算机生成的虚拟物体信息及其他信息叠加真实的场景中，这也是增强现实技术实现虚实融合的关键。目前许多学者在跟踪注册领域提出了很多有价值的方法，其中主要包括基于人工标志的跟踪注册方法、基于自然特征的跟踪注册方法以及基于模型的跟踪注册方法，这些方法都是基于视觉上的方法。但在工业机械产品装配领域，工业零部件呈现出纹理稀疏、表面光滑的特点，同时在头戴设备运动的过程中，容易产生因运动而造成的图像模糊抖动的问题，在这种情况下，基于视觉的跟踪注册方法在特征的提取与跟踪上会产生很大的困难，进而导致跟踪注册的精度及鲁棒性都无法得到保障。所以，为了解决上述问题，可以在视觉传感器测量的方法基础上，引入其他类型传感器的测量信息，来克服纯视觉的跟踪注册方法在工业机械产品装配环境下的跟踪注册鲁棒性差的问题。针对上述问题，可以在视觉测量传感器的基础上，引入其他类型的体积小，成本低廉的传感器进行测量，并通过多传感器融合的思想，实现不同传感器之间的优势互补，进而提高增强现实跟踪注册过程的鲁棒性。
5.相比于普通的rgb相机，深度传感器可在表面光滑、纹理稀疏的机械产品表面中获得稠密且稳定的深度特征信息。同时，深度信息的引入还可以克服单目相机下的尺度不确定性及双目相机下的尺度估算不准确性的问题，可以辅助rgb特征信息通过icp算法进行更为稳定的跟踪注册。但是这种方式存在如下的问题：一是在相机快速运动的时候，会导致3d特征缺失，从而致使icp(迭代最近点)算法在迭代的过程中容易陷入局部最优解而失效，二是，在超出深度传感器的工作范围(0.15～4m)时，容易造成“丢帧”现象，从而导致跟踪注册失败。
6.惯性跟踪器也称为惯性测量单元(inertial measurement unit,imu),包括惯性陀螺仪和微加速计,采用捷联式惯性导航原理计算方向和位置。具有体积小、免遮挡、工作范围不受限制、更新频率高的特点，这可以有效帮助基于rgbd的跟踪注册方法克服因快速运动以及在深度测量范围受限而引起的跟踪注册鲁棒性下降的问题。虽然惯性测量单元容
易受到噪声和自身偏置的相应，随着系统的运行时间增加误差也在不断的累计，由于rgbd相机的存在，可以对imu自身的陀螺仪偏置进行标定。
7.针对上述问题，并结合机械产品装配环境特点，在传统视觉测量传感器的基础上，引入体积小、成本低廉的深度传感器及惯性测量单元(imu)，通过多传感器融合算法提升增强现实跟踪注册过程的鲁棒性，提升复杂机械产品的装配效率与稳定性。
8.申请公开号cn111462231a，一种基于rgbd传感器和imu传感器的定位方法，过程如下：首先，移动机器人搭载rgbd传感器和imu传感器采集数据；其次，采用视觉slam算法对rgbd传感器数据进行初始化获得其位姿信息，同时对imu传感器数据进行预积分，然后对移动机器人初始化获取其初始状态信息；基于重投影误差优化跟踪移动机器人的位姿信息、速度和imu传感器偏置；同时，采用基于滑动窗口的非线性优化算法，实时优化位姿信息和地图；加入回环检测，避免零偏情况的出现。该定位方法综合运用rgbd传感器和imu传感器，克服纯视觉slam算法在纯旋转以及背景单一时容易跟踪失败和imu传感器在长时间运动中产生零偏现象的缺点。
9.上述专利中，虽然同样提出了基于rgbd信息和imu信息融合的跟踪定位方法，但是本专利与其在细节处理上存在如下差异：
10.1.由于工业装配现场下的机械零部件具有纹理稀疏、表面光滑的特性，这会直接导致rgb相机无法提取出足够数量且稳定的特征点来进行跟踪注册，所以，在权利要求1中所述的视觉测量前端中，提出在rgb相机的基础上通过深度相机采集的深度信息来获得稠密点云，并以点云配准的方式来实现跟踪注册，虽然也通过rgb相机提取了2d的orb特征，但是这个orb特征在弱纹理场景下是稀疏的，本发明中通过该特征计算出的位姿信息只有两种用途：第一种是作为点云配准过程中的初始位姿信息来加速迭代过程；第二种是在深度相机深度不可测的情况下，作为视觉跟踪注册过程的唯一依据，其所产生测量的误差，通过后面的imu进行弥补。而上述专利中，在视觉的特征跟踪与匹配上，仅仅采用了rgb相机采集的orb特征，而没有采用深度传感器采集的深度信息，所以在本专利所描述的场景中跟踪注册的鲁棒性将无法得到保障。
11.2.本专利在后端优化的误差函数中，引入了边缘化先验信息，在滑动窗口中，如果有变量被移除窗口，本方法并不会将该变量及相关约束信息直接移除，而是将与该变量有约束关系的变量之间产生新的约束关系，从而将丢弃变量所携带的信息传递给剩余变量，为后续优化过程提供先验信息，从而优化得到更为准确的位姿估计，同时，本专利在滑动窗口的优化策略上，做出了相关优化，旨在保证一定的跟踪注册精度同时提升算法的实时性。而在上述专利中，后端优化过程中仅与相邻帧建立约束关系，进而被边缘化的变量的约束信息并没有参与优化，所以在跟踪注册的精度上并不会优于本专利，同时上述专利也并没有在边缘化策略上进行优化，所以在跟踪注册的实时性可能比较差。


技术实现要素：

12.本发明旨在解决以上现有技术的问题。提出了一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法。本发明的技术方案如下：
13.一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法，其包括以下步骤：
14.视觉测量前端步骤：首先，通过rgbd相机获取rgb信息与深度信息，进行特征提取
与跟踪，并采用基于rgbd的跟踪注册方法，计算当前时刻的帧的位置及观测点的3d信息，并将跟踪匹配好的特征信息加入到关键帧队列中，交给后端进行优化处理，同时imu将采集到的位姿信息进行预积分操作，得到相邻帧之间的位姿信息，作为基于rgbd的跟踪注册方法计算时的初始位姿，并交给后端进行优化处理；
15.多传感器联合初始化步骤：对三个传感器的外参、偏置、绝对尺度进行标定，通过slam算法估计滑动窗口内开始两帧的位姿及观测点的逆深度，并与imu预积分结果进行对齐求解相关参数；
16.后端优化步骤：采用基于滑动窗口的方法对将视觉测量前端传过来的初始位姿信息做进一步优化，通过先验信息约束、视觉约束、imu约束构建目标误差函数，通过基于滑动窗口的图优化算法不断优化所有帧的位姿及偏置信息；
17.回环检测与优化步骤：利用dbow视觉词袋算法进行回环检测，如果检测到回环产生，则通过重定位的方式对整个轨迹进行闭环优化。
18.进一步的，所述特征提取与跟踪是采用orb算法进行特征点的提取与匹配，orb算法的步骤具体包括：首先，通过比较当前像素点与周围像素点之间的差异来找出零部件的角点特征，其次，通过在选取的角点附近，以某种规则选取n个点对，并对这n个点对的差异结果进行组合作为该特征点的描述子，最后，在特征匹配阶段，通过角点的描述子的相似度进行特征匹配；
19.并通过ransac算法对误匹配点进行剔除，ransac算法对误匹配点进行剔除的步骤具体包括：首先，将匹配到角点特征点对通过深度信息采集映射到三维空间，并随机假设一组局内点即正确匹配点为初始值，将其拟合出一个模型；然后，用这个假设的模型去尝试拟合其他的特征点，如果适合该模型则扩充该模型，否则将该点视为外点即无匹配点进行剔除；最后，在达到最大迭代次数后，如果这假设模型内的局内点数量少于某个阈值则放弃该假设模型并重新假设迭代，否则，则认为该模型置信度高，保留该模型，并将模型的外点也即无匹配点进行剔除。
20.进一步的，所述基于rgbd的跟踪注册方法，计算当前时刻的帧的位置及观测点的3d信息，具体包括：
21.在orb特征匹配的基础上，判断对应特征点对深度信息是否可测，对于可测的特征点对，则测量对应特征点对的深度信息，通过icp算法计算当前帧的初始位姿信息，再通过深度图像获得稠密点云信息并将该初始位姿作为icp迭代的初值，进行进一步的icp迭代优化，以获得更为精准的位姿信息；对于无法获得深度的特征点对信息，则仍使用pnp方法进行位姿估计。
22.进一步的，所述sfm方法的步骤是：
23.所述通过icp算法计算当前帧的初始位姿信息，具体包括：首先，计算两组匹配的点云p1和p2的每个点的去质心坐标，
p
i和p’i
，然后，通过将变换矩阵t将点云p1点变换至p2，并构建误差函数；最后，通过迭代的方法不断优化变换矩阵t，直至误差函数低于某个阈值或者达到最大迭次数为止；
24.所述对于无法获得深度的特征点对信息，则仍使用pnp方法进行位姿估计，具体包括：首先，根据特征匹配点找出当前帧和上一帧有共视关系的观测点q，将该点通过变换矩阵t转换到当前帧的像素坐标中；然后，构建重投影误差函数，并通过迭代最小二乘的方法
不断优化变换矩阵t，进而求解当前帧的位姿。进一步的，所述imu将采集到的位姿信息进行预积分操作，通过imu测量，并结合第i时刻的pvq位姿信息，得到第j时刻的pvq位姿信息，计算公式为：
[0025][0026][0027][0028]
其中，是j，i时刻下，imu在世界坐标系下的位置信息；是在j，i时刻下，imu在世界坐标系下的速度；是在j，i时刻下，imu在世界坐标系下的旋转信息；是在t时刻下的imu加速度；是在t时刻下的imu的角速度。上述三个式子的积分部分，表示从i时刻到j时刻之间的与积分增量，分别记作，这三个量将作为后端的视觉测量的帧间约束信息来进行非线性优化。
[0029]
进一步的，所述多传感器联合初始化的具体步骤为：
[0030]
step1：利用旋转约束，估计imu与rgbd相机之间的外参q
bc
；其中旋转约束如下，其中是imu自身从k时刻到k 1时刻的旋转四元数，是rgbd相机自身从k时刻到k 1时刻的旋转四元数；
[0031][0032]
step2：利用上述旋转约束，通过视觉观测值和陀螺仪测量值构建误差函数进行最小二乘迭代求解陀螺仪偏置bg，并重新计算当前偏置下的imu预积分。
[0033]
step3：通过第k时刻和第k 1时刻的imu积分出的绝对尺度平移及rgbd的相对尺度平移的以及imu与rgbd之间旋转外参q
bc
、平移外参p
bc
，将imu轨迹与rgbd相机轨迹对齐，得到绝对尺度s；
[0034]
step4：通过平移约束构建最小化imu预积分误差，求解系统的初始化速度及初始化重力具体的，平移约束计算公式如下：
[0035][0036]
其中，为第k时刻下imu系到第0时刻rgbd相机之间的相对尺度位移，为第k时刻到第0时刻两帧rgbd相机间的相对尺度位移，为第k时刻下imu系相对于rgbd相机系的旋转矩阵。
[0037]
具体的第k时刻和第k 1时刻之间的imu预积分误差计算式为：
[0038][0039]
其中，是第k时刻和第k 1时刻之间imu的位移变化量误差；是第k时刻和第k 1时刻之间imu的位移变化量误差；是第k时刻和第k 1时刻之间imu位移变化量的测量值及预测值，第k时刻和第k 1时刻之间imu位移变化量的测量值及预测值，是第k时刻和第k 1时刻之间的imu变化量的测量值和预测值。剩余符号含义参考上述注释。
[0040]
step5：通过重力模长||g||这一约束信息，对上述step4中的重力进行参数化并代入上述imu预积分误差计算式进行计算，从而修正初始化的重力尺量。参数化表达式为：
[0041][0042]
式子中，||g||是已知的重力大小，是重力方向单位向量，b1、b2是跨越切平面的两个正交基向量，w1、w2是待优化变量，表示沿着两个正交基向量上的位移，他们共同组成
[0043]
进一步的，所述采用基于滑动窗口的方法对将视觉测量前端传过来的初始位姿信息做进一步优化，通过先验信息约束、视觉约束、imu约束构建目标误差函数，通过基于滑动窗口的图优化算法不断优化所有帧的位姿及偏置信息，具体包括：
[0044]
step1：确定优化的状态向量，需要优化的状态向量有：滑动窗口中的所有相机的状态、m 1个观测点的逆深度；
[0045][0046][0047]
其中，代表滑动窗口中所有的状态量，n是滑窗内的帧数，m代表滑窗内的特征点总数，λk代表每个观测点的逆深度信息；xk代表每一帧下的位姿信息，其中包括在世界坐标系下的imu的位移速度角速度加速度计偏置陀螺仪偏置
[0048]
step2：通过视觉误差和惯性测量误差构建如下的基于最小二乘的目标误差函数，来不断优化滑窗内的状态向量
[0049][0050]
其中，e
prior
是滑窗边缘化的先验信息，rb是相邻帧间imu预积分误差函数，是第k时刻和k 1时刻之间imu预积分观测，是imu预积分噪声项的协方差，rc是相邻帧间视觉重投影误差，是在j时刻rgbd相机对观测点l的观测，是rgbd观测的协方差；
[0051]
step3：将step2中的最小二乘目标函数展开并进行求导，计算每个误差项关于状态向量的雅克比矩阵j；
[0052]
step4：通过上述求解出的雅克比矩阵j，通过带阻尼因子的高斯牛顿法对状态向量进行迭代优化，来加速优化的迭代过程。
[0053]
step5：更新状态向量，当目标误差函数低于某一阈值或者达到最大迭代次数n，则停止，否则跳转至step3。
[0054]
进一步的，所述step2中，边缘化的先验信息的是通过滑动窗口移除老的变量重新建立变量间约束信息后得到：滑动窗口的变量移除策略为：
[0055]
(1)在新一帧加入滑窗后，寻找当前滑窗内所有帧都可以观测到的路标点l1
，
通过这些帧进行三角化优化，优化完成后，将这个观测点移除滑窗，同时在所有帧之间建立约束；
[0056]
(2)对于新的帧看不到的观测点l2，则将该观测点移除，并在可以观测到该点的旧的帧之间建立约束；
[0057]
(3)在移除上述的观测点之后，如果滑窗内某些帧没有可以观测的点，则直接移除这些观测帧。
[0058]
进一步的，在移除这些变量后，根据舒尔补的方式计算移除的变量对滑动窗口内的其他状态向量影响，即边缘化的先验信息；
[0059]
进一步的，所述用dbow视觉词袋算法进行回环检测，具体包括：首先，对当前帧的新老角点进行orb算子描述，然后，计算当前帧与词袋的相似度分数，并与关键帧数据库中所有帧进行比对，并进行闭环一致性检测，最后，如果发现闭环，则通过slam算法对状态向量进行回环优化。
[0060]
本发明的优点及有益效果如下：
[0061]
(1)由于机械零部件具有纹理稀疏、表面光滑的特点，这对2d特征点提取带来了很大困难，从而致使跟踪注册的鲁棒性无法得到保证，而深度相机可以在弱纹理环境中提取稠密点云信息，可以对稀疏的2d特征进行补充。所以权利要求3提出了一种基于rgbd的跟踪注册方法，在2d特征上引入深度信息，通过orb特征计算初始位姿，作为后续稠密点云icp配准过程的位姿初值，在加速icp配准的同时可以获得更为准确的位姿估计。
[0062]
(2)在机械产品装配过程中，由于操作者头部运动，会引起rgb图像和深度图像出现模糊抖动的问题，导致基于rgbd特征的跟踪注册方法无法进行正确的位姿估计，而imu传感器在运动过程中可以实时的对自身的位姿进行估计。所以，在权利要求5-8上，将imu测量信息引入到基于rgbd特征的跟踪注册方法中，将imu测量信息也进行了融合，在基于rgbd特征的跟踪注册方法可以正常工作的时候，可以通过imu的帧间约束信息，进一步优化位姿信息；在基于rgbd特征的跟踪注册方法因运动而无法正常工作的时候，imu依然可以实时提供自身的位姿估计，从而很好的弥补了在运动场景下，rgbd相机的在跟踪注册上鲁棒性差的缺陷的同时，也进一步提升了基于rgbd特征的跟踪注册方法的位姿估计精度。
[0063]
(3)在提升增强现实系统跟踪注册鲁棒性的同时，权利要求7-8提出了一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法，在滑动窗口移动的过程中，将被移除变量的约束信息以先验信息方式传递给剩余变量，参与后端非线性优化，进一步提升跟踪注册的精度；同时在此基础上提出一种滑动窗口变量剔除策略，剔除滑动窗口内冗余的状态变量，缩短后端非线性优化的响应时间，提升跟踪注册过程的实时性。
附图说明
[0064]
图1是本发明提供优选实施例基于rgbd和imu融合增强现实混合跟踪注册方法流程图；
[0065]
图2是视觉惯性约束拓扑图及滑动窗口边缘化图；
[0066]
图3是基于rgbd的位姿估计方法流程图；
[0067]
图4是基于滑动窗口的多传感器融合策略示意图。
具体实施方式
[0068]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
[0069]
本发明解决上述技术问题的技术方案是：
[0070]
针对上述增强现实技术在工业机械产装配场景下存在的问题，提出一种基于rgbd与惯性测量单元融合的混合跟踪注册方法。本发明的技术方案如下：
[0071]
一种基于rgbd与惯性测量单元融合的混合跟踪注册方法，该方法的流程框架如图1所示，包括以下步骤：
[0072]
视觉测量前端步骤：首先，通过rgbd相机获取rgb信息与深度信息，进行特征提取与跟踪，并采用基于rgbd的跟踪注册方法，计算当前时刻的帧的位置及观测点的3d信息，并将跟踪匹配好的特征信息加入到关键帧队列中，交给后端进行优化处理，同时imu将采集到的位姿信息进行预积分操作，得到相邻帧之间的位姿信息，作为基于rgbd的跟踪注册方法计算时的初始位姿，并交给后端进行优化处理；
[0073]
多传感器联合初始化步骤：对三个传感器的外参、偏置、绝对尺度进行标定，通过slam算法估计滑动窗口内开始两帧的位姿及观测点的逆深度，并与imu预积分结果进行对齐求解相关参数；
[0074]
后端优化步骤：采用基于滑动窗口的方法对将视觉测量前端传过来的初始位姿信息做进一步优化，通过先验信息约束、视觉约束、imu约束构建目标误差函数，通过基于滑动窗口的图优化算法不断优化所有帧的位姿及偏置信息；
[0075]
回环检测与优化步骤：利用dbow视觉词袋算法进行回环检测，如果检测到回环产生，则通过重定位的方式对整个轨迹进行闭环优化。
[0076]
进一步的，所述特征提取与跟踪是采用orb算法进行特征点的提取与匹配，orb算法的步骤具体包括：首先，通过比较当前像素点与周围像素点之间的差异来找出零部件的角点特征，其次，通过在选取的角点附近，以某种规则选取n个点对，并对这n个点对的差异结果进行组合作为该特征点的描述子，最后，在特征匹配阶段，通过角点的描述子的相似度进行特征匹配；
[0077]
并通过ransac算法对误匹配点进行剔除，ransac算法对误匹配点进行剔除的步骤具体包括：首先，将匹配到角点特征点对通过深度信息采集映射到三维空间，并随机假设一组局内点即正确匹配点为初始值，将其拟合出一个模型；然后，用这个假设的模型去尝试拟合其他的特征点，如果适合该模型则扩充该模型，否则将该点视为外点即无匹配点进行剔除；最后，在达到最大迭代次数后，如果这假设模型内的局内点数量少于某个阈值则放弃该假设模型并重新假设迭代，否则，则认为该模型置信度高，保留该模型，并将模型的外点也
即无匹配点进行剔除。
[0078]
进一步的，所述基于rgbd的跟踪注册方法，计算当前时刻的帧的位置及观测点的3d信息，具体包括：
[0079]
在orb特征匹配的基础上，判断对应特征点对深度信息是否可测，对于可测的特征点对，则测量对应特征点对的深度信息，通过icp算法计算当前帧的初始位姿信息，再通过深度图像获得稠密点云信息并将该初始位姿作为icp迭代的初值，进行进一步的icp迭代优化，以获得更为精准的位姿信息；对于无法获得深度的特征点对信息，则仍使用pnp方法进行位姿估计。
[0080]
进一步的，所述sfm方法的步骤是：
[0081]
所述通过icp算法计算当前帧的初始位姿信息，具体包括：首先，计算两组匹配的点云p1和p2的每个点的去质心坐标，pi和p’i
，然后，通过将变换矩阵t将点云p1点变换至p2，并构建误差函数；最后，通过迭代的方法不断优化变换矩阵t，直至误差函数低于某个阈值或者达到最大迭次数为止；
[0082]
所述对于无法获得深度的特征点对信息，则仍使用pnp方法进行位姿估计，具体包括：首先，根据特征匹配点找出当前帧和上一帧有共视关系的观测点q，将该点通过变换矩阵t转换到当前帧的像素坐标中；然后，构建重投影误差函数，并通过迭代最小二乘的方法不断优化变换矩阵t，进而求解当前帧的位姿。进一步的，所述imu将采集到的位姿信息进行预积分操作，通过imu测量，并结合第i时刻的pvq位姿信息，得到第j时刻的pvq位姿信息，计算公式为：
[0083][0084][0085][0086]
其中，是j，i时刻下，imu在世界坐标系下的位置信息；是在j，i时刻下，imu在世界坐标系下的速度；是在j，i时刻下，imu在世界坐标系下的旋转信息；是在t时刻下的imu加速度；是在t时刻下的imu的角速度。上述三个式子的积分部分，表示从i时刻到j时刻之间的与积分增量，分别记作，这三个量将作为后端的视觉测量的帧间约束信息来进行非线性优化。
[0087]
进一步的，所述多传感器联合初始化的具体步骤为：
[0088]
step1：利用旋转约束，估计imu与rgbd相机之间的外参q
bc
；其中旋转约束如下，其中是imu自身从k时刻到k 1时刻的旋转四元数，是rgbd相机自身从k时刻到k 1时刻的旋转四元数；
[0089]
[0090]
step2：利用上述旋转约束，通过视觉观测值和陀螺仪测量值构建误差函数进行最小二乘迭代求解陀螺仪偏置bg，并重新计算当前偏置下的imu预积分。
[0091]
step3：通过第k时刻和第k 1时刻的imu积分出的绝对尺度平移及rgbd的相对尺度平移的以及imu与rgbd之间旋转外参q
bc
、平移外参p
bc
，将imu轨迹与rgbd相机轨迹对齐，得到绝对尺度s；
[0092]
step4：通过平移约束构建最小化imu预积分误差，求解系统的初始化速度及初始化重力具体的，平移约束计算公式如下：
[0093][0094]
其中，为第k时刻下imu系到第0时刻rgbd相机之间的相对尺度位移，为第k时刻到第0时刻两帧rgbd相机间的相对尺度位移，为第k时刻下imu系相对于rgbd相机系的旋转矩阵。
[0095]
具体的第k时刻和第k 1时刻之间的imu预积分误差计算式为：
[0096][0097]
其中，是第k时刻和第k 1时刻之间imu的位移变化量误差；是第k时刻和第k 1时刻之间imu的位移变化量误差；是第k时刻和第k 1时刻之间imu位移变化量的测量值及预测值，第k时刻和第k 1时刻之间imu位移变化量的测量值及预测值，是第k时刻和第k 1时刻之间的imu变化量的测量值和预测值。剩余符号含义参考上述注释。
[0098]
step5：通过重力模长||g||这一约束信息，对上述step4中的重力进行参数化并代入上述imu预积分误差计算式进行计算，从而修正初始化的重力尺量。参数化表达式为：
[0099][0100]
式子中，||g||是已知的重力大小，是重力方向单位向量，b1、b2是跨越切平面的两个正交基向量，w1、w2是待优化变量，表示沿着两个正交基向量上的位移，他们共同组成
[0101]
进一步的，所述采用基于滑动窗口的方法对将视觉测量前端传过来的初始位姿信息做进一步优化，通过先验信息约束、视觉约束、imu约束构建目标误差函数，通过基于滑动窗口的图优化算法不断优化所有帧的位姿及偏置信息，具体包括：
[0102]
step1：确定优化的状态向量，需要优化的状态向量有：滑动窗口中的所有相机的状态、m 1个观测点的逆深度；
[0103][0104]
[0105]
其中，代表滑动窗口中所有的状态量，n是滑窗内的帧数，m代表滑窗内的特征点总数，λk代表每个观测点的逆深度信息；xk代表每一帧下的位姿信息，其中包括在世界坐标系下的imu的位移速度角速度加速度计偏置陀螺仪偏置
[0106]
step2：通过视觉误差和惯性测量误差构建如下的基于最小二乘的目标误差函数，来不断优化滑窗内的状态向量
[0107][0108]
其中，e
prior
是滑窗边缘化的先验信息，rb是相邻帧间imu预积分误差函数，是第k时刻和k 1时刻之间imu预积分观测，是imu预积分噪声项的协方差，rc是相邻帧间视觉重投影误差，是在j时刻rgbd相机对观测点l的观测，是rgbd观测的协方差；
[0109]
step3：将step2中的最小二乘目标函数展开并进行求导，计算每个误差项关于状态向量的雅克比矩阵j；
[0110]
step4：通过上述求解出的雅克比矩阵j，通过带阻尼因子的高斯牛顿法对状态向量进行迭代优化，来加速优化的迭代过程。
[0111]
step5：更新状态向量，当目标误差函数低于某一阈值或者达到最大迭代次数n，则停止，否则跳转至step3。
[0112]
进一步的，所述step2中，边缘化的先验信息的是通过滑动窗口移除老的变量重新建立变量间约束信息后得到：滑动窗口的变量移除策略为：
[0113]
(1)在新一帧加入滑窗后，寻找当前滑窗内所有帧都可以观测到的路标点l1，通过这些帧进行三角化优化，优化完成后，将这个观测点移除滑窗，同时在所有帧之间建立约束；
[0114]
(2)对于新的帧看不到的观测点l2，则将该观测点移除，并在可以观测到该点的旧的帧之间建立约束；
[0115]
(3)在移除上述的观测点之后，如果滑窗内某些帧没有可以观测的点，则直接移除这些观测帧。
[0116]
进一步的，在移除这些变量后，根据舒尔补的方式计算移除的变量对滑动窗口内的其他状态向量影响，即边缘化的先验信息；
[0117]
进一步的，所述用dbow视觉词袋算法进行回环检测，具体包括：首先，对当前帧的新老角点进行orb算子描述，然后，计算当前帧与词袋的相似度分数，并与关键帧数据库中所有帧进行比对，并进行闭环一致性检测，最后，如果发现闭环，则通过slam算法对状态向量进行回环优化。
[0118]
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
[0119]
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。