CN109712172A - 一种初始位姿测量结合目标追踪的位姿测量方法 - Google Patents

Abstract

一种初始位姿测量结合目标追踪的位姿测量方法，属于计算机视觉领域。本发明的目的是针对不规则运动的目标，实现视频序列中运动目标位姿参数的实时输出。技术要点：目标的初始位姿测量，目标的初始位姿测量用于启动后续的目标追踪，实现位姿参数的实时输出；包括参考位姿的测量、特征点的提取与匹配、基于多点匹配的位姿测量；目标追踪，包括边缘特征的搜索与匹配、点特征的追踪与匹配、位姿参数的非线性优化求解。本发明将目标的初始位姿与目标追踪方法相结合，实现视频序列中运动目标位姿参数的实时输出；本发明对环境的变化具有一定的适应能力，而且针对物体被局部遮挡时也能测量出其位姿。

Description

一种初始位姿测量结合目标追踪的位姿测量方法

技术领域

本发明涉及一种基于视觉信息的位姿测量方法，具体涉及目标的初始位姿测量、特征提取与匹配，以及目标追踪，属于计算机视觉领域。

背景技术

运动目标在进行位姿测量时，往往因缺乏参照物、无法得知其先验运动参数等因素，导致无法获得其准确的位姿参数，且实时性难以保证。近年来，随着机器人技术、计算机技术和传感器技术的飞速发展，利用视觉信息对运动目标进行位姿测量，已成为计算机视觉领域的研究热点之一。

文献号为CN104596502A的专利文献公开了一种基于CAD模型与单目视觉的物***姿测量方法，通过模板拍摄摄像机运动假设和迭代计算得到假设运动的模板拍摄摄像机与固定摄像机之间的运动关系，得到假设运动的模板拍摄摄像机和固定摄像机组成的双目***外参数后，对目标进行三维重建，得到目标的三维点云数据，并与包含物体三维结构信息的CAD模型进行配准，得到当前世界坐标系下目标物***姿与CAD模型的对应关系，准确计算出目标物体的位姿。但对于不规则运动的目标，如何以视觉信息作为运动目标的先验信息，将目标的初始位姿与目标追踪方法相结合，实现视频序列中运动目标位姿参数的实时输出没有给出技术措施。

发明内容

本发明的目的是提供一种初始位姿测量结合目标追踪的位姿测量方法，以针对不规则(不规律)运动的目标，实现视频序列中运动目标位姿参数的实时输出。

本发明为解决上述技术问题采取的技术方案是：

一种初始位姿测量结合目标追踪的位姿测量方法，所述方法的实现过程为：

步骤1、目标的初始位姿测量

目标的初始位姿测量用于启动后续的目标追踪，实现位姿参数的实时输出；

(1)参考位姿的测量

为测量运动目标的初始位姿，在含有n帧图像的视频序列的前n-1帧图像中提取任意一帧图像，作为初始位姿测量的参考图像，利用此参考图像测量被测图像中目标的位姿参数；

利用POSIT位姿求解算法对所选参考点非共面的情况进行位姿解算；针对参考点共面的情况，采用适用于共面参考点的Coplanar-POSIT位姿求解算法进行位姿求解；在对运动目标的参考位姿进行求取的过程中，需要通过对输入的三维参考点的数据进行共面判定，根据参考点共面性的判定结果在POSIT算法和Coplanar-POSIT算法之间的切换，进而求得准确的运动目标参考位姿；

(2)特征点的提取与匹配

采用SIFT特征提取算子对参考图像和被测图像中的SIFT特征点进行提取，进而利用所提取到的特征点进行匹配，在剔除背景特征点和误匹配以后通过2D-3D重投影建立模型上三维特征点与被测图像上二维SIFT特征点的对应关系，

具体实现过程为：

首先，需要对参考图像提取SIFT特征点，提取SIFT特征点的过程为对参考图像的学习过程；在对二维图像提取SIFT特征点时，对提取出的SIFT特征点进行甄别，只保留位于图像中目标上的SIFT特征；根据目标模型(指目标实物)和参考位姿，在二维图像上进行投影以剔除背景图像中的SIFT特征点；

