一种基于RGB-D和IMU信息融合的位姿估计方法与流程

本发明涉及多传感器融合技术，特别是一种基于rgb-d和imu信息融合的位姿估计方法。

背景技术：

多传感器信息融合的位姿估计技术是指不同的传感器在相近的时间段获取的数据进行结合，利用相关算法进行数据结合，优势互补，从而得到更可信的分析结果。由于相机低廉的价格和具备丰富的信息、惯性测量单元短时间内积分准确的特性，以相机和惯性测量单元的融合逐渐成为研究的热点。

目前相机和惯性测量单元数据融合的位姿估计技术主要分为两类：基于滤波器的方法和基于优化的方法。根据是否把图像特征信息加入到状态变量进行联合优化，可以进一步分为松耦合和紧耦合的方法。

基于滤波器的松耦合方法以ethz的ssf方法为代表，ethz实验室使用携带单目摄像头和imu的无人机进行了松耦合方法的实验，取得了较高精度的位姿估计。与松耦合算法恰恰相反的是紧耦合算法，基于滤波的紧耦合算法中系统的优化变量不仅包含imu在世界坐标系下的位姿、旋转、加速度bias和陀螺仪bias，而且还包含地图点在世界坐标系下的坐标。采用紧耦合方法的另一个算法是ethz的rovio算法。两种算法都是基于ekf框架。其中rovio算法将系统外参也加入到系统优化变量中，此外把特征点在世界坐标系下的三维坐标参数化为二维的相机归一化坐标和特征点的逆深度(即深度的倒数)，另外为了减小计算规模，加速计算，算法对系统代价函数的雅克比进行qr分解。由于紧耦合算法将特征点的坐标也考虑到系统优化变量中，相比松耦合算法能够获得更高的定位精度。

和基于滤波器的方法相比，基于捆集优化的方法能够获得更高的精度，虽然增加了计算量，但是随着近年来处理器算力的快速增长，当前基于视觉惯导融合的位姿估计方法多采用基于优化的方法。

目前国内外流行的基于优化的位姿估计算法有：vins-mono。该算法后端优化的变量包含系统在世界坐标系下的位置、姿态、imu加速度和陀螺仪的bias、系统外参以及特征点的逆深度。算法最小化imu测量残差和视觉测量残差，从而得到系统状态的最优估计。该算法的创新点是视觉惯导的初始化和后端优化。同时该系统加入回环检测，如果检测到回环，系统进行4个自由度的全局优化进行消除累计误差。该算法在实际环境测试中发现系统在回到原始位置时，进行全局优化后，整体系统位姿变化比较大，这说明系统位姿估计精度不高。另外，提出的okvis算法将imu测量残差和相机重投影误差的范数的和作为最小二乘代价函数进行联合优化，从而得到系统的实时位姿，通过采用滑动窗口方法进行约束计算量，以及通过采用边缘化方法不丢失历史状态的约束信息。由于该算法没有加入回环检测机制，所以从本质上来说是一个视觉惯导里程计，如果进行长时间的位姿估计的话，累计误差不能纠正。融合imu数据的orbslam2算法是一个完整的视觉惯导slam系统。系统加入了回环检测，能够进行全局优化，从而消除累积的误差。该算法的创新点之一是视觉惯导系统的初始化。首先利用运动恢复结构(structfrommotion,缩写为sfm)得到连续几个关键帧的相对位姿，将该结果作为imu的约束，进一步优化得到尺度、速度、imu的陀螺仪和加速度计的bias以及重力方向。由于该初始化方法需要经过一定的时间进行系统尺度的收敛，对于实时系统，比如无人机的定位导航来说会出现一定的问题。

技术实现要素：

针对现有技术中的问题，本发明提供一种基于rgb-d和imu信息融合的位姿估计方法。该方法利用rgb-d相机的深度信息，加快特征点深度的收敛，使得特征点深度估计更加准确，同时提高系统的定位精度。

本发明提供一种基于rgb-d和imu信息融合的位姿估计方法，包括：

s1、在rgb-d相机数据和imu数据的时间同步之后，对rgb-d相机采集的灰度图像和深度图像以及imu采集的加速度、角速度信息进行预处理，获取世界坐标系下相邻帧匹配的特征点和imu状态增量；

s2、依据位姿估计系统的系统外参，对位姿估计系统中视觉惯导装置进行初始化，以恢复陀螺仪bias、尺度、重力加速度方向和速度；

s3、根据初始化后的视觉惯导装置的信息和世界坐标系下相邻帧匹配的特征点、imu状态增量构建位姿估计系统的最小二乘优化函数，使用优化方法迭代求解出最小二乘优化函数的最优解，将最优解作为位姿估计状态量；

进一步地，如果位姿估计系统在预设时间段内回到之前的位置时，采用当前时刻rgb-d相机的帧数据和之前位置时刻rgb-d相机的帧数据作为约束条件，对预设时间段内位姿估计系统的位姿估计状态量进行全局优化，获取全局一致的位姿估计状态量。

可选地，所述步骤s1包括：

s11、对图像数据和imu数据进行时间同步，使用改进的基于ransac的光流跟踪算法跟踪上一帧的关键点，并提取当前灰度图像新的特征点；

s12、对提取的特征点像素坐标进行畸变校正，并通过内参模型得到特征点在相机坐标系下的归一化坐标；

