CN113298796A - 一种基于最大后验imu的线特征slam初始化方法 - Google Patents

Abstract

本发明涉及一种基于最大后验IMU的线特征SLAM初始化方法，通过合并梯度相近的像素点来实现像素合并，通过计算NFA检测获得的近似矩形是否可以作为可被提取的线特征。为在尺度空间内对线特征构建矩形支撑域，分解支撑域分解为大小相等的子区域，并结合全局和局部高斯权重系数对子区域进行梯度计算，从而获得的描述矩阵。通过相机采集到的连续三帧图像对应的LBD描述子构建三焦点张量的矩阵并求解。将地图点边缘化构建最小化重投影误差方程，获得轨迹的先验信息,以及先关的信息矩阵,用这些信息进行IMU数据的优化。此方法初始化精度更高、耗时更少，能够为后续定位带来良好的初值。

Description

一种基于最大后验IMU的线特征SLAM初始化方法

技术领域

本发明属于图像制导领域，涉及一种图像惯导初始化定位方法，具体涉及一种基于最大后验IMU的线特征SLAM初始化方法。

背景技术

同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)被认为是实现移动机器人自主作业的核心技术，已被广泛应用于无人机、无人车和虚拟现实等领域。对于室内环境，由于建筑物会对GPS信号进行遮蔽，无人机定位多采用SLAM技术，为了克服单一传感器自身精度的不足，常采用多传感器融合策略，如视觉惯性就是一种有效的融合方法，且相机和惯性测量单元(Inertial Measurement Unit，IMU)均具有重量轻、成本低的特点，符合室内无人机轻量化和低功耗的需求。

随着技术的发展和越来越多开源系统的出现，视觉SLAM技术逐渐地成熟，然而仍有很多实际的问题需要解决。其中视觉SLAM的局限之一是过于依赖场景特征，如大部分视觉SLAM中采用的点特征。当场景中纹理信息缺失时或相机快速运动导致图像模糊时，点特征的数量往往较少，影响了位姿估计的精度。现有主流的视觉惯性SLAM有VINS-MONO，ORB-SLAM等，在光照条件好的环境中定位较为精准。但在弱纹理、弱照明及光照变化明显等场景会产生初始化失败以及特征点提取变少导致的定位失效。线特征在此类场景具有较高的光照不变性，在较大的视角变化下也比较稳定。但基于线特征的视觉惯性SLAM初始化时间远超过点特征，由此本发明专利提出一种基于后验IMU的点线特征SLAM初始化方法减少点线特征SLAM初始化时间。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于最大后验IMU的线特征SLAM初始化方法。

本发明由视觉特征处理，IMU初始化和联合初始化三大部分组成。首先利用LSD线特征提取算法对接收到的图像进行处理，用LBD方法对提取的线特征添加描述子，构建线特征空间信息；其次将连续三帧图像对应的LBD描述子构建三焦点张量的矩阵并求解得到纯视觉的位姿；接下来使用最大后验估计处理IMU的预积分数据，求得旋转矩阵，速度，位移的误差；最后，主要任务为对齐相机与IMU信息以及对状态变量进行初始估计，包括相机与IMU信息对齐，初始估计的状态向量则主要包括重力向量g、速度v、尺度s与偏差bias，为后端优化模块提供良好初值。

技术方案

一种基于最大后验IMU的线特征SLAM初始化方法，其特征在于步骤如下：

步骤1：采用LSD算法对相机拍摄图像进行边缘提取，得到若干条带；

步骤2、添加LBD描述子：计算出每个条带对应的特征向量BD_j，将特征向量BD_j与条带并将生成LBD描述子；

步骤3、线特征初始化：将连续三帧图像对应的LBD描述子构建一个三焦点张量的矩阵并求解，取数值最小的一组解得到位姿信息获得轨迹的先验信息，完成位姿初始化；

步骤4、IMU初始化：以轨迹的先验信息进行IMU数据的优化；

将所有轨迹和关键帧的位姿固定，仅仅优化IMU的相关参数：

其中,s也就是整个地图的尺度；R_wg∈SO₍₃₎是重力加速度方向，表示为g＝R_wgg_i g_i＝(0,0,G)^T,

表示IMU的加速度和角速度偏差，