然后，将提取到的特征点及其描述子作为学习样本与被测图像中的SIFT特征点和描述子进行匹配，建立二者之间SIFT特征点的一一映射关系；所述匹配的具体过程为：SIFT特征点的匹配采用BruteForce匹配算法(其具有匹配效果稳定、计算效率高等优点)，通过将参考图像中的特征描述子与被测图像中的特征描述子进行匹配，返回最优匹配并设置阈值消除错误匹配，进而建立参考图像与被测图像中SIFT特征点的一一映射关系；

最后，在消除二维图像上的背景干扰点以后，求取保留的SIFT特征点所对应的模型坐标系中点的三维坐标，保留的SIFT特征点在模型坐标系上的三维坐标及其对应的特征描述子将用于后续的基于多点匹配的位姿测量；

(3)基于多点匹配的位姿测量，具体过程为：

定义最小化图像特征的期望状态参数为s*，实际状态参数为s，则在理想的条件下，存在一个唯一的相机位姿以最小化二者之间的误差；

假设被测目标上有大量的3D特征点^oP，^oP为被测目标上的点在模型坐标系中的3D坐标，然后设定一个虚拟相机，其在模型坐标系中的位置和姿态定义为r，通过最小化观测数据s*(图像中SIFT特征点的2D坐标p，此时s*＝p)和3D特征点依据位姿参数r进行的前向投影到图像平面所得的2D坐标的实际状态s之间的误差Δ，可求解出被测目标相对于相机的真实位姿参数；

式中，pr_ε(r,^oP_i)为基于相机内部参数ξ和相机位姿r所得的投影模型；N为SIFT特征点的个数；

在位姿解算的过程中，初始位于r_i位置的虚拟相机遵循虚拟视觉伺服控制算法进行移动，以最小化误差参数Δ，在算法的收敛位置，虚拟相机到达r_d位姿以最小化Δ，r_d即为要求解的位姿参数；

在经典视觉伺服过程中，定义任务函数e：

e＝(s(r)-s^*) (2)

式(2)的导数由式(3)得出：

式中，L为图像雅克比矩阵；

将误差参数e进行指数解耦，可得：

式中，λ为控制衰减率的比例系数；

由此，可得到如下的控制律：

式中，为L的伪逆矩阵，

最后，将速度向量v映射到其对应的瞬时位移上，以更新目标的位姿参数：

式中，exp(v)为齐次矩阵形式的指数映射；k为迭代优化过程中的迭代次数；

通过对特征点的2D-3D匹配关系进行随机采样求解以避免在特征匹配和3D信息求解的过程中出现粗大误差，并且不断地测试排除错误的匹配点对，最后利用所有的正确匹配点对重新求解目标的位姿参数，进而求得目标的初始位姿；

步骤2、目标追踪

(1)边缘特征的搜索与匹配

将边缘特征转化为边缘上间隔一定距离的采样点特征后再进行法向一维搜索，从而确定目标模型边缘点与图像边缘点的匹配；

(2)点特征的追踪与匹配

定义为从第I_k帧图像中提取到的特征点，且第I_k帧图像对应的目标位姿参数r_k已知；根据已知的目标的三维模型(目标模型)精确求解图像中二维特征点的深度值，进而利用位姿参数r_k，将图像中二维特征点逆投影到三维模型上，得到目标3D模型上一系列3D特征点

(3)位姿参数的非线性优化求解

定义目标3D模型投影在图像上的模型投影取得2D点集为{x_i,j}，其对应的二维图像上对应点集为{x′_i,j}，误差函数Δ(r)用于指代图像上点x′_i,j到模型投影点x_i,j的距离，Δ(r)可表示为：

式中，d_⊥(x_i,j,x′_i,j)为图像上特征点x′_i,j到图像平面上模型投影点x_i,j的距离；

采用高斯-牛顿迭代法最小化目标误差函数Δ(r)，并求解最优位姿参数估计从初始位姿估计r₀开始，在每个迭代循环k中，位姿参数r_k做微小位移变换δr，通过式(8)更新位姿估计参数；

式(8)中为参数空间中的求和算子，其最终目标为确定在每个迭代循环中使得Δ(r)最小的r_k值，其数学描述如式(9)所示：

在迭代过程中，通过式(12)求解微小位移变换δr：

