[发明专利]一种基于原始CAD模型的三维目标跟踪算法研究

权利要求书

1.一种基于原始CAD模型的三维目标跟踪算法研究，其特征在于，其研究方法包括以下步骤：

S1、建立VISP：建立VISP平台，使其实现对实时图像进行处理和计算机视觉算法进行跟踪，在使用校准摄像机的同时，利用能够提供对象3D位置信息的CAD模型先验知识进行无标记的目标跟踪；

S2、CAO格式模型建立：选择cao格式，分别对规则图形和不规则图形进行CAO格式建模，在对不规则图形建模时，将模型以索引三角网格为储存格式，实现模型有效网格表示，对于三角网格中的每个三角形都会与其相邻的其他三角形共享边，三角网格所要存储的信息包括顶点、边和面的信息；

S3、三维目标跟踪：对边缘特征进行检测，根据前一帧的目标位置来计算模型边缘在当前帧的图像空间中可见边缘的投影直线，计算方法是根据模型节点的投影求直线坐标的方程参数，第二步是沿投影直线方向采样，设置采样步长，求取采样点像素坐标，根据每一个采样点像素坐标及直线角度参数，选取边缘检测的单元；

S4、投影边缘的距离计算：将图像空间特征变化变换到笛卡尔坐标中，变换关系是利用点到直线距离的交互矩阵来表达的，此处，交互矩阵就是能够获取目标在三维空间平移及旋转方向的速度与图像空间坐标变化速度的关系矩阵，给定相机坐标系点P(x_c，y_c，z_c)，与其对应的平面投影点为p(x，y)，根据投影关系求导可得：

交互矩阵是利用相机内参及深度信息进行估计的，图像空间直线参数方程xcosθ+ysinθ-ρ＝0，式中，θ代表直线和图像坐标系u轴的夹角，ρ是点到直线的距离，空间直线所在的平面方程A₁x_c+B₁y_c+C₁z_c+D₁＝0，由此可将其对应焦距归一化图像平面的直线投影方程表示为：

Ax+By+C＝1/z_c (3.2)

式中A＝-A₁/D₁，B＝-B₁/D₁，C＝-C₁/D₁，直线极坐标方程求导得

ρ′+(xsinθ-ycosθ)θ′＝x′cosθ+y′sinθ (3.3)

由式(3.1)，(3.2)，(3.3)，且参考文献[6]能够得到

式中λ_ρ＝-Aρcosθ-Bρsinθ-C，λ_θ＝-Asinθ+Bcosθ，即直线的交互矩阵为：

点到直线的距离可以根据图像坐标点到斜率相等的两条直线的距离差求出，点(x_d，y_d)到直线(ρ，θ)的距离可表达为

d＝ρ-ρ_d＝ρ-x_dcosθ+y_dsinθ (3.6)

对上式求导得

d′＝ρ′-(x_dcosθ+y_dsinθ)θ′＝ρ′-αθ′ (3.7)

式中α＝x_dcosθ+y_dsinθ，根据直线交互矩阵得点到直线的交互矩阵

L_d＝L_ρ-αL_θ，表示为

式中λ_d＝λ_ρ+θλ_θ，用边缘点到对应模型投影直线之间的距离表示每个跟踪单元中的图像特征，当目标中存在多个跟踪单元时，应融合目标全部的图像特征，构造系统的交互矩阵，完成将所有可见模型投影边缘收敛于图像边缘点，系统的交互矩阵表示为

L_s＝[L₁ L₂…L_m] (3.9)

跟踪单元特征的交互矩阵表示为

式中，λ_di＝λ_pi+α_iλ_θi(i＝1，...，m)，矩阵中的行数表示模型投影边缘采样的特征单元个数，列数表示模型在笛卡尔空间上自由度的个数，使用伪逆将图像空间特征偏差变换为模型速度变化：

式中，是交互矩阵的伪逆，v_x，v_y，v_z，w_x，w_y，w_z代表模型在旋转、平移方向的速度，d₁，d₂，...，d_m即为目标边缘点到对应模型可见投影边缘的距离；

S5、三维模型与目标对准：使模型与目标间的距离收敛到零的过程，本文使用带权重的最小二乘法，在图像空间创建表示目标边缘与模型投影特征之间偏差的目标函数，函数如下：

S6、KLT的目标跟踪算法：提取图像上的特征点，然后匹配下一帧中的特征点，得到目标在下一帧的位置，该过程的核心就是将目标跟踪转化为特征点的跟踪：

A、Harris特征点提取算法：

将图像窗口平移[u，v]，产生的灰度变化E(u，v)，则

式中，w(x，y)为窗口函数，I(x，y)为图像灰度，I(x+u，y+v)为平移后的图像灰度，利用泰勒级数对式(3.16)进行简化得

式中对窗口中每一个像素在x及y方向上的梯度I_x和I_y进行统计分析即可求得M，再利用M来计算每个像素的角点响应函数R：

R＝detM-k(traceM)² (3.18)

式中，detM＝λ₁λ₂，traceM＝λ₁λ₂，在矩阵R中，某点同时满足R(i，j)＝det(M)-Ktrace²(M)＞T且R(i，j)是区域内局部极大值时，该点被认为是角点，其中，K的取值范围为0.04～0.06，T为阈值，检测过程中将低于该阈值的R设为0；

B、KLT匹配算法理论：

I(x，y，t+τ)＝I(x-Δx，y-Δy，t) (3.19)

给定两幅图像I和J，定义能够使

SSDSum of Squared intensity Difference，此处用ε表示)最小化的d，则有：

式中，W为给定的特征窗口，w(X)为加权函数，d＝(Δx，Δy)为点X(x，y)的运动偏移量，由于d远小于X，则可将J(X+d)进行泰勒展开，再结合式(3.20)对d求导可得：

Zd＝e (3.21)

式中，g为展开的一阶泰勒系数，对每一个点通过(3.21)式进行牛顿迭代，当迭代结果达到要求精度时就实现了图像点的跟踪，即找到了最优匹配，最终解为：

其中，d为特征窗口中心的平移量，d_k为第k次牛顿迭代法所得的d的值，在迭代计算时本文将初始迭代值设为0，当目标移动速度较慢时其相对的帧间位移也小，该情况下利用上述KLT匹配算法可以在降低匹配搜索范围的同时达到很高的匹配精度，但当目标运动过快时帧间位移也大，根据匹配获取的特征点对的可靠性也会下降，此时，若匹配窗口选的小，会有漏匹配，若匹配窗口选的大，会影响匹配精度，该问题的解决方法是对KLT匹配算法进行分层迭代，分层迭代匹配方法是通过高斯金字塔分解来实现的，操作步骤是对图像I及J进行精度递减的L层金字塔分解，得到I^l和J^l，l的取值范围为l＝0，1，2，...，L，(1)令l＝L，初始化n＝0，(2)令n＝n+1，在I^l和J^l上由式(3.22)求得再判断迭代的次数是否执行完或者的精度是否已经满足要求，只要满足条件之一就执行下一步，否则返回到第(2)步，(3)当l＝0时迭代结束，最后否则令返回到步骤(2)，分层迭代过程中选择小的搜索窗口，然后从高层到底层不断迭代，直到最低层为止，从而提高目标作快速移动时的点对匹配精度；

S7、点线协同的目标跟踪：将基于纹理信息的相机位移估计集成在基于边缘特征的相机姿态计算过程中。