[发明专利]一种基于目标三维模型的实时位姿跟踪方法

摘要

本发明涉及一种基于目标三维模型的实时位姿跟踪方法。本发明基于目标的三维模型，利用轮廓特征，建立轮廓部件模型，依据局部轮廓辨识度将轮廓分割成若干局部轮廓段；针对每一轮廓段，在梯度域基于相位匹配准则建立二维‑三维对应；利用刚体目标各部件之间的几何关系，消除错误匹配对；基于建立的二维‑三维匹配对，通过RPNP算法求解位姿参数，迭代整个过程直至收敛，实现序列图像中目标位姿跟踪。本发明算法步骤简单明了，易于应用，且由于引入目标的三维模型，可实现全程无漂移跟踪。

主权项

1.一种基于目标三维模型的实时位姿跟踪方法，其特征在于：基于目标的三维模型及初始位姿估计投影得到二值图像，目标区域为1，背景区域为0，提取目标轮廓，利用轮廓特征，建立轮廓部件模型，依据局部轮廓辨识度将轮廓分割成若干局部轮廓段；针对每一轮廓段，在梯度域基于相位匹配准则建立二维‑三维对应；利用刚体目标各部件之间的几何关系，消除错误匹配对；基于建立的二维‑三维匹配点对，通过RPNP鲁棒的N点透视算法求解位姿参数，迭代整个过程直至收敛，实现序列图像中目标位姿跟踪，其步骤为：(1)轮廓部件模型生成利用目标的轮廓特征建立投影图像与输入图像之间匹配对的建立，利用目标三维模型采用的是顶点和三角面片的数据格式，利用目标的三维模型及目标的初始位姿估计，采用公式(1)将目标三维模型中的每个顶点投影到图像上，式中R及T分别为描述刚体运动的旋转矩阵及平移向量，[X_W Y_W Z_W]^T、[X_C Y_C Z_C]^T及[u v]^T分别为顶点在世界坐标系、相机坐标系及图像坐标系中的坐标，F_x、F_y、C_x及C_y为相机焦距及主点参数，通过标定获得，依据顶点投影结果，计算面片对应的投影区域，最终确定在投影图像中整个目标对应的区域，将目标对应的区域置1，背景区域置0，得到二值投影图像，记为T；采用腐蚀数学形态学操作算子对二值投影图像进行处理，其中核元素为3*3的十字模板，再与原始二值图像做差，得到目标投影区域的最外围轮廓，对应的顺序点集记为N为轮廓长度；在原始二值图像中，对于轮廓中每一个轮廓点对应的位置s_i，计算其对应的Shi‑Tomasi角点响应值，记为v_i，角点响应值越大，说明对应位置的轮廓走向存在较大改变，更具辨识度，依据{v_i}_N从大到小对所有轮廓点进行排序，并采用非最大值抑制方法选出Q个辨识度高的轮廓点，记为Q为选取的轮廓点个数，针对以其为中心选择长度L的轮廓段S_j，则{S_j}_Q即为得到的目标在当前位姿参数下的轮廓部件模型，对应S_j，在投影计算的过程中保存深度信息，依据相机参数可反求出对应的物体上的点的三维坐标，记为轮廓部件模型的点对集合为(2)梯度域相位匹配在梯度域进行匹配操作，旨在建立可靠的匹配对，采用3*3大小的Sobel算子求取投影到得到的二值图像T及输入图像I对应的梯度图；针对每一个轮廓段S_j，记T中轮廓段S_j对应的图像子块为T'，在输入图像中待匹配图像子块记为I'，(dx_pT,dy_pT)及(dx_pI,dy_pI)分别为T'及I'中的像素点梯度向量，定义梯度相位角为梯度向量与图像坐标系轴正方向的夹角，对应的梯度相位角度记为θ_pT及θ_pI，梯度相位角的范围为[0,2π]，则基于余弦和准则的梯度相位匹配准则如公式(2)所示式中X和Y为图像子块的尺寸，Score为相似度值，(θ_pT‑θ_pI)即为梯度向量之间的夹角，将梯度向量归一化式中norm(g)求取向量的模值，则cos(θ_pT‑θ_pI)＝dx'_pT*dx'_pI+dy'_pT*dy'_pI   (4)本发明认为向量方向相同或相反完全等价，即(θ_pT‑θ_pI)＝0与(θ_pT‑θ_pI)＝π等价，改写公式(2)中相似度准则为有二倍角公式知cos(2(θ_pT‑θ_pI))＝2cos(θ_pT‑θ_pI)²‑1，则相似度准则进一步转化为将公式(4)带入公式(6)得在投影二值图像中，仅提取最外围目标轮廓，为消除实时输入图像中目标外围轮廓附近的梯度造成干扰，采用原始二值图像子块T'对应的梯度幅值对匹配准则进行加权，即其中w_pT＝norm((dx_pT,dy_pT))，Score的取值范围为[0,1]；针对每一个轮廓段S_j，在其初始位置附近一定尺寸邻域内采用滑动窗口策略搜索最佳匹配位置，若邻域内最大相似度值大于预先设定的阈值，则认为成功匹配，否则此轮廓段匹配失败，以轮廓段中心表示匹配对，则最终得到的成功匹配对集合记为K为成功匹配对数；(3)误匹配剔除针对的刚体目标的位姿跟踪，刚体目标不存在形变，自身各部件间相对几何位置关系固定，采用局部结构保持的匹配策略，剔除误匹配点；针对依据结构保持策略，建立如下目标函数式中P＝{p_k}_K,p_k∈{0,1}为标识向量，p_k＝1表示匹配点正确，反之为误匹配点，N_p为点p的邻域，以欧氏距离度量，λ为控制参数，通过最小化公式(9)中所示目标函数，即求得标识向量P，进而剔除误匹配点，得到正确的匹配点对集合，记为结合目标三维模型投影中得到的点对集合记为建立输入图像与目标三维模型之间的二维‑三维对应关系，对应的二维‑三维匹配点对集合为(4)位姿参数求解基于得到的二维‑三维匹配点对集合采用RPNP鲁棒的N点透视算法求解目标位姿参数，迭代整个过程，并依据迭代中位姿参数更新量判断迭代是否终止，输出最终的位姿更新结果。