[发明专利]一种初始位姿测量结合目标追踪的位姿测量方法

摘要

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

主权项

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)，利用高斯‑牛顿迭代优化算法对运动目标的位姿参数进行实时解算和更新。