用于平面三自由度位姿测量的高速微视觉追踪方法及系统

1.本发明属于密定位平台的位姿高速视觉追踪领域，尤其涉及用于平面三自由度位姿测量的高速微视觉追踪方法及系统。


背景技术：

2.随着信息技术的发展和计算机视觉技术的广泛应用，人们能够利用图像处理技术来实现对运动目标的实时监测和跟踪。所谓视觉追踪，就是指对图像序列中的运动目标进行检测，提取、识别和跟踪，获得运动目标的运动参数(如位置、速度、加速度等)，以及运动轨迹，从而进行进一步处理与分析，实现对运动目标的追踪。现在该项技术广泛应用于在人机交互、智能交通、智能机器人等各领域中。
3.基于模板匹配的目标追踪方法是现有视觉追踪算法中的一种，其本质就是以待跟踪目标为模板图像在下一帧图像中进行模板匹配，以最佳匹配位置作为目标跟踪结果。模板更新策略是基于模板匹配的目标追踪中有一个重要研究内容，其目的在于使目标模板信息与图像信息变化尽可能保持同步，适应相机运动、目标自身运动或者其他噪声对图像造成的几何或者灰度畸变，从而提高目标追踪精度。通过模板匹配获得最佳匹配位置后，即在该位置截取一定大小的子区域作为更新后的模板，进行下一帧的模板匹配跟踪。这种模板一定程度上降低了模板失效的风险，但是每一次更新的模板都包含了当前模板匹配中的误差，随着这种误差的逐帧累积，最后导致跟踪漂移的出现。
4.因此，文献(li h,zhang x,zhu b,et al.online precise motion measurement of 3-dof nanopositioners based on image correlation[j].ieee transactions on instrumentation and measurement,2018,pp:1-9.)为了解决迭代过程中目标模板更新所产生的误差积累，实现高效、高精度、高带宽的目标，提出了以在第一帧中选取的目标区域为模板，并且以差平方和函数为匹配准则，以高斯牛顿迭代法为优化算法，同时结合逆向合成法对迭代过程中的扭曲函数进行更新，从而实现高速视觉追踪。文献(li h,zhang x,yao s,et al.an improved template-matching-based pose tracking method for planar nanopositioning stages using enhanced correlation coefficient[j].ieee sensors journal,2020,pp(99):1-1.)针对微视觉感知中存在的测量精度与测量范围相互抑制问题及计算效率低问题进行研究，提出了基于增强差平方和匹配准则的追踪方法，并以ecc算子为基础，对求解算法进行推导，通过结合平面欧式射影变换及模板匹配，将多自由度运动问题转化为多变量参数优化问题。在使用逆向合成式高斯牛顿优化算法的基础上，为进一步提高优化效率及收敛精度，提出了一种优化模板选取策略和一种分段惩罚加速方案。解决由于目标函数复杂度提升而导致的计算效率下降问题，该方案在角度测量方面精度更高，并且能够实现对三自由度精密定位平台的有效追踪。
[0005]
但上述两种方法都没有从算法迭代的次数考虑，没有简化算法迭代的过程，因此也就不能提高相机追踪的帧率。


技术实现要素：