s13、利用imu加速度和角速度信息，使用预积分技术得到两个相邻图像帧之间的imu状态增量。

可选地，所述步骤s11包括：

对rgb-d相机采集的灰度图像中选取代表性的特征点进行处理；

特征点的选取包括：

具体地，给定图像i和关键点(u,v)，以及该关键点的转角θ；

描述子d表示为：d＝[d1,d2,...,d256]；

对任意i＝1,...,256,di的计算为：取(u,v)附近的任意两点p,q，并按照θ进行旋转：

其中up,vp为p的坐标，对q同样处理，记旋转后的p,q为p′,q′,那么比较i(p′)和i(q′),若i(p′)大，记为di＝0，反之为di＝1；得到orb的描述子；

所述orb的描述子中每一特征点的深度值为对应深度图像中的像素值。

可选地，ransac算法的实现过程如下：

初始：假设s是n个特征点对应的集合；s为orb初始确定的描述子；

开启循环：

1)随机选择集合s中8个特征点对；

2)通过使用8个特征点对拟合出一个模型；

3)使用拟合出的模型计算s集合内每个特征点对的距离；如果距离小于阈值，那么该点对为内点；存储内点集合d；

4)返回1)重复进行，直到达到设定的迭代次数；

选择内点最多的集合作为最后输出的orb的描述子。

可选地，步骤s3包括：

将imu状态增量、系统外参和特征点的逆深度作为优化变量，最小化边缘化残差、imu预积分测量残差和视觉测量残差和深度残差，以构建成最小二乘问题，使用高斯牛顿法迭代求解得到系统状态的最优解。

可选地，构建最小二乘问题，基于深度约束的视觉惯导融合的位姿估计优化的代价函数可写为：

上式中的误差项包括边缘化残差、imu测量残差、视觉重投影残差和加入的特征点的深度残差；

x代表优化变量；

代表第k帧对应的imu到第k+1图像帧对应的imu之间imu的预积分测量；

b代表图像帧的集合；

代表第k帧对应的imu到第k+1图像帧对应的imu之间imu的预积分测量的协方差矩阵；

代表第l个特征点在第j帧图像帧上的像素坐标测量；

代表第l个特征点在第j帧图像帧上的测量的协方差矩阵；

c代表图像帧的集合；

代表代表第d个特征点在第j帧图像帧上的深度值测量；

代表代表第d个特征点在第j帧图像帧上的深度值测量的协方差矩阵；

d代表图像帧的集合。

可选地，imu测量残差包括：位置残差、速度残差、姿态残差、加速度bias残差和陀螺仪bias残差；

世界坐标系到第k帧图像帧对应的imu的旋转；

第k+1帧图像帧对应的imu在世界坐标系下的旋转；

g^w代表世界坐标系下的重力加速度；

第k+1帧图像帧对应的imu在世界坐标系下的速度；

第k帧图像帧对应的imu和第k+1帧图像帧对应的imu之间的预积分测量之平移部分；

是预积分测量之平移部分对加速度bias的一阶雅克比；

是预积分测量之平移部分对角速度bias的一阶雅克比；

是加速度bias小量；

是陀螺仪bias小量；

是第k+1帧图像帧对应的imu到世界坐标系的旋转；

第k帧图像帧对应的imu和第k+1帧图像帧对应的imu之间的预积分测量之旋转部分；

第k帧图像帧对应的imu和第k+1帧图像帧对应的imu之间的预积分测量之速度部分；

预积分测量之速度对加速度bias的一阶雅克比；

预积分测量之速度对陀螺仪bias的一阶雅克比；

第k+1帧对应的加速度bias；

第k帧对应的加速度bias；

第k+1帧对应的陀螺仪bias；

公式(3)中第一行表示位置残差，第二行表示姿态残差，第三行表示速度残差，第四行表示加速度bias残差，第五行表示陀螺仪bias残差；

优化变量为4个，包括：

视觉重投影残差为：

第l个特征点在第j帧像素坐标测量；

[b1,b2]^t代表切平面上的一对正交基；

第l个特征点的在第j帧下的投影的归一化相机坐标；

第l个特征点的在第j帧下的投影的相机坐标；

第l个特征点的在第j帧下的投影的相机坐标的模；

参与相机测量残差的状态量有以及逆深度λl；

深度测量残差模型：

代表第d个特征点在第j帧下的深度测量值；

λl优化变量之深度；

其中λl为待优化的变量，为通过深度图像获取的深度信息。

可选地，imu状态量包括：位置、旋转、速度、加速度bias、陀螺仪bias；

系统外参包括：相机到imu的旋转和平移；

或者，

系统外参的获取方式：通过离线的外参标定算法来获取，或者通过在线外参标定算法来获取。

可选地，所述步骤s1之前，所述方法还包括：

同步相机数据和imu数据的时钟，具体包括：

在位姿估计系统中，以imu的时钟作为系统的时钟，首先创建2个缓冲区用于存储图像消息和同步消息；

图像消息的数据结构包含当前图像的时间戳、帧号和图像信息；

同步消息包含当前图像的时间戳、帧号和图像信息；

每当相机捕获一张图片，一个同步消息就产生；且同步消息的时间戳更改为和imu最近的时间作为同步消息的时间戳，此时，实现了相机数据和imu数据的时间同步。