是所有的关键帧对应的无尺度的速度；

相对于真值s*v表示的速度,使用速度

同时，在初始化的过程中偏置的值固定不变，将重力加速度纳入到优化公式中；所有的测量值都是IMU的变量，同时用L_i,j为IMU从第i帧到第j帧之间的预积分，L_0,k表示在初始化窗口的连续关键帧之间的预积分；

制定一个最大后验估计：

p(χ_k|L_0:k)∝p(L_0:k|χ_k)p(χ_k)

其中，p(L_0:k|χ_k)表示在给定第k帧IMU数据的情况下,从0到k帧的可能的IMU测量值分布可能性；p(χ_k)为IMU状态的先验；

将其分解为：

找到参数使后验概率最大，意味着使其负对数的值更小：

IMU的预积分都是服从高斯分布的，则MAP问题等价于：

其中，r_p和

为先验值和IMU测量值的残差，而∑_p和

为他们的协方差；

优化的过程中；

定义旋转矩阵，速度，位移的误差如下：

步骤5、视觉惯性联合初始化：将视觉初始位姿与IMU初始化结果即旋转矩阵，速度，位移的误差作一阶线性相似，对其求解最小二乘得到速度，加速度和尺度信息，完成初始化。

有益效果

本发明提出的一种基于最大后验IMU的线特征SLAM初始化方法，通过合并梯度相近的像素点来实现像素合并，通过计算NFA(Number of False Alarms)检测获得的近似矩形是否可以作为可被提取的线特征。为在尺度空间内对线特征构建矩形支撑域，分解支撑域分解为大小相等的子区域，并结合全局和局部高斯权重系数对子区域进行梯度计算，从而获得的描述矩阵。通过相机采集到的连续三帧图像对应的LBD描述子构建三焦点张量的矩阵并求解。将地图点边缘化构建最小化重投影误差方程，获得轨迹的先验信息,以及先关的信息矩阵,用这些信息进行IMU数据的优化。

本发明提出的一种基于后验IMU的线特征SLAM初始化方法，该方法将传统的IMU初始化使用最小二乘法求初值转变为MAP(最大后验估计)求初值，降低了计算复杂度，这缩同时IMU初始化结果更稳健，短了整体初始化时间。满足无人机在弱纹理、弱照明及光照变化明显等场景的快速部署。相较于线特征原始初始化方法，此方法初始化精度更高、耗时更少，能够为后续定位带来良好的初值。

附图说明

图1：本发明中具体实施步骤的流程图

图2：LBD算子描述图

图3：线特征初始化位姿示意图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

由图1看出本发明的思路：首先通过LSD算法提取图像中的线特征信息，再添加LBD描述子来进行前后帧的特征匹配；视觉初始化首先进行相机位姿初始化利用PNP求解位姿，再进行空间线特征初始化，利用三焦点张量的矩阵并求解得到旋转矩阵；IMU进行预积分将帧与帧之间的IMU相对测量信息转换为约束节点(载体位姿)参与到优化框架中，对IMU相对测量进行处理，使得它与绝对位姿解耦，从而大大提高优化速度。最后通过联合初始化求解最终的无人机位姿。

具体过程：

1、采用LSD算法对相机拍摄图像进行边缘提取，得到若干条带；

接收外部传感器传输的图像后，使用LSD算法将线段定义为一个连续的图像区域，在该图像区域内所有的点都大致拥有相同的图像梯度方向。线段检测LSD算法就能根据赫姆霍兹原理通过有效的融合梯度方向和线段校验从而生成准确的线段，并且控制错误检测率在一个较低的水准。

2、添加LBD描述子：计算出每个条带对应的特征向量BD_j，将特征向量BD_j与条带并将生成LBD描述子；

利用改进的LBD描述子描述线段的局部外观，并结合几何约束来实现线段匹配。

如图1所示，由若干个相互平行的条带组成一个线段支持域LSR，并定义d⊥与dL两个方向来实现旋转不变性，其中d⊥为线段的方向，d⊥为dL的顺时针垂直方向。设条带数目为m，宽度为长度为线段的长度。考虑到距离中心较远的像素梯度对描述子的影响，LBD算法引入全局高斯权重函数f_g，以降低LSR区域内沿d⊥方向远离中心行的权重。同时，引入局部高斯权重函数f_t，以削弱边界效应，避免条带间描述子的骤然变化。

