CN112465877A - 一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法 - Google Patents

Abstract

本发明公开了一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：基于经过标定的双目视觉，注册真实标志物，识别与评估物固定在一起的标志物，建立标志物坐标系，求解双目视觉坐标系与标志物坐标系的位姿变换矩阵，进行标志物位姿检测；评估标志物在各自由度的运动状态；根据不同运动状态，采用不同的卡尔曼滤波模型，融合标志物的状态值与卡尔曼滤波的预测值；求解最优估计值作为标志物当前状态值，并预测下一时刻的状态。通过判断与评估物固定在一起的标志物的状态，在标志物处于准静止状态或运动状态时，采用不同的卡尔曼滤波模型，滤除掉图像中产生的噪声和干扰，还原真实的数据，提高定位精度。

Description

一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法

技术领域

本发明涉及双目立体视觉定位技术领域，尤其是涉及一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法。

背景技术

双目立体视觉定位技术是一种由多幅图像获取标志物三维信息的技术，区别于传统单目视觉依靠目标运动获取结构信息(二维信息)，双目视觉还可以通过视差效应获取物体的深度信息(三维信息)，可以以最低的成本获得最高的鲁棒性，以满足控制系统或智能系统对视觉系统的要求。双目视觉已经逐渐成为计算机视觉中最热门的研究方向，拥有广阔的应用前景和非常高的研究价值。

当前的双目视觉定位技术框架相对成熟：通过双目相机同时获取两幅图像，分别对两幅图像中的特征点进行识别，然后进行特征点匹配，对匹配的特征点通过视差计算三维坐标，实现标志物的识别，计算标志物相对于相机坐标系的位姿，最后在导航系统中进行显示。但是目前基于可见光的双目视觉定位技术的精度还受限于硬件--摄像机--的性能。

双目视觉追踪系统对相机有各方面的要求：实时跟踪需要做到相机的传输帧率足够高；特征点识别准确要求图像的像素数要大，分辨率高，那么每张图像占用的内存就越大；使用电荷耦合器件(charge coupled device，CCD)作为感光元件的相机仅有一个输出节点统一读出，其信号输出的一致性非常好，但也导致了图像读出速度慢；而以互补金属氧化物半导体(Complementary Metal Oxide Semiconductor，CMOS)为感光元件的相机有多个电荷-电压转换器和行列开关控制，读出速度快很多，CMOS芯片中每个像素都有各自的信号放大器，各自进行电荷-电压的转换，其信号输出的一致性较差，导致图像产生噪声。

在可见光这种复合频段光线条件下，由于CMOS相机信号输出产生的微小噪声，在二维特征点匹配时，会造成二维图像上的特征点坐标测量产生随机误差。而双目视觉测量特征点深度时，采用的是基于视差的三角测量方法，视差的误差是两幅图像在x方向误差的和，导致特征点在相机Z方向-深度方向-上的误差加倍，导致欧拉角-β角-的随机误差变大，产生小尺度范围的抖动，影响定位精度，这里只关注深度方向，是由于深度方向的误差最大，β角的抖动也最大，虽然其他各个自由度也同样存在误差，只不过没有深度方向的大。

因此，如何提高双目视觉的定位精度，是目前亟待解决的问题。

发明内容

本发明的目的是提供一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，通过判断标志物的状态，在标志物处于准静止状态或运动状态时，采用不同的卡尔曼滤波模型，滤除掉图像中产生的噪声和干扰，还原真实的数据，提高定位精度。

第一方面，本发明的上述发明目的通过以下技术方案得以实现：

一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，基于经过标定的双目视觉，注册真实标志物，识别与评估物固定在一起的标志物，建立标志物坐标系，求解双目视觉坐标系与标志物坐标系的位姿变换矩阵，进行标志物位姿检测；评估标志物在各自由度的运动状态；根据不同运动状态，采用不同的卡尔曼滤波模型，融合标志物的状态值与卡尔曼滤波的预测值；求解最优估计值作为标志物当前状态值，并预测下一时刻的状态。