本发明具有的有益效果：

本发明的方法在数据输入部分，预先设计了相机imu(惯性测量单元)数据时间同步方案。在硬件和软件上实现相机imu数据的时间同步，给多传感器数据融合的位姿估计算法提供可靠的输入数据。

在前端特征点追踪部分，基于随机采样一致性方法，改进金字塔lucas-kanade光流法。在前后帧追踪得到特征点对的基础上随机选择8对点，算出基础矩阵，然后利用该基础矩阵对应的对极约束，对匹配点进行测验，满足设定的阈值为内点，进一步提升光流的追踪精度。

在后端优化部分，通过rgb-d相机引入特征点深度的先验，构造深度残差；然后采用紧耦合的方法，最小化imu测量残差、视觉重投影误差和深度残差，将问题构建成最小二乘问题，使用高斯牛顿迭代求解得到系统状态的最优解；进一步采用滑动窗口和边缘化技术，在约束计算量的同时不丢失历史信息的约束。本发明提出的方法，利用rgb-d相机的深度信息，加快特征点深度的收敛，使得特征点深度估计更加准确，同时提高系统的定位精度。

附图说明

图1为本发明所涉及的基于rgb-d和imu信息融合的位姿估计系统框图；

图2为相机数据和imu数据的时间戳同步的示意图；

图3为相机数据和imu数据时间同步说明的过程示意图；

图4为提取fast特征点的示意图；

图5为采用lk光流法提取特征点的示意图；

图6为没有使用ransac的orb特征点光流跟踪的示意图；

图7为使用ransac的orb特征点光流跟踪的示意图；

图8为rgb-d相机的深度图像的示意图；

图9为特征点深度提取流程的示意图；

图10为当滑动窗口中最新的图像帧x4不是关键帧的边缘化策略示意图；

图11为当滑动窗口中最新的图像帧x4是关键帧的边缘化策略示意图；

图12为边缘化方法示意图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

实施例一

一种基于rgb-d和imu信息融合的位姿估计的方法的整体框架如图1所示。

rgb-d和imu信息融合的位姿估计系统(下述简称系统)的位姿估计算法可以分为四个部分：数据预处理，视觉惯导初始化，后端优化和回环检测。四个部分分别作为独立的模块，可以根据需求进行改进。

数据预处理部分：其用于对rgb-d相机采集的灰度图像、深度图像以及imu采集的加速度和角速度信息进行处理。这一部分的输入包含灰度图像、深度图像、imu加速度和角速度信息，输出包含相邻匹配的特征点和imu状态增量。

具体为：由于相机和imu有两个时钟源，首先对图像数据和imu数据进行时间同步；然后使用改进的基于ransac的光流跟踪算法跟踪上一帧的关键点，并提取当前灰度图像新的特征点，对特征点像素坐标进行畸变校正，并通过内参模型得到特征点在相机坐标系下的归一化坐标；同时利用imu加速度和角速度信息，使用预积分技术得到两个图像帧之间的imu状态增量。

相机每时每刻都会发布图像，前后两帧图像之间有很多imu数据，利用这些数据，使预积分技术可以得到两帧之间的imu状态增量。

视觉惯导初始化：首先判断系统外参是否已知，系统外参指的是相机到imu的旋转和平移，可以通过离线或者在线外参标定算法来获取系统外参，然后在这个基础上进行视觉惯导系统的初始化。

具体为：通过相机图像采用运动恢复结构(structfrommotion，缩写为sfm)得到旋转和无尺度的平移量，结合imu预积分旋转，建立基本方程，最终可以得到相机到imu的旋转。在这个基础上，进行视觉惯导系统的初始化，恢复陀螺仪bias，尺度，重力加速度方向和速度。

后端优化部分：通过构建最小二乘优化函数并使用优化方法迭代求解出系统状态的最优解。使用滑动窗口技术进行约束计算量，并且使用边缘化技术使得不丢失历史状态的约束信息，通过构建边缘化残差、视觉重投影残差和加入的特征点深度残差，最小化代价函数，使用优化方法迭代求解系统状态的最优解。

具体为：首先计算imu测量残差、视觉重投影误差和本发明提出的深度残差的误差以及协方差，使用高斯牛顿或列文伯格-马夸尔特方法进行迭代求解，从而得到系统状态的最优解。

回环检测部分：用于检测回环。当系统回到之前的位置时，得到当前帧和历史帧的约束，通过全局位姿图优化，对系统历史位姿状态量进行全局优化，消除累计误差，得到全局一致的位姿估计。

1.1相机imu数据时间同步的设计和实现

为了验证视觉惯导融合的位姿估计算法在实际环境下的效果，使用相机和imu产生的数据作为位姿估计算法的输入。

问题是：相机和imu有各自的时钟，导致相机和imu数据的时间戳不一致。然而位姿估计算法要求相机imu数据的时间戳一致。如图2所示为相机imu数据时间戳的同步情况，主要分为3种：

(1)完美同步，采样间隔固定，在有图像采样的时刻都有imu采样对应。这样的情况是理想的。

(2)两个传感器有一个公共的时钟，采样间隔固定。这样的情况是比较好的。

(3)两个传感器有不同的时钟，采样间隔不固定，这样的情况不好。