计算出每个条带尽对应的特征向量BD_j，并将其合并以生成LBD描述子

对于每个条带B_j，其条带描述子BD_j由B_J-1和B_J+1计算得到，具体计算过程为：对于B_j的第K行，累计该行中四个方向(d⊥和dL的正反方向)。

像素的梯度：

其中，为λ＝f_g(k)f_l(k)为高斯权重系数。通过条带B_j的四个累积梯度，可以构造条带描述矩阵BDM_j：

其中

最后计算该矩阵的均值向量M_j和标准差S_j，进而得到LBD特征向量：

此时，LBD描述子为8m维的特征向量。为了提高描述子的匹配效率，还需要将LBD描述子转化为二进制描述子。二进制编码的规则是：对于特征向量LBD中,若第P个元素比第q个元素大，编码为1，否则编码为0。按照一定的顺序取出若干对后，可以形成一个与BRIEF描述子类似的二值编码串，从而可以使用汉明距离来度量两个LBD描述子之间的距离。

3、线特征初始化：将连续三帧图像对应的LBD描述子构建一个三焦点张量的矩阵并求解，取数值最小的一组解得到位姿信息获得轨迹的先验信息，完成位姿初始化；

如图2所示，设定空间线特征端点P,Q在三帧相机视图上投影分别为{P₁,Q₁}，{P₂,Q₂}，{P₃,Q₃}，其中I₁,I₂,I₃∈R³为计算投影时相对应的线系数。

本文基于相机运动为小旋转且连续的假设，使相邻相机视图之前旋转相同，即R¹＝R^T,R₂＝I,R₃＝R,其中I为3*3单位向量，R可以近似表示为：

线系数I_i,i＝{1,2,3}的几何含义是垂直于投影中心C_i与p_i,Q_i形成平面的向量参数。在同一个参考坐标系中，两个线系数向量叉乘后平行于直线PQ，垂直于第三个线系数向量。可构建几何约束式如下：

以此对三帧匹配线特征进行此约束，可得三个二次方程形成的方程组，未知数为式(1.1)中的r₁,r₂,r₃，采用多项式求解器即可以得到r₁,r₂,r₃的八组解。针对每组解的旋转矩阵R使用三焦点张量方程在假定t₂＝0线性近似可得到t₁,t₃，最后对八组解的t₁,t₃回代式(1.2)中进行验证，取数值最小的一组解，可完成位姿初始化。

4、以轨迹的先验信息进行IMU数据的优化；

这一步的主要目标是通过上一步中视觉计算出的位姿，从而获得IMU相关参数的优化结果。位姿优化需要一个很好的初值。因此，将所有轨迹和关键帧的位姿固定，仅仅优化IMU的相关参数。

其中,s也就是整个地图的尺度；R_wg∈SO₍₃₎是重力加速度方向，表示为g＝R_wgg_i g_i＝(0,0,G)^T,

表示IMU的加速度和角速度偏差，

是所有的关键帧对应的无尺度的速度。

相对于真值s*v表示的速度,使用速度

使得优化过程更加容易；同时，假设在初始化的过程中偏置的值固定不变，随机游走几乎没有什么影响，并且从一开始就将重力加速度纳入到优化公式中。所有的测量值都是IMU的变量，同时用L_i,j为IMU从第i帧到第j帧之间的预积分，L_0,k表示在初始化窗口的连续关键帧之间的预积分。有了定义的状态和测量值，我们可以制定一个最大后验估计(MAP估计问题)，其中后验分布是：

p(χ_k|L_0:k)∝p(L_0:k|χ_k)p(χ_k) (0.7)

其中，p(L_0:k|χ_k)表示在给定第k帧IMU数据的情况下,从0到k帧的可能的IMU测量值分布可能性。p(χ_k)为IMU状态的先验。考虑到测量值之间相互独立，可以将其分解为:

我们需要找到参数使后验概率最大，意味着使其负对数的值更小：

因为假设IMU的预积分都是服从高斯分布的，则MAP问题等价于：

其中，r_p和

为先验值和IMU测量值的残差，而∑_p和

