CN111982103A - 一种权值优化的点线综合视觉惯性里程计方法 - Google Patents

Abstract

本发明公开了一种权值优化的点线综合视觉惯性里程计方法，其步骤如下：利用载体搭载的相机和IMU采集环境信息与运动信息，并实现点特征和直线特征的提取与管理，IMU运动信息的预积分；利用点特征、直线特征与IMU的残差获得初始的位姿解算目标函数；利用基于词频的非线性权值抑制不稳定特征，优化各项残差构成的目标函数模型；利用优化后的目标函数计算载体位子与运动情况。本发明提供了一种权值优化的点线综合视觉惯性里程计方法，利用特征直线解决弱纹理场景下特征数量较少的问题，并使用非线性权值优化了特征残差模型，实现弱纹理场景下视觉惯性里程计的稳定有效运行。

Description

一种权值优化的点线综合视觉惯性里程计方法

技术领域

本发明涉及计算机视觉技术领域，更具体的说是涉及一种权值优化的点线综合视觉惯性里程计方法。

背景技术

视觉惯性里程计是常见的组合导航方法之一，其具有低成本和小型便捷的特点。近年来，随着小型平台的发展，视觉惯性里程计被广泛应用，对于算法的精度、适用环境与稳定性的要求也在日益增高。但是在惯性视觉里程计的视觉部分，常因光照、环境纹理条件差等环境因素导致采集不到足够多的特征点使得系统出现故障，这种情况对算法的鲁棒性和稳定性造成了很大的影响。此外，在特征匹配当中出现的误匹配也会对系统的解算结果与精度产生负面影响。

因此，如何提供一种在弱纹理场景及特征不稳定情况下稳定有效运行的视觉惯性里程计方法是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种权值优化的点线综合视觉惯性里程计方法，利用特征直线补充特征数量，利用基于词频的非线性权值抑制不稳定特征，实现弱纹理场景及特征不稳定情况下视觉惯性里程计稳定有效运行。

为了实现上述目的，本发明提供如下技术方案：

一种权值优化的点线综合视觉惯性里程计方法，包括如下步骤：

S1、利用载体搭载的相机和IMU采集环境信息与运动信息，选取关键帧，对关键帧进行滑窗边缘化处理，并实现点特征和直线特征的提取与管理，IMU运动信息的预积分；

S2、计算点特征残差、直线特征残差、IMU残差和边缘化先验信息；

S3、利用基于词频的非线性权值抑制不稳定特征，优化点特征和直线特征残差，获得目标函数模型；

S4、利用优化后的所述目标函数模型计算载体位姿与运动情况。

优选的，所述步骤S3中的非线性权值包括：特征所在图像帧的重要程度和相对于同一图像帧中其它特征的重要程度。

优选的，所述步骤S1包括：

S11、利用载体搭载的相机和IMU采集环境信息与运动信息，选取关键帧并进行边缘化；

S12、在相机采集的环境信息图像中提取FAST特征点，使用Brief描述子进行特征点描述，并利用光流法进行特征点的追踪；

S13、在相机采集的环境信息图像中使用直线分割算子LSD提取特征直线，使用直线描述子LBD进行特征直线描述，使用普吕克坐标与正交坐标对特征直线进行表示与管理。

优选的，所述步骤S3包括：

S31、统计图像帧采集频次、特征点和特征直线出现频次，根据频次计算非线性权值；

S32、根据所述非线性权值优化目标函数，获得目标函数模型。

优选的，所述步骤S4包括：利用权值优化后的特征点、特征直线、IMU数据及边缘化先验信息组成的目标函数进行位姿求解，获得载体动态位姿。

优选的，所述步骤S31中的基于词频的非线性权值为

用于衡量特征所在图像帧的重要程度的权值：

其中M_max为随着新图像帧特征提取更新的所有帧当中特征数量最大值，M_this为当前帧检测到的特征数量，M_threshold为用于模型修正的阈值。

用于衡量特征相对于同一图像帧中其它特征的重要程度：

其中，m_this为特征提取到的频次，m_average为特征所在图像帧中特征提取的平均频次。

优选的，所述步骤S32中非线性权值优化后的目标函数为：

其中，各项依次为系统的边缘化先验信息残差、IMU残差、特征点残差和特征直线残差；χ为包含所有参数、状态在内的估计向量；r_m为旧边缘化残差，H_m为多元高斯分布建立的信息矩阵；B为IMU测量集合，b为IMU测量数据帧数，r_B为IMU残差即观测量

与χ中估计量的误差，

为IMU协方差矩阵；P为当前滑动窗口中跟踪匹配到的特征点集合，p为当前帧特征点，q为该特征点被观测到的帧数，r_P为特征点重投影误差即观测特征点

与χ中估计特征点投影的误差，