δr＝-J⁺e(r) (10)

式中，J⁺代表矩阵J的伪逆矩阵；J为误差向量e(r)的雅可比矩阵，

微小位移变换δr＝(υ,ω)表征位姿参数的螺旋位移，其中υ表征平移位移参数，ω表征旋转位移参数，为利用微小位移变换δr实现位姿参数的更新，利用指数映射将微小位移变换δr转换为其对应的刚体瞬时位移δM：

δM＝exp(δr) (11)

其中，

定义^cM_o为刚体变换过程中的齐次变换矩阵，则^cM_o表征了运动目标相对于CCD相机的位姿参数；δM为刚体瞬时位移，根据式(8)，利用δM对^cM_o进行更新：

M_k+1＝exp([δr])M_k (13)

在上述的位姿参数非线性求解过程中，需要求解点特征和边缘特征的误差函数，边缘特征的误差函数如式(14)所示：

式中，d_⊥(l_i(r),x′_i)代表—图像上特征点x′_i与轮廓线l_i(r)的距离，ρ^g为M估计器，σ^g是的标准差的归一化因子，

点特征的误差目标函数Δ^p可表述如下：

其中，

式(16)中，x_i(r)为三维点X_i基于待求解位姿参数r投影到图像平面上的结果，x_i(r)＝pr(X_i,r)，ρ^p代表M估计器，σ^p是的标准差的归一化因子，

为实现基于点特征与边缘特征融合的目标追踪，进而利用非线性优化算法对位姿参数进行求解，将点特征与边缘特征各自的误差函数组合为一个全局误差函数Δ：

Δ＝ω^gΔ^g+ω^pΔ^p (17)

式中，Δ^g为边缘特征的误差函数，Δ^p为点特征的误差函数，ω^g为权重参数(0<ω^g<1)，ω^p为权重参数(0<ω^p<1)；

同理，将点特征与边缘特征的误差向量组合为一个全局误差向量e，对应的雅可比矩阵也要组合为全局雅克比矩阵J：

其中，

全局雅克比矩阵J由下式给出：

其中，

在式(18)、式(20)中，λ^g和λ^p由下式表示：

求解全局权重矩阵D以进行基于M估计的高斯-牛顿优化，再求解位移变化，全局权重矩阵D由下式表示：

D＝blockdiag(D^g,D^p) (23)

最后，高斯-牛顿迭代优化位移算式可更新为：

根据式(24)，利用高斯-牛顿迭代优化算法对运动目标的位姿参数进行实时解算和更新。

进一步地，在参考位姿的测量的过程中，当采用POSIT位姿求解算法对参考位姿(被测目标在参考图像中的位姿)进行求解时，首先通过人机交互的方式，选取与目标模型上特征点对应的图像平面点，再对模型上3D特征点的共面性进行判定，当平面点非共面时，利用POSIT位姿求解算法求解参考图像中目标的参考位姿。

进一步地，在参考位姿的测量的过程中，Coplanar-POSIT算法将平面法向量和平面内投影分开进行迭代计算，Coplanar-POSIT算法求解获得两组位姿参数，在迭代求解位姿参数的过程中，对两组位姿参数进行判别，只采用误差较小的解进行每次迭代，使得最后迭代收敛的两组位姿参数基本接近，其中任意一组数据都可以作为位姿测量的最终结果。

进一步地，在点特征的追踪与匹配过程中，点特征由Harris特征点检测算子求得，点特征追踪由基于光流法的KLT追踪算法实现；通过解算从第I_k帧图像到第I_k+1帧图像的光流，在第I_k+1帧图像中求得与第I_k帧图像特征点集对应的特征点集该二维特征点集与前述的三维特征点集具有一一映射的关系。

进一步地，在参考位姿的测量过程中，视频序列中被测图像位于参考图像之后。

本发明的有益效果是：

本发明针对不规则运动的目标，以视觉信息作为运动目标的先验信息，将目标的初始位姿与目标追踪方法相结合，实现了视频序列中运动目标位姿参数的实时输出。背景技术中现有技术是对目标进行三维重建，得到目标的三维点云数据，而本发明没有进行三维重建，所得到的信息为目标的边缘特征和点特征。

