基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法与流程

本发明涉及运动体自主导航技术，特别涉及一种基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法。

背景技术：

本发明涉及运动体自主导航技术，具体涉及容积卡尔曼滤波技术、次优无偏极大后验噪声估计技术、单点RANSAC特征匹配技术和视觉测量技术等。本发明在航天器超近距离相对导航、低空无人飞行器自主导航、移动机器人自主即时定位和无人水下航行器自主导航等领域具有广阔的应用前景。

视觉传感器最接近人类感知效果，具有信息量大，特征丰富，采样周期短，使用方便经济等特点。近年来，随着图像处理技术的进步和空间探索的需求，基于视觉信息完成航天器超近距离相对导航逐渐成为航天领域研究的热点。计算机视觉最主要的特点是图像特征易于提取且数量大，而且图像间的特征关联较其它传感数据更容易。因而，对计算机视觉导航的研究不仅可以为空间交会对接、在轨服务、空间碎片清除和行星车导航等领域提供技术支撑，而且可以广泛推广应用到基于多种平台的自主导航领域。

基于上述，因此本发明提出一种基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法。

技术实现要素：

本发明的目的是提供一种基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法，有效地克服系统的严重非线性以及噪声统计特性时变的情况，证特征提取实时性的同时，也保证了特征点的旋转不变性和尺度不变性。

为了实现以上目的，本发明是通过以下技术方案实现的：

一种基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法，其特征点是，包含如下步骤：

S1、建立自适应容积卡尔曼滤波器时间预测和量测更新模型；

S2、建立视觉系统的状态模型和量测模型；

S3、采用ORB特征算子对目标进行特征提取和描述；

S4、采用最小汉明距准则完成特征初匹配和单点RANSAC完成特征精匹配，并将全状态模型中对应点坐标进行替换更新，同时完成最终的时间预测和量测更新。

所述的步骤S1中，采用次优无偏极大后验估值器和容积卡尔曼滤波器建立时间预测模型、量测更新模型、过程噪声模型和量测噪声模型。

所述的S2中，建立六自由度视觉系统运动全状态模型和量测模型，全状态信息包括视觉系统状态和特征点信息。

所述的步骤S3中，采用ORB特征算子对目标进行特征提取和描述。

所述的S4中，利用二进制最小汉明距方法完成ORB特征点的初始匹配，并采用单点随机抽样完成特征点的匹配，且将全状态模型中对应点坐标进行替换更新，同时完成最终的时间预测和量测更新。

本发明与现有技术相比，具有以下优点：

1、在滤波器设计方面，通过对相机噪声方差阵的分析可知，相机成像噪声是一类与成像点位置相关的噪声，即随着成像点的不同测量噪声方差特性也随之发生相应的改变。因而，采用所提出的自适应CKF算法，可以更有效地克服系统的严重非线性以及噪声统计特性时变的情况。

2、在特征提取算子方面，采用FAST算子提取目标特征点，并采用Brief算子进行描述，构成ORB特征点，在保证特征提取实时性的同时，也保证了特征点的旋转不变性和尺度不变性。

3、在特征配准方面，充分地利用了CKF得到的状态预测值这一先验知识，采用单点RANSAC技术，只需要从观测数据集选取一个数据就可以完成精确配准迭代，并完成滤波器更新。该成果对基于视觉的自主导航技术研究具有重要的指导意义。

附图说明

图1为本发明视觉相机透视投影坐标系示意图

图2为本发明立体视觉成像示意图；

图3为本发明基于自适应容积卡尔曼滤波和单点随机抽样的视觉导航方法的流程图。

具体实施方式

如图1-3所示，本发明提供一种基于自适应容积卡尔曼滤波和单点随机抽样视觉的导航方法，包含以下步骤：

S1、建立自适应容积卡尔曼滤波器时间预测和量测更新模型；

S2、建立视觉系统的状态模型和量测模型；

S3、采用ORB特征算子对目标进行特征提取和描述；

S4、采用最小汉明距准则完成特征初匹配和单点RANSAC完成特征精匹配，并将全状态模型中对应点坐标进行替换更新，同时完成最终的时间预测和量测更新。

