中国科技核心期刊      中国指挥与控制学会会刊     军事装备类重点期刊
指挥控制与仿真

指挥控制与仿真 >

2020 , Vol. 42 >Issue 1: 47 - 53

DOI: https://doi.org/10.3969/j.issn.1673-3819.2020.01.010

理论研究

基于多源信息融合的单目定位算法

  • 周扬 ,
  • 王勇 ,
  • 徐国亮
展开
  • 江苏自动化研究所, 江苏 连云港 222061

作者简介：周 扬(1995—),男,重庆人,硕士研究生,研究方向为视觉SLAM。

王 勇(1979—),男,高级工程师。

Copy editor: 张培培

收稿日期: 2019-10-13

  修回日期: 2019-11-14

  网络出版日期: 2022-05-19

收起

Monocular Positioning Algorithm Based on Multi-source Information Fusion

  • ZHOU Yang ,
  • WANG Yong ,
  • XU Guo-liang
Expand
  • Jiangsu Automation Research Institute of CSIC, Lianyungang 222061, China

Received date: 2019-10-13

  Revised date: 2019-11-14

  Online published: 2022-05-19

Fold

摘要

针对低纹理环境下或相机快速旋转时单目相机定位中存在的精度和鲁棒性差的问题,给出了一种集成线特征的单目视觉惯性里程计算法(Visual Inertial Odometry,VIO),并采用Dog-Leg方法代替Levenberg-Marquard方法进行非线性求解,降低了运算量。基于实测数据的实验结果也表明,相比于VINS-Mono算法和OKVIS-Mono算法,所给出的定位方法可以获得更高的定位精度。

关键词： 低纹理; 线特征; Dog-Leg; 定位精度

本文引用格式

周扬 , 王勇 , 徐国亮 . 基于多源信息融合的单目定位算法[J]. 指挥控制与仿真, 2020 , 42(1) : 47 -53 . DOI: 10.3969/j.issn.1673-3819.2020.01.010

Abstract

In this paper, the accuracy and robustness of monocular camera positioning in low-texture environment or camera rotation are presented. A monocular visual inertial odometry (VIO) method with integrated line features is presented. The Dog-Leg method is used instead of the Levenberg-Marquard method for nonlinear solution, which reduces the amount of computation. The experimental results based on the measured data also show that the positioning method can achieve higher positioning accuracy than the VINS-Mono algorithm and the OKVIS-Mono algorithm.

Key words： low texture; line feature; Dog-Leg; positioning accuracy

里程计算法属于同时定位与建图(Simultaneous Localization and Mapping,SLAM)中的导航定位部分。SLAM是指搭载传感器的载体在移动时建立对环境的描述模型,同时估计自己的运动,这对无人系统的导航、控制、任务规划等领域有重大意义。未来的战争必将是无人化战争,无人系统会应用在军事领域的各个方面。例如无人车、无人机的前线侦察,特别是在无GPS信号的环境下,无人系统对前线侦察有着重大意义,里程计算法成了无人系统导航定位的关键技术。
由于前线环境的复杂性,单一的传感器无法满足,需要更加轻量型的无人系统,所以在传感器的选择上,相机比激光雷达更适合,也更经济。视觉传感器分为深度相机、多目相机和单目相机。深度相机能直接获得深度数据但售价较高;多目相机准确度高,但需要处理的图片信息增多,且镜头的差异性对算法影响较大;单目相机价格低廉,且算法复杂度适中。因为纯视觉对图像的依赖过大,相机可能由于被遮挡或者运动过快会导致系统失效,需要选择与相机融合的传感器进行融合定位。IMU与相机存在互补性,IMU可以在图像丢失的短暂时间提供位姿估计,同时更好地确定系统尺度,相机对IMU也有互补作用,相机不会产生漂移而IMU漂移较大。所以,IMU与视觉融合对系统的鲁棒性有很大的提升。目前,主流的视觉惯性系统仅使用点要素作为视觉信息。然而,低纹理环境中的点检测是具有挑战性的[1],相比之下,线特征提供了比点特征更多的环境结构信息[2],可以提高数据关联的准确性。

