目标物体的六自由度姿态跟踪方法及终端设备与流程

[0001]
本发明涉及目标姿态跟踪的技术领域，尤其涉及一种目标物体的六自由度姿态跟踪方法及终端设备。


背景技术：

[0002]
圆形特征是人造物体上最常见的几何特征之一，其广泛存在于大量的工业产品。相比其他几何特征，圆形特征的图像具有封闭性、闭合的数学表达式、对噪声鲁棒等优点。如果事先已知某目标上圆形特征的三维几何信息，便可从一幅圆形特征的图像中，计算出目标的六自由度姿态参数。因此，基于圆形特征的姿态估计算法一直是基于计算机视觉的姿态计算领域的热点问题，受到了广泛研究。目前，基于圆形特征的姿态估计方法已经应用于工业装配、航天器空间交会对接、无人机导航、航天器着陆等众多领域。
[0003]
现有的基于圆形特征的姿态估计方法主要分为两类。第一类为基于椭圆拟合的姿态计算方法，对目标图像中圆形特征的图像边缘进行椭圆拟合，目标的姿态与位置可以直接从拟合的椭圆计算出来；但当目标的圆形特征被遮挡时，通过不完整的圆形特征拟合的椭圆与圆形特征的真实投影相差很大，这将严重影响姿态估计的精度，甚至使该方法失效。第二类为基于空间圆位置关系的姿态计算方法，利用目标上圆形特征的三维信息与其图像信息的匹配关系，实现目标的姿态估计；但是该类算法的运行效率不高，且精度容易受到误匹配的影响。


技术实现要素：