所述的步骤S1中，采用次优无偏极大后验估值器和容积卡尔曼滤波器建立时间预测模型、量测更新模型、过程噪声模型和量测噪声模型。考虑具有加性噪声的非线性系统状态方程和量测方程如下：

x_k＝f(x_k-1,u_k-1)+w_k-1 (1)

z_k＝h(x_k,u_k)+v_k (2)

式中：x_k∈Rⁿ是系统在k时刻的状态；u_k∈Rⁿ是系统的输入；z_k∈R^m是系统的量测值；w_k-1和v_k为不相关零均值高斯白噪声，其协方差阵分别为Q_k-1和R_k。在CKF滤波器设计中，假设在第k时刻，已知前k-1时刻的后验密度函数为其中，z_1:k-1为初始时刻到k-1时刻的观测值；为k-1时刻的状态估计值；P_k-1为k-1时刻的状态协方差矩阵。则对第k时刻的状态估计流程如下所示：

(1)时间预测

1)计算容积点

式中：

2)容积点传播

3)估计预测均值和协方差阵

(2)量测更新

1)计算容积点

2)容积点传播

Z_i,k＝h(X_i,k,u_k)i＝1,...,2n (9)

3)计算量测预测值新息方差和协方差矩阵P_xz,k

4)计算量测更新

式中：

(3)过程噪声和量测噪声

当系统存在噪声统计特性不准确或者时变的情况，会导致CKF估计精度降低甚至滤波发散。Sage-Husa的噪声统计估值器是次优无偏极大后验估值器，适合估计统计特性为常值的噪声。本文在CKF的基础上，利用Sage-Husa噪声估计器估计噪声，从而实现自适应CKF算法。Sage-Husa估计器的本质是基于白噪声采样估算噪声统计值，对于均值为零的过程和量测噪声，其方差估计值分别为

由于其中K为滤波器增益，因此式(16)可以写作

由于对式(17)两端取期望，易得

因此是有偏的，由式(18)得Q_k次优无偏估计为

从而得到的递推形式

相应的可得

从而R_k的次优无偏估计为

类似Sage-Husa滤波器估计统计特性为时变噪声的方法，引入渐消记忆指数d_k-1可得

式中：遗忘因子b取值范围[0.95 0.995]。

所述的S2中，建立六自由度视觉运动系统全状态模型和量测模型，全状态信息包括视觉系统状态和特征点信息。

(1)状态模型

一般用视觉系统的光心运动状态等效其运动状态，光心的空间位置变化可以通过平移和旋转得到，平移和旋转可以通过线速度和角速度得到，而且在运动过程中必然存在着一定的运动干扰。本发明采用13个变量描述视觉系统的运动模型，光心在世界坐标系下的位置r^w，视觉坐标系到世界坐标系的姿态四元数q^wc，光心在世界坐标系下的线速度v^w，光心的自旋角速度ω^c。为了简化模型，对线速度V^w和角速度Ω^c采用匀速运动模型，即线加速度和角加速度满足零均值，且相互独立的高斯正态分布。因此，视觉系统的状态模型为：

k和k+1分别表示第k时刻和第k+1时刻。在视觉系统运动过程中，每当检测到新特征y_i时，便将其加入到状态向量中组成全状态空间。

x_v＝(x_k y_1k y_2k ...)^T (26)

(2)量测模型

假设空间点P在世界坐标系下的坐标为世界坐标系到视觉坐标系的旋转矩阵为R^cw，视觉坐标系到世界坐标系的平移向量在世界坐标系中的表示为t，P点在视觉坐标系下的坐标为P点在像面坐标系下的坐标为hⁱ＝(u v)^T。

M为视觉系统的内参数矩阵，其中f为焦距，dx和dy为像素横向尺寸和纵向尺寸，u₀和v₀为像面的主点位置。