1 算法框架

传统SLAM算法框架大致有前端的数据关联、后端的优化求解、建图、回环检测和重定位五个部分。本文研究的VIO算法只包含前端与后端两个部分,与主流方法不同的是,加入了LSD线特征以提高数据关联的准确性,以及采用改进的DL方法提高算法效率,如图1所示。 前端模块用于预处理来自IMU和相机的测量,后端模块用于估计和优化系统状态。

2 基于点线结合的特征提取

图像中具有代表性的显著特征有点特征和线特征。点特征主要是图像中的角点;线特征主要是图像中物体的边缘信息,集中在梯度明显的地方。点特征虽然能有效体现图像中特征明显的区域,但对纹理信息极其依赖,无法描述环境的几何信息,经常因为低纹理、图像模糊、视角变化而失效;线特征侧重于图像中的几何约束关系,在低纹理场景下以及相机快速旋转时有比较好的鲁棒性、光照不变性、旋转不变性。因此,两者的融合能够更好地利用环境信息,发挥互补作用,提高数据关联的准确性。对于点特征,本文采用ORB点特征[3],具有效率高、旋转不变性等优势。线特征本文采用LSD方法。LSD是一种实时有效的直线检测分割算法,能在线性时间内得出亚像素级精度的检测结果[4],其主要思想是将梯度方向相近的像素点合并。
图2是EuRoc数据集MH_03_medium序列下的点线特征提取,可以看到,在特征采集时,低纹理的平面上线特征的采集比较容易,而点特征很难提取。
图2 低纹理环境下的特征提取

2.1 线的几何表示

在算法中,表示线要素最简单方法是使用两个端点,但是,匹配来自不同视图的线的端点是困难的;此外,3D空间线仅具有四个自由度,而两个3D端点引入六个参数,这将导致过度参数化。所以,本文将3D空间中的直线视为无限直线,并采用两个参数化作为3D线。由六个参数组成的Plücker线坐标[2]由于其简单性而用于变换和投影,由四个参数组成的正交表示[5]由于其紧凑性而用于优化,如图3所示。在图3a)中,Plücker坐标中的3D空间线L= n T , d T T∈R6,其中d∈R6是线方向向量,n∈R3是由平面的法向量线和坐标原点确定的。然而,Plücker坐标过度参数化,因为有一个向量n和d之间的隐式约束,即nTd=0。因此,Plücker坐标不能直接用于无约束优化,但是,用法向量和方向矢量表示的3D线矢量很容易在两个视图中执行三角测量,它也可以方便地对线几何变换进行建模。
图3 线的空间表示

2.2 数据关联

前端从相机和IMU的原始测量值中提取信息,并通过每次新IMU测量的传播来更新IMU状态,最新的IMU状态被用作优化中的初始值。另外,新的IMU测量被预积分在优化期间约束连续的IMU状态。对于图像处理,在两个单独的线程中检测点和线特征。当新的帧到来时,通过特征匹配从前一帧跟踪到点特征,如果跟踪的点特征的数量小于阈值,则将添加由FAST检测器检测到的新拐角特征。对于线特征,LSD检测器检测新帧中的线特征,并通过基于描述子LBD与前一帧中的线段匹配。另外,我们使用几何约束来删除线匹配的异常值。在特征检测和匹配之后,将点特征和线特征端点投影到归一化图像平面上。

3 后端紧耦合视觉惯性融合