本发明将目标的初始位姿与目标追踪方法相结合，实现视频序列中运动目标位姿参数的实时输出；

本发明对环境的变化具有一定的适应能力。本发明采用SIFT特征提取算子对参考图像和被测图像中的SIFT特征点进行提取，进而利用所提取到的特征点进行匹配，在剔除背景特征点和误匹配以后通过2D-3D重投影建立模型上三维特征点与被测图像上二维SIFT特征点的对应关系。由于SIFT特征点具有旋转不变性、尺度不变性、对光照有一定的适应能力等优点，所以本发明对环境的变化具有一定的适应能力，而且针对物体被局部遮挡时也能测量出其位姿。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。在附图中：

图1是目标位姿测量方法的总体流程图；图2是目标初始位姿测量的流程框图；图3是目标追踪的流程图；图4是边缘特征法向一维搜索。

具体实施方式

具体实施方式一：本实施方式结合图1至4，对本发明所述的初始位姿测量结合目标追踪的位姿测量方法进行如下阐述：

1目标的初始位姿测量

目标的初始位姿测量用于启动后续的目标追踪，实现位姿参数的实时输出，对目标进行初始位姿测量的过程实质上为在三维环境中对目标进行探测和定位的过程。

(1)参考位姿的测量

为测量运动目标的初始位姿，需要首先提取一帧图像作为初始位姿测量的参考图像，利用此参考图像测量被测图像中目标的位姿参数。本发明采用POSIT位姿求解算法对参考位姿进行求解，通过人机交互的方式选取与目标模型上特征点对应的图像平面点，在对模型上3D特征点的共面性进行判定以后，利用POSIT/共面POSIT位姿测量算法求解参考图像中目标的参考位姿。

POSIT算法是一种经典的计算三维物***置和姿态的算法，用于计算三维物体的位置和姿态参数，其具有计算复杂度低，收敛速度快，鲁棒性较好等优势，适用于本发明中求取参考位姿的要求。

然而，经典的POSIT位姿测量算法只能对所选参考点非共面的情况进行位姿解算，而针对参考点共面的情况，需要采用适用于共面参考点的Coplanar-POSIT(共面POSIT)算法进行位姿求解。Coplanar-POSIT算法将平面法向量和平面内投影分开进行迭代计算，可求解获得两组位姿参数，在迭代求解位姿参数的过程中，对两组位姿参数进行判别，只采用误差较小的解进行每次迭代，可以使得最后迭代收敛的两组位姿参数基本接近，其中任意一组数据都可以作为位姿测量的最终结果。

在对运动目标的参考位姿的进行求取过程中，通过对输入的三维参考点的数据进行共面判定，根据参考点共面性的判定结果，完成参考位姿求取算法在POSIT算法和Coplanar-POSIT算法之间的切换，进而求得准确的运动目标参考位姿。

(2)特征点的提取与匹配

由于SIFT特征点具有旋转不变性、尺度不变性、对光照有一定的适应能力等优点，本发明采用SIFT特征提取算子对参考图像和被测图像中的SIFT特征点进行提取，进而利用所提取到的特征点进行匹配，在剔除背景特征点和误匹配以后通过2D-3D重投影建立模型上三维特征点与被测图像上二维SIFT特征点的对应关系。

在位姿测量的过程中，首先需要对参考图像提取SIFT特征点，这个过程为对参考图像的学习过程，提取到的特征点及其描述子作为学习样本与被测图像中的SIFT特征点和描述子进行匹配，建立二者之间SIFT特征点的一一映射关系。

SIFT特征点的匹配采用BruteForce匹配算法，其具有匹配效果稳定、计算效率高等优点。通过将参考图像中的特征描述子与被测图像中的特征描述子进行匹配，返回最优匹配并设置阈值消除错误匹配，可以建立参考图像与被测图像中SIFT特征点的一一映射关系。

在对二维图像提取SIFT特征点时，需要对提取出的SIFT特征点进行甄别，只保留位于目标图像上的SIFT特征。由于目标模型和参考位姿已知，因此利用这些信息可以通过在二维图像上进行投影以剔除背景图像中的SIFT特征点。

