基于特征观测数和IMU预积分的自适应协方差方法

基于特征观测数和imu预积分的自适应协方差方法
技术领域
1.本发明属于人工智能及计算机视觉测量技术领域，具体涉及一种基于特征观测数和imu预积分的自适应协方差方法。


背景技术：

2.在过去的几十年里，计算机视觉方向得到了广泛的研究，为自主定位系统提供了精确和低成本的解决方案。传统的基于视觉的定位方案都是基于一个理想的静态环境中，这样对于特征点匹配计算出来的运动仅是由相机运动造成的，从而可以得到相机的位姿估计。但是实际场景中可能存在动态目标，这些动态目标将会导致错误的特征匹配，错误地使用动态特征点的信息进行相机运动估计，这对于基于特征匹配的视觉定位系统是致命的。因而，研究适用于动态环境下的视觉定位系统一直是一个热点问题。
3.目前动态环境下slam问题较多地聚焦于如何将动态区域与静态区域区分开，将动态区域剔除，仅使用静态背景中的特征点进行相机位姿的计算。通过剔除动态物体从而减少动态物体对相机位姿求解的影响。然而，与ransac失效的场景一样，当场景中存在大量动态物体中，直接去除动态物体上的特征会导致过度的特征剔除，不仅会严重影响特征分布的几何形状，并且会严重减少参与视觉定位的特征点对，导致有限的视觉测量，影响定位精度。此外，大多数vio系统都没有考虑到用于相对姿态估计的特征的不确定度，在从特征匹配剔除异常值之后，假定所有特征点具有相同的噪声分布，即每对匹配的特征点对位姿的贡献相等，然而，真实环境中，各个点的噪声分布情况会因为位置不同、环境不同而具有不同的类型，因此不同的二维特征应该具有不同的贡献度。图4展示了不同的特征点具有不同不确定性的情况，其中图4a为各向同性且独立同分布的协方差矩阵图、图4b为各向异性且非独立同分布的协方差矩阵图，特征点的不确定度越高，对优化函数的贡献就要越小，反之则应该越大。


技术实现要素：