本发明进一步设置为：对双目视觉获得的两幅真实标志物图像进行角点识别，并对识别到的角点进行匹配，获取相互匹配的角点的三维坐标及相关信息，存储真实标志物信息，完成注册；将当前双目视觉系统识别到的标志物信息与存储的真实标志物信息进行对比，识别标志物。

本发明进一步设置为：真实标志物信息包括：真实标志物角点个数、真实标志物角点所构成的真实标志物面积最大三角形、以及真实标志物面积最大三角形中的最小角度及每个真实标志物角点在真实标志物坐标系中的坐标；

当前双目视觉系统识别到的标志物信息包括标志物角点信息，在标志物角点中任选三个形成一个标志物三角形，与真实标志物面积最大三角形的信息进行对比，寻找标志物角点中面积最大的三角形对应的三个标志物角点，在其余的标志物角点中，继续寻找与真实标志物角点距离最近的点，确定标志物与真实标志物是否相同。

本发明进一步设置为：在识别标志物后，以标志物各角点的坐标均值为原点，建立标志物坐标系，采用SVD分解法，求取双目视觉坐标系与标志物坐标系的位姿变换矩阵，得到标志物在双目视觉坐标系下的位姿矩阵。

本发明进一步设置为：分别构建标志物在标志物坐标系下的角点坐标矩阵M、双目视觉识别到的匹配角点坐标矩阵L，以标志物相对于双目视觉的位姿变换矩阵

和角点坐标矩阵M的积作为综合项，求解匹配角点坐标矩阵L与综合项之间的最小差值，获得标志物相对于双目视觉的位姿变换矩阵

本发明进一步设置为：提取每一时刻标志物相对于双目视觉坐标系的旋转矩阵R及平移向量P，并转换为欧拉角(αβγ)表示，获得位姿状态向量

根据每个时刻的位姿向量

评估标志物在各自由度的速度，其中P_x表示x方向位移，P_y表示y方向位移，P_z表示z方向位移。

本发明进一步设置为：根据标志物每一时刻在各自由度上位姿状态，求取标志物在各自由度上的速度向量

对速度向量取模，根据速度向量模的大小，确定标志物是处于准静止状态或运动状态，其中，ω_α表示绕z轴的速度，ω_β表示绕y轴的速度，ω_γ表示绕x轴的速度，υ_x表示沿x轴的速度，υ_y表示沿y轴的速度，υ_z表示沿z轴的速度。

本发明进一步设置为：在标志物处于准静止状态，对欧拉角进行变换，使变换之后的角度变化连续，建立静态卡尔曼滤波模型，包括状态转换矩阵A_j、状态向量

初始状态向量

观测向量

状态转移协方差矩阵Q_j，观测噪声方差矩阵R_j，其中：

状态向量

状态转换矩阵A_j：A_j＝I(h,h)(1)；

状态转移协方差矩阵Q_j初始值：Q_j0＝I(h,h)×c₁(2)；

观测噪声方差矩阵R_j初始值：R_j0＝I(h,h)×c₂(3)；

式中，I表示单位矩阵，h表示矩阵维度，c₁、c₂表示初始值常数，下标j表示静态。

本发明进一步设置为：在标志物是处于运动状态，建立动态卡尔曼滤波模型，包括状态向量

状态转移矩阵A_d，其中，

状态向量

状态转移矩阵A_d如下：

式中，dt表示系统采样时间，下标d表示动态，下标m表示自由度速度与位置总和。

本发明进一步设置为：根据状态转换矩阵由前一状态值预测当前的状态值，同时预测状态协方差矩阵：

根据双目视觉的测量值更新系统状态值以及卡尔曼增益和状态协方差：

式中，

表示当前时刻状态的最优估计值，

表示状态的先验预测值，

表示状态的先验协方差矩阵，A表示状态转移矩阵，Q表示状态转移协方差矩阵，H表示观测矩阵，R表示观测噪声方差矩阵，z_κ表示观测值，K表示卡尔曼增益，B表示控制矩阵，P_κ表示状态协方差矩阵；