相机、IMU以及点线的视觉观测的简化示意图如图4所示。图中不仅包含视觉测量,还包含预积分的IMU测量作为边缘来约束连续的IMU状态。其中,C1表示时间t=i时的相机帧,同时bi作为同时刻的IMU帧。四元数qxy表示y到x的旋转,相应的旋转矩阵为Rxy。四元数qbc和向量pbc是相机与IMU的外参,这些外参是通过Kalibr方差[6]标定得到。 z f j C 1、 z L j C 2表示特征点fi与线段Lj的观测。对于视觉惯性融合,我们可以使用紧耦合框架以及滑动窗口[7]方法,以优化视觉惯性系统的所有状态变量。
图4 视觉惯性传感器以及点线特征示意图
在后端线程中,首先对点和线进行三角测量以构建重新投影残差。为了获得良好的地标估计,利用所有观测值估计点特征的逆深度。对于线三角测量,我们只在滑动窗口中选择具有最远空间距离的两个帧来初始化Plücker坐标。在获得点、线的初始估计以及IMU初始状态之后,使用滑动窗口优化找到最佳系统状态(相机位姿)。为了限制状态向量χ的大小,采用双向边缘化策略[8]从滑动窗口中去除旧状态。当第二个最新的帧是一个关键帧时,将所有测量结果边缘化掉最早的帧。否则,如果第二个最新帧不是关键帧,我们将丢弃此帧中的相机测量值,并在预积分中保留其IMU测量值。基于边缘化信息获得新的先验信息,从被移除的状态保留约束信息。最后,剔除无效的特征点和线。如果点的逆深度为负,将从优化中删除此点;如果线的重新投影残差超过阈值,则将从优化中移除。

3.1 基于滑动窗口的残差模型建立

在VIO中,通常采用集束调整(Bundle Adjustment,简称BA)[9]用于优化相机姿势和3D特征最小化图像平面中的重投影误差。为了紧密有效地融合来自视觉和惯性传感器的信息,本文引入了具有IMU预积分[10]约束和点、线特征的滑动窗口模型。本文建立IMU与视觉观测的统一模型,通过最小化各自的残差和来优化滑动窗口中的所有状态变量,目标残差函数如下

minxρ r p - J p χ Σ p 2+

∑i∈Bρ r b z b i b i + 1 , χ Σ b i b i + 1 2+∑(i,j)∈Fρ r f z f j c i , χ Σ f j c i 2+∑(i,l)∈Lρ r l z L i c i , χ Σ i c i 2

式(1) 中,rb z b i b i + 1 , χ是状态xi和xi+1之间的IMU测量残差,B是在滑动窗口中进行所有预积分的IMU测量,rf z f j c i , χ和rl z L i c i , χ分别是点特征和线特征重投影残差,F和L是相机框架观察到的点特征和线特征集, r p , J p是在滑动窗口边缘化一个关键帧后计算出的先验信息,ρ是柯西鲁棒核函数。
在i时刻滑动窗口内待优化的系统状态量定义如下

χ= x n , x n + 1 , … , x n + N , λ m , λ m + 1 , … , λ m + M , O o , O o + 1 , … , O o + 0 T

xi= p w b i , q w b i , v i w , b a b i , b g b i T,i∈[n,n+N]

其中:
1)xi包含i时刻IMU在惯性坐标系中的位置、速度、姿态以及IMU坐标系中的加速度和角速度的偏置量估计。
2)下标n,m,o是状态、点线特征在滑动窗口中的起始时刻,λ表示逆深度。
3)N为滑动窗口中关键帧数量,M和O是由所有关键帧观察到的点特征和线特征的数量。

3.2 非线性优化算法(DL)及改进

主流的VIO系统求解公式(1)一般采用Levenberg-Marquard(LM)[11]非线性方法求解,LM方法虽然求解稳定,但是优化求解迭代的次数较多,尤其是迭代失败回滚的次数较多;而Dog-Leg方法结合了高斯牛顿法和最快下降法的优点,更合理地选择迭代步长,使得迭代次数相对较少,求解效率高。由于本文新加入了线特征,增加了算法的模型复杂度以及求解时间,所以本文使用速度更快的DL方法进行求解并加以改进。DL通过判断高斯牛顿法与最快下降法的下降方向求得DL的迭代步长;同时,DL相比LM显式地利用信赖域,思想是在 h≤μ范围内,F(x+h)=F(x)+hTJTf+ 1 2hTJTJh能够较好地近似,通过增益比p加快迭代。经实验发现,采用Nielsen策略[12]代替原来的增益比更新策略,能够使得信赖域μ更好地利用增益比ρ以加快迭代。Nielsen策略如下
i f p > 0 　 μ : = μ · m a x 1 3 , 1 - ( 2 p - 1 ) 3 , v : = 2 e l s e μ : = μ · v , v : = 2 · v
图5是改进的DL方法的流程图。
图5 改进的DL方法流程图
采用上述DL算法,系统状态增量Δx可以被定义为:

