一种点线融合的视觉惯性导航方法

1.本发明涉及视觉惯性导航技术领域，尤其涉及一种融合点线特征的视觉惯性导航方法。


背景技术：

2.近年来，研究人员提出了不同类型的视觉惯性导航方案，每种方法都有其特点，并且适用于不同的应用场景。在处理缺失的纹理或无纹理场景时，或当特征点由于运动模糊和其他因素而暂时消失时，传统的基于点特征的方法很容易失败，且缺乏稳定性。导航系统缺乏稳定性的主要原因是环境中缺乏可靠的特征点。但是，如果在处理点的特征时考虑了线和平面等高级特征，则可以提高视觉惯性导航系统的稳定性。
3.线特征在户外环境中非常丰富，与点特征相比，线特征对光照的变化不敏感。线特征可以提供关于图像的几何内容等更重要的信息，并且在结构化环境中比点特征更好地表示环境。将点特征和线特征相结合，可以更好地表达环境，提供丰富的视觉信息，使视觉惯性导航系统更加健壮。然而，线特征的应用存在许多困难。首先，线段的提取和匹配存在一些问题，如端点位置不准确，没有明确的强几何约束(如短基线匹配、线端点极约束等)，在部分场景中缺乏丰富的纹理等。第二个问题是空间线段的表达式和参数化。现有的空间直线表达式一般采用与端点相关的表示式。但是，该方法并不能保证在运行过程中能够稳定地提取线段的端点和追踪。此外，当直线在后端进行优化时，经常存在过参数化问题，从而导致优化解的不稳定性。此外，当根据线段的对应关系计算姿态时，由于可能存在部分遮挡，线段的可靠性会进一步受到影响。这些问题使得线段在视觉惯性导航中的应用充满了挑战。


技术实现要素：