状态转移矩阵，在不同状态下，采用不同的表达式。

第二方面，本发明的上述发明目的通过以下技术方案得以实现：

一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端，包括存储器、处理器、以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法。

第三方面，本发明的上述发明目的通过以下技术方案得以实现：

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被所述处理器执行时实现所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法。

第四方面，本发明的上述发明目的通过以下技术方案得以实现：

一种基于运动状态估计的卡尔曼滤波视觉追踪稳定系统，包括相连接的双目视觉系统、计算中心，双目视觉系统用于获取标志物的两个图像，计算中心用于对标志物的两个图像进行注册与识别，进行位姿检测，评估标志物运动状态，基于不同的运动状态，采用不同的卡尔曼滤波模型，融合标志物的状态值与卡尔曼滤波的预测值；求解最优估计值作为标志物当前状态值，并预测下一时刻的系统状态。

与现有技术相比，本申请的有益技术效果为：

1.本申请通过对标志物状态的评估，针对不同的状态采用不同的卡尔曼滤波模型，实现了对评估物视觉的精确追踪；

2.进一步地，本申请通过设置角点面积最大三角形，在识别时能够快速选择出最大面积三角形，同时筛选掉许多不可能的组合，大大提高了图像的处理速度；

3.进一步地，本申请通过在运动状态时考虑各自由度的速度，进一步确保视觉追踪的精度，抑制抖动。

附图说明

图1是本申请的一个具体实施例的稳定方法流程示意图；

图2是本申请的一个具体实施例的相机标定流程示意图；

图3是本申请的一个具体实施例的相机标定用左图像图集；

图4是本申请的一个具体实施例的相机标定用右图像图集；

图5是本申请的一个具体实施例的标志物样本示意图；

图6是本申请的一个具体实施例的角点计算示意图；

图7是本申请的一个具体实施例的姿态变换示意图；

图8是本申请的一个具体实施例的标志物运动状态示意图；

图9是本申请的一个具体实施例的欧拉角变化范围示意图；

图10是本申请的一个具体实施例的评估物与标志物实际图像；

图11是本申请的一个具体实施例的评估物与标志物追踪效果图。

具体实施方式

以下结合附图对本发明作进一步详细说明。

具体实施例一

本申请的一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，如图1所示，包括：标定双目视觉系统，注册真实标志物，识别与评估物固定在一起的标志物，进行标志物位姿检测，评估标志物的运动状态，根据运动状态的不同，采用不同卡尔曼滤波模型，确定位置姿态的最优估计值。

具体地，一、双目相机标定：

双目视觉系统采用双目相机，双目相机标定的参数主要用于矫正图像的畸变、相机的校正，实现左右两幅图像行对齐，并在对齐后的两幅图像基础上，进行特征点匹配与三角测量，如图2所示。

双目相机标定的方法如下：

固定双目相机，采用一个棋盘标定板，改变棋盘标定板在双目相机下的位姿，采集一组棋盘不同姿态时的左右图像，如图3、4所示，其中图3中为左相机图像组，图4中为右相机图像组，采用Matlab中的Stereo Camera Calibrator工具包进行双目相机标定，获取双目相机的五个内部参数以及六个外部参数，将所述参数应用到双目相机上，即完成相机标定。

采用标定过的双目相机，能够同时获取两张行对齐的图像，从而进行特征点匹配和三角测量，将特征点的三维信息与二维坐标进行匹配。

二、真实标志物注册：

在本实施例中，采用具有黑白X角点的标志物作为真实标志物，如图5所示，进行真实标志物注册，对单独真实标志物的各个特征点，即X形角点之间的距离关系记录并存储。

由于X角点是由交替阵列排列的黑白区块的交界点，通过计算每个像素周围像素值之间的关系，可以找到X形角点，响应值越大，越具备角点的特征。

在标志物坐标系中，计算X角点的响应值并进行亚像素角点识别。