为特征点协方差矩阵；L为特征直线集合，i为当前帧特征直线，j为该特征直线被观测到的帧数，r_L为特征点重投影误差即观测特征直线

与χ中估计特征直线投影的误差，

为特征点协方差矩阵；

为当前帧特征点p对应的两项权值，

为当前帧特征直线l对应的两项权值。

本发明实施例提供的上述技术方案的有益效果至少包括：

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种权值优化的点线综合视觉惯性里程计方法，利用直线特征增加特征数量，利用非线性权值抑制不稳定特征残差影响，实现弱纹理场景及特征不稳定情况下视觉惯性里程计的稳定运行。

本发明的其它特征和优点将在随后的说明书中阐述，而且部分关键技术细节从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明实施例提供的权值优化的点线综合视觉惯性里程计方法的流程图。

具体实施方式

下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。

本发明实施例公开了一种权值优化的点线综合视觉惯性里程计方法，如图1所示，包括以下具体步骤：

S1、利用载体搭载的相机和IMU采集环境信息与运动信息，选取关键帧，对所述关键帧进行滑窗边缘化处理，并实现点特征和直线特征的提取与管理、IMU运动信息的预积分；

使用载体上经过标定的相机与IMU进行数据的测量，当相机采集的环境信息具有足够视差时，选取同一时刻的IMU数据共同作为关键帧以供视觉惯性里程计使用。使用滑窗进行关键帧管理，当关键帧数量超出滑窗大小时，对最旧的关键帧进行边缘化处理。对关键帧中视觉数据进行特征提取、匹配与管理，对IMU数据进行预积分。

S2、计算点特征残差、直线特征残差、IMU残差和边缘化先验信息；

点特征和直线特征残差分别定义为点特征和直线特征的重投影误差，IMU残差为IMU观测误差，边缘化先验信息由当前关键帧与已存在关键帧的匹配即回环检测获得。

S3、利用基于词频的非线性权值抑制不稳定特征，优化点特征和直线特征残差，获得目标函数模型；

利用特征管理数据进行词频统计，即特征出现频率的统计，出现频率高的特征重要程度高，特征点数量多的图像帧重要程度高，重要程度高则具有更大的权值。用统计词频获得的权值优化残差模型，获得权值优化后的目标函数。

S4、利用优化后的所述目标函数模型计算载体位姿与运动情况。

利用权值优化后的目标函数进行非线性优化，使其迭代下降获得最优位姿与运动估计。

为了进一步优化上述技术方案，上述用于优化特征残差模型与解算目标函数的非线性权值包括：特征所在图像帧的重要程度和相对于同一图像帧中其它特征的重要程度。

为了进一步优化上述技术方案，所述步骤S1包括：

S11、利用载体搭载的相机和IMU采集环境信息与运动信息，选取关键帧并进行边缘化；

S12、在相机采集的环境信息图像中提取FAST特征点，使用Brief描述子进行特征点描述，并利用光流法进行特征点的追踪；

S13、在相机采集的环境信息图像中使用Line Segment Detector(LSD)算法提取特征直线，使用Line Band Descriptor(LBD)描述子进行特征直线描述，使用普吕克坐标与正交坐标对特征直线进行表示与管理。

为了进一步优化上述技术方案，所述步骤S3包括：

S31、统计图像帧采集频次、特征点和特征直线出现频次，根据频次计算非线性权值；

S32、根据所述非线性权值优化目标函数，获得目标函数模型。

为了进一步优化上述技术方案，所述步骤S4包括：利用权值优化后的特征点、特征直线、IMU数据及边缘化先验信息组成的目标函数进行位姿求解，获得载体动态位姿。

为了进一步优化上述技术方案，步骤S31中的基于词频的非线性权值为：

用于衡量特征所在图像帧的重要程度的权值：

其中M_max为随着新图像帧特征提取更新的所有帧当中特征数量最大值，M_this为当前帧检测到的特征数量，M_threshold为用于模型修正的阈值。

用于衡量特征相对于同一图像帧中其它特征的重要程度：

其中，m_this为特征提取到的频次，m_average为特征所在图像帧中特征提取的平均频次。

为了进一步优化上述技术方案，所述步骤S32中非线性权值优化后的目标函数为：

其中，各项依次为系统的边缘化先验信息残差、IMU残差、特征点残差和特征直线残差，χ为包含所有参数、状态在内的估计向量；r_m为旧边缘化残差，H_m为多元高斯分布建立的信息矩阵；B为IMU测量集合，b为IMU测量数据帧数，r_B为IMU残差即观测量

与χ中估计量的误差，

为IMU协方差矩阵；P为当前滑动窗口中跟踪匹配到的特征点集合，p为当前帧特征点，q为该特征点被观测到的帧数，r_P为特征点重投影误差即观测特征点

与χ中估计特征点投影的误差，