δχ=[δxn,δxn+1,…,δxn+N,δλm,δλm+1,…,δλm+M,δOo,δOo+1,…,δOo+0]T

δxi= δ p , δ θ , δ v , δ b a b i , δ b g b i T,i∈[n,n+N]

每次迭代,通过解方程(1)求得增量Δx: H p + H b + H f + H lδχ= b p + b b + b f + b l,其中,
Hp、Hb、Hf、Hl分别是先验残差、IMU测量残差、点特征残差和线特征残差的信息矩阵。 H=JTΣ-1J,b=-JTΣ-1r,其中J是关于增量Δx的残差向量r的雅克比矩阵,Σ是测量协方差矩阵。IMU、点特征的残差和雅克比参考文献[8],本文只给出线特征的残差与雅克比。

3.3 线特征的残差与雅克比

线特征的重新投影误差定义为空间直线与线特征对应端点的距离,对于针孔相机模型,将3D空间线L=[n,d]T投影到相机图像平面为
l= l 1 l 2 l 3=Knc= f y 0 0 0 f x 0 - f y c x - f x c y f x f ync
其中,K是线特征投影矩阵。将线特征投影到归一化平面时,K是一个单位矩阵,由上述投影公式,3D线投影的坐标仅与法向量有关。将世界坐标系下的3D线段投影到图像屏幕获得投影线 l l c i,线特征重投影误差定义为:rl z L ' l c i , χ= d s l c i , l l c i d e l c i , l l c i,用d(s,l)表示端点s到投影线l的距离函数:d(s,l)= s T l l 1 2 + l 2 2,可获得相应的雅克比矩阵
Jl= ∂ r l ∂ l c i ∂ l c i ∂ L i c ∂ L i c ∂ δ x i ∂ L c i ∂ L w ∂ L w ∂ δ O
∂ r l ∂ l c i= - l 1 s l c i T l l 1 2 + l 2 2 3 2 + u s l 1 2 + l 2 2 1 2 - l 2 s l c i T l l 1 2 + l 2 2 3 2 + v s l 1 2 + l 2 2 1 2 1 l 1 2 + l 2 2 1 2 - l 1 e l c i T l l 1 2 + l 2 2 3 2 + u e l 1 2 + l 2 2 1 2 - l 2 e l c i T l l 1 2 + l 2 2 3 2 + v e l 1 2 + l 2 2 1 2 1 l 1 2 + l 2 2 1 2
∂ l i c i ∂ L c i= K 0 3 × 6
∂ L c i ∂ δ x i= J b c - 1 R T w b d w × 0 3 × 3 J b c - 1 R T w p n w + d w × p w b × R T w b d w × 0 0 0
∂ L c i ∂ L w ∂ L w ∂ δ O= J w c - 1 0 - w 1 u 3 w 1 u 2 - w 2 u 1 w 2 u 3 0 - w 2 u 1 w 1 u 2 6 × 4(9)

4 实验结果分析