4.本发明正是针对现有技术中存在的问题，提供一种基于特征观测数和imu预积分的自适应协方差方法，将动态物体上的特征点作为视觉约束信息进行相机位姿求解，以提高视觉定位系统的性能。首先，提取特征点并将图像特征数据与imu数据进行对齐；通过使用图像金字塔光流跟踪，实现特征匹配；再对特征点跟踪次数进行计算，在时间维度上获取当前帧中每一个特征点的观测数，通过计算特征点的观测数，计算当前帧中特征点观测数的均值和标准差，从而确定当前帧中的特征点的静态观测权重；通过imu预积分对逆深度不确定性进行建模，再通过特征点观测数以及imu预积分进行自适应协方差的计算，得到自适应的协方差矩阵，最后将特征点的协方差矩阵与位姿求解的误差函数进行融合构建不确定性加权的重投影误差，实现基于特征观测数和imu预积分的自适应协方差方法，有效缓解了动态目标的影响，并且避免过度剔除导致特征分布扭曲以及过少的视觉测量。
5.为了实现上述目的，本发明采取的技术方案是：基于特征观测数和imu预积分的自
适应协方差方法，包括如下步骤：
6.s1：提取特征点并将图像特征数据与imu数据进行对齐，所述对齐策略为：第一个imu数据的时间戳小于上一帧图像结束的时间戳，最后一个imu数据的时间戳是第一个大于当前帧图像结束的时间戳；
7.s2：通过使用图像金字塔光流跟踪，实现特征匹配；
8.s3：对特征点跟踪次数进行计算，在时间维度上获取当前帧中每一个特征点的观测数，通过计算特征点的观测数，计算当前帧中特征点观测数的均值和标准差，从而确定当前帧中的特征点的静态观测权重；
9.s4：通过imu预积分对逆深度不确定性进行建模，所述特征点的逆深度不确定性与该特征点在相机帧下归一化坐标和帧间平移向量的叉乘成反比，与特征匹配不确定性δx成正比，用imu的预积分项表示逆深度不确定性具体如下：
[0010][0011]
其中，xi＝k-1
pi，pi是特征点在图像平面上的齐次坐标，表示xi的斜对称矩阵，为第i帧图像与第j帧图像之间imu预积分得到的平移向量，δx＝f-1
δp为归一化平面上不确定度形成的圆的半径，f为相机的焦距，δp为特征点特征匹配时产生的误差，δd表示第j帧中特征点的不确定性；
[0012]
s5：通过步骤s3获得的特征点观测数以及步骤s4获得的imu预积分进行自适应协方差的计算，得到自适应的协方差矩阵，对于第l个特征点，其静态观测权重为w(p
l
)，其逆深度不确定性为δd
l
，其自适应协方差矩阵q
l
为：
[0013]ql
＝wcw(p
l
)δd
l
i2×2[0014]
其中，wc为特征点原始协方差矩阵，i2×2为单位矩阵；
[0015]
s6：将上述计算出的特征点的协方差矩阵与位姿求解的误差函数进行融合构建不确定性加权的重投影误差，所述不确定性加权重投影误差为：
[0016][0017]
p
l
为第l个特征点在当前帧图像上的二维坐标，n为特征点总个数，将特征点的协方差矩阵进行分解可以得到q-1
＝u∑-1ut
，其中，
[0018][0019][0020]
与现有技术相比，本发明在对动态物体的处理上做了部分改进，不再拘泥于剔除动态物体，提出了一种基于特征观测数和imu预积分的自适应协方差方法，将动态物体上的特征点作为视觉约束信息进行相机位姿求解，以提高视觉定位系统的性能。在进行相机位
姿求解时，通过在时间维度上对特征点的观测数进行统计建模得到每个特征点的特征观测权重，并且通过imu预积分得到特征点逆深度的不确定性，联合上述二项因子对协方差进行改进，根据特征观测数以及imu预积分生成的平移与旋转约束来建模每一个特征点的不确定性，并且将这种不确定性传播为相对姿态不确定性，该方法有效缓解了动态目标的影响，并且避免过度剔除导致特征分布扭曲以及过少的视觉测量，准确度更高。
附图说明
[0021]
图1是本发明基于特征观测数和imu预积分的自适应协方差方法的流程图；
[0022]
图2是对极几何约束原理图；
[0023]
图3是特征匹配误差图；
[0024]
图4是不同分布的协方差矩阵图，其中
[0025]
图4a为各向同性且独立同分布的协方差矩阵图；
[0026]
图4b为各向异性且非独立同分布的协方差矩阵图；
[0027]
图5是本发明实验结果对比图，其中
[0028]
图5a为原vins-fusion实验结果图；
[0029]
图5b为改进后的vins-fusion实验结果图。
具体实施方式
[0030]
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
[0031]
实施例1
[0032]
一种基于特征观测数和imu预积分的自适应协方差技术，如图1所示，具体方法如下：
[0033]
步骤s1：提取特征点并将图像特征数据与imu数据进行对齐；
[0034]
本方法中，提取shi-tomasi角点，并且使用掩码策略使提取到的特征点分布均匀，定义如下的对齐策略：第一个imu数据的时间戳小于上一帧图像结束的时间戳，最后一个imu数据的时间戳是第一个大于当前帧图像结束的时间戳；
[0035]
步骤s2：对点云进行分类使用图像金字塔光流跟踪，进而实现特征匹配；
[0036]
使用金字塔光流跟踪实现特征点的匹配，对每一帧图像建立图像金字塔，实现尺度空间里进行光流跟踪，具有空间尺度不变性，并且速度快，不必提取特征点的描述子进行匹配；
[0037]
步骤s3：对特征点的跟踪次数进行计算；
[0038]
如果一个特征点是静态特征点，那么该特征点被判断为静态特征点的次数会非常多，特征跟踪的次数决定特征跟踪质量，特征点在帧间跟踪的次数越多，特征点的质量越好，因此我们在时间维度上统计当前帧中每一个特征点的观测数。假设第k帧图像中的第i个特征点用表示，
[0039]
从第一帧到当前帧，如果特征点pi被第k帧观察到，则pi的观测数；
[0040]
nk(pi)＝n
k-1
(pi)+1
[0041]
其中，nk(pi)是特征点pi在第k帧的观测数，特征点提取之后第一次被光流跟踪到，
此时该特征点已经经历了两帧图像，因此设置初始值为1；
[0042]
如果没有观测到特征点pi，则nk(pi)＝n
k-1
(pi)，在当前帧特征观测数计算完成后，清除该特征点的观测数信息，并令nk(pi)＝0；
[0043]
如果nk(pi)大于观测数阈值tv,则nk(pi)＝tv；
[0044]
计算当前帧的特征点观测数的均值μk和标准差σk，具体如下式：
[0045][0046][0047]
其中，nk为第k帧特征点数量，使用特征点观测数的均值μk和标准差σk，可以计算当前帧的每个特征点的静态观测权重，即
[0048][0049]
其中α为大于零的实数。
[0050]
特征点跟踪的次数越少，其属于动态特征点的概率越大，该特征点的不确定性越高；
[0051]
步骤s4：通过imu预积分对逆深度不确定性进行建模：
[0052]
对于基于特征法的视觉定位而言，相对姿态估计最广泛使用的策略就是使用基础矩阵进行求解，这种方法非常依赖于特征点之间的准确的对应关系，很多的视觉定位系统中，都没有考虑特征点的不确定性，在从特征匹配中删除异常之后，剩下的特征匹配结果对目标函数具有相同的贡献。然后，不同的二维特征点应具有不同的误差分布，这与图像质量、特征点是否具有稳定深度信息有关。特征点的深度估计在数值上是不稳定的，同时，姿态估计的不确定性也会增加，同时由于逆深度信息比深度信息具有更好的数值稳定性，因此本发明采用基于imu预积分的不确定性估计方法估计这种深度不确定性；
[0053]
假设已知空间点p在世界坐标系下的位置信息pw＝[x y z]
t
，假设空间点p在第i帧中投影特征点的位置为pi，在第j帧中投影特征点的位置为pj，根据如图2所示对极几何约束，可以得到下式：
[0054][0055][0056][0057]
其中，zi是第i帧中pw的深度，pi＝[u
i v
i 1]
t
是特征点在图像平面上的齐次坐标，k是相机内参，为世界坐标系下第i帧的旋转矩阵和平移向量。
[0058]
根据推导可得：
[0059]
[0060]
令
[0061]
可以得到：
[0062][0063]
上式两边同时乘以表示xi的斜对称矩阵。
[0064]
得到：
[0065][0066][0067][0068]
通过上式可以看出，特征点逆深度的不确定性与特征匹配的误差有关，与第i帧到第j帧之间的位姿变换有关。假设特征匹配时产生的不确定性呈各向同性且均匀分布，即所有特征点特征匹配时都会产生一个不确定性为δp的误差项，在归一化平面上，不确定度会形成半径为δx＝f-1
δp的圆，其中f为相机的焦距。下式给出了逆深度δd的不确定度；
[0069][0070]
根据外积的性质可以得到：
[0071][0072]
已知||δxj||≤δx，rr
t
＝i；
[0073]
可得特征点逆深度不确定性与该特征点在相机帧下归一化坐标和帧间平移向量的关系，具体为：
[0074][0075]
由上式中可以得出，特征点的逆深度不确定性与该特征点在相机帧下归一化坐标和帧间平移向量的叉乘成反比，与特征匹配不确定性δx成正比，特征匹配的不确定性如图3所示，对于帧间的旋转矩阵和平移向量，本发明使用imu预积分来得到的帧间旋转矩阵和平移向量，特征匹配的不确定性取决于不同的特征提取方法和特征匹配方法，本发明假设此时特征匹配的不确定性为一个恒定的经验值，因此可以通过imu的预积分项表示逆深度不确定性；
[0076]
步骤s5：通过特征点跟踪质量以及imu预积分进行自适应协方差的计算；
[0077]
由上述两个方面对特征的不确定性进行度量，自适应协方差矩阵可推导为：
[0078][0079]
步骤s6：基于自适应协方差的位姿估计方法；
[0080]
将上述计算出的二维特征点不确定性与位姿求解的误差函数进行融合构建不确
定性加权的重投影误差，以体现具有不同不确定性的特征点在求解过程中对解的贡献应该是不同的；
[0081]
用q来表示自适应协方差矩阵，二维特征点的协方差矩阵是一个半正定对称矩阵，将协方差矩阵进行奇异值分解可以得到：
[0082]
q-1
＝u∑-1ut
[0083]
其中，
[0084]
根据上式得到仿射矩阵h2×2：
[0085][0086]
通过仿射矩阵矩阵h2×2将原始数据空间中的二维测量点和重投影点坐标变换到不确定性加权协方差数据空间中的坐标，是特征不确定性对目标函数贡献大小的度量，特征不确定性越小的特征点对目标函数有更大的贡献度，u
t
是一个旋转矩阵，将协方差得到的各异椭圆进行旋转得到正向椭圆，通过结合将椭圆进行缩放得到圆，在原始数据空间中，特征点处的噪声各向异性且非独立同分布，通过仿射矩阵h2×2将数据转换到加权协方差数据空间中，噪声就变成各向同性且独立同分布。根据变换后的数据构造出不确定性加权重投影误差如下式所示：
[0087][0088]
通过变换矩阵h2×2将特征点不确定性分配到测量点和重投影点中，使每个特征点的不确定性对目标函数作出不同的贡献，从而适应于不同特征不确定性下的位姿求解；通过上述步骤完成特征观测数和imu预积分的自适应协方差技术。
[0089]
应用本发明的方法在vins-fusion中进行了测试，在kitti00数据集总轨迹长度为3724.2m下进行了多次测试，并使用gnss定位结果作为真值，评估轨迹精度，实验结果如图5所示，图5a为原vins-fusion实验结果图，图5b为改进后的vins-fusion实验结果图，从左到右依次为三维轨迹对比图、三轴位置对比图、三轴姿态角对比图，结果表明轨迹的均方根误差由4.52m减小为3.46m，大大提高了视觉定位系统的性能，准确度更高。
[0090]
需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。