如图6所示，选择中间一点像素点，对其周围的二个圆周上的像素点依次编号，然后计算其响应值。

角点响应值计算公式如下：

(1)；

(2)；

(3)；

式中，I表示像素点的灰度值，R₁表示总响应值，R₂表示微分响应值，R₃表示平均响应值，n表示像素点序号。

其加权响应值R为：

R＝R₁-R₂-16R₃

响应值R越大，越具备X形黑白角点的特征。

在相机坐标系中，对左右两幅图像识别到的角点进行匹配，对相互匹配的角点使用三角测量方法获取其三维坐标：

(4)；

其中，设左右二个相机焦距相同，f表示相机的焦距，T_l-r表示两个相机光轴的距离，x与y分别是左右相机中角点的像素坐标，c_x分别表示光轴的x像素坐标值,c_y分别表示光轴的y像素坐标值。

通过以上计算，得到真实标志物角点个数。

在真实标志物所有X角点中任意取三个X角点组成一个三角形，得到多个三角形，求取所有三角形中面积最大的一个三角形为第一三角形，提取第一三角形对应的三个X角点的坐标信息，其三个X角点作为第一角点组；第一三角形中的最小角度、面积信息；并存储真实标志物信息。真实标志物X角点中除第一角点组外的其余X角点作为第二角点组。

三、标志物识别：

将真实标志物与评估物固定在一起，用于对评估物进行评估时进行参照，为了区别不同场景，与评估物一起的真实标志物称为标志物。

标志物与评估物固定在一起，为了方便简称评估组合。

将真实标志物信息与标志物信息进行比对，实现标志物的识别。

双目相机拍摄评估组合的组合图像，基于组合图像，采用前述角点计算方法，进行组合角点识别，并任选三个组合角点组成组合三角形，将组合三角形面积与第一三角形面积进行比对，找到组合三角形中面积最大的一个三角形为第二三角形，得到第二三角形对应的三个组合角点坐标，作为标志物的三个角点。

再在其余组合角点中，寻找与第二角点组中的X角点距离最近的点，也作为标志物角点，一个标志物包括3～5个角点。

四、标志物与评估物位姿检测：

姿态变换示意图如图7所示。

在找到标志物的角点后，完成了标志物的识别，以各角点的坐标均值为原点，运用主成分分析方法，建立标志物坐标系，再采用SVD分解法，求取双目相机坐标系到标志物坐标系的位姿变换矩阵

基于标志物的N个X角点，构造标志物坐标系下，标志物X角点坐标矩阵M，M＝(m₁，…，m_N)，其中m₁＝(m_x，m_y，m_z)′。

双目相机识别到的标志物角点，与真实标志物角点匹配的角点矩阵L，L＝(l₁，…，l_N)。

理想状态下，标志物X角点坐标矩阵M经过位姿变换矩阵

后，能够得到角点矩阵L，即

实际上，理想状态是无法实现的，因此，通过求解最小化值，进而求解得到位姿变换矩阵

即求解：

(6)。

五、运动状态评估：

运动状态估计是指对Marker在六个自由度上的速率进行评估，确定标志物的运动状态，根据不同的运动状态，选择相应的卡尔曼滤波模型。

提取每一时刻标志物相对于双目相机的旋转矩阵R及平移向量P，然后变换到欧拉角表示，变换公式如下：

α＝arc tan(r₂₁，r₁₁) (7)；

γ＝arc tan(r₃₂，r₃₃) (9)；

式中，r_ij表示旋转矩阵R中的第i行、第j列的元素。

由于此处的β角不会到达90°，不存在多解的问题，因此，能够直接得到一个关于位姿的状态向量

式中，P_x P_y P_z分别表示x、y、z三个方向的位移。

采用位姿状态向量

代替位姿变换矩阵

便于运动评估及卡尔曼滤波。

记录第κ个时刻的系统时间为t_κ，状态向量为

计算第κ个时刻标志物在各自由度的速度：

(11)；