为特征点协方差矩阵；L为特征直线集合，i为当前帧特征直线，j为该特征直线被观测到的帧数，r_L为特征点重投影误差即观测特征直线

与χ中估计特征直线投影的误差，

为特征点协方差矩阵；

为当前帧特征点p对应的两项权值，

为当前帧特征直线l对应的两项权值。

将上述过程进行迭代执行则可以使用视觉惯性里程计获得载体的动态位姿与运动估计。

以下通过一个更具体的实施例对步骤进行说明。

步骤一：利用载体搭载的相机和IMU采集环境信息与运动信息，选取关键帧，对关键帧进行滑窗边缘化处理，并实现点特征和直线特征的提取与管理，IMU运动信息的预积分，其具体步骤为：

(1)使用载体上经过标定的相机与IMU进行数据的采集与测量。设定视差阈值为d_视差，当相机采集的图像与前一关键帧的视差大于该阈值时，选取同一时刻的IMU数据共同作为关键帧。设置滑窗宽度为w_滑窗，当关键帧数量超出该滑窗宽度时，对最旧的关键帧进行边缘化处理。

(2)在关键帧图像中提取FAST特征点，使用Brief描述子进行特征点描述，并利用光流法进行特征点的追踪。

(3)在相机采集的环境信息图像中使用Line SegmentDetector(LSD)算法提取特征直线，使用Line Band Descriptor(LBD)描述子进行特征直线描述，使用普吕克坐标与正交坐标对特征直线进行表示与管理。

LSD方法主要步骤为：

①对图像进行高斯下采样；

②对图像中的像素点进行灰度梯度求解，获得各点的梯度值和梯度方向记为g，将梯度的法向量方向作为像素方向h；

③通过像素方向获得备选直线域，求解该区域的最小外接矩形与该外接矩形的主方向H；

④如果最小外接矩形是细长的，则是要检测的特征直线的可能性大，再判断矩形内各个像素点方向h与矩形主方向H差值，进行直线检测。

将直线支持域划分为m个子区域，子区域宽度为w长度与直线长度相等，将划分的子区域称为条带，那么直线的LBD描述子为

其中M_j为BDM_j的均值，S_j为BDM_j的标准方差。中间条带的描述子BDM的计算公式为

其中，num_band为总行数，由于在j＝1,m即两边缘条带处由于只有一个相邻的条带，因此总行数为两倍的条带宽度；

和

分别为第i行像素在直线正反方向与法线正反方向梯度投影和。

直线的普吕克坐标表示记为L＝(n^T,d^T)^T，其中d为直线的方向向量，n为直线与坐标原点构成平面的法向量，二者有正交约束d^T·n＝0，即普吕克坐标表示具有5个自由度。

对直线的普吕克坐标进行QR分解可得到正交表示的正交矩阵U与旋转矩阵W为

步骤二：计算点特征残差、直线特征残差、IMU残差和边缘化先验信息，其中本发明实施例对直线特征残差的定义：

直线特征的重投影误差为观测直线的起始点s与终止点e到重投影预测直线的点到直线距离，其计算公式为：

其中，l＝(l₁ l₂ l₃)为像素平面的投影直线，可由相机坐标系下直线投影到像素平面获得，其计算公式为：

其中，K为相机的内参矩阵；f_x、f_y、c_x和c_y分别为图像x轴和y轴的焦距和中心像素位置；n_c为相机坐标系下特征直线的法向量。

在使用直线残差进行优化求解位姿的过程中，后端优化函数需要残差的雅可比矩阵进行优化求解，对于直线残差雅可比矩阵分为对位姿求解雅可比矩阵

和对直线变量求解雅可比矩阵

两部分，根据链式法则可对二者进行求解，其求解公式分别为

其中，r_l为残差；l为图像平面投影直线；

与L^w分别为相机坐标系和世界坐标系下直线；x为位姿；O为特征直线的正交参数表示。

步骤三：利用基于词频的非线性权值抑制不稳定特征，优化点特征和直线特征残差，获得新的目标函数模型，其具体步骤为：

(1)统计图像帧采集频次、特征点和特征直线出现频次，根据频次计算非线性权值。

对于图像特征，本发明实施例主要从两个方面来衡量其重要性，一是特征所在图像帧的重要程度，二是特征在同一图像帧中相对于其它特征的重要程度。

用于衡量特征所在图像帧的重要程度的权值：

其中M_max为随着新图像帧特征提取更新的所有帧当中特征数量最大值，M_this为当前帧检测到的特征数量，M_threshold为用于模型修正的阈值。

用于衡量特征相对于同一图像帧中其它特征的重要程度：

其中，m_this为特征提取到的频次，m_average为特征所在图像帧中特征提取的平均频次。

(2)根据非线性权值优化目标函数

权值优化后的目标函数为