4.本发明针对现有技术的不足，提出了一种融合点线特征的视觉惯性导航方法，提高导航的精度。
5.本发明采用下述技术方案实现：
6.融合gps的视觉惯性导航方法，该方法采用下述步骤实现：
7.步骤一，双目相机采集前方的灰度图像，imu传感器采集加速度和角速度，主控进行数据的对齐；
8.步骤二，几何表示点线特征，为视觉惯性导航系统的特征追踪模块引入线特征；
9.步骤三，提取与合并线特征，剔除无用的线特征，减小线的误匹配率；
10.步骤四，求出点线得重投影误差，并进行点线误差融合；
11.步骤五，求出点线重投影误差的雅可比矩阵，为后续的非线性优化做准备。
12.本发明的有效效果：一、本发明对采集的数据进行时间校准，提高位姿预估的精度；对imu数据进行预积分，减小了积分时所消耗的计算量。二、本发明将线特征加入视觉惯性导航系统，并进行点线特征融合，提高了导航系统的精度和稳定性。三、本发明提取与合
并线特征，剔除无用线特征，减小了运行的计算量并提高了线匹配的准确率。
13.本发明的效果通过如下实验得到验证：
14.本提出的方法与vins-fusion进行比较，vins-fusion具有闭环、重装载和高精度地图复用等功能。本发明已经在公共kitti数据集和现实世界中评估了所提出的方法。为了使估计的轨迹与地面实况对齐，本发明使用了horn’s的方法。本发明通过绝对位姿误差和绝对轨迹误差来评估导航的准确性。绝对位姿误差和绝对轨迹误差由evo工具计算。本发明采用绝对轨迹均方根误差进行定量评估。绝对轨迹误差比较估计和地面实况轨迹的平移分量之间的绝对距离。在时间步i处的绝对轨迹误差计算公式如下：
[0015][0016]
其中，t表示对齐两个轨迹的变换；g表示真实地面；e表示预估的轨迹。
[0017]
对于n个姿势的序列，绝对轨迹误差的绝对轨迹均方根误差的计算公式如下：
[0018][0019]
考虑到系统的不确定性，所有值都是通过五次实验获得的，并显示了估计轨迹准确性的中值结果。本发明使用kitti数据集来评估所提出的方法，该数据集适用于评估视觉惯性slam在自动驾驶车辆中的性能。在这个数据集中，为序列00-10提供了真实地面。为了提供准确的地面实况，对激光雷达、摄像头和gps进行校准和同步。kitti数据集是从现实世界中的各种场景数据中收集的，例如农村、高速公路和城市。
[0020]
在现实世界的实验中，本发明利用自制传感器套件来演示所提出的方法。本发明在户外大规模环境中驾驶汽车，并使用四种不同的组合进行状态估计。为了对该实验进行误差评估，本发明通过rtk-gps传感器设置了控制点，控制点作为真实地面点。为了获得绝对误差，本发明将估计的轨迹与控制点对齐。
[0021]
在kitti数据集和真实世界上的实验表明，本发明提出的方法与vins-fusion相比，将定位精度提高了约27％。对比实验结果表明，本方法在纹理特征少和人造环境中提高了导航系统的准确性和鲁棒性。
附图说明
[0022]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0023]
图1是本发明中步骤一到步骤五的流程示意图。
[0024]
图2是本发明中的相机模型框架示意图。
[0025]
图3是本发明中点线特征重投影误差示意图。
[0026]
图4是实验中本发明在现实世界中的实验设备。
[0027]
图5是本发明和传统方法绝对轨迹误差的绝对轨迹均方根误差对比图。
具体实施方式
[0028]
下面结合具体实施例对本发明作详细的说明。
[0029]
融合点线特征的视觉惯性导航方法，该方法采用下述步骤实现，具体见图1的流程：
[0030]
步骤一，双目相机采集前方的灰度图像，imu传感器采集加速度和角速度，主控进行数据的对齐。
[0031]
步骤二，几何表示点线特征，为视觉惯性导航系统的特征追踪模块引入线特征。
[0032]
步骤三，提取与合并线特征，剔除无用的线特征，减小线的误匹配率。
[0033]
步骤四，求出点线得重投影误差，并进行点线误差融合。
[0034]
步骤五，求出点线重投影误差的雅可比矩阵，为后续的非线性优化做准备。
[0035]
所述步骤一中，具体步骤包括：
[0036]
(1)双目相机以10hz的频率采集左右目的灰度图像；
[0037]
(2)imu以100hz的频率采集车体的三轴加速度和角速度；
[0038]
(3)主控进行数据的对齐；由于触发延迟、传输延迟和不同步的时钟，生成的时间戳不等于实际采样时间，导致摄像机与imu之间的时间失调。对齐数据的计算公式为：
[0039]
t
imu
＝t
cam
td；
[0040]
其中，t
imu
和t
cam
分别表示imu和相机的时间戳，td表示一个未知常数的时间偏移量。
[0041]
所述步骤二中，具体步骤包括：
[0042]
(1)几何表示点特征。设o
w-x
wywzw
为世界坐标系，o
c-xcyczc为相机坐标系，o-xyz为图像坐标系，uv为像素坐标系。(u,v)为图像坐标，f
x
、fy、c
x
、cy为相机内参，c
x
、cy是像素坐标相对于图像坐标的偏移量。
[0043]
空间中一点pw＝[xw,yw,zw]
t
转换到相机坐标系记为：
[0044][0045]
其中，t
cw
是世界坐标系w到相机坐标系c的变换关系，r
cw
为旋转矩阵，t
cw
为平移向量。
[0046]
由图2的相机投影模型，得到像素坐标为：
[0047][0048]
其中，kc为内参矩阵。由于单目相机具有尺度不确定性，定义尺度变换因子为s＝1/zc。
[0049]
(2)几何表示线特征。设空间直线l两个端点的齐次坐标为p1＝(x1,y1,z1,w1)
t
和p2＝(x2,y2,z2,w2)
t
，非齐次坐标分别为和则空间中直线的普吕克坐标为：
[0050][0051]
其中，l是一个6
×
1向量，n是直线的矩向量，v是直线的方向向量，w1和w2表示其次因子。
[0052]
将空间中直线lw转换到相机坐标系，即lc，lc用普吕克坐标表示为：
[0053][0054]
其中，h
cw
为直线变换矩阵，为直线旋转矩阵，为直线平移向量，表示平移向量的反对称矩阵。
[0055]
将lc投影到图像平面得到lc，lc的普吕克坐标为：
[0056][0057]
其中，k
l
表示直线的投影矩阵。
[0058]
所述步骤三中，具体步骤包括：
[0059]
(1)提取线特征。本发明采用edline算法来检测线段，而不是采用广泛使用的lsd算法。对于大多数采用lsd提取线特征的视觉slam算法，需要采用lbd来跟踪两个连续图像帧之间的线。然而lbd获取线的描述子进行匹配的过程是耗时的。本发明选用edline来提取线特征不需要参数调整，它的运行速度比lsd快11倍并且不影响提取精度。然而，edline提取的线段存在着分段、短小、无用等问题，这将导致线段匹配和跟踪的失败。为了解决上述问题，本发明提出了一种新的线合并算法来进行特征跟踪，即改进的edline。
[0060]
(2)合并线。本发明设计了两个标准来衡量每一条线对的相似性，并确定匹配的标准。本发明的方法在以下几个方面得到了改进。本发明先合并提取的线，然后再匹配它们。考虑到可能的分段线段，合并它们有助于提高算法对错误检测和噪声的鲁棒性。
[0061]
第一步：得到两条线，l1和l2，l1和l2为检测出的2条相近线段。线段表示为l(s,e)，s表示线段起点，e表示线段终点，l1和l2为检测出的2条相近线段，l3为合并后的线段。根据线段长度进行筛选，忽略长度小于阈值td的线段。l1和l2与x轴的夹角为θ1与θ2，两条线之间的夹角为θ
12
＝abs(θ
1-θ2)。当θ
12
小于设定的角度t
θ
时可以认为是同一条直线。经过实验，本发明发现把t
θ
设为10
°
的效果较好。
[0062]
第二步：本发明用下面两个标准来判断是否合并l1和l2。
[0063]
标准1:端点间的欧氏距离。将两条线的端点间的欧氏距离定义为d
min
＝min{d1,d2,d3,d4}，式中，d1,d2,d3,d4是l1的端点s1、e1和l2的端点s2、e2的之间欧氏距离。如果d
min
小于设定的阈值te时，则将这两条线合并；本发明将te设为10个像素。
[0064]
标准2：端点和线之间的关系。本发明根据点与线之间的关系设计了标准2，以处理其它情况。如果l1和l2可以看作是同一行，则：
[0065]
b1＝t1∈l2&&d
11
＜t
p
,b2＝t2∈l2&&d
12
＜t
p
[0066]
b3＝t3∈l1&&d
21
＜t
p
,b4＝t4∈l1&&d
22
＜t
p
；
[0067]
b1||b2||b3||b4＝＝true
[0068]
其中代表{s1,e1}与l2之间的欧氏距离，具有垂直点t1和t2。代表{s2,e2}与l1之间的欧氏距离，具有垂直点t3和t4。t
p
是一个被设为5个像素的预定义阈值。t1∈l2意味着t1位于l2上。在实际应用中，如果该点非常靠近该线的位置，则该点及对应的线可以合并。例如，点s1和线l2在松散的情况下形成一个三角形，表述为：
[0069][0070]
其中，ε是一个固定的阈值，(x.,y.)是点s.对应的坐标。
[0071]
所述步骤四中，点线重投影误差示意图详见图3，具体步骤包括：
[0072]
(1)点特征的重投影误差。对于空间中一点p在第k-1帧、第k帧的投影像素位置坐标分别为p
p,k-1
、p
p,k
。通过帧间变换关系t
k,k-1
将相机坐标系下第k-1帧的空间点p
c,k-1
转化到第k帧下p
c,k
。点特征在第k帧的观测像素位置为：
[0073]
p
′
p,k
＝skp
c,k
＝skct
k,k-1
p
c,k-1
＝skt
k,k-1
π-1
(p
p,k-1
)；
[0074]
其中，π-1
函数表示将空间中的点在参考帧的像素位置p
p,k-1
反投影到相机坐标系下。
[0075]
点特征重投影误差是图像上投影点和实际测量点之间的误差。由于t
k,k-1
不准，观测位置与投影位置不完全重合，点特征重投影误差表示为：
[0076]ep
＝p
p,k-p
′
p,k
。
[0077]
(2)线特征的重投影误差。根据相机投影模型得出空间中的直线l在当前帧中的投影位置为l，通过帧间变换关系得到预测位置坐标为l
′
。线特征重投影误差是线段端点到投影线的距离。代数几何距离定义的线特征重投影误差为：
[0078][0079]
其中，l1、l2为直线方程l
′
的系数，d1、d2分别为线段端点ps、pe到预测线段的距离。
[0080]
(3)融合点线重投影误差。代价函数表示为：
[0081][0082]
其中，w
p
、w
l
分别为点、线特征观测误差权重系数，表示点线误差的观测协方差。ρ
p
、ρ
l
为抑制异常值的huber鲁棒性代价函数，如下式所示：
[0083][0084]
上式中，当误差e大于阈值δ时，目标函数的增长由二次函数降为一次，从而可以抑制误匹配对的影响，使得整个优化求解过程更加稳定。
[0085]
本发明基于图像纹理复杂度和成功匹配的特征数来设置权重系数w
p
、w
l
。设第k帧成功匹配的点特征数目n
p
和线特征数目n
l
之比为rk＝n
p
/n
l
。权重系数w
p
、w
l
的计算公式如下：
[0086][0087]
其中，s为的第k帧提取出的特征点数量而得出的图像纹理复杂度，r是设为0.7的阈值。
[0088]
所述步骤五中，具体步骤包括：
[0089]
(1)点特征重投影误差的雅可比矩阵。点特征重投影误差相对于投影点的导数为：
[0090][0091]
投影点相对于李代数的导数为：
[0092][0093]
根据链式法则，将以上两式相乘得到点特征重投影误差相对于位姿增量的雅可比矩阵：
[0094][0095]
投影点相对于三维坐标的导数为：
[0096][0097]
以上两式相乘，得到点特征重投影误差相对于三维坐标的雅可比矩阵：
[0098][0099]
(2)线特征重投影误差的雅可比矩阵。根据链式法则，线特征重投影误差相对于位姿增量的雅可比矩阵表示为：
[0100][0101]
线特征征重投影误差相对于空间直线正交增量的雅可比矩阵表示为：
[0102][0103]
设线段起始点和终止点的坐标为：
[0104]
ps＝[u1,v1,1]，pe＝[u2,v2,1]。
[0105]
线特征重投影误差相对于投影像素坐标lc的导数表示为：
[0106][0107]
投影像素坐标lc对普吕克坐标lc的雅可比矩阵表示为：
[0108][0109]
普吕克坐标lc相对于位姿的导数表示为：
[0110][0111]
其中，
[0112]
三维直线在相机坐标系下的投影坐标相对于三维坐标的导数为：
[0113][0114]
由直线普吕克坐标得出其正交表示为：
[0115]
(u,w)；
[0116]
其中，
[0117]
空间中的直线相对于正交增量的雅可比矩阵为：
[0118][0119]
本发明在现实世界中采集数据集的装置如图4所示，图4中包含双目深度相机(mynteye-s1030)、惯性测量单元(imu)和gps传感器(gps-u-blox)。在得到数据集后进行算法的精度验证，精度对比结果如图5所示，图5中的传统算法为vins-fusion。
[0120]
以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。