式中，ω_α表示绕z轴的速度，ω_β表示绕y轴的速度，ω_γ表示绕x轴的速度，υ_x表示沿x轴的速度，υ_y表示沿y轴的速度，υ_z表示沿z轴的速度。

求出各自由度的速度后，对速度向量取模，根据速度向量模的大小，判断标志物的运动状态。

具体地，当速度向量模的平方小于设定值时，认为标志物处于准静止状态，大于等于设定值时，则认为标志物处理运动状态。

在本具体实施例中，设定值为0.02。

即：满足下式的条件时，认为标志物处于准静止状态。

标志物运动状态示意图如图8所示。

六、卡尔曼滤波模型：

卡尔曼滤波本质上是数据融合的过程，本申请将双目视觉识别到的标志物(Marker)姿态的各个状态值与卡尔曼滤波的预测值进行融合，得到最优估计值，作为当前状态值，并用于预测下一时刻的系统状态。

卡尔曼滤波解决的是隐马尔科夫模型中的预测问题，根据已知模型和观测序列，求解预测概率最大的真实状态序列。

6.1、卡尔曼滤波前进行角度范围变换，本申请采用的是ZYX欧拉角，按照式(7)、(8)、(9)，如图9所示，欧拉角的范围如下：

α∈(-pi,pi) (13)；

β∈(-pi/4,pi/4) (14)；

γ∈(-pi,-3pi/4)∪(3pi/4,pi) (15)。

6.2、卡尔曼滤波在应用时，包括预测和更新两个步骤：

预测步骤：

根据状态转移矩阵由前一状态值预测当前的状态值，同时预测状态协方差矩阵。

更新步骤：

根据双目视觉的测量值更新系统状态值以及卡尔曼增益和状态协方差：

式中，

表示当前时刻状态的最优估计值，

表示状态的先验预测值，

表示状态的先验协方差矩阵，A表示状态转移矩阵，Q表示状态转移协方差矩阵，H表示观测矩阵，R表示观测噪声方差矩阵，z_κ表示观测值，K表示卡尔曼增益，B表示控制矩阵，P_κ表示状态协方差矩阵。

6.3、准静止状态卡尔曼滤波：

在准静止状态的卡尔曼滤波模型中，转移矩阵A为单位阵，即认为该标志物相对于相机的欧拉角是不变的，如果有变化，则利用测量值z会对其进行修正。这样，最优预测值也会在其上一时刻状态值的附近。然而在-pi与pi的分界点上，虽然对于欧拉角来说意义是相同的，都是围绕Z轴旋转了180度，但是对于卡尔曼滤波来说却有一个高达2pi的跃变。因此根据图9所显示的欧拉角变化范围，分别对α角和γ角减去2pi，将其变换到下式表达的范围中：

α∈(-2pi,0) (21)；

β∈(-pi/4,pi/4) (22)；

γ∈(-5pi/4,-3pi/4) (23)。

经过上述变换，欧拉角的变化将会是连续的，不会出现跳变，如图10、11所示，导航显示的运动状态与实际探针的运动状态相同。

准静止状态下的卡尔曼滤波模型是不考虑标志物的速度变量的，当前标志物的状态满足式(12)，状态变量为式(10)，即：

准静止状态下的卡尔曼滤波模型认为当前时刻的姿态与前一时刻的姿态保持一致，因而，选择的状态转移矩阵A_j为：

A_j＝I(6,6) (24)；

式中，6表示6个维度，包括三个平移与三个旋转；I表示单位矩阵。

通常情况下，维度采用6个。

还需要给定：初始状态向量

观测向量

状态转移协方差矩阵Q_j，观测噪声方差矩阵R_j。

状态转移协方差矩阵Q_j的初始化为：Q_j0＝I(6,6)×c₁(25)；观测噪声方差矩阵R_j初始值：R_j0＝I(6,6)×c₂(26)；

式中，c₁、c₂表示初始值常数，下标j表示静态。

在本实施例中，c₁＝0.0001，c₂＝0.1。

将上述参数代入式(16)--(20)，进行准静止状态下的卡尔曼滤波。