在消除二维图像上的背景干扰点以后，需要求取剩下的SIFT特征点所对应的模型坐标系中点的三维坐标，这些特征点在模型坐标系上的三维坐标及其对应的特征描述子将用于后续的基于多点匹配的位姿测量算法。

(3)基于多点匹配的位姿测量

定义最小化图像特征的期望状态参数为s*，实际状态参数为s，则在理想的条件下，存在一个唯一的相机位姿以最小化二者之间的误差。

假设被测目标上有大量的3D特征点^oP，^oP为被测目标上点在模型坐标系中的3D坐标，然后设定一个虚拟相机，其在模型坐标系中的位置和姿态定义为r，通过最小化观测数据s*(图像中SIFT特征点的2D坐标p，此时s*＝p)和3D特征点依据位姿参数r进行的前向投影到图像平面所得的2D坐标s之间的误差Δ，可以求解出被测目标相对于相机的真实位姿参数。

式中，pr_ε(r,^oP_i)为基于相机内部参数ξ和相机位姿r所得的投影模型；N为SIFT特征点的个数。

在位姿解算的过程中，初始位于r_i位置的虚拟相机遵循虚拟视觉伺服控制算法进行移动，以最小化误差参数Δ，在算法的收敛位置，虚拟相机到达r_d位姿以最小化Δ，r_d即为要求解的位姿参数。

在经典视觉伺服过程中，定义任务函数e：

e＝(s(r)-s^*) (2)

式(2)的导数由式(3)得出：

式中，L为图像雅克比矩阵。

将误差参数e进行指数解耦，可得：

式中，λ为控制衰减率的比例系数。

由此，可以得到如下的控制律：

式中，为L的伪逆矩阵，

最后，将速度向量v映射到其对应的瞬时位移上，以更新目标的位姿参数：

式中，exp(v)为齐次矩阵形式的指数映射；k为迭代优化过程中的迭代次数。

为了避免在特征匹配和3D信息求解的过程中出现粗大误差，通过对特征点的2D-3D匹配关系进行随机采样求解，并且不断地测试排除错误的匹配点对，最后利用所有的正确匹配点对重新求解目标的位姿参数，进而求得目标的初始位姿。

2目标追踪

(1)边缘特征的搜索与匹配

边缘特征具有特征提取效果稳定、抗干扰能力强等优势。然而边缘特征难以量化表示，因此，本发明通过将边缘特征转化为边缘上间隔一定距离的采样点特征，进而进行法向一维搜索确定模型边缘点与图像边缘点的匹配，如图4所示.

(2)点特征的追踪与匹配

本发明将特征点作为用于位姿追踪的另一种视觉特征。定义为从第I_k帧图像中提取到的特征点，且第I_k帧图像对应的目标位姿参数r_k已知。由于目标的三维模型已知，因此，这些二维特征点的深度值可以得到精确的求解，进而利用位姿参数r_k，可以将这些二维特征点逆投影到三维模型上，得到目标3D模型上一系列3D特征点

本发明中，点特征由Harris特征点检测算子求得，点特征追踪由基于光流法的KLT追踪算法实现。通过解算从第I_k帧图像到第I_k+1帧图像的光流，可以在第I_k+1帧图像中求得与第I_k帧图像特征点集对应的特征点集这个二维特征点集与前述的三维特征点集具有一一映射的关系。

(3)位姿参数的非线性优化求解

定义目标3D模型投影在图像上的模型投影取得2D点集为{x_i,j}，其对应的二维图像上对应点集为{x′_i,j}，误差函数Δ(r)用于指代图像上点x′_i,j到模型投影点x_i,j的距离，Δ(r)可表示为：

式中，d_⊥(x_i,j,x′_i,j)为图像上特征点x′_i,j到图像平面上模型投影点x_i,j的距离。

对Δ(r)进行非线性优化的过程，实质上就是基于位姿参数r对模型在图像平面上的投影特征和图像中提取到的图像特征之间的距离进行重组优化的过程。

本发明采用高斯-牛顿迭代法最小化目标误差函数Δ(r)，并求解最优位姿参数估计在本发明中，从初始位姿估计r₀开始，在每个迭代循环k中，位姿参数r_k做微小位移变换δr，通过式(8)更新位姿估计参数。