本文使用公共基准数据集EuRoc MAV来评估本文算法,该数据集由无人机挂载 VI-Sensor记录200 Hz的IMU数据和20 Hz的相机数据。EuRoc微型飞行器(MAV)数据集由MAV在两个室内场景中收集,每个数据集提供由VICON运动捕捉系统给出的地面实况轨迹,所有的外在和内在参数也在数据集中提供。
本文算法与两种优秀的单目视觉惯性方法进行了比较一单目模式下的OKVIS[13]和没有闭环的VINS-Mono[14]。OKVIS是一种滑动窗口优化算法,具有单点或立体模式的点特征;VINS-Mono是一个完整的VIO-SLAM系统,采用滑动窗口中紧耦合优化IMU状态和点特征,并执行回环。为了公平比较,本文使用了文献[13-14]提供的默认参数。
1)定位精度分析
本文选择绝对姿态误差(APE)作为评估指标,直接比较估计与地面实况之间的轨迹误差[15]。 开源软件包evo提供了一个易于使用的界面来评估测距和里程计算法的轨迹。我们在本节中使用此工具来评估这些方法。 表1显示了沿着所有轨迹的平移和旋转的均方根误差(RMSE)。所有的实验都在计算机上进行,配备Intel Core i7-8750H CPU,2.20 GHz,8 GB RAM和ROS Kinetic。
表1 RUROC数据集下RMES单位:m
EUROC数据集 VINS-Mono OKVIS-Mono 本文算法
MH_03_medium 0.1724 0.2573 0.1264
MH_04_difficult 0.1837 0.2758 0.1394
MH_05_difficult 0.3035 0.3964 0.1736
V1_01_easy 0.0864 0.1058 0.0891
V1_02_medium 0.0747 0.1421 0.0758
V2_03_difficult 0.2965 0.3251 0.2609
本文算法共同优化了点和线特征,为六个序列提供了最佳性能。表1显示, 除了V1_01_easy,V1_02_medium之外,本文算法在另外四个序列上表现最佳,相对于最佳结果的差异仅在亚毫米级别。表1中的结果表明,将线特征集成到VIO中可以提高运动估计的准确性。为了直观地展示结果,本文算法估计的轨迹的几个热图(左)和绝对轨迹误差(右)如图6所示。热图中颜色从蓝到红,平移误差逐渐增大,绝对轨迹误差图与热图相对应,根据实验发现,误差较大的红色部分为视觉信息丢失区域,其原因为相机旋转角度较大或光照变化大,此时,特征无法跟踪,图像信息无法利用,算法仅依靠IMU数据运行导致定位误差较大。从上向下三个轨迹分别为MH_04_difficult、MH_05_difficult、V2_03_difficult,都具有相机快速旋转、低纹理、光照变换的特性。通过三个轨迹可以得出:当相机快速旋转移动以及在光照变换、低纹理场景中,具有线特征的本文算法比VINS-Mono和OKVIS-Mono的定位误差减少了一半。
图6 MH_04_difficult、MH_05_difficult、V2_03_difficult(从上到下)的轨迹和轨迹误差图
2)求解时间分析
本文还做了两种非线性方法DL与LM求解效率对比分析,如图7、表2所示。图7为Euroc数据集MH_05_difficult下DL与LM的总迭代次数与求解时间对比,可以看出图中DL点的数量少于LM,意味着迭代次数DL少于LM,求解效率更高,同时,DL在整个迭代过程中的平均求解时间相较于LM减短了很多。另外,本文算法只是对数据观测异常值做了简单的处理,因此,图中存在一些求解时间过短的异常值。为了定量分析,表2中,可以看出一次优化的平均迭代成功次数DL相较于LM少7次,失败次数少5次,一次优化总的平均迭代次数少11次,总的迭代次数少298次,与图7相对应。所以DL相比于LM的迭代次数少了接近三分之一,因此在同等条件下,DL比LM效率更高。
图7 MH_05_difficult下DL与LM求解效率对比
表2 MH_05_difficult下DL与LM迭代次数对比单位:次
MH_05_difficult DL LM
一次优化的平均迭代成功次数 17 24
一次优化的平均迭代失败次数 10 15
一次优化总的平均迭代次数 29 39
总体迭代次数 865 1163
为了进一步分析,表3列出了不同方法做一次优化的平均计算时间。OKVIS-Mono计算效率最高,但其精度最低,本文算法是最耗时的方法,但其计算时间的增加主要受线特征检测和匹配的限制。另外,在本文算法的基础上,DL方法的求解时间比LM方法减短了20~30 ms,提高了算法的实时性,这对无人系统有着重大意义。
表3 RUROC数据集下求解时间对比单位:ms
EUROC数据集 OKVIS-
Mono		 VINS-
Mono		 本文算法—
LM		 本文算法—
DL
MH_03_medium 43 71 132 102
MH_04_difficult 37 72 126 93
MH_05_difficult 44 78 135 98
V1_01_easy 43 74 125 97
V1_02_medium 42 75 114 86
V2_03_difficult 40 61 118 87