所述的S3中，采用ORB特征算子对目标进行特征提取和描述，提高图像特征提取的快速性和准确性，可以保证具有一定的旋转不变性和尺度不变性。ORB特征检测采用FAST特征算子，特征描述采用BRIEF描述子，这两种算法的速度都非常快，非常适用于实时测量系统。FAST特征检测的原则为：若某像素点与其周围邻域内足够多的像素点处于不同的区域，在该像素点可能为角点。对于灰度图而言，若该点的灰度值比其周围邻域内足够多的像素点的灰度值大或者小，则该点可能为角点。本发明中选择以任意像素点为圆心的圆形区域作为该点的邻域。选取某一待判断点p，在以p为中心，以3个像素为半径的离散化Bresenham圆环上，将p的灰度值I(u,v)和它邻域内16个像素点的灰度值进行比较，若圆环上存在9个连续的像素点的灰度值大于p点的灰度值加th或者小于p点的灰度值减th，则p点位角点。

为了给检测到的角点价格方向信息，本发明采用了一个简单有效的角点定向方——灰度质心法，灰度质心法假设角点的灰度和质心之间存在一个偏移，这个向量可以用于表示一个方向。本发明定义的邻域距为：

质心为：

特征点与质心的夹角定义为FAST特征点的方向：

θ＝arctan(m₀₁,m₁₀) (33)

为了提高方法的旋转不变性，需要确保u、v在半径为3的圆形区域内，即u,v∈[-3,+3]。

BRIEF描述子定义S*S大小的图像邻域准则τ为：

I(x)为图像块在x(u,v)处的灰度值，同理I(y)为图像块在y(u,v)处的灰度值。选择n个(x,y)像素位置对，就可以得到BRIEF描述子的二进制比特串：

n可以取值为128、256和512等，选择不同的值会影响速度、存储效率和识别率等方面。为了解决ORB算子的噪声敏感问题，采用高斯核滤波器对图像进行预处理；为了解决尺度不变性问题，采用8尺度、1.2倍因子建立图像金字塔，对每一层进行高斯滤波，并提取ORB特征。

经过以处理，ORB算子在抗光照变化、抗噪声、旋转不变性和尺度不变性方面都具有了较强的鲁棒性。

所述的S4中，采用最小汉明距准则完成特征初匹配和单点RANSAC完成特征精匹配，并将全状态模型中对应点坐标进行替换更新，同时完成最终的时间预测和量测更新。汉明距表示两个相同的数据对应位不同的数量，我们以d_Hamming(a,b)表示数据a和b之间的汉明距，对这两个数据进行异或运算，并统计结果为1的数量，那么这个数就是汉明距。记录汉明距满足小于一定阈值的点对，这些点对即为初始匹配点对。

利用了CKF得到的状态预测值这一先验知识，从初始匹配点对中随机抽取一点，进行图像点的匹配。RANSAC算法需要关注的参数主要有：构建模型需要的最少数据数量m，算法迭代次数ik，模型验证条件con，判断模型合理性的数据点数量num。w表示每次迭代时从数据集中选取一个点别切该点是正确数据点的概率，每次选取的m个点均正确的概率为w^m，1-w^m表示m个数据中至少有一点是异常数据的概率。因此，ik次迭代过程中从来没有选取到m个数据均为正确数据的概率为1-pr＝(1-w^m)^ik。那么迭代次数为：

通常情况下，pr为事先给定的常数。本发明的验证条件con为预测的特征点与选取的特征点的欧氏距离。当有足够的点满足验证条件时，迭代结束，并记录最终匹配的特征点。在记录下最终匹配的特征点后，将全状态模型中对应点坐标进行替换更新，同时完成最终的时间预测和量测更新。

综上所述，本发明所提供的基于自适应容积卡尔曼滤波的单点随机抽样视觉导航方法，与现有技术相比，具有以下优点和有益效果：在滤波器设计方面，通过对相机噪声方差阵的分析可知，相机成像噪声是一类与成像点位置相关的噪声，即随着成像点的不同测量噪声方差特性也随之发生相应的改变。因而，采用所提出的自适应CKF算法，可以更有效地克服系统的严重非线性以及噪声统计特性时变的情况。在特征提取算子方面，采用FAST算子提取目标特征点，并采用Brief算子进行描述，构成ORB特征点，在保证特征提取实时性的同时，也保证了特征点的旋转不变性和尺度不变性。在特征配准方面，充分地利用了CKF得到的状态预测值这一先验知识，采用单点RANSAC技术，只需要从观测数据集选取一个数据就可以完成精确配准迭代，并完成滤波器更新。该成果对基于视觉的自主导航技术研究具有重要的指导意义。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。