其中，各项依次为系统的边缘化先验信息残差、IMU残差、特征点残差和特征直线残差，χ为包含所有参数、状态在内的估计向量；r_m为旧边缘化残差，H_m为多元高斯分布建立的信息矩阵；B为IMU测量集合，b为IMU测量数据帧数，r_B为IMU残差即观测量

与χ中估计量的误差，

为IMU协方差矩阵；P为当前滑动窗口中跟踪匹配到的特征点集合，p为当前帧特征点，q为该特征点被观测到的帧数，r_P为特征点重投影误差即观测特征点

与χ中估计特征点投影的误差，

为特征点协方差矩阵；L为特征直线集合，i为当前帧特征直线，j为该特征直线被观测到的帧数，r_L为特征点重投影误差即观测特征直线

与χ中估计特征直线投影的误差，

为特征点协方差矩阵；

为当前帧特征点p对应的两项权值，

为当前帧特征直线l对应的两项权值。

步骤四：利用权值优化后的目标函数进行非线性优化，使其迭代下降获得最优位姿与运动估计。

将上述过程进行迭代执行则可以使用视觉惯性里程计获得载体的动态位姿与运动估计。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (7)

1.一种权值优化的点线综合视觉惯性里程计方法，其特征在于，包括步骤：

S1、利用载体搭载的相机和IMU采集环境信息与运动信息，选取关键帧，对所述关键帧进行滑窗边缘化处理，并实现点特征和直线特征的提取与管理、IMU运动信息的预积分；

S2、计算点特征残差、直线特征残差、IMU残差和边缘化先验信息；

S3、利用基于词频的非线性权值抑制不稳定特征，优化点特征和直线特征残差，获得目标函数模型；

S4、利用优化后的所述目标函数模型计算载体位姿与运动情况。

2.根据权利要求1所述的一种权值优化的点线综合视觉惯性里程计方法，其特征在于，所述步骤S3中的非线性权值包括：特征所在图像帧的重要程度和相对于同一图像帧中其它特征的重要程度。

3.根据权利要求1所述的一种权值优化的点线综合视觉惯性里程计方法，其特征在于，所述步骤S1包括：

S11、利用载体搭载的相机和IMU采集环境信息与运动信息；

S12、在相机采集的环境信息图像中提取FAST特征点，使用Brief描述子进行特征点描述，并利用光流法进行特征点的追踪；

S13、在相机采集的环境信息图像中使用直线分割算子LSD提取特征直线，使用直线描述子LBD进行特征直线描述，使用普吕克坐标与正交坐标对特征直线进行表示与管理。

4.根据权利要求2所述的一种权值优化的点线综合视觉惯性里程计方法，其特征在于，所述步骤S3包括：

S31、统计图像帧采集频次、特征点和特征直线出现频次，根据频次计算非线性权值；

S32、根据所述非线性权值优化目标函数，获得目标函数模型。

5.根据权利要求1所述的一种权值优化的点线综合视觉惯性里程计方法，其特征在于，所述步骤S4包括：利用权值优化后的特征点、特征直线、IMU数据及边缘化先验信息组成的目标函数进行位姿求解，获得载体动态位姿。

6.根据权利要求4所述的一种权值优化的点线综合视觉惯性里程计方法，其特征在于，所述步骤S31中的基于词频的非线性权值为：

用于衡量特征所在图像帧的重要程度的权值：

其中M_max为随着新图像帧特征提取更新的所有帧当中特征数量最大值，M_this为当前帧检测到的特征数量，M_threshold为用于模型修正的阈值。

用于衡量特征相对于同一图像帧中其它特征的重要程度：

其中，m_this为特征提取到的频次，m_average为特征所在图像帧中特征提取的平均频次。

7.根据权利要求4所述的一种权值优化的点线综合视觉惯性里程计方法，其特征在于，所述步骤S32中非线性权值优化后的目标函数为：

其中，各项依次为系统的边缘化先验信息残差、IMU残差、特征点残差和特征直线残差；χ为包含所有参数、状态在内的估计向量；r_m为旧边缘化残差，H_m为多元高斯分布建立的信息矩阵；B为IMU测量集合，b为IMU测量数据帧数，r_B为IMU残差即观测量

与χ中估计量的误差，

为IMU协方差矩阵；P为当前滑动窗口中跟踪匹配到的特征点集合，p为当前帧特征点，q为该特征点被观测到的帧数，r_P为特征点重投影误差即观测特征点

与χ中估计特征点投影的误差，

为特征点协方差矩阵；L为特征直线集合，i为当前帧特征直线，j为该特征直线被观测到的帧数，r_L为特征点重投影误差即观测特征直线

与χ中估计特征直线投影的误差，

为特征点协方差矩阵；

为当前帧特征点p对应的两项权值，

为当前帧特征直线l对应的两项权值。

Images