在模型确定后，对Q_j、R_j参数分别进行调整。其中，Q_j主要代表了建模的误差，也就是说状态转换矩阵不完全准确，为非完全静止状态。Q_j越小就越信任模型的预测值，越大则越不信任预测值。

R_j表示了观测噪声，R_j的选取与双目相机的特性有关，同样地，R_j越小代表测量值越准确，这个模型也就越信任之后的测量值，相反R_j的值越大则越不信任测量值。

实验证明，在准静止状态下静态卡尔曼滤波模型展现了较好的效果，能够很明显的抑制抖动。

6.4、准静止状态卡尔曼滤波：

相对于准静止状态下的一阶滤波模型，运动状态下的卡尔曼滤波采用二阶的滤波模型。

二阶的滤波模型考虑了标志物在各个自由度的速度状态，因此，状态变量

由下式表示：

状态转移矩阵A_d如下：

式中，dt表示系统采样时间，下标d表示动态，下标12表示6自由度的速度与6个自由度的位置总和。

将上述参数代入式(16)--(20)，进行运动状态下的卡尔曼滤波。

如图10所示的评估物与标志物，采用准静止状态的卡尔曼滤波，导航效果图如11所示，导航图中显示的运动状态与实际探针的运动状态相同。

具体实施例二

本申请的一种基于运动状态估计的卡尔曼滤波视觉追踪稳定系统，包括相连接的双目视觉系统、计算中心，双目视觉系统用于获取标志物的两个图像，计算中心采用具体实施例一所述的方法，实现对卡尔曼滤波视觉追踪。

具体实施例三

本发明一实施例提供的一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备，该实施例的终端设备包括：处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，例如印章真伪判别计算机程序，所述处理器执行所述计算机程序时实现实施例1中的方法。

示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备中的执行过程。

所述一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，所述上述示例仅仅是基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备的示例，并不构成对基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备的限定，可以包括比图示更多或更少的部件，或组合某些部件，或不同的部件，例如所述基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备还可以包括输入输出设备、网络接入设备、总线等。

所述处理器可以是中央处理单元(Central Processing Unit，CPU),还可以是其他通用处理器、数据信号处理器(Digital Signal Processor，DSP)、专用集成电路