为他们的协方差。在本次优化的过程中，不在考虑视觉投影误差，只有IMU的误差。我们定义旋转矩阵，速度，位移的误差如下：

5、视觉惯性联合初始化：视觉惯性联合初始化：将视觉初始位姿与IMU初始化结果即旋转矩阵，速度，位移的误差作一阶线性相似，对其求解最小二乘得到速度，加速度和尺度信息，完成初始化。

在进行陀螺仪偏置初始化的时候要同时使用到IMU测量的旋转和视觉测量的旋转，即联合视觉和惯性的数据。同时求得的尺度S的值不仅仅是IMU的，还是视觉和IMU整个系统的尺度。旋转我们可以通过两种方式求得，一种是陀螺仪测量值，另一种就是视觉观测值。陀螺仪的误差有两部分测量噪声和陀螺仪偏置，噪声暂时忽略，而视觉的误差就只有观测噪声，因此两者差值的绝对值就是陀螺仪偏置，将整个滑动窗口的所有的旋转做差构成了一个最小化误差模型：

公式1.12中第一个式子的第一项和第二项作四元数旋转的广义乘积就可以得到相机从b_k到b_k+1下的相对旋转(b_k+1坐标系下)，第三项是陀螺仪从b_k+1到b_k下的相对旋转(b_k坐标系下)，两者在做广义乘积，就是首先从b_k到b_k+1旋转，然后再从b_k+1到b_k旋转，相当于做差，第二个式子就是前面预积分提到的一阶线性近似。然后取最小二乘，也可以使用SVD分解等方法求解。在求得陀螺仪偏置之后要再次将陀螺仪偏置代入到预积分中再求一次预积分的值会更加精确。

将状态量统一到一个状态向量中，如公式1.13所示：

速度是在b_k坐标系下的，重力加速度在初始相机坐标系下，求解这几个量是由P、V数学模型而求得，同时考虑两个连续关键帧b_k和b_k+1。即求出速度、重力加速度和尺度标定值。

Claims (1)

1.一种基于最大后验IMU的线特征SLAM初始化方法，其特征在于步骤如下：

步骤1：采用LSD算法对相机拍摄图像进行边缘提取，得到若干条带；

步骤2、添加LBD描述子：计算出每个条带对应的特征向量BD_j，将特征向量BD_j与条带并将生成LBD描述子；

步骤3、线特征初始化：将连续三帧图像对应的LBD描述子构建一个三焦点张量的矩阵并求解，取数值最小的一组解得到位姿信息获得轨迹的先验信息，完成位姿初始化；

步骤4、IMU初始化：以轨迹的先验信息进行IMU数据的优化；

将所有轨迹和关键帧的位姿固定，仅仅优化IMU的相关参数：

其中,s也就是整个地图的尺度；R_wg∈SO₍₃₎是重力加速度方向，表示为g＝R_wgg_ig_i＝(0,0,G)^T,

表示IMU的加速度和角速度偏差，

是所有的关键帧对应的无尺度的速度；

相对于真值s*v表示的速度,使用速度

同时，在初始化的过程中偏置的值固定不变，将重力加速度纳入到优化公式中；所有的测量值都是IMU的变量，同时用L_i,j为IMU从第i帧到第j帧之间的预积分，L_0,k表示在初始化窗口的连续关键帧之间的预积分；

制定一个最大后验估计：

p(χ_k|L_0:k)∝p(L_0:k|χ_k)p(χ_k)

其中，p(L_0:k|χ_k)表示在给定第k帧IMU数据的情况下,从0到k帧的可能的IMU测量值分布可能性；p(χ_k)为IMU状态的先验；

将其分解为：

找到参数使后验概率最大，意味着使其负对数的值更小：

IMU的预积分都是服从高斯分布的，则MAP问题等价于：

其中，r_p和

为先验值和IMU测量值的残差，而∑_p和

为他们的协方差；

优化的过程中；

定义旋转矩阵，速度，位移的误差如下：

步骤5、视觉惯性联合初始化：将视觉初始位姿与IMU初始化结果即旋转矩阵，速度，位移的误差作一阶线性相似，对其求解最小二乘得到速度，加速度和尺度信息，完成初始化。

Images