式(8)中⊕为参数空间中的求和算子，其最终目标为确定在每个迭代循环中使得Δ(r)最小的r_k值，其数学描述如式(9)所示：

在迭代过程中，通过式(12)求解微小位移变换δr：

δr＝-J⁺e(r) (10)

式中，J⁺代表矩阵J的伪逆矩阵；J为误差向量e(r)的雅可比矩阵，

微小位移变换δr＝(υ,ω)表征位姿参数的螺旋位移，其中υ表征平移位移参数，ω表征旋转位移参数，为利用微小位移变换δr实现位姿参数的更新，利用指数映射将微小位移变换δr转换为其对应的刚体瞬时位移δM：

δM＝exp(δr) (11)

其中，

定义^cM_o为刚体变换过程中的齐次变换矩阵，则^cM_o表征了运动目标相对于CCD相机的位姿参数。δM为刚体瞬时位移，根据式(8)，利用δM，可以对^cM_o进行更新：

M_k+1＝exp([δr])M_k (13)

在上述的位姿参数非线性求解过程中，需要求解点特征和边缘特征的误差函数，边缘特征的误差函数如式(14)所示：

式中，d_⊥(l_i(r),x′_i)代表—图像上特征点x′_i与轮廓线l_i(r)的距离，ρ^g为M估计器，σ^g是的标准差的归一化因子，

点特征的误差目标函数Δ^p可表述如下：

其中，

式(16)中，x_i(r)为三维点X_i基于待求解位姿参数r投影到图像平面上的结果，x_i(r)＝pr(X_i,r)，ρ^p代表M估计器，σ^p是的标准差的归一化因子，

为实现基于点特征与边缘特征融合的目标追踪，进而利用非线性优化算法对位姿参数进行求解，本发明将点特征与边缘特征各自的误差函数组合为一个全局误差函数Δ：

Δ＝ω^gΔ^g+ω^pΔ^p (17)

式中，Δ^g为边缘特征的误差函数，Δ^p为点特征的误差函数，ω^g为权重参数(0<ω^g<1)，ω^p为权重参数(0<ω^p<1)。

同理，本发明将点特征与边缘特征的误差向量组合为一个全局误差向量e，对应的雅可比矩阵也要组合为全局雅克比矩阵J：

其中，

全局雅克比矩阵J由下式给出：

其中，

在式(18)、式(20)中，λ^g和λ^p由下式表示：

为了求解位移变化，需要求解全局权重矩阵D以进行基于M估计的高斯-牛顿优化，全局权重矩阵D由下式表示：

D＝blockdiag(D^g,D^p) (23)

最后，高斯-牛顿迭代优化位移算式可更新为：

根据式(24)，可以利用高斯-牛顿迭代优化算法对运动目标的位姿参数进行实时解算和更新。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种初始位姿测量结合目标追踪的位姿测量方法，其特征在于，所述方法的实现过程为：

步骤1、目标的初始位姿测量

目标的初始位姿测量用于启动后续的目标追踪，实现位姿参数的实时输出；

(1)参考位姿的测量

为测量运动目标的初始位姿，在含有n帧图像的视频序列的前n-1帧图像中提取任意一帧图像，作为初始位姿测量的参考图像，利用此参考图像测量被测图像中目标的位姿参数；

利用POSIT位姿求解算法对所选参考点非共面的情况进行位姿解算；针对参考点共面的情况，采用适用于共面参考点的Coplanar-POSIT位姿求解算法进行位姿求解；在对运动目标的参考位姿进行求取的过程中，需要通过对输入的三维参考点的数据进行共面判定，根据参考点共面性的判定结果在POSIT算法和Coplanar-POSIT算法之间的切换，进而求得准确的运动目标参考位姿；

(2)特征点的提取与匹配

采用SIFT特征提取算子对参考图像和被测图像中的SIFT特征点进行提取，进而利用所提取到的特征点进行匹配，在剔除背景特征点和误匹配以后通过2D-3D重投影建立模型上三维特征点与被测图像上二维SIFT特征点的对应关系，

具体实现过程为：

首先，需要对参考图像提取SIFT特征点，提取SIFT特征点的过程为对参考图像的学习过程；在对二维图像提取SIFT特征点时，对提取出的SIFT特征点进行甄别，只保留位于图像中目标上的SIFT特征；根据目标模型和参考位姿，在二维图像上进行投影以剔除背景图像中的SIFT特征点；