(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备的控制中心，利用各种接口和线路连接整个一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

具体实施例四

所述一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端设备集成的模块/单元，如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

本具体实施方式的实施例均为本发明的较佳实施例，并非依此限制本发明的保护范围，故：凡依本发明的结构、形状、原理所做的等效变化，均应涵盖于本发明的保护范围之内。

Claims (13)

1.一种基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：基于经过标定的双目视觉，注册真实标志物，识别与评估物固定在一起的标志物，建立标志物坐标系，求解双目视觉坐标系与标志物坐标系的变换矩阵，进行标志物位姿检测；评估标志物在各自由度的运动状态；根据不同运动状态，采用不同的卡尔曼滤波模型，融合标志物的状态值与卡尔曼滤波的预测值；求解最优估计值作为标志物当前状态值，并预测下一时刻的状态。

2.根据权利要求1所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：对双目视觉获得的两幅真实标志物图像进行角点识别，并对识别到的角点进行匹配，获取相互匹配的角点的三维坐标及相关信息，存储真实标志物信息，完成注册；将当前双目视觉系统识别到的标志物信息与存储的真实标志物信息进行对比，识别标志物。

3.根据权利要求2所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：

真实标志物信息包括：真实标志物角点个数、真实标志物角点所构成的真实标志物面积最大三角形、以及真实标志物面积最大三角形中的最小角度及每个真实标志物角点在真实标志物坐标系中的坐标；

当前双目视觉系统识别到的标志物信息包括标志物角点信息，在标志物角点中任选三个形成一个标志物三角形，与真实标志物面积最大三角形的信息进行对比，寻找标志物角点中面积最大的三角形对应的三个标志物角点，在其余的标志物角点中，继续寻找与真实标志物角点距离最近的点，确定标志物与真实标志物是否相同。

4.根据权利要求1所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：在识别标志物后，以标志物各角点的坐标均值为原点，建立标志物坐标系，采用SVD分解法，求取双目视觉坐标系与标志物坐标系的齐次变换矩阵，得到标志物在双目视觉坐标系下的位姿矩阵。

5.根据权利要求4所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：分别构建标志物在标志物坐标系下的角点坐标矩阵M、双目视觉识别到的匹配角点坐标矩阵L，以标志物相对于双目视觉的齐次变换矩阵T和角点坐标矩阵M的积作为综合项，求解匹配角点坐标矩阵L与综合项之间的最小差值，获得标志物相对于双目视觉的齐次变换矩阵T。

6.根据权利要求1所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：提取每一时刻标志物相对于双目视觉坐标系的旋转矩阵R及平移向量P，并转换为欧拉角(α β γ)表示，获得位姿状态向量

根据每个时刻的位姿向量

评估标志物在各自由度的速度，其中P_x表示x方向位移，P_y表示y方向位移，P_z表示z方向位移。

7.根据权利要求1所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：根据标志物每一时刻在各自由度上位姿状态，求取标志物在各自由度上的速度向量

对速度向量取模，根据速度向量模的大小，确定标志物是处于准静止状态或运动状态，其中，ω_α表示绕z轴的速度，ω_β表示绕y轴的速度，ω_γ表示绕x轴的速度，υ_x表示沿x轴的速度，υ_y表示沿y轴的速度，υ_z表示沿z轴的速度。

8.根据权利要求7所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：在标志物处于准静止状态，对欧拉角进行变换，使变换之后的角度变化连续，建立静态卡尔曼滤波模型，包括状态转换矩阵A_j、状态向量

初始状态向量

观测向量

状态转移协方差矩阵Q_j，观测噪声方差矩阵R_j，其中：

状态向量

状态转换矩阵A_j：A_j＝I(h，h) (1)；

状态转移协方差矩阵Q_j初始值：Q_j＝I(h，h)×c₁ (2)；

观测噪声方差矩阵R_j初始值：R_j＝I(h，h)×c₂ (3)；

式中，I表示单位矩阵，h表示矩阵维度，c₁、c₂表示初始值常数，下标j表示静态。

9.根据权利要求7所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：在标志物是处于运动状态，建立动态卡尔曼滤波模型，包括状态向量

状态转移矩阵A_d，其中，状态向量

状态转移矩阵A_d如下：

式中，dt表示系统采样时间，下标d表示动态，下标m表示自由度速度与位置总和。

10.根据权利要求8或9所述基于运动状态估计的卡尔曼滤波视觉追踪稳定方法，其特征在于：根据状态转换矩阵由前一状态值预测当前的状态值，同时预测状态协方差矩阵：

根据双目视觉的测量值更新系统状态值以及卡尔曼增益和状态协方差：

式中，

表示当前时刻状态的最优估计值，

表示状态的先验预测值，

表示状态的先验协方差矩阵，A表示状态转移矩阵，Q表示状态转移协方差矩阵，H表示观测矩阵，R表示观测噪声方差矩阵，z_κ表示观测值，K表示卡尔曼增益，B表示控制矩阵，P_κ表示状态协方差矩阵；

状态转移矩阵，在不同状态下，采用不同的表达式。

11.一种基于运动状态估计的卡尔曼滤波视觉追踪稳定终端，其特征在于：包括存储器、处理器、以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1-10任一所述方法。

12.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-10任一所述方法。

13.一种基于运动状态估计的卡尔曼滤波视觉追踪稳定系统，其特征在于：包括相连接的双目视觉系统、计算中心，双目视觉系统用于获取标志物的两个图像，计算中心用于对标志物的两个图像进行注册与识别，进行位姿检测，评估标志物运动状态，基于不同的运动状态，采用不同的卡尔曼滤波模型，融合标志物的状态值与卡尔曼滤波的预测值；求解最优估计值作为标志物当前状态值，并预测下一时刻的系统状态。

Images