5 结束语

本文设计了基于点、线特征结合的单目VIO算法,在传统的VIO算法中加入线特征,并采用了改进的DL方法代替主流的LM方法。通过仿真评估,在EuRoc数据集下,与两种较优秀的单目VIO方法相比得出:线特征解决了低纹理场景中相机快速旋转定位精度低的问题;改进的DL方法提高了算法效率,解决了线特征检测和匹配耗时问题。在后续的工作中,计划加入回环检测以及引入三维空间线的结构约束,进一步提高定位精度,同时为无人系统提供地图支持。

参考文献

原文顺序 | 文献年度倒序 | 文中引用次数倒序
[1]
Kong X, Wu W, Zhang L, et al. Tightly-coupled Stereo Visual-inertial Navigation Using Point and Line Features.[J]. Sensors, 2015, 15(6):12816-12833.

DOI

[2]
Zhang G, Lee J H, Lim J, et al. Building a 3-D Line-Based Map Using a Stereo SLAM[J]. IEEE Transactions on Robotics, 2016, 31(6):1-14.

DOI

[3]
Rublee E, Rabaud V, Konolige K, et al. ORB: An Efficient Alternative to SIFT or SURF[C]// International Conference on Computer Vision, 2012.

[4]
Grompone v G R, Jakubowicz J, Morel J M, et al. LSD: a Fast Line Segment Detector with A False Detection Control.[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 2010, 32(4):722-732.

[5]
Bartoli A, Sturm P. The 3D Line Motion Matrix and Alignment of Line Reconstructions[J]. International Journal of Computer Vision, 2004, 57(3):159-178.

DOI

[6]
Furgale P, Rehder J, Siegwart R. Unified Temporal and Spatial Calibration for Multi-sensor Systems[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems, 2013.

[7]
Shen S, Michael N, Kumar V. Tightly-coupled Monocular Visual-inertial Fusion for Autonomous Flight of Rotorcraft MAVs[C]// IEEE International Conference on Robotics & Automation, 2015.

[8]
Yijia H, Ji Z, Yue G, et al. PL-VIO: Tightly-Coupled Monocular Visual-Inertial Odometry Using Point and Line Features[J]. Sensors, 2018, 18(4):1159-1184.

DOI

[9]
Zach C. Robust Bundle Adjustment Revisited[C]// European Conference on Computer Vision. Springer International Publishing, 2014.

[10]
Forster C, Carlone L, Dellaert F, et al. On-Manifold Preintegration for Real-Time Visual--Inertial Odometry[J]. IEEE Transactions on Robotics, 2015, 33(1):1-21.

DOI

[11]
Tra V, Kim J, Khan S A, et al. Bearing Fault Diagnosis under Variable Speed Using Convolutional Neural Networks and the Stochastic Diagonal Levenberg-Marquardt Algorithm.[J]. Sensors, 2017, 17(12):2834.

DOI

[12]
Madsen K, Nielsen H B. Finite Alogorithms for Robust Linear Regression[J]. Bit Numerical Mathematics, 1990, 30(4):682-699.

DOI

[13]
Leutenegger S, Lynen S, Bosse M, et al. Keyframe-based Visual-inertial Odometry Using Nonlinear Optimization[J]. International Journal of Robotics Research, 2014, 34(3):314-334.

DOI

[14]
Tong Q, Li P, Shen S. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[J]. IEEE Transactions on Robotics, 2017, PP(99):1-17.

[15]
Sturm J, Engelhard N, Endres F, et al. A Benchmark for the Evaluation of RGB-D SLAM Systems.[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems, 2012.

Options
文章导航

/

〈 〉