然后，将提取到的特征点及其描述子作为学习样本与被测图像中的SIFT特征点和描述子进行匹配，建立二者之间SIFT特征点的一一映射关系；所述匹配的具体过程为：SIFT特征点的匹配采用BruteForce匹配算法，通过将参考图像中的特征描述子与被测图像中的特征描述子进行匹配，返回最优匹配并设置阈值消除错误匹配，进而建立参考图像与被测图像中SIFT特征点的一一映射关系；

最后，在消除二维图像上的背景干扰点以后，求取保留的SIFT特征点所对应的模型坐标系中点的三维坐标，保留的SIFT特征点在模型坐标系上的三维坐标及其对应的特征描述子将用于后续的基于多点匹配的位姿测量；

(3)基于多点匹配的位姿测量，具体过程为：

定义最小化图像特征的期望状态参数为s*，实际状态参数为s，则在理想的条件下，存在一个唯一的相机位姿以最小化二者之间的误差；

假设被测目标上有大量的3D特征点^oP，^oP为被测目标上的点在模型坐标系中的3D坐标，然后设定一个虚拟相机，其在模型坐标系中的位置和姿态定义为r，通过最小化观测数据s*和3D特征点依据位姿参数r进行的前向投影到图像平面所得的2D坐标的实际状态s之间的误差Δ，可求解出被测目标相对于相机的真实位姿参数；

式中，pr_ε(r,^oP_i)为基于相机内部参数ξ和相机位姿r所得的投影模型；N为SIFT特征点的个数；

在位姿解算的过程中，初始位于r_i位置的虚拟相机遵循虚拟视觉伺服控制算法进行移动，以最小化误差参数Δ，在算法的收敛位置，虚拟相机到达r_d位姿以最小化Δ，r_d即为要求解的位姿参数；

在经典视觉伺服过程中，定义任务函数e：

e＝(s(r)-s^*) (2)

式(2)的导数由式(3)得出：

式中，L为图像雅克比矩阵；

将误差参数e进行指数解耦，可得：

式中，λ为控制衰减率的比例系数；

由此，可得到如下的控制律：

式中，为L的伪逆矩阵，

最后，将速度向量v映射到其对应的瞬时位移上，以更新目标的位姿参数：

式中，exp(v)为齐次矩阵形式的指数映射；k为迭代优化过程中的迭代次数；

通过对特征点的2D-3D匹配关系进行随机采样求解以避免在特征匹配和3D信息求解的过程中出现粗大误差，并且不断地测试排除错误的匹配点对，最后利用所有的正确匹配点对重新求解目标的位姿参数，进而求得目标的初始位姿；

步骤2、目标追踪

(1)边缘特征的搜索与匹配

将边缘特征转化为边缘上间隔一定距离的采样点特征后再进行法向一维搜索，从而确定目标模型边缘点与图像边缘点的匹配；

(2)点特征的追踪与匹配

定义为从第I_k帧图像中提取到的特征点，且第I_k帧图像对应的目标位姿参数r_k已知；根据已知的目标的三维模型精确求解图像中二维特征点的深度值，进而利用位姿参数r_k，将图像中二维特征点逆投影到三维模型上，得到目标3D模型上一系列3D特征点

(3)位姿参数的非线性优化求解

定义目标3D模型投影在图像上的模型投影取得2D点集为{x_i,j}，其对应的二维图像上对应点集为{x_i′_,j}，误差函数Δ(r)用于指代图像上点x_i′_,j到模型投影点x_i,j的距离，Δ(r)可表示为：

式中，d_⊥(x_i,j,x′_i,j)为图像上特征点x′_i,j到图像平面上模型投影点x_i,j的距离；

采用高斯-牛顿迭代法最小化目标误差函数Δ(r)，并求解最优位姿参数估计从初始位姿估计r₀开始，在每个迭代循环k中，位姿参数r_k做微小位移变换δr，通过式(8)更新位姿估计参数；

式(8)中为参数空间中的求和算子，其最终目标为确定在每个迭代循环中使得Δ(r)最小的r_k值，其数学描述如式(9)所示：