为了实现相机和imu数据的时间同步，目前的做法是在硬件上进行硬同步和在软件上进行软同步。硬件同步方法是在嵌入式设备上连接imu，imu每隔一定的时间通过嵌入式板子上的引脚给相机一个硬触发，从而完成硬件设备的同步。软件同步是在软件端进行相机和imu的时间同步，保证相机imu数据时间戳的一致。本发明进行的是软件同步。

本发明实施例中设计了相机imu数据时间同步方法，如图3所示，图3中2个传感器以不同的速率和各自的时钟源运行，imu的速率更快，以imu的时间戳作为系统的时间戳。其中n代表缓冲区的大小。

以imu的时钟作为系统的时钟。首先创建2个缓冲区用于存储图像消息和同步消息。每个缓冲区的大小为10。图像消息的数据结构包含当前图像的时间戳、帧号和图像信息。同步消息包含当前图像的时间戳、帧号和图像信息。每当相机捕获一张图片，一个同步消息就产生。同步消息的时间戳更改为和imu最近的时间作为同步消息的时间戳，进行后续操作或者在ros话题上进行发布。这样在软件上实现了相机imu数据的时间同步。

1.2基于ransac改进的光流追踪算法

1.2.1特征点提取和跟踪

使用相机采集图像，相邻两帧图像会存在一些重叠的区域。利用这些重叠的区域，根据多视图几何原理，可以计算出两个图像帧的相对位姿关系。但是一幅图像中有很多像素，对于分辨率640×480的图像，一幅图像中包含307200个像素点，对每个像素点进行匹配会使计算量非常大，同时也是不必要的，因此可以在图像中选取一些具有代表性的部分进行处理。可以作为图像特征的部分：角点、边缘、区块。

特征点由关键点和描述子组成。关键点用角点检测算法(比如harris、shi-tomasi、fast等)从图像中提取角点。提取的特征点具有旋转不变性、光照不变性、尺度不变性。

旋转不变性指相机斜着也能识别该特征点。为了保证特征点具有旋转不变性，orb(orientedfastandrotatedbrief)特征点通过计算特征点的主方向，使得后续的描述子具有旋转不变性。orb特征点改进了fast检测子不具有方向性的问题，并采用速度极快的二进制描述子brief，使得整个图像特征点的提取大大加速。

光照不变性指提取的角点对亮度和对比度不敏感。即使光照变化，也能识别该特征点。

尺度不变性指相机走近和走远都能识别该特征点。为了保证相机走近和走远都能识别该特征点，orb特征点通过构建图像金字塔，对每层图像提取fast特征点。

在不同的场景下，orb比sift和surf的耗时小，这是因为①fast的提取时间复杂度低。②surf特征为64维描述子，占据256字节空间；sift为128维描述子，占据512字节空间。而brief描述每个特征点仅需要一个长度为256的向量，占据32字节空间，降低了特征点匹配的复杂度。因此本发明采用orb特征进行稀疏光流跟踪，减少时间耗费。

orb即orientedfast的简称。它实际上是fast特征再加一个旋转量。使用opencv自带的fast特征提取算法，然后完成旋转部分的计算。

fast特征点的提取流程，如图4所示：

(1)在图像中选取像素p，假设它的灰度为ip。

(2)设定一个阈值t(比如为ip的20％)。

(3)以像素p为圆心，选择半径为3的圆上的16个像素点。

(4)如果选取的圆上有连续n个点的灰度大于ip+t或小于ip-t，那么该像素p可以认为是特征点(n一般取12)。

为了加快特征点检测，在fast-12特征点算法中，检测1、5、9、13这4个像素中有3个像素大于ip+t或小于ip-t，当前像素才有可能是特征点，否则直接排除。

旋转部分的计算描述如下：

首先找到图像块的质心。质心是以灰度值作为权重的中心。

(1)在一个小块的图像块b中，定义图像块的矩：

其中，(x,y)代表图像中特征点的像素坐标。

(2)通过矩可以找到图像块的质心：

(3)连接几何中心和质心，得到方向向量。特征点的方向定义为：

orb描述即带旋转的brief描述。所谓brief描述是指一个0-1组成的字符串(可以取256位或128位)，每一个bit代表一次像素间的比较。算法流程如下：

(1)给定图像i和关键点(u,v)，以及该点的转角θ。以256位描述为例，那么最终描述子d为：

d＝[d1,d2,...,d256](4)

(2)对任意i＝1,...,256,di的计算如下，取(u,v)附近的任意两点p,q，并按照θ进行旋转：

其中，up,vp为p的坐标，对q也是这样。记旋转后的p,q为p′,q′,那么比较i(p′)和i(q′),若前者大，记为di＝0,反之为di＝1。这样就得到了orb的描述。在这里要注意，通常会固定p、q的取法，称为orb的pattern，否则每次都随机进行选取，会使得描述不稳定。

1.2.2基于ransac改进的光流追踪算法

光流法基于灰度(光度)不变假设，即同一个空间点的像素值在各个图片中的灰度值是固定不变的。

灰度不变假设是说对于t时刻位于(x,y)的像素，在t+dt时刻移动到(x+dx,y+dy)，如图5所示。由于灰度不变假设，得到:

i(x,y,t)＝i(x+dx,y+dy,t+dt)(6)

依据一阶泰勒展开对式(6)进行一阶泰勒展开，得到式(7)：