[0006]
为了解决上述问题，本发明提出用于平面三自由度位姿测量的高速微视觉追踪方法，因为卡尔曼滤波可以将目标追踪问题转换为系统状态后验概率密度的估计问题，当输入是白噪声产生的随机信号时，卡尔曼滤波可以使期望输出和实际输出之间的均方差达到最小。同时利用卡尔曼滤波器的预测功能，预测运动目标在下一帧中的位置，将图像全局搜索问题转换为局部搜索，减少了搜索空间，加快匹配速度，提高了系统的实时性。使用该方法进行高速视觉追踪不仅可以克服多传感器组合测量方案中的众多缺点，而且兼具可视化和为微纳定位平台全闭环反馈提供新的思路，可以扩展微纳定位平台的使用范围，可以有效提高追踪过程中的采样频率和追踪精度，为后续精密定位平台的全反馈提供新的方法。
[0007]
本发明至少通过如下技术方案之一实现。
[0008]
用于平面三自由度位姿测量的高速微视觉追踪方法，包括以下步骤：
[0009]
s1、采集精密运动平台的运动信息，并选取图片信息作为目标区域进行追踪；
[0010]
s2、根据对运动平台的运动规划得到追踪目标的运动状态表达式，根据运动状态表达式得出卡尔曼滤波的状态转移矩阵和控制矩阵，利用卡尔曼滤波来得到当前时刻状态的最优估计值，再根据前一状态值预测当前值，同时预测状态协方差矩阵：
[0011]
s3、根据当前时刻状态的最优估计值，得到当前时刻的扭曲函数更新值，并将追踪过程转化为非线性最优化问题；
[0012]
s4、利用模板匹配得到每次迭代过程的运动参数：
[0013]
s5、在扭曲函数更新之后，得到目标模板在该帧中的位置，找到目标图像在每帧中的位置后实现目标的追踪。
[0014]
进一步地，步骤s2包括以下步骤：
[0015]
根据状态转移矩阵由前一状态值预测当前值，同时预测状态协方差矩阵：
[0016][0017][0018]
根据视觉的测量值得到追踪目标在相机坐标系中的位置，更新系统状态值以及卡尔曼增益和状态协方差：
[0019][0020][0021][0022]
式中，表示当前时刻状态的最优估计值，表示状态的先验预测值，表示状态的先验协方差矩阵，a表示状态转移矩阵，q表示状态转移协方差矩阵，h表示观测矩阵，r表示观测噪声方差矩阵，zk表示观测值，k表示卡尔曼增益，b表示控制矩阵，pk表示状态协方差矩阵，uk表示系统的控制矩阵，表示预测状态的协方差矩阵，i表示单位矩阵。
[0023]
进一步地，用w定义坐标系o
t-x
′y′
和坐标系o
i-xy之间的扭曲变换函数；对于平面欧几里得变换，将扭曲函数表示为：
[0024]
[0025]
式中：w(p；x
′
)表示变换后的目标位置坐标的矩阵，p表示位姿变化向量，x
′
表示变化前图像的坐标位置；θ表示图像变换旋转的角度；t
x
和ty表示图像变换过程中的位移矢量；w
x
与wy表示图像变换后在x轴和y轴的坐标。
[0026]
进一步地，在步骤s3中，根据当前时刻的最优估计值得出当前时刻的基于模板匹配进行视觉追踪的扭曲函数在迭代开始时更新的初值，将追踪过程转化为非线性最优化问题，追踪目标在当前时刻的目标函数为：
[0027][0028]
p＝(t
x t
y θ)
t
[0029]
其中为匹配标准，p是参数向量，θ表示图像变换旋转的角度；t
x
和ty表示图像变换过程中的位移矢量；通过目标函数的优化得到迭代过程中的位姿变化向量p，通过卡尔曼滤波预测的初值得到，t
xk
＝x(1,1),t
yk
＝x(2,1),θk＝x(3,1)，x分别代表卡尔曼滤波所获得状态变量所表示的矩阵；t
xk
和t
yk
表示状态变量中目标位置在x轴和y轴的位移；θk表示状态变量中目标位置变化的旋转角度；i(w(p；x
′
))表示图像变化后的目标区域灰度值，t(x
′
)表示选取模板区域的灰度值；ω表示图像上所有点的集合。
[0030]
进一步地，选择逆向合成法完成模板匹配，实现对目标函数进行优化求解，得到在第k次迭代的目标函数表示为：
[0031][0032]
其中，h(w(δpk；x
′
))表示模板区域的灰度值；g(w(pk；x
′
))表示子图像区域的灰度值；是模板区域的图像灰度值均值；为子图像区域的灰度值均值；δh表示模板区域灰度值和灰度值均值的范数；δg为目标子区域灰度值和灰度值均值的范数。
[0033]
进一步地，根据每次迭代的参数δpk得到每次迭代过程的δt
xk
、δt
xy
、δθk，则扭曲函数按如下方式进行更新：
[0034][0035]
式中：表示扭曲函数的复合过程，δt
xk
、δt
xy
、δθk分别为目标区域移动在x轴和y轴的位移矢量和旋转角度，x
′
表示图像上的点，w(pk；x
′
)是扭曲函数，w-1
(δpk；x
′
)为逆向扭曲参数矩阵增量。
[0036]
进一步地，利用基于灰度值的模板匹配来完成目标的追踪过程，根据目标函数优化，用逆向合成的方法来完成模板匹配的过程，得到每次迭代过程的运动参数：
[0037][0038]
式中，g(w(pk；x
′
))表示子图像区域的灰度值，是是模板区域的图像灰度值均值；δh表示模板区域灰度值和灰度值均值的范数，为子图像区域的灰度值均值，j表示雅可比矩阵，其表达式表示为：
[0039][0040]
其中w
x
与wy表示图像变换后在x轴和y轴的坐标，和是当t＝1时，即选取的第一帧图像中的目标区域的模板的梯度，和是关于p的扭曲函数w的雅可比矩阵，p表示位姿变化向量；
[0041][0042][0043][0044][0045]
其中δg为目标子区域灰度值和灰度值均值的范数，n表示模板区域的像素个数；g(w(p；x
′
))表示子区域像素的灰度值，h(x
′
)是目标模板区域像素的灰度值向量，g(w(pk；x
′
))是迭代后图像区域像素的灰度值。
[0046]
进一步地，末端运动平台的运动规划是做加速度为a的匀加速直线运动，末端运动平台的状态变量x＝[x v]，状态转移矩阵控制矩阵观测矩阵h＝[1 1]；δt是图像采样间隔，x代表目标位置在x轴的位移，v表示目标图像的运动速度，其中w(k)与v(k)为互不相关的均值为零的白噪声序列，满足：
[0047][0048]
其中qk和rk分别为w(k)和v(k)的协方差矩阵，δ
kj
是克罗内克函数，即
[0049][0050]
其中w(j)、v(j)表示均值为零的白噪声序列，k和j只是代表自变量。
[0051]
进一步地，精密定位平台的输出(x,y,θ)与电容传感器的位移关系如下：
[0052][0053]
式中：u1、u2、u3分别为三个电容传感器的测量值，d为1号和2号电容之间的距离；精
密定位平台做角速度为ω的匀速圆周运动，运动的状态变量为：x＝[x y v
x vy]
t
，其中，x和y分别代表目标位置在x轴和y轴方向的位移，v
x
和vy分别代表目标位置在x轴和y轴方向的速度。相应的运动规划的状态转移矩阵和控制矩阵如下：
[0054][0055][0056]
观测矩阵为：δt是图像采样间隔，其中w(k)与v(k)为互不相关的均值为零的白噪声序列，满足：
[0057][0058]
其中qk和rk分别为w(k)和v(k)的协方差矩阵，δ
kj
是克罗内克函数，即
[0059][0060]
实现所述的用于平面三自由度位姿测量的高速微视觉追踪方法的系统，包括隔振台、镜筒、精密运动装置、工业相机、成像系统定位平台、标志图案、镜头固定滑台、同轴光源、显微镜头；所述工业相机与镜筒连接，工业相机采集数据传给计算机进行处理，镜筒设置在镜头固定滑台上，显微镜头与镜筒连接，镜头固定滑台安装于成像系统定位平台上，成像系统定位平台设置在隔振台，标志图案位于精密运动装置上，并且镜筒位于标志图案的上方，便于信息采集，精密运动装置固定在隔振台上，同轴光源在显微镜头上。
[0061]
与现有的技术相比，本发明的有益效果为
[0062]
(1)在以模板匹配为基础的视觉追踪过程中，增加了卡尔曼滤波技术，利用其预测功能，预测追踪目标在下一帧中的位置，将图像全局搜索问题转换为局部搜索，减少了搜索空间，加快匹配速度，提高了系统的实时性。
[0063]
(2)在优化求解算法的过程中，与早期研究中使用的ssd算子相比，本发明采用了鲁棒性更好的ecc算子对算法进行推导,该方法可以减少追踪过程所需的平均迭代时间，而且追踪的误差明显减少，该方法可以明显提高精密定位平台位姿高速视觉追踪性能。
附图说明
[0064]
图1为实施例用于平面三自由度位姿测量的高速微视觉追踪方法结构流程示意图；
[0065]
图2为实施例用于平面三自由度位姿测量的高速微视觉追踪系统结构图；
[0066]
图3为实施例用于平面三自由度位姿测量的高速微视觉追踪系统总体结构图；
[0067]
图4是实施例一种用于平面三自由度位姿测量的高速微视觉追踪方法的流程示意图；
[0068]
图中示出：1-隔振台、2-镜筒、3-精密运动装置、4-工业相机、5-成像系统定位平台、6-标志图案、7-镜头固定滑台、8-同轴光源、9-显微镜头。
具体实施方式
[0069]
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0070]
实施例1
[0071]
本技术的一种基于运动预测与迭代优化的精密定位平台位姿高速视觉追踪方法，如图1所示，包括：采集精密运动平台的运动信息，运动信息的预处理过程，精密运动平台的运动规划，卡尔曼滤波预测下一帧中追踪目标的最优估计值，基于模板匹配完成视觉追踪。具体地，包括以下步骤：
[0072]
s1、调整精密运动装置，使其末端运动平台的原点与基座原点保持重合，且两者都处在零点处；
[0073]
s2、调整相机定位平台上面的滑台使镜头下的标志图案清晰；
[0074]
s3、工业相机获取标志图案的初始位置图像信息并传输至计算机，并且选取大小合适的模板作为目标追踪的对象；
[0075]
s4、根据精密运动平台的理论运动轮廓，并将其转换成脉冲信号传递给电机控制器，使精密运动平台产生相应的运动轨迹；
[0076]
s5、根据理论运动状态，利用卡尔曼滤波来得到当前时刻状态的最优估计值得出过程如下：
[0077]
在追踪平台搭载完成之后，对相机标定完成之后，采集标志图案的第一帧位置，并选取合适的图片信息作为目标区域进行追踪。其次，根据对运动平台的运动规划可以求出追踪目标的运动状态空间表达式：
[0078]
x
k+1
＝axk+bu
[0079]
其中u表示系统的控制矩阵，xk表示系统在k时刻的状态变量
[0080]
根据运动空间表达式，可以得出卡尔曼滤波的状态转移矩阵a和控制矩阵为b，根据这些参数可以根据前一状态值预测当前值，同时预测状态协方差矩阵：
[0081][0082][0083]
根据视觉的测量值更新系统状态值以及卡尔曼增益和状态协方差：
[0084][0085][0086]
[0087]
式中，表示当前时刻状态的最优估计值，表示状态的先验预测值，表示状态的先验协方差矩阵，a表示状态转移矩阵，q表示状态转移协方差矩阵，h表示观测矩阵，r表示观测噪声方差矩阵，zk表示观测值，k表示卡尔曼增益，b表示控制矩阵，pk表示状态协方差矩阵，uk表示系统的控制矩阵，表示预测状态的协方差矩阵，i表示单位矩阵。
[0088]
在卡尔曼滤波完成目标状态的预测完成之后，获得当前时刻状态的最优估计值，即可得到目标模板的位置(t
xk
＝x(1，1)，t
yk
＝x(2，1)，θk＝x(3，1))，此时，扭曲函数可以更新为：
[0089][0090]
根据当前时刻的最优估计值得出当前时刻的基于模板匹配进行视觉追踪的扭曲函数在迭代开始时更新的初值，将追踪过程转化为非线性最优化问题，追踪目标在当前时刻的目标函数为：
[0091][0092]
其中为匹配标准，而p＝(t
x t
y θ)
t
是参数向量，通过卡尔曼滤波预测的初值得到，t
xk
＝x(1，1)，t
yk
＝x(2，1)，θk＝x(3，1)，x分别代表卡尔曼滤波所获得状态变量所表示的矩阵；t
xk
和t
yk
表示状态变量中目标位置在x轴和y轴的位移；θk表示状态变量中目标位置变化的旋转角度；i(w(p；x
′
))表示图像变化后的目标区域灰度值；t(x
′
)表示选取模板区域的灰度值；ω表示图像上所有点的集合。
[0093]
用w定义坐标系o
t-x
′y′
和坐标系o
i-xy之间的扭曲变换函数。对于平面欧几里得变换，可以将扭曲函数表示为：
[0094][0095]
对上述所求的目标函数进行优化求解，主要包括一下几个步骤：
[0096]
1、对于基于区域灰度的视觉追踪，一般选用灰度模板匹配，常用的匹配函数有差平方和函数(ssd)、差绝对值和函数(sad)、归一化互相关函数(ncc)、归一化ssd等。
[0097]
2、对于上述的匹配函数，其都是属于非线性优化问题。而优化计算的求优方法是按照一定的逻辑结构进行反复的数值计算，寻求目标函数数值不断下降的设计点，知道获得足够的精度为止。
[0098]
3、综合比较正向叠加法、正向合成法、逆向合成法，本实施例选择逆向合成法对目标函数进行优化处理。该方法与另两种方法相比计算过程更加简单，模板的梯度、雅可比矩阵、海塞矩阵都只需计算一次，不用重复计算，极大减少计算量，减少迭代时间。
[0099]
选用逆向合成法对上述算法进行优化，可以得到在第k次迭代的目标函数可以表示为：
[0100][0101]
其中，δpk表示迭代完成后所得到的位移矢量和旋转角度；表示目标函数；h(w(δpk；x
′
))表示目标子区域的灰度值；g(w(pk；x
′
))表示模板区域的灰度值；是目
标子区域的图像灰度值均值；为模板区域的灰度值均值；δh表示目标子区域灰度值和灰度值均值的范数；δg为模板区域灰度值和灰度值均值的范数。
[0102]
利用模板匹配来完成目标的追踪过程，根据所述的根据ecc算子来优化算法(目标函数)，并用逆向合成的方法来完成模板匹配的过程，结果可以得到每次迭代过程的运动参数：
[0103][0104]
式中：j表示雅可比矩阵，，其表达式可以表示为：
[0105][0106]
其中和是当t＝1时，即选取的第一帧图像中的目标区域的模板的梯度，和是关于p的扭曲函数w的雅可比矩阵。
[0107]
h(x
′
)是目标模板区域像素的灰度值向量，g(w(pk；x
′
))是子图像区域像素的灰度值向量，
[0108][0109]
根据上述所求δpk的表达式，可以得到每次迭代过程的δt
xk
、δt
xy
、δθk，扭曲函数就可以按如下方式进行更新，其过程如下：
[0110][0111]
式中：表示扭曲函数的复合过程，δt
xk
、δt
xy
、δθk分别表示上述步骤3中的迭代结果，为目标区域移动在x轴和y轴的位移矢量和旋转角度。
[0112]
s6、在扭曲函数更新之后，就可以得到目标模板在该帧中的位置，找到目标图像在每帧中的位置后，就可以实现目标追踪的过程。
[0113]
实施例2
[0114]
如图2所示，本实施例提供一种基于宏动精密运动平台的高速视觉追踪装置，包括隔振台1、镜筒2、精密运动装置3、工业相机4、成像系统定位平台5、标志图案6、镜头固定滑台7、同轴光源8、显微镜头9；所述工业相机4与镜筒2通过螺纹连接，工业相机4通过以太网将采集数据传给计算机进行处理，镜筒2用螺栓固定在镜头固定滑台7上，显微镜头9通过螺纹连接与镜筒2连接，镜头固定滑台7通过螺栓固定于成像系统定位平台5上，成像系统定位平台5通过螺栓连接于隔振台1，标志图案6固定在精密运动装置3上，并且镜头2位于标志图案6的上方，便于信息采集，精密运动装置3通过螺栓固定在隔振台1上。
[0115]
在成像模块中，面阵相机选用高速数据传输接口，镜头选用可支持同轴光源的低畸变远心镜头或显微镜头。实时追踪算法中，将迭代优化模板追踪与运动预测相结合，通过
卡尔曼滤波方法估计当前最优状态并预测下一时刻的状态，从而减少不同图像帧之间的优化初值偏差与迭代次数，实现高速成像下的高精度实时图像处理。
[0116]
在本实例中，工业相机为flare cl系列的4m180mcl，分辨率为2048
×
2048，像素尺寸为5.5μm
×
5.5μm；镜头型号为0.06x goldtl远心镜头，整体成像系统的放大倍率为0.06倍，理论测量范围为187.73mm
×
187.73mm。单自由度往复运动平台的运动范围设置为80mm，该参数在视觉系统的视场范围内，符合设计要求。
[0117]
在该实例中，往复平台的运动规划为加速度为a的匀加速直线运动，因此该状态变量可以用x＝[x v]表示，其状态转移矩阵为：控制矩阵为：观测矩阵为：h＝[1 1]；δt是图像采样间隔，其中w(k)与v(k)为互不相关的均值为零的白噪声序列，满足：
[0118][0119]
其中qk和rk分别为w(k)和v(k)的协方差矩阵，δ
kj
是克罗内克函数，即
[0120][0121]
由上述公式即可完成卡尔曼滤波的预测功能，后续基于模板匹配的视觉追踪过程与实施例1相似。
[0122]
实施例3
[0123]
如图3所示，本实例的区别与实例2不同之处在于精密运动平台的结构，实例3的精密运动平台的结构三自由度的柔顺机构，该结构由压电陶瓷驱动，具有x-y-z三轴的运动能力，每个轴的最大行程为100μm，分辨率可达0.2nm。而实例3所装备的视觉系统：相机为flare cl系列的2m360mcl,其分辨率为2048
×
1088，像元尺寸为5.5μm
×
5.5μm；镜头型号为来自日本的mitutoyo放大倍数为20的物镜，其与来自美国的navitar的镜筒组合，用来自navitar的适配器连接镜筒和相机。该系统的理论测量范围为563.2μm
×
299.2μm。由于该柔顺机构的单轴最大行程为100μm，该值小于本实例设计的视觉系统的视场范围。
[0124]
该精密定位平台的输出(x,y,θ)与电容传感器的位移关系如下：
[0125][0126]
式中：u1、u2、u3分别为三个电容传感器的测量值，d为1号和2号电容之间的距离。该精密定位平台做角速度为ω的匀速圆周运动，运动的状态变量为：x＝[x y v
x vy]
t
，因此，得到该运动规划的状态转移矩阵和控制矩阵如下：
[0127][0128][0129]
观测矩阵为：δt是图像采样间隔，其中w(k)与v(k)为互不相关的均值为零的白噪声序列，满足：
[0130][0131]
其中qk和rk分别为w(k)和v(k)的协方差矩阵，δ
kj
是克罗内克函数，即
[0132][0133]
由上述公式即可完成卡尔曼滤波的预测功能，后续基于模板匹配的视觉追踪过程与实施例1相似。本发明可用于精密测量领域，通过选配不同型号的相机与镜头并结合所提算法，可实现平面耦合三自由度运动的实时高精度检测，测量范围与测量分辨率的比值可达104～105，追踪频率可到1000hz以上。
[0134]
以上仅为本发明的实施例，并非因此限制本发明的保护范围，凡是利用本发明说明书内容所作为的等效结构，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的保护范围内。