在迭代过程中，通过式(12)求解微小位移变换δr：

δr＝-J⁺e(r) (10)

式中，J⁺代表矩阵J的伪逆矩阵；J为误差向量e(r)的雅可比矩阵，

微小位移变换δr＝(υ,ω)表征位姿参数的螺旋位移，其中υ表征平移位移参数，ω表征旋转位移参数，为利用微小位移变换δr实现位姿参数的更新，利用指数映射将微小位移变换δr转换为其对应的刚体瞬时位移δM：

δM＝exp(δr) (11)

其中，

定义^cM_o为刚体变换过程中的齐次变换矩阵，则^cM_o表征了运动目标相对于CCD相机的位姿参数；δM为刚体瞬时位移，根据式(8)，利用δM对^cM_o进行更新：

M_k+1＝exp([δr])M_k (13)

在上述的位姿参数非线性求解过程中，需要求解点特征和边缘特征的误差函数，边缘特征的误差函数如式(14)所示：

式中，d_⊥(l_i(r),x′_i)代表—图像上特征点x′_i与轮廓线l_i(r)的距离，ρ^g为M估计器，σ^g是的标准差的归一化因子，

点特征的误差目标函数Δ^p可表述如下：

其中，

式(16)中，x_i(r)为三维点X_i基于待求解位姿参数r投影到图像平面上的结果，x_i(r)＝pr(X_i,r)，ρ^p代表M估计器，σ^p是的标准差的归一化因子，

为实现基于点特征与边缘特征融合的目标追踪，进而利用非线性优化算法对位姿参数进行求解，将点特征与边缘特征各自的误差函数组合为一个全局误差函数Δ：

Δ＝ω^gΔ^g+ω^pΔ^p (17)

式中，Δ^g为边缘特征的误差函数，Δ^p为点特征的误差函数，ω^g为权重参数(0<ω^g<1)，ω^p为权重参数(0<ω^p<1)；

同理，将点特征与边缘特征的误差向量组合为一个全局误差向量e，对应的雅可比矩阵也要组合为全局雅克比矩阵J：

其中，

全局雅克比矩阵J由下式给出：

其中，

在式(18)、式(20)中，λ^g和λ^p由下式表示：

求解全局权重矩阵D以进行基于M估计的高斯-牛顿优化，再求解位移变化，全局权重矩阵D由下式表示：

D＝blockdiag(D^g,D^p) (23)

最后，高斯-牛顿迭代优化位移算式可更新为：

根据式(24)，利用高斯-牛顿迭代优化算法对运动目标的位姿参数进行实时解算和更新。

2.根据权利要求1所述的一种初始位姿测量结合目标追踪的位姿测量方法，其特征在于，在参考位姿的测量的过程中，

当采用POSIT位姿求解算法对参考位姿进行求解时，首先通过人机交互的方式，选取与目标模型上特征点对应的图像平面点，再对模型上3D特征点的共面性进行判定，当平面点非共面时，利用POSIT位姿求解算法求解参考图像中目标的参考位姿。

3.根据权利要求2所述的一种初始位姿测量结合目标追踪的位姿测量方法，其特征在于，在参考位姿的测量的过程中，

Coplanar-POSIT算法将平面法向量和平面内投影分开进行迭代计算，Coplanar-POSIT算法求解获得两组位姿参数，在迭代求解位姿参数的过程中，对两组位姿参数进行判别，只采用误差较小的解进行每次迭代，使得最后迭代收敛的两组位姿参数基本接近，其中任意一组数据都可以作为位姿测量的最终结果。

4.根据权利要求1、2或3所述的一种初始位姿测量结合目标追踪的位姿测量方法，其特征在于，

在点特征的追踪与匹配过程中，点特征由Harris特征点检测算子求得，点特征追踪由基于光流法的KLT追踪算法实现；通过解算从第I_k帧图像到第I_k+1帧图像的光流，在第I_k+1帧图像中求得与第I_k帧图像特征点集对应的特征点集该二维特征点集与前述的三维特征点集具有一一映射的关系。

5.根据权利要求1所述的一种初始位姿测量结合目标追踪的位姿测量方法，其特征在于，在参考位姿的测量过程中，视频序列中被测图像位于参考图像之后。