根据式(8)：

i(x,y,t)＝i(x+dx,y+dy,t+dt)(8)

可以得到式(9):

其中，为像素在x轴方向的运动速度，记为u(该参数和公式(5)中的参数表示式不一样的)。记为v,为图像在x轴方向的梯度，记为ix,记为iy。为图像对时间的变化量，记为it。

可以得到式:

式(10)中的u、v代表的是像素在x、y方向的速度。

其中式(10)中有2个未知数u,v,至少需要两个方程。

在lk光流中，假设某一窗口内的像素具有相同的运动，考虑一个w×w的窗口，有w×w个像素，由于该窗口内的像素具有相同的运动，可得到w×w个方程，如式(11)所示:

即为式(12)：

这是关于u,v的超定方程，可以用最小二乘法进行求解，这样就可以得到像素在图像间的运动了。当t取离散时刻时，就能得到某些像素在若干个图像中出现的位置了。上述描述的是基于orb特征点的光流追踪过程，属于数据预处理改进的部分内容。

本发明采用基于随机采样一致算法，改进金字塔lucas-kanade光流法，提高特征点的追踪精度。

lk算法的3个假设在实际场景中难以满足，包括目标图像亮度一致、图像空间连续和图像变换在时间上连续。

传统的解决办法包括：(1)保证足够大的图像帧率；(2)引入图像金字塔，通过不同尺度的图像光流提取保证连续性。

为了提高前后帧特征点追踪的准确性，本申请的系统在传统方法的基础上采用随机采样一致算法(randomsamplingconsensus，缩写为ransac)提高光流的追踪精度。

本发明借助随机采样一致算法的思想，在前后帧追踪得到特征点对的基础上随机选择8对点，算出基础矩阵，然后利用该基础矩阵对应的对极约束，对剩余的匹配点对进行测验，满足设定的阈值为内点(即正确的点)。

ransac算法的核心思想是：内点个数越多，构建的模型就更准确。最终选择内点个数最多的模型所对应的匹配点对进行后续的操作。

ransac算法的实现过程如下：

步骤a1初始：假设s是n个特征点对应的集合。

步骤a2开启循环：

(11)随机选择集合s中8个特征点对；

(12)通过使用8个特征点对拟合出一个模型；

(13)使用拟合出的模型计算s集合内每个特征点对的距离。如果距离小于阈值，那么该点对为内点。存储内点集合d；

(14)返回(11)重复进行，直到达到设定的迭代次数。

步骤a3选择内点最多的集合进行后续的操作。

如图6和图7所示，基于ransac算法的orb特征的光流算法，能够进一步去除错误的匹配，从而提高特征点追踪的精度。

ransac算法是对lk光流法的改进，从而去除错误的匹配，得到更为精确的特征点匹配。

即使用图像特征点进行匹配，从而求解相机位姿，而imu状态增量用于约束两帧图像帧之间的位姿变换，这个约束是放在后端优化进行的。

1.3加入深度约束的后端优化

针对当前位姿估计算法定位精度低的问题，本发明提出一种基于深度约束的视觉惯导位姿估计方法。将imu状态量(包含位置、旋转、速度、加速度bias,陀螺仪bias)、系统外参(相机到imu的旋转和平移)和特征点的逆深度(逆深度是深度分之一，特征点的深度是系统要估计的，而通过rgb-d相机获取的深度，当成深度的先验，用于加快优化过程中深度的收敛)作为优化变量，最小化边缘化残差、imu预积分测量残差和视觉测量残差和深度残差，将问题构建成最小二乘问题，使用高斯牛顿法迭代求解得到系统状态的最优解。采用滑动窗口策略约束不断增长的计算量；采用边缘化技术，使得历史状态变量间的约束信息得以保存。本发明提出的算法在后端优化中利用了rgb-d相机的深度信息，加入深度残差项，加快特征点深度的收敛，使得特征点深度的估计更加准确，同时提高系统位姿估计的精度。

滑动窗口策略可理解为：通过设定一定数量的优化变量来使得系统状态估计的计算量不上升。

对于后端优化方法，从大体上来讲，存在两种选择，其一是假设马尔科夫性，简单的马尔可夫性认为，k时刻状态只与k-1时刻状态有关，而与之前的状态无关。如果做出这样的假设，就会得到以扩展卡尔曼滤波为代表的滤波器方法。其二是考虑k时刻状态与之前所有状态有关，就会得到以捆绑调整为代表的非线性优化方法。接下来对捆绑调整进行阐述。

捆绑调整(bundleadjustment，缩写为ba)是从视觉重建中提炼出最优的3d模型和相机参数。ba对相机姿态和特征点的空间位置做出最优的调整。人们逐渐意思到slam问题中ba的稀疏性，才能用于实时的场景。举最小化重投影误差的例子。重投影误差是将像素坐标(观测到的投影位置)与3d点按照当前估计的位姿进行投影得到的位置相比较得到的误差。重投影误差分为3步。

(1)投影模型。首先把空间点转换到归一化相机坐标系下，然后用畸变模型得到畸变的归一化坐标，最后用内参模型得到畸变的像素坐标。通过畸变模型计算像素坐标(u,v)对应的畸变像素坐标，找到畸变后的像素坐标，在把这个像素上的值给到(u,v)，这是去畸变的过程。