[0004]
有鉴于此，本发明实施例提供了一种目标物体的六自由度姿态跟踪方法及终端设备，以解决运动目标的姿态参数估计的精确性低和速度慢的问题。
[0005]
本发明实施例的第一方面提供了一种目标物体的六自由度姿态跟踪方法，
[0006]
包括：
[0007]
提取视频数据中第n帧图像的边缘图像；所述视频数据为实时拍摄目标物体得到；所述目标物体的外形具有圆形特征；n>2；
[0008]
根据所述视频数据中第n-1帧图像对应的下一帧(n帧)图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线；
[0009]
在所述边缘图像中查找属于所述投影曲线的像素点，得到若干边缘点；
[0010]
根据所述若干边缘点与所述投影曲线之间的几何残差，得到所述第n帧图像中所述目标物体的姿态参数估计值；
[0011]
对所述姿态参数估计值进行数据平滑处理并根据所述姿态参数估计值对下一帧(n+1帧)图像中所述目标物体的姿态进行预测，得到姿态参数最终估计值以及所述下一帧(n+1帧)图像的姿态参数预测值。
[0012]
在一个实施示例中，在提取视频数据中第n帧图像的边缘图像之前，还包括：
[0013]
获取所述视频数据的第一帧图像，采用椭圆检测算法提取出所述第一帧图像中所
述目标物体的圆形特征对应的特征像素点；
[0014]
根据所述特征像素点确定在所述第一帧图像中所述目标物体的圆形特征的第二投影曲线；
[0015]
基于所述第一帧图像的第二投影曲线，得到所述第一帧图像中所述目标物体的姿态参数的初始估计值；
[0016]
根据所述第一帧图像的姿态参数初始估计值，确定在所述第一帧图像中所述目标物体的圆形特征的第三投影曲线；
[0017]
对每一条所述第三投影曲线，在所述对应的特征像素点中查找属于所述第三投影曲线的像素点，得到若干第三边缘点；
[0018]
根据所述若干第三边缘点与所述第三投影曲线之间的几何残差，得到所述第一帧图像中所述目标物体的姿态参数估计值。
[0019]
在一个实施示例中，当n等于2时，所述根据所述视频数据中第n-1帧图像对应的下一帧(n帧)图像的姿态参数预测值，确定在所述第n帧图像中所述目标物体的圆形特征的投影曲线，包括：
[0020]
根据所述视频数据中第一帧图像对应的下一帧图像的姿态参数估计值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线。
[0021]
在一个实施示例中，所述在所述边缘图像中查找属于所述投影曲线的像素点，得到若干边缘点，包括：
[0022]
在所述投影曲线中等间距采样若干个控制点；
[0023]
以各个所述控制点为中心，在所述边缘图像中查找位于各个所述控制点的法线所在的预设范围内的像素点，得到各个所述控制点对应的候选边缘点；
[0024]
若任一所述控制点对应的任一候选边缘点满足预设条件，则确定所述候选边缘点为边缘点。
[0025]
在一个实施示例中，所述若任一所述控制点对应的任一候选边缘点满足预设条件，则确定所述候选边缘点为边缘点，包括：
[0026]
对于每一所述控制点对应的每一所述候选边缘点，若所述候选边缘点的法线与所述候选边缘点对应的控制点的法线之间的夹角小于预设阈值，则设定所述候选边缘点为预选边缘点；
[0027]
从每一所述控制点对应的所述预选边缘点中，确定与所述控制点距离最近的一个预选边缘点为边缘点。
[0028]
在一个实施示例中，所述根据所述若干边缘点与所述投影曲线之间的几何残差，得到所述第n帧图像中所述目标物体的姿态参数估计值，包括：
[0029]
计算所述若干边缘点与所述投影曲线之间的几何距离的平方和，得到所述第n帧图像中所述目标物体的姿态参数估计值；
[0030]
采用广义期望最大化方法对所述姿态参数估计值进行优化得到姿态参数的极大似然估计值。
[0031]
在一个实施示例中，所述对所述姿态参数估计值进行数据平滑处理并根据所述姿态参数估计值对下一帧(n+1帧)图像中所述目标物体的姿态进行预测，得到姿态参数最终估计值以及所述下一帧图像的姿态参数预测值，包括：
[0032]
根据所述姿态参数估计值得到姿态参数的协方差矩阵；
[0033]
采用卡尔曼滤波算法对所述姿态参数估计值和所述协方差矩阵进行计算，得到姿态参数最终估计值以及下一帧图像的姿态参数预测值。
[0034]
在一个实施示例中，所述根据所述视频数据中第n-1帧图像对应的下一帧图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线，包括：
[0035]
将姿态参数预测值代入预设的空间圆投影方程，得到目标物体的外表圆形特征投影在图像中的投影曲线方程。
[0036]
在一个实施示例中，在所述根据所述视频数据中第n-1帧图像对下一帧(n帧)图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线之前，还包括：
[0037]
根据所述目标物体的空间坐标系与视频数据的图像坐标系之间的映射关系构建与姿态参数相关的姿态模型；
[0038]
对所述姿态模型进行圆形特征参数转换得到空间圆投影方程。
[0039]
本发明实施例的第三方面提供了一种终端设备，包括：存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现第一方面中目标物体的六自由度姿态跟踪方法。
[0040]
本发明实施例提供的一种目标物体的六自由度姿态跟踪方法及终端设备，通过提取视频数据中第n帧图像的边缘图像；所述视频数据为实时拍摄目标物体得到；所述目标物体的外形具有圆形特征；n>2；根据所述视频数据中第n-1帧图像对应的下一帧图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线；结合视频数据中n-1帧图像对应的下一帧(n帧)图像的姿态参数预测值快速预测当前第n帧图像中目标物体的圆形特征的投影曲线，避免圆形特征被遮挡而导致投影方程拟合出错的问题，具备圆形特征遮挡的鲁棒性。在所述边缘图像中查找属于所述投影曲线的像素点，得到若干边缘点；根据所述若干边缘点与所述投影曲线之间的几何残差，得到所述第n帧图像中所述目标物体的姿态参数估计值；通过直接最小化圆形特征的参数化投影曲线的几何残差，获得目标物体的姿态参数估计值，保证了姿态估计的速度、精度和对噪声的鲁棒性。对所述姿态参数估计值进行数据平滑处理并根据所述姿态参数估计值对下一帧(n+1帧)图像中所述目标物体的姿态进行预测，得到姿态参数最终估计值以及所述下一帧图像的姿态参数预测值，实现对当前第n帧图像的目标物体的姿态参数估计值的准确矫正和对下一帧图像中目标物体的姿态参数的精确预测。
附图说明
[0041]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0042]
图1是本发明实施例一提供的目标物体的六自由度姿态跟踪方法的流程示意图；
[0043]
图2是本发明实施例一提供的第n帧图像中目标物体的圆形特征；
[0044]
图3是本发明实施例二提供的目标物体的六自由度姿态跟踪方法的流程示意图；
[0045]
图4是本发明实施例三提供的终端设备的结构示意图。
具体实施方式
[0046]
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
[0047]
本发明的说明书和权利要求书及上述附图中的术语“包括”以及它们任何变形，意图在于覆盖不排他的包含。例如包含一系列步骤或单元的过程、方法或系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。此外，术语“第一”、“第二”和“第三”等是用于区别不同对象，而非用于描述特定顺序。
[0048]
实施例一
[0049]
如图1所示，是本发明实施例一提供的目标物体的六自由度姿态跟踪方法的流程示意图。本实施例可适用于对目标物体进行姿态跟踪的应用场景，该方法可以由终端设备执行，该装置可为处理器、智能终端、平板或pc等；在本申请实施例中以终端设备作为执行主体进行说明，该方法具体包括如下步骤：
[0050]
s110、提取视频数据中第n帧图像的边缘图像；所述视频数据为实时拍摄目标物体得到；所述目标物体的外形具有圆形特征；n>2。
[0051]
圆形特征是人造物体上最常见的几何特征之一，其广泛存在于大量的工业产品。相比其他几何特征，圆形特征的图像具有封闭性、闭合的数学表达式、对噪声鲁棒等优点。如果事先已知某目标物体的外形上的圆形特征的三维几何信息，便可从一幅圆形特征的图像中，计算出姿态参数。根据得到的姿态参数就能确定目标物体的姿态。详细地，该圆形特征可为目标物体的圆形开口、圆形本体或圆形底部等圆形线条，或者是设置于目标物体外部的圆形外设的圆形线条。因此，在对运动目标物体进行姿态跟踪的应用场景，通常采用拍摄装置对外形具有圆形特征的目标物体进行实时拍摄得到视频数据，视频数据中的每帧图像均记录对应时刻目标物体的圆形特征的形状变化。可选的拍摄装置可为针孔照相机或工业相机。
[0052]
由于拍摄装置对目标物体进行拍摄时具有拍摄时间间隔，可通过调节拍摄时间间隔调节目标物体的图像采样频率，使得视频数据中每帧图像均对应一个采样时刻。为获得任一帧图像中目标物体的姿态参数，可以根据目标物体的圆形特征在该帧图像中对应的投影曲线得到该帧图像中目标物体的姿态参数。具体的，为获得拍摄得到的第n帧图像中目标物体的姿态参数，需确定视频数据中第n帧图像中目标物体的圆形特征对应的投影曲线，可以通过提取视频数据中第n帧图像的边缘图像，得到第n帧图像中目标物体的所有边缘信息。如图2所示，由于目标物体的圆形特征在拍摄得到的图像中呈现边缘轮廓的形式，即图2中的白圈，因此提取到的边缘图像包含第n帧图像中目标物体的圆形特征对应的像素点；其中，n为整数且n>2。可选的，可采用canny算子方法提取视频数据中第n帧图像的边缘图像。
[0053]
s120、根据所述视频数据中第n-1帧图像对应的下一帧(n帧)图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线。
[0054]
在提取视频数据中第n帧图像的边缘图像后，为确定边缘图像中目标物体的圆形特征对应的像素点(边缘点)，需得到在第n帧图像中所述目标物体的圆形特征的投影曲线。具体的，可以根据视频数据中第n-1帧图像对应的姿态参数预测值，预测在第n帧图像中目标物体的圆形特征的投影曲线。由于拍摄装置对目标物体进行拍摄时每帧图像之间的间隔时间非常短，帧率通常在25fps左右，因此在与上一帧图像中的目标物体相比当前第n帧图像中的目标物体的姿态变化很小，可以将上一帧(n-1)图像对应的下一帧(n帧)图像的姿态参数预测值作为当前第n帧的目标物体的姿态参数的初始估计。由于目标物体在现实的空间坐标系与拍摄得到的图像中的图像坐标系之间存在映射关系，根据初始估计的姿态参数预测第n帧图像中目标物体的圆形特征在现实空间坐标的位置，采用映射关系对圆形特征在现实空间坐标的位置进行转换，便可得到在第n帧图像中目标物体的圆形特征的投影曲线。避免目标物体的圆形特征自遮挡而导致投影方程拟合出错的问题，具备圆形特征遮挡的鲁棒性。
[0055]
在一个实施示例中，由于视频数据中每帧图像的图像坐标系与目标物体所在的空间坐标系具有映射关系，目标物体的姿态参数确定目标物体的圆形特征在空间坐标系的位置又与目标物体的姿态参数有关，则可以根据映射关系得到圆形特征对应在拍摄到的图像中的投影曲线与目标物体的姿态参数相关的表达式。在所述根据所述视频数据中第n-1帧图像对应的下一帧图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线之前，还包括：根据所述目标物体的空间坐标系与视频数据的图像坐标系之间的映射关系构建与姿态参数相关的姿态模型；对所述姿态模型进行圆形特征参数转换得到空间圆投影方程。
[0056]
具体的，根据目标物体的空间坐标系与视频数据的图像坐标系之间的映射关系构建与姿态参数相关的姿态模型，使得将确定的姿态参数输入姿态模型后就能够直接转换得到目标物体上任一点位置在图像中对应的像素点。由于圆形特征为空间坐标系的空间圆，空间圆能够由圆心的三维坐标、圆平面的法向量、以及圆半径这三个参数决定。因此，将圆心的三维坐标、圆平面的法向量、以及圆半径这三个参数作为圆形特征参数，将姿态模型中的参数转换为与圆形特征参数有关的参数得到空间圆在视频数据的图像坐标系上的投影方程，即空间圆投影方程。从而实现根据映射关系得到圆形特征对应在拍摄到的图像中的投影曲线与目标物体的姿态参数相关的表达式，减少在确定在所述第n帧图像中所述目标物体的圆形特征的投影曲线过程中的计算量。
[0057]
详细举例说明，设拍摄装置的焦距为f，像素纵横比为k，倾斜因子为s，相主点为u与v，因此拍摄装置的内参数矩阵定义为：在目标物体上建立目标坐标系即空间坐标系(object coordinate system,ocs)。目标物体上的圆形特征可通过如下圆形特征参数唯一确定圆形特征在空间坐标系的形状与位置：圆心的三维坐标c
o
、圆平面的法向量n
o
、圆半径r；上述三个参数也为圆形特征的形状参数。使用旋转矩阵r与平移向量t描述目标物体相对于图像坐标系即拍摄装置坐标系(camera coordinate system,ccs)的姿态，则根据所述目标物体的空间坐标系与视频数据的图像坐标系之间的映射关系构建与姿态参数相关的姿态模型为：其中，假设空间中某一点在ocs中的坐标为x，则x为x
在图像中对应的图像点；和分别为x和x的齐次坐标；符号表示差一个比例常数意义下相等。
[0058]
为了无歧义地表示目标的旋转，可以采用基于李群、李代数的旋转矩阵表示方法。设旋转向量为ω，则旋转矩阵r可表示为：其中，i为单位向量，[ω]
x
为由ω的分量构成的反对称矩阵根据上述给定的旋转矩阵r能够计算得到旋转向量ω为：
[0059]
||ω||＝arccos((trace(r)-1)/2),
[0060]
[ω]
x
＝||ω||/(2sin||ω||)(r-r
t
).
[0061]
因此，目标物体的姿态参数p可通过由旋转向量ω的三个分量ω1，ω2，ω3和平移向量t的三个分量构成，其中，p＝[ω
t
t
t
]
t
；即目标物体的姿态参数p为六自由度姿态参数。
[0062]
在图像坐标系中空间圆的圆心c为c＝rc
o
+t，空间圆的圆平面法向量n为n＝rn
o
。设f1和f2为垂直于n的两个正交单位向量，利用姿态模型与圆形特征参数可得到空间圆的投影方程，即空间圆上任何一点在图像坐标系中可表示如下：
[0063][0064]
其中，表示图像点x的齐次坐标；d＝c
t
n。
[0065]
在一个实施示例中，根据所述视频数据中第n-1帧图像对应的下一帧图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线的具体过程为：将姿态参数预测值代入预设的空间圆投影方程，得到目标物体的外表圆形特征投影在图像中的投影曲线方程。
[0066]
具体的，将上一帧(n-1)图像对应的下一帧图像的姿态参数预测值作为当前第n帧的目标物体的姿态参数的初始估计，然后将姿态参数初始预估代入上述空间圆投影方程，从而得到目标物体的外表圆形特征投影在第n帧图像中的投影曲线方程。
[0067]
s130、在所述边缘图像中查找属于所述投影曲线的像素点，得到若干边缘点。
[0068]
得到在第n帧图像中所述目标物体的圆形特征的投影曲线后，为确定边缘图像中目标物体的圆形特征对应的像素点(边缘点)，在视频数据中第n帧图像的边缘图像中查找属于第n帧图像中圆形特征的投影曲线的像素点，得到若干边缘点。
[0069]
在一个实施示例中，为提高计算速率，使得姿态参数估计算法具备实时性，可以通过直接最小化圆形特征的参数化投影曲线的几何残差，减少计算量。在所述边缘图像中查找属于所述投影曲线的像素点，得到若干边缘点的具体过程包括步骤21至步骤23：
[0070]
步骤21、在所述投影曲线中等间距采样若干个控制点；
[0071]
具体的，在得到的第n帧图像中目标物体的圆形特征的投影曲线上等间距采样若干点控制点{q
c
}。
[0072]
步骤22、以各个所述控制点为中心，在所述边缘图像中查找位于各个所述控制点的法线所在的预设范围内的像素点，得到各个所述控制点对应的候选边缘点；
[0073]
以采样得到的各个控制点为中心，在第n帧图像的边缘图像中查找位于各个控制
点的法线所在的预设范围内的像素点，得到各个控制点对应的候选边缘点m。
[0074]
步骤23、若任一所述控制点对应的任一候选边缘点满足预设条件，则确定所述候选边缘点为边缘点。
[0075]
若任一控制点对应的任一点候选边缘点m满足预设条件，则确定该候选边缘点m为该控制点的边缘点。其中，通过预设条件寻找满足距离控制点最近的以及相对于控制点的偏离角度位于最大角度范围内的一点候选边缘点为边缘点。
[0076]
在一个实施示例中，若任一所述控制点对应的任一候选边缘点满足预设条件，则确定所述候选边缘点为边缘点，包括：
[0077]
对于每一所述控制点对应的每一所述候选边缘点，若所述候选边缘点的法线与所述候选边缘点对应的控制点的法线之间的夹角小于预设阈值，则设定所述候选边缘点为预选边缘点；从每一所述控制点对应的所述预选边缘点中，确定与所述控制点距离最近的一个预选边缘点为边缘点。
[0078]
具体的，为从各个控制点对应的候选边缘点m中寻找满足距离控制点最近的以及相对于控制点的偏离角度位于最大角度范围内的一点候选边缘点为边缘点，预设条件需包含两个限定条件，条件一：候选边缘点的法线与所述候选边缘点对应的控制点的法线之间的夹角小于预设阈值，可选的该预设阈值为15度；条件二：确定满足条件一的所有预选边缘点中与对应的控制点距离最近的一个预选边缘点为边缘点。
[0079]
详细的，得到各个控制点对应的候选边缘点m后，对于每一控制点对应的各个候选边缘点，若任一候选边缘点的法线与该候选边缘点对应的控制点的法线之间的夹角小于预设阈值，则设定该候选边缘点为预选边缘点；从每一控制点对应的预选边缘点中，确定与该控制点距离最近的一个预选边缘点为边缘点。从而实现在视频数据中第n帧图像的边缘图像中查找属于第n帧图像中圆形特征的投影曲线的像素点，得到若干边缘点。
[0080]
s140、根据所述若干边缘点与所述投影曲线之间的几何残差，得到所述第n帧图像中所述目标物体的姿态参数估计值。
[0081]
每一边缘点y与该边缘点对应的投影曲线之间的几何残差的计算过程实际为计算该边缘点y与在该边缘点y对应的投影曲线上距离该边缘点y最近的点之间的距离。其中，在该边缘点y对应的投影曲线上距离该边缘点y最近的点满足如下条件：点处的切线垂直于连接y与的直线。精确的需要通过矩阵分解或者解一元四次方程的方法获得，因此不具备闭合的解析表达式。然而，在三维姿态跟踪过程中的一阶近似已经足够精确，因此使用的一阶近似估计，也就是辛普森近似，来近似地计算其计算公式如下
[0082][0083]
其中e是空间圆投影方程中的系数矩阵，e2是e的前两行构成的矩阵。最后计算边缘点y与在该边缘点y对应的投影曲线上距离该边缘点y最近的点之间的距离得到几何距离(几何残差)。根据步骤130得到的各个边缘点与各个边缘点对应的投影曲线之间的几何残差，计算得到第n帧图像中目标物体的姿态参数估计值。
[0084]
在一个实施示例中，为优化根据几何残差得到的姿态参数估计值，可以使用广义期望最大化原理降低优化问题的非线性程度，从而使算法具备实时性。根据所述若干边缘
点与所述投影曲线之间的几何残差，得到所述第n帧图像中所述目标物体的姿态参数估计值的具体过程包括步骤31至步骤32：
[0085]
步骤31、计算所述若干边缘点与所述投影曲线之间的几何距离的平方和，得到所述第n帧图像中所述目标物体的姿态参数估计值；
[0086]
第n帧图像中目标物体的姿态参数估计值可以通过最小化n个圆形特征对应的投影曲线与n个圆形特征对应的投影曲线对应的边缘点之间的几何残差平方和得到。设目标物体上有n个圆形特征，则每个圆形特征对应的投影曲线可以根据空间圆投影方程表示成姿态参数的闭合解析方程f
i
(x,p)＝0,i＝1,
…
,n,，姿态参数估计值的计算公式为：
[0087][0088]
其中，式中x
ij
为与f
i
(x,p)匹配的边缘点；d(f
i
(x,p),x
ij
)表示x
ij
与曲线f
i
(x,p)之间的几何距离，该几何距离被称为几何残差。具体的，几何残差为每一边缘点与该边缘点对应的投影曲线之间的几何距离。通过计算每一边缘点x
ij
与该边缘点对应的投影曲线f
i
(x,p)之间的几何距离得到d(f
i
(x,p),x
ij
)；将计算得到的几何距离代入上述目标物体的姿态参数估计值计算公式，计算得到第n帧图像中所述目标物体的姿态参数估计值。
[0089]
步骤32、采用广义期望最大化方法对所述姿态参数估计值进行优化得到姿态参数的极大似然估计值。
[0090]
为对上述姿态参数估计值的计算公式进行优化，以得到更精确的姿态参数估计值，可以采用广义期望最大化方法对所述姿态参数估计值进行优化得到姿态参数的极大似然估计值。具体的，姿态参数估计值的计算公式优化问题为最小二乘问题，首先使用m估计方法将上述姿态参数估计值的计算公式写成m估计公式：其中，d
ij
＝d(f
i
(x,p),x
ij
)；σ是d
ij
的方差；ρ是一个对称、正定、最小值为零的凸函数，被称为鲁棒估计子。由于ρ的形式通常比较复杂，因此直接优化上述m估计公式是非常困难的。可使用迭代加权最小二乘算法(irls)解决这一问题。设在经过k-1次迭代之后，我们得到了姿态参数的估计p
(k-1)
，从而将m估计公式优化问题转换为irls的第k次迭代问题。然后使用广义期望最大化方法(generalized expectation-maximization gem)解决irls的第k次迭代问题：
[0091]
上式中d
ij(k-1)
＝d2(f
i
(x,p
(k-1)
),x
ij
)，ω是权值函数。
[0092]
具体的，使用广义期望最大化方法解决irls的第k次迭代问题的过程为：设为在曲线f
i
(x,p
(k-1)
)上距离x
ij
最近的点，该点可通过公式计算。在ccs中，对应的三维空间点可通过辛普森近似得到。点在ocs中对应的坐标值为
[0093][0094]
因此根据空间坐标系与图像坐标系的映射关系，点可以表示为
[0095]
[0096]
其中k
12
是拍摄装置内参数矩阵k的第一、第二行构成的矩阵；是的第三个分量；t3是t的第三个分量。因此irls的第k次迭代问题可以改写成：
[0097][0098]
其中ω
ij(k-1)
＝ω(d
ij(k-1)
/σ
(k-1)
)为常数；l(p|{x
ij
},{ξ
ij
})被称为似然函数。我们将{x
ij
}看成观测数据，而将{ξ
ij
}看作缺失数据(missing data)，则视频数据中第n帧图像的姿态参数估计值p
(k)
可通过gem方法获得。gem分为两个步骤，分别为e步与m步。e步计算似然函数关于缺失数据的数学期望。已知姿态的初始估计值为p
(k-1)
，则利用公式(2)可唯一地确定缺失数据的估计值{ξ
ij(k-1)
}，因此ξ
ij
的边缘概率密度函数为狄利克雷函数
[0099]
h(ξ
ij
|x
ij
,p
(k-1)
)＝δ(ξ
ij-ξ
ij(k-1)
).
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0100]
所以，似然函数关于缺失数据的数学期望为
[0101][0102]
接下来，m步寻找一个新的姿态参数估计值p
(k)
，使得q(p
(k)
|{x
ij
})≤q(p
(k-1)
|{x
ij
})。可选的，使用levenberg-marquardt(lm)算法寻找p
(k)
。m步寻找到的满足q(p
(k)
|{x
ij
})≤q(p
(k-1)
|{x
ij
})条件的姿态参数估计值为姿态参数的极大似然估计值，确保得到的姿态参数的极大似然估计值一定收敛于局部极小点且具备实时性。
[0103]
s150、对所述姿态参数估计值进行数据平滑处理并根据所述姿态参数估计值对下一帧(n+1帧)图像中所述目标物体的姿态进行预测，得到姿态参数最终估计值以及所述下一帧图像的姿态参数预测值。
[0104]
目标物体在运动过程的自遮挡可能导致目标物体的外形信息上产生歧义，因此自遮挡可能使基于计算机视觉的姿态跟踪算法陷入局部极小点，最终丢失目标。克服这一影响的有效方法是根据第n帧图像的姿态参数估计值对目标物体在下一帧图像(下一时刻)的姿态参数进行预测，然后以该姿态参数预测值作为下一帧图像的目标物体的姿态参数的初始值。具体的，对第n帧图像的姿态参数估计值进行数据平滑处理，以进一步提高姿态参数估计值的精确度；然后根据第n帧图像的姿态参数估计值对下一帧(n+1)图像中目标物体的姿态进行预测，得到第n帧图像的姿态参数最终估计值以及下一帧(n+1)图像的姿态参数预测值。根据第n帧图像的姿态参数最终估计值就可得到在第n帧图像的采样时刻目标物体在空间坐标系的姿态。实现对当前第n帧图像的目标物体的姿态参数估计值的准确矫正和对下一帧图像中目标物体的姿态参数的精确预测。
[0105]
在一个实施示例中，可利用卡尔曼滤波对目标物体在下一帧图像(下一时刻)的姿态参数进行预测，然后以该姿态参数预测值作为下一帧图像的目标物体的姿态参数的初始值。对所述姿态参数估计值进行数据平滑处理并根据所述姿态参数估计值对下一帧图像中所述目标物体的姿态进行预测，得到姿态参数最终估计值以及所述下一帧图像的姿态参数预测值，具体过程包括：
[0106]
步骤41、根据所述姿态参数估计值得到姿态参数的协方差矩阵；
[0107]
根据第n帧图像的姿态参数估计值能够计算得到姿态参数的协方差矩阵。姿态参数的协方差矩阵对第n帧图像的姿态参数估计值的修正和对下一帧图像(下一时刻)的姿态参数的预测至关重要。基于优化理论的k.k.t.原理和多元函数的有限增量定理，给出了姿
态参数估计值协方差矩阵的一阶近似闭合表达式。因此当获得姿态参数估计值后，便可一步获得参数的协方差矩阵。
[0108]
详细的，设将m估计公式优化问题转换为irls的第k次迭代问题进行优化后，在第k
*
步收敛，所得的姿态参数为p
*
，此时公式(1)可改写成
[0109][0110]
由于此时m估计公式的目标函数达到极值点，p
*
必使公式(7)达到极值点。因此在p
*
处，公式(7)满足k.k.t.条件，由此可得姿态参数p
*
的协方差矩阵的一阶近似表达式
[0111][0112]
其中，
[0113][0114]
式中f
i*
＝f
i
(x,p
*
)；由空间圆投影方程可知：
[0115][0116]
其中{
□
}
12
为矩阵
□
的第一行与第二行构成的矩阵。可通过空间圆投影方程公式由链式法则求得。在计算时，最复杂的步骤为计算现将计算的方法概述如下。令ω＝[ω1ω2ω3]
t
，则：
[0117][0118]
根据r为正交矩阵的性质可知：
[0119][0120]
其中为矩阵的第i列。
[0121]
步骤42、采用卡尔曼滤波算法对所述姿态参数估计值和所述协方差矩阵进行计算，得到姿态参数最终估计值以及下一帧图像的姿态参数预测值。
[0122]
具体的，得到姿态参数的协方差矩阵后，采用二阶自回归模型的扩展卡尔曼滤波算法对姿态参数估计值和协方差矩阵进行计算，得到姿态参数最终估计值以及下一帧图像的姿态参数预测值。
[0123]
详细的，定义状态向量如下：
[0124]
s
t
＝[p
tt p
t-1t
]
t
＝[ω
tt t
tt ω
t-1t t
t-1t
]
t
,
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0125]
其中t为时刻索引，p
t-1
表示当前第n-1帧图像对应的姿态参数，n>2；p
t
表示第n帧图像对应的姿态参数；并且令p-1
＝p0。二阶自回归模型的扩展卡尔曼滤波的动态模型为
[0126][0127]
其中log
so(3)
(r
t
)通过以下公式计算旋转向量ω
t
得到：
[0128][0129]
γ
t
和ψ
t
分别表示旋转向量与平移向量的噪声向量，其均值为零。协方差矩阵分别为a2i
3 and b2i3的高斯白噪声。卡尔曼滤波的观测方程为：
[0130]
p
t
＝[i
6 06]s
t
+ε
t
,
ꢀꢀꢀ
(16)
[0131]
式中p
t
为当前第n帧图像对应的姿态参数估计值；06为六阶全零矩阵；ε
t
为观测噪声，其均值为零。协方差矩阵为v
t
，v
t
由公式(8)计算得到。公式(14)的雅克比矩阵为
[0132][0133]
其中，
[0134][0135]
式中m(ω)通过矩阵计算得到。至此，扩展卡尔曼滤波算法可通过公式(13)到公式(18)实现，流程综述如下：
[0136]
状态预测
[0137]
s
t+1|t
＝f(s
t|t
),
ꢀꢀꢀ
(19)
[0138]
其中f()表示公式14.
[0139]
误差的协方差预测
[0140][0141]
状态估计校正
[0142]
s
t+1|t+1
＝s
t+1|t
+k
t+1
[p
t+1-[i
6 06]s
t+1|t
].
ꢀꢀꢀ
(21)
[0143]
误差协方差估计校正
[0144]
p
t+1|t+1
＝p
t+1|t-k
t+1
[i
6 06]p
t+1|tt
.
ꢀꢀꢀ
(22)
[0145]
增益矩阵
[0146]
k
t+1
＝p
t+1|t
[i
6 06]
t
([i
6 06]p
t+1|t
[i
6 06]
t
+v
t
).
ꢀꢀꢀ
(23)
[0147]
采用公式(19)到公式(23)对第n帧图像的姿态参数估计值和协方差矩阵进行计算，得到姿态参数最终估计值以及下一帧图像的姿态参数预测值的具体过程包括：将第n帧图像的姿态参数估计值代入公式21得到姿态参数最终估计值。将得到的姿态参数最终估计值代入公式19，计算得到下一帧(n+1)图像的姿态参数预测值。
[0148]
本发明实施例提供的一种目标物体的六自由度姿态跟踪方法，通过提取视频数据中第n帧图像的边缘图像；所述视频数据为实时拍摄目标物体得到；所述目标物体的外形具有圆形特征；n>1；根据所述视频数据中第n-1帧图像对应的下一帧图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线；结合视频数据中n-1帧图像对应的下一帧图像的姿态参数预测值快速预测当前第n帧图像中目标物体的圆形特征的投影曲线，避免圆形特征被遮挡而导致投影方程拟合出错的问题，具备圆形特征遮挡的鲁棒性。在所述边缘图像中查找属于所述投影曲线的像素点，得到若干边缘点；根据所述若干边缘点与所述投影曲线之间的几何残差，得到所述第n帧图像中所述目标物体的姿态参数估计值；通过直接最小化圆形特征的参数化投影曲线的几何残差，获得目标物体的姿态参数估计值，保证了姿态估计的精度和对噪声的鲁棒性。对所述姿态参数估计值进行数据平滑处理并根据所述姿态参数估计值对下一帧图像中所述目标物体的姿态进行预测，得到姿态参数最终估计值以及所述下一帧图像的姿态参数预测值，实现对当前第n帧图像的目标物体的姿态参数估计值的准确矫正和对下一帧图像中目标物体的姿态参数的精确预测。
[0149]
实施例二
[0150]
如图3所示是本发明实施例二提供的目标物体的六自由度姿态跟踪方法的流程示意图。在实施例一的基础上，本发明实施例还提供了当n等于1或2时，第n帧图像中目标物体的姿态参数估计值的计算过程。
[0151]
对于视频数据中的第一帧图像，为拍摄得到的目标物体的初始图像。由于第一帧图像为初始时刻拍摄得到的图像，没有上一帧图像对应的目标物体的姿态参数预测值，为得到第一帧图像中目标物体的投影曲线需要将目标物体的圆形特征与第一帧图像进行匹配。在提取视频数据中第n帧图像的边缘图像之前，还包括第一帧图像中目标物体的姿态参数计算过程，包括以下步骤：
[0152]
s210、获取所述视频数据的第一帧图像，采用椭圆检测算法提取出所述第一帧图像中所述目标物体的圆形特征对应的特征像素点；
[0153]
当n等于1时，为了找到目标物体的圆形特征在视频数据的第一帧图像中对应的像素，使用椭圆检测算法检测出第一帧图像中所有椭圆和每个椭圆对应的特征像素。人为指定目标物体的圆形特征与检测出的椭圆的对应关系，从而得到第一帧图像中目标物体的圆形特征对应的特征像素点。
[0154]
s220、根据所述特征像素点确定在所述第一帧图像中所述目标物体的圆形特征的第二投影曲线；
[0155]
根据第一帧图像中目标物体的圆形特征对应的特征像素点，基于几何残差最小化的方法拟合曲线方程，从而得到第一帧图像中目标物体的m个圆形特征的第二投影曲线。
[0156]
s230、基于所述第一帧图像的第二投影曲线，得到所述第一帧图像中所述目标物
体的姿态参数的初始估计值；
[0157]
计算所有第二投影曲线的周长与其对应的特征像素点个数的比值，然后选择比值最大的两个第二投影曲线。设选出的两个投影曲线为e1和e2，他们对应的圆形特征c1和c2在空间坐标系的形状与位置分别为：圆心的三维坐标c
1o
，c
2o
、圆平面的法向量n
1o
，n
2o
、圆半径r1，r2。由投影几何理论可知，c1和c2的对偶二次曲面为对偶空间中的圆锥，圆锥的系数矩阵表示如下
[0158][0159]
令|w
1-λw2|＝i1λ4+i2λ3+i3λ3+i4λ+i5，式中|
□
|表示矩阵的行列式值，则c1和c2的绝对不变量为
[0160][0161]
根据计算机视觉理论可知，利用已知的照相机内参数k和半径r
i
，i＝1,2，从每个e
i
可以重建出两个空间圆c
i1
，c
i2
。因此从e1和e2可以获得4对可能的空间圆：(c
11
，c
21
)、(c
12
，c
21
)、(c
11
，c
22
)、(c
12
，c
22
)。接下来，我们需要从这四对空间圆中选出与(c1，c2)对应的一对。因此，我们利用公式(1)和(2)计算每一对空间圆的绝对不变量。在四个不变量中，与v最接近的不变量对应的空间圆对即为与(c1，c2)对应的空间圆对，记为(c
1*
，c
2*
)。设c
1*
和c
2*
的位置参数分别为：圆心的三维坐标c1，c2；圆平面的法向量n1，n2，又设目标物体的旋转矩阵和平移向量分别为r
10
和t
10
，因此可以得到如下表达式
[0162][0163]
使用单位四元数表示旋转矩阵r
10
，则通过奇异值分解可从公式(3)的第三个方程中解出旋转矩阵r
10
。然后利用最小二乘法从公式(3)的前两个方程中解出平移向量t
10
。利用下述公式可将r
10
转换成旋转向量ω
10
[0164][0165]
因此p
10
＝[ω
10t
t
10t
]
t
为目标姿态参数的初始估计。
[0166]
s240、根据所述第一帧图像的姿态参数初始估计值，确定在所述第一帧图像中所述目标物体的圆形特征的第三投影曲线；
[0167]
s250、对所述每一条第三投影曲线，在所述对应的特征像素点中查找属于所述第三投影曲线的像素点，得到若干第三边缘点；
[0168]
为确定第一帧图像中目标物体的圆形特征的第三投影曲线对应的像素点(边缘点)，在视频数据中第一帧图像的所述特征像素点中查找属于第一帧图像中圆形特征的第三投影曲线的像素点，得到若干第三边缘点。
[0169]
s260、根据所述若干第三边缘点与所述第三投影曲线之间的几何残差，得到所述第一帧图像中所述目标物体的姿态参数估计值。
[0170]
每一第三边缘点y与该边缘点对应的第三投影曲线之间的几何残差的计算过程实际为计算该第三边缘点y与在该第三边缘点y对应的第三投影曲线上距离该第三边缘点y最近的点之间的距离。其中，在该第三边缘点y对应的第三投影曲线上距离该第三边缘点y最近的点满足如下条件：点处的切线垂直于连接y与的直线。精确的需要通过矩阵分解或者解一元四次方程的方法获得，因此不具备闭合的解析表达式。然而，在三维姿态跟踪过程中的一阶近似已经足够精确，因此使用的一阶近似估计，也就是辛普森近似，来近似地计算其计算公式如下
[0171][0172]
其中e是空间圆投影方程中的系数矩阵，e2是e的前两行构成的矩阵。最后计算第三边缘点y与在该第三边缘点y对应的第三投影曲线上距离该第三边缘点y最近的点之间的距离得到几何距离(几何残差)。根据步骤130得到的各个第三边缘点与各个第三边缘点对应的第三投影曲线之间的几何残差，计算得到第一帧图像中目标物体的姿态参数估计值。
[0173]
第一帧图像中所述目标物体的姿态参数估计值可以通过最小化m个圆形特征对应的投影曲线与m个圆形特征对应的边缘点之间的几何残差平方和得到。设目标物体上有m个圆形特征，则每个圆形特征对应的第三投影曲线可以根据空间圆投影方程表示成姿态参数的闭合解析方程f
i
(x,p)＝0,i＝1,
…
,n,，姿态参数估计值的计算公式为：
[0174][0175]
其中，式中x
ij
为与f
i
(x,p)匹配的边缘点；d(f
i
(x,p),x
ij
)表示x
ij
与曲线f
i
(x,p)之间的几何距离，该几何距离被称为几何残差。具体的，几何残差为每一第三边缘点与该第三边缘点对应的第三投影曲线之间的几何距离。通过计算每一第三边缘点x
ij
与该第三边缘点对应的第三投影曲线f
i
(x,p)之间的几何距离得到d(f
i
(x,p),x
ij
)；将计算得到的几何距离代入上述目标物体的姿态参数估计值计算公式，计算得到第一帧图像中所述目标物体的姿态参数估计值。
[0176]
在一个实施示例中，由于当n等于1时，仅计算得到第一帧图像中目标物体的姿态参数估计值，因此直接将第一帧图像对应的姿态参数估计值作为下一帧图像的目标物体的姿态参数预测值。当n等于2时，所述根据所述视频数据中第n-1帧图像对应的下一帧图像的姿态参数预测值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线，包括：根据所述视频数据中第一帧图像对应的下一帧图像的姿态参数估计值，预测在所述第n帧图像中所述目标物体的圆形特征的投影曲线。
[0177]
实施例三
[0178]
图4是本发明实施例三提供的终端设备的结构示意图。该终端设备包括：处理器41、存储器42以及存储在所述存储器42中并可在所述处理器41上运行的计算机程序43，例如用于目标物体的六自由度姿态跟踪方法的程序。所述处理器41执行所述计算机程序43时实现上述目标物体的六自由度姿态跟踪方法实施例中的步骤，例如图1所示的步骤s110至
s150。
[0179]
示例性的，所述计算机程序43可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述存储器42中，并由所述处理器41执行，以完成本申请。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序43在所述终端设备中的执行过程。例如，所述计算机程序43可以被分割成边缘图像提取模块、投影曲线确定模块、边缘点确定模块、姿态参数估计值计算模块和姿态参数最终估计值和下一帧图像的姿态参数预测值确定模块，各模块具体功能如下：
[0180]
边缘图像提取模块，用于提取视频数据中第n帧图像的边缘图像；所述视频数据为实时拍摄目标物体得到；所述目标物体的外形具有圆形特征；n>2；
[0181]
投影曲线确定模块，用于根据所述视频数据中第n-1帧图像对应的下一帧(n帧)图像的姿态参数预测值，确定在所述第n帧图像中所述目标物体的圆形特征的投影曲线；
[0182]
边缘点确定模块，用于在所述边缘图像中查找属于所述投影曲线的像素点，得到若干边缘点；
[0183]
姿态参数估计值计算模块，用于根据所述若干边缘点与所述投影曲线之间的几何残差，得到所述第n帧图像中所述目标物体的姿态参数估计值；
[0184]
姿态参数最终估计值和下一帧图像的姿态参数预测值确定模块，用于对所述姿态参数估计值进行数据平滑处理并根据所述姿态参数估计值对下一帧(n+1帧)图像中所述目标物体的姿态进行预测，得到姿态参数最终估计值以及所述下一帧图像的姿态参数预测值。
[0185]
所述终端设备可包括，但不仅限于，处理器41、存储器42以及存储在所述存储器42中的计算机程序43。本领域技术人员可以理解，图4仅仅是终端设备的示例，并不构成对终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线等。
[0186]
所述处理器41可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
[0187]
所述存储器42可以是所述终端设备的内部存储单元，例如终端设备的硬盘或内存。所述存储器42也可以是外部存储设备，例如终端设备上配备的插接式硬盘，智能存储卡(smart media card，smc)，安全数字(secure digital，sd)卡，闪存卡(flash card)等。进一步地，所述存储器42还可以既包括终端设备的内部存储单元也包括外部存储设备。所述存储器42用于存储所述计算机程序以及目标物体的六自由度姿态跟踪方法所需的其他程序和数据。所述存储器42还可以用于暂时地存储已经输出或者将要输出的数据。
[0188]
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可
以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
[0189]
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。