(2)构建代价函数。比如在位姿ξi处观测到路标pj，得到一次观测zij。一段时间内的观测都考虑进来，那么可以得到一个代价函数，如式：

式(13)中，p是观测数据，i代表第i次观测，j代表第j个路标。所以m代表一段时间内的观测次数，n代表一段时间内的路标个数。

对式(13)进行最小二乘求解，相当于对位姿和路标同时做出调整，即ba。测量值减去真实值(包含待优化的变量)，最小化残差。

(3)求解。不管使用高斯牛顿法还是列文伯格-马夸尔特方法，最终都面临求解增量方程，如式(14):

hδx＝g(14)

h是海森矩阵，deltax是状态增量，g是定义的一个量。式(14)用于求解系统状态。

高斯牛顿法还是列文伯格-马夸尔特方法的主要差别在于h取的是j^tj还是j^tj+λi。

接下来本发明对系统进行建模、转换为最小二乘问题进行求解。

1.3.1系统建模和求解

在介绍提出的基于深度约束的视觉惯融合的位姿估计算法之前，首先明确系统要优化的状态变量：

其中，xk表示第k个图像帧对应的状态变量，包含第k个图像帧对应的imu在世界坐标系下的平移速度姿态加速度biasba和陀螺仪biasbg。n表示滑动窗口的大小，这里设置为11。表示系统的外参，包含相机到imu的旋转和平移。

现在构建最小二乘问题，基于深度约束的视觉惯导融合的位姿估计优化的代价函数可写为：

式(16)右边的函数为代价函数，需要做的就是找到一个最优的参数，使得代价函数无限接近于零。本系统涉及的误差项为边缘化残差、imu测量残差、视觉重投影残差和深度残差。每个待估计的变量和误差项之间都有关联，对此要进行求解。

以一个简单的最小二乘问题为例子：

其中x∈rⁿ,f是任意非线性函数，设f(x)∈r^m。

如果f是个形式上很简单的函数，那么可以用解析形式来进行求解。令目标函数的倒数为0，然后求解x的最优值，就像求解二元函数的极值一样，如式所示：

通过求解此方程，可以得到导数为0的极值。他们可能是极大、极小或者鞍点处的值，可以通过比较他们的函数值大小即可。而方程是否容易求解取决于f导函数的形式。所以对于不便求解的最小二乘问题，可以采用迭代的方法，从一个初始值出发，不断更新当前的优化变量，使得目标函数下降。步骤描述如下：

(1)给定一个初始值x0；

(2)对于第k次迭代，找一个自变量的增量δxk,使得达到最小值。

(3)如果δxk足够小，则停止迭代。

(4)否则令xk+1＝xk+δxk,返回第二步。

上面将求解导函数为0的问题变为一个不断寻找梯度并下降的过程。接下来就是确定δxk的过程。

梯度法和牛顿法：梯度下降法又称一阶梯度法，牛顿法又称二阶梯度法。它们对目标函数进行泰勒展开：

保留1阶项为一阶梯度法，保留2阶项为二阶梯度法。一阶梯度法，增量的解是：

δx＝-j^t(x)(20)

二阶梯度法，增量的解是:

hδx＝-j^t(21)

一阶梯度方法得到的是局部最优解。如果目标函数是一个凸优化问题，那么局部最优解就是全局最优解，值得注意一点的是，每一次迭代的移动方向都与出发点的等高线垂直。一阶梯度法过于贪心，容易走出锯齿路线，反而增加迭代次数。二阶梯度法需要计算目标函数的h矩阵，这在问题规模较大的时候特别困难，通常避免计算h矩阵。

高斯牛顿法:高斯牛顿法对f(x+δx)进行一阶泰勒展开，不是对目标函数进行泰勒展开。

增量的解为hδx＝-j(x)^tf(x)，其中h＝j^tj进行近似。

它的问题是：

(1)原则上要求h是可逆而且是正定的，但是使用h＝j^tj进行近似，得到的却是半正定，即h为奇异或者病态，此时增量的稳定性差，导致算法不收敛。(补充：对于任意一个非0向量x，正定矩阵满足x^tax>0，大于0是正定的，大于等于0是半正定的)

(2)假设h非奇异非病态，算出来的δx过大，导致采用的局部近似不够准确，这样无法保证算法迭代收敛，让目标函数变大都是有可能的。

列文伯格-马夸尔特方法：列文伯格-马夸尔特方法(又称阻尼牛顿法)，一定程度上修正这些问题。高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以很自然地就想到应该给添加一个信赖区域，不能让它太大。那么如何确定这个信赖区域呢？通过近似模型和实际函数之间的差异来确定，如果差异小，就扩大这个信赖区域，如果差异大，就缩小这个信赖区域。

来判断泰勒近似是否够好。ρ的分子是实际函数下降的值，分母是近似模型下降的值。如果ρ接近于1，则近似是好的。如果ρ太小，说明实际减小的值少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，实际减小的值大于近似减小的值，需要扩大近似范围。

1.3.2观测模型

本节详细介绍imu测量残差、视觉重投影残差和深度测量残差。

imu测量残差：相邻两个图像帧之间的imu测量残差。imu测量残差包含位置残差、速度残差、姿态残差、加速度bias残差和陀螺仪bias残差。

imu测量模型可表示为式(23)，式(23)左侧为使用带噪声的加速度计和陀螺仪观测数据进行预积分的结果，这些参数可以通过加速度计和陀螺仪数据的观测数据计算得到，在初始化阶段，只需要估计陀螺仪的bias,但是后端优化部分，必须对加速度的bias和陀螺仪的bias进行同时估计。imu的加速度和陀螺仪bias在每次迭代优化后进行更新。

所以imu残差等于真实值减去测量值，其中测量值包含bias的更新，可表示为：

第一行表示位置残差，第二行表示姿态残差，第三行表示速度残差，第四行表示加速度bias残差，第五行表示陀螺仪bias残差。

这个地方很容易出错，原因在于没搞清楚求偏导的对象。整个优化过程中，imu测量模型这一块，涉及到的状态量是xk,但是这个地方的雅克比矩阵是针对变化量δxk,所以求4个部分雅克比的时候，是对误差状态量求偏导。

一共有4个优化变量，

视觉重投影残差：视觉重投影残差本质还是特征的重投影误差。将特征点p从相机i系转换到相机j系，即把相机测量残差定义为式：

为归一化坐标,是真实值。因为最终要将残差投影到切平面上,[b1,b2]是切平面的正交基。反投影后的可表示为：

其中是相机坐标i系的特征点空间坐标。是特征点l在世界坐标系下的位置。

反投影之前的归一化坐标写为：

参与相机测量残差的状态量有以及逆深度λl。对这些状态量求偏导，得到高斯迭代过程中的雅克比矩阵。

深度测量残差：在实际室内环境中,为了提高视觉惯导融合的位姿估计的定位精度，本发明结合rgb-d相机中的深度图像。rgb-d相机可以直接获得特征点对应的深度信息。

为了获得稳定可靠的深度信息，本文首先进行特征点深度的预处理。rgb-d相机深度图像有效测量范围：0.8-3.5m，需要在使用时对不在这个范围内的值进行剔除。由于rgb-d相机的红外发射相机和红外接收相机在空间中的位置不一样，所以rgb-d传感器对于物体边缘深度值的检测跳变严重，如图8所示。

为了位姿估计稳定性的考虑，把空间中的物体边缘进行标记，在位姿估计的时候不参与计算。此外，深度图像由于受到光照等因素的影响，图像中存在噪声，本申请进行高斯平滑进行抑制噪声影响。最后可以得到稳定可靠的特征点深度值。特征点深度提取流程如图9所示。

在后端优化中，本申请在原有的观测模型中加入深度残差模型，将特征点对应的深度信息作为初始值，然后进行迭代优化，深度残差模型可表示为：

其中，λl为待优化的变量，为通过深度图像获取的深度信息。通过构建深度残差，加快了特征点深度的收敛，特征点的深度更准确，同时系统位姿估计更加准确，从而提高了整套系统的定位精度。

为了得到特征点的深度信息，还有一个方案是采用双目相机，双目相机的深度z需要进行计算，如式(30)所示。

其中式(30)中d为左右图横坐标之差，称为视差。根据视差，可以计算特征点和相机之间的距离。视差与距离成反比，距离越远，视差越小。视差最小为一个像素，理论上双目的深度存在一个最大值，由fb决定。从式(30)可以知道，当基线b的值越大，双目能够测量的最大距离就越大，反之只能测量很近的距离。

1.3.3滑动窗口技术

在基于图优化的slam技术中，无论是位姿图优化(posegraphoptimization)还是集束调整(bundleadjustment)都是最小化损失函数来达到优化位姿和地图的目的。然而当待优化的位姿或者特征点不断增多时，式(18)所示最小二乘问题的规模也会不断增大，使得优化求解的计算量不断增加，因而不能无限制地添加待优化的变量，一种解决思路是系统不再使用所有历史测量数据来估计历史所有时刻的系统状态量，在当前时刻，只使用最近几个时刻的测量数据来估计对应的最近几个时刻的系统状态量，而时间较久远的系统状态量则被认为是与真实值非常接近了，后续将不再进行优化，这其实就是滑动窗口的基本思想，通过固定滑动窗口的大小，可以维持计算量不上涨，从而可以进行实时求解系统的状态变量。

比如说，一开始窗口中有三个关键帧kf1、kf2、kf3，经过一段时间，窗口中又加入第4个关键帧kf4，此时需要去掉kf1,只对关键帧kf2、kf3、kf4进行优化，从而保持了优化变量的个数，达到固定计算量的目的。

在上面的优化过程中，来一帧新的关键帧，直接丢弃了关键帧1和关键帧2，3之间的约束，只用新的关键帧4和2、3构建的约束来对关键帧2,3和4进行优化，很明显可以看出，优化后关键帧2和3的位姿和关键帧1之间的约束就破坏了，这样关键帧1的一些约束信息就丢失了。所以要做到既使用滑动窗口，又不丢失约束信息。接下来分别对滑动窗口和边缘化技术进行分析。

为了维持整个系统的计算量不增加，本系统使用滑动窗口技术。当系统处于悬停状态或运动较慢时，相机采集到的相邻两个图像帧之间的视差很小。如果只根据时间先后来进行取舍，之前的图像数据被丢弃，而这会导致滑动窗口中相似的图像帧过多，这些图像帧对系统状态估计贡献很小。

针对这个问题，本系统采用关键帧机制，判断当前图像帧是否是关键帧，如果是的话，则放入滑动窗口参与优化，否则直接将该帧丢弃。在本系统的算法中，主要使用以下两个原则来判断一个图像帧是否为关键帧：

(1)计算当前帧与上一关键帧所有匹配的特征点平均视差(所有匹配特征点像素坐标的欧氏距离平方和除以匹配特征点数量)，当平均视差大于阈值时，则将当前图像帧选定为关键帧；

(2)判断当前帧中使用光流跟踪算法跟踪到的特征点数量是否小于阈值，小于则认为该帧为关键帧。

将从一个例子进行分析滑动窗口的过程。

状态量满足以下两个条件之1加才能被加入到滑窗中：

(p1)两帧之间的时间差不能超过阈值。

(p2)两帧之间的视差超过一定的阈值。

条件p1避免两帧图像帧之间的长时间imu积分，而出现漂移。条件p2保证2个关键帧之间有足够的视差，才能加入到滑窗中。

因为滑窗的大小是固定的，要加入新的关键帧就要从滑窗中剔除旧的关键帧。本系统中剔除旧帧有两种方式，剔除滑窗中的首帧或倒数第二帧。

比如说滑窗中一个有4个状态：1,2,3,4，要加入1个新的状态5。

(1)状态4和状态5有足够的视差→边缘化状态0→接受状态5。如图10所示。其中灰色虚线框表示滑动窗口，黑色框表示imu预积分得到的两帧之间的约束，f为特征点，x为系统位姿状态量，xcb为视觉惯导系统外参。当新的图像帧x4进入滑动窗口，如果该帧不是关键帧，则将该帧的对应的特征点的观测和该帧对应的系统位姿丢弃，imu预积分保留。

(2)状态4和状态5视差过小→去掉该图像帧对应的信息的特征点观测和该帧对应的位姿状态，imu约束继续保留。如图11所示。当新的图像帧x4进入滑动窗口，如果该帧是关键帧，则保留该帧图像帧并将红色虚线框中的特征点以及系统位姿边缘化进行保留约束。

1.3.4边缘化技术

如果直接将系统状态量滑出窗口，也就是说将相关的测量和观测数据丢弃，那么会破坏系统状态量之间的约束关系，这样会导致状态估计的求解精度下降。视觉slam中比如orbslam2中的marg是为了加速计算，那些marg的特征点也计算了。和vio中滑动窗口中的不太一样。vio中的marg是计算窗口外的约束zm对窗口内的影响，即不丢失信息。

通过将被边缘化的和边缘化有约束关系的变量之间的约束封装成和边缘化有约束关系的变量的先验信息。

那么如何求解先验信息呢。假设要边缘化掉的变量为xm,和这些待丢弃的变量有约束关系的变量用xb表示，滑动窗口中的其他变量为xr,所以滑动窗口中的变量为x＝[xm,xb,xr]^t。相应的测量值为z＝zb,zr,其中zb＝zm,zc。从图12进行详细分析。

从图12中可以看出一共有5个状态变量:x0,x1,x2,x3,x4，需要边缘化掉x1,但是x1和x0,x3,x3有约束关系，定义：xm＝x1,xb＝[x0,x2,x3]^t,xr＝x4，相应的约束为zm＝{z01,z12,z13},zc＝{z0,z03,z23},zr＝{z04,z34}。

现在，系统需要丢掉变量xm,优化xb,xr。为了不丢失信息，正确的做法是把zm分装成和被边缘化的变量有约束关系的变量xb的先验信息，分装成先验信息，即在zm条件下xb的概率：

上面就是把xm,xb之间的约束封成了先验信息了，带着先验信息去优化xb,xr,这样就不会丢失约束信息了。

为了求解只需要求解这个非线性最小二乘：

如何求解这个非线性最小二乘呢，得到海森矩阵，表示为：

正常情况下，通过hx＝b就能得到x,这里不要求解xm,因此对h矩阵进行schurcomplement分解就能直接得到xb:

这样得到又得到这样就得到先验信息了。这样就可以直接丢掉xm而不丢失约束信息了，现在式子可表示为：

构建最小二乘优化问题，本来通过hδx＝b就能求解xm,xb，这里应用舒尔补(schur消元(schurelimination))，只求解xb，而不求解xm，这样就得到先验信息了。这样就可以去掉xm，只优化xb,xr。

这里要注意，把xm去掉，最多丢了信息，但是上述要注意xb的值，否则一不小心就引入错误信息，造成系统崩溃，对xb进行求雅克比，用的是marg时的xb，而不能用和xr一起优化后的xb，这就是边缘化的一致性问题。用的是fej(firstestimatejacobian)。

另一方面，对于特征点来说，如何保证h矩阵的稀疏性呢，要边缘化(marg)不被其他帧观测到的路标点，这样不会变的稠密，对于被其他帧观测到的路标点，要么就别marg,要么就直接丢弃。

本系统及方法的应用场景为室内环境下的移动机器人的定位，包含地面机器人、无人机等，通过相机imu的紧耦合、再结合深度相机的深度测量，提供提高定位的鲁棒性和精确性。

上述各个实施例可以相互参照，本实施例不对各个实施例进行限定。

最后应说明的是：以上所述的各实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或全部技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。