CN105180904A - 基于编码结构光的高速运动目标位姿测量方法 - Google Patents

Abstract

本发明基于编码结构光的高速运动目标位姿测量方法属于计算机视觉测量技术领域，涉及一种大视场小目标高速运动物***姿测量方法。测量方法采用彩色投影仪向测量区域投射含有编码信息的彩色栅线，使用高速摄像机连续拍摄经运动物体表面调制而变形的结构光编码栅线图案；经过数字图像解码后，实现成像平面与投影平面的对应点匹配，获取相位变化信息，由***结构参数反算出高速运动物体的三维动态形貌；通过将测量点与基于待测物几何参数建立的先验模型进行匹配，最终准确地测量出高速运动物体的位置、姿态信息。本发明采用彩色条纹伪随机序列与灰度栅线相位相结合的方式，有效解决了立体视觉中的匹配难题。测量***成本低，机构简单、操作简易。

Description

基于编码结构光的高速运动目标位姿测量方法

技术领域

本发明属于计算机视觉测量技术领域，涉及一种大视场内非合作高速运动目标空间位置和姿态的测量方法。

背景技术

物***姿信息在航空航天、机器人导航以及汽车工业领域都有着十分重要的地位，为了保证在各种工况下的目标位姿实时可控，使得对物***姿进行测量是十分必要的，这也对物***姿测量技术提出了很多新的要求。特别针对大视场高速运动目标位姿测量时，为保证真实还原工况，不对目标物做任何处理的情况下，快速准确地测量目标物位姿信息是现阶段所要解决的主要问题。

目前利用结构光投影技术测量高速运动物***姿的研究较少，多数利用视觉测量配合结构光完成静态尺寸测量，而不能对运动目标进行位姿测量。高学海等2012年在宇航学报发表的《非合作大目标位姿测量的线结构光视觉方法》中提出了一种利用单目摄像机结合投射在大目标上的激光矩形特征进行对接位姿测量和控制的方法，但该方法只针对大目标且物体运动变化不大时的情况，仍不能解决大视场、小目标高速位姿测量。

发明内容

本发明要解决的技术难题是克服现有技术的缺陷，发明一种基于编码结构光的高速运动目标位姿测量方法，该方法采用单目高速摄像机与彩色投影仪组成的高速测量***进行大视场高速运动目标位姿测量，解决了传统测量方法无法在非合作目标表面安装特征标记点而影响测量的可达性难题；利用单目结构光投影三维测量技术结合彩色编码栅线，通过基于先验模型的匹配运算，实现了在不对高速运动目标物做任何处理的条件下，位姿信息的高精度动态测量；与双目位姿测量***相比结构简易，降低了设备投入，扩大了测量视场；解决了在大测量视场、非合作目标高速运动工况下，准确、快速测量目标运动位姿信息的问题。

本发明所采用的技术方案是一种基于编码结构光的高速运动目标位姿测量方法，采用单目摄像机，其特征是，测量方法采用彩色投影仪向测量区域投射含有编码信息的彩色栅线，使用单目高速摄像机连续拍摄经运动物体表面调制而变形的结构光编码栅线图案；经过数字图像解码后实现成像平面与投影平面的对应点匹配，获取相位变化信息，由***结构参数反算出高速运动物体的三维动态形貌；通过将测量点与基于待测物几何参数建立的先验模型进行匹配，最终准确地测量出高速运动物体的位置、姿态信息；测量方法的具体步骤如下：

1、彩色栅线编码设计

采用一种彩色条纹伪随机序列与灰度栅线相位相结合的彩色栅线空域编码图案。由于灰度栅线图案的相位信息周期规律难以在单幅图像中恢复，而彩色条纹伪随机序列空域编码在对条纹位置编码时充分利用了条纹自身及其周边的色彩信息，可实现单幅图像动态测量。将两种方法相结合，利用色彩编码解决单幅图案周期定位问题，同时保留灰度栅线相位编码的高分辨率优点，在单幅图像中构成像素级别唯一编码。

2、彩色栅线图像解码

彩色栅线图像解码即求解每一像素点对应的有效完全相位φ(x,y)：包括获取相展开，获取周期序列号n和相主值折叠在0～2π中两部分，如公式(1)所示。

采用由粗到细的分步彩色栅线图像解码技术，在粗识别阶段根据周期边界及色彩信息划分周期区域进行相展开，在细分阶段根据局部区域亮度余弦变化规律进行空域解相(解算相主值)。具体解码过程如下：

1)由RGB表示模式转化为HIS表示模式

根据各颜色通道亮度大小及差值比例关系将RGB表示模式转换为HIS表示模式，将色彩信息与亮度信息分离。

R、G、B分别表示RGB模式下某颜色在三个通道的色调值，色调值H对应彩色栅线图案中的六种颜色，分六种情况计算。用于获取颜色信息从而确定周期序列号，色调的计算如公式(2)所示：

$\left\{\begin{array}{c}\max (R,G,B)=R,\min (R,G,B)=B,H=(G-B)/(R-B)\\ \max (R,G,B)=G,\min (R,G,B)=B,H=2-(R-B)/(G-B)\\ \max (R,G,B)=G,\min (R,G,B)=R,H=2+(B-R)/(G-R)\\ \max (R,G,B)=B,\min (R,G,B)=R,H=4-(G-R)/(B-R)\\ \max (R,G,B)=B,\min (R,G,B)=G,H=4+(R-G)/(B-G)\\ \max (R,G,B)=B,\min (R,G,B)=G,H=6-(B-G)/(R-G)\end{array}\right.---\left(2\right)$

将求得的色调定义为六种色彩，当H值处于0～0.5或者5.5～6区间时为红色，处于0.5～1.5区间时为黄色，处于1.5～2.5区间时为绿色，处于2.5～3.5区间时为青色，处于3.5～4.5区间时为蓝色，处于4.5～5.5区间时为品红色，6个颜色区间分别对应周期序列号0-5。

亮度值I余弦变换规律用于相主值的求解。其计算公式为：

$I=\frac{R+G+B}{3}---\left(3\right)$

2)对彩色栅线图像进行相展开

在彩色栅线编码的伪随机序列中，只要提取出高亮度中心边界和其左右相邻两个周期高亮度中心边界色彩信息，即可确定中心边界对应的周期顺序号，实现周期解码。即解算出完全相位中的n值。

3)对彩色栅线图像进行相主值解算

接下来通过等步长相移法对彩色栅线图像进行局域空域解相，即对相主值进行求解。假设连续5个像素对应相同的物体表面反射率r，环境光分量a，条纹调制幅度b和基本畸变相位每两个像素间的相位步长均为2ε。则有：

其中：

反正切值在0～π/2之间。待入测量亮度值进行求解展开至0～2π之间，取两者平均值解出最终根据公式(1)实现完全解相。

3、高速运动物体三维动态形貌解算

经过彩色栅线图像的解码后，即可获取即时的相位变化信息，通过预先标定好的测量***参数，反算出高速运动物体测量表面高度信息，进而得到三维动态形貌，测量原理见图2，计算公式如下：

$AB=pl\·\left({\mathrm{\φ}}_A\right(x,y)-{\mathrm{\φ}}_B(x,y\left)\right)/2\mathrm{\π},h(x,y)=\frac{AB\·L}{AB+d}---\left(7\right)$

投影光线照射到参考面A点，成像平面上对应的像素点其相位为φ_A(x,y)，放上被测物体后，像素点对应的是投影至物体表面D点的光线，该光线投影至参考平面的B点，相位为φ_B(x,y)。d为投影出瞳P和摄影入瞳C的距离，L为摄影入瞳C与参考面的距离，pl为参考面上投影光栅的周期长度。距离AB包含了高度信息h(x,y)，而该距离可由两点间的相位差计算。

通过对彩色光栅图像解码，解算出测量表面的高度信息，最终获得高速运动物体的三维动态形貌。

4、基于先验模型的三维测量点匹配与位姿求解，

采用一种基于先验模型的测量点匹配技术，以实现高速运动物***置、姿态参数的精确求解；分为预估阶段和匹配求解两个阶段：

1)预估阶段

①在预校准前，首先通过定义两个指标V和K_u，对起始测量数据点集的可靠度进行预估。

V＝λ_max(Cov(P))(8)

其中

P＝{p_i}(9)

表示将要进行预校准的测量点数据集。

Cov(P)＝E[(p-μ)(p-μ)^T](10)

μ＝E[p](11)

K_u定义为

$K_u=\frac{1}{V^2}\left\{\frac{1}{N_p}\underset{i=1}{\overset{N_p}{\Σ}}{(p_i-\mathrm{\μ})}^4\right\}-3---\left(12\right)$

N_p表示集合P中点的数量。V表示P的特征矩阵的主要特征值，反应点的分布情况。如果有很多点沿着一个特定的方向分布，V将会取大值，并且相应的特征向量表示出点分布的主要方向。K_u是表示点集分布集中化程度的一个指标，当K_u＝0时表明符合正态分布，这样的点分布是可靠的。结合这两个指数，定义具有相对较大的V值和相对较小的K_u值的测量点分布是可靠的。

第一预估阶段将依次循环直至在时序测量点集中找到第一个可信点集并定义为P₁。那么，对起始测量数据点集(即P₁，P₂)的预估结果存在两种可能性：如果P₁和P₂均被认定为可靠点集，那么就用Cov(P₁)、Cov(P₂)的特征向量和模型点集X与测量点集P₁的中心点，分别对X的姿态与位置进行预校准；如果P₂在第二个预估阶段被认定为不可靠，那么预校准阶段将会采用在第一个时序图像P₁中获取的相对位置与姿态信息作为估计值对先验模型点集X进行预校准。

②先验模型的前表面模型建立。通过预校准时所使用的信息建立摄像机可视部分的先验模型点集代替完整的先验模型点集，可以有效的提高匹配算法的可靠性与精度。这一局部先验模型称为前表面模型。

2)匹配求解阶段

完成总流程中的预校准与前表面模型的建立后，利用所得数据进行匹配与位姿求解。P^(m)为测量数据点集，X^(m)为位置与姿态经预校准的前表面先验模型数据点集，上标m表示循环次数。其中，最优的平移变换矩阵T^(m)和旋转变换矩阵R^(m)通过基于特征值问题的封闭求解算法直接进行估算，得到表示平移变换矩阵T的相对位置的和表示旋转变换矩阵R的四元数矩阵该算法将一直循环下去，直至达到收敛条件E≤T_E或者循环次数达到预先设定的阈值m≤T_m，最终得到该时刻目标物体的相对位置与相对姿态信息。

通过执行上述计算过程直到k＝k_max，即完成对所有时序图像对应的测量数据点集与先验模型点集的匹配，解算出每一时刻高速运动物体的相对姿态与相对位置进而从时序图象中获取高速运动物体的位姿状态信息。

本发明的有益效果是在大测量视场下，利用彩色栅线图案在非合作工况下对高速运动目标表面不作任何处理就可准确、快速地求取其位姿信息；采用彩色条纹伪随机序列与灰度栅线相位相结合的彩色栅线空域编码图案，克服了时序编码结构光三维测量动态性能差的问题；测量***成本低，机构简单、操作简易，有效解决了立体视觉法中的匹配难题。

附图说明

图1为测量方法原理图。其中，1-高速摄像机，2-彩色投影仪，3-彩色栅线，4-彩色栅线变形部分，5-高速运动目标物体，6-图形工作站。

图2为结构光三维形貌测量原理图。A-投影光线照射到参考面上的一个点，D-放上被测物体后对应着成像平面上相同点的投影光线照射到物体表面上的点，B-移去物体后通过D点的光线照射在参考面上的点，P—出瞳，C—入瞳，d—投影出瞳P和摄影入瞳C的距离，L—摄影入瞳C与参考面的距离，h—D点到参考平面的距离。O—两条光轴在参考平面上的焦点。

图3为匹配与位姿求解算法流程图。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

本发明采用一种彩色条纹伪随机序列与灰度栅线相位相结合的彩色栅线空域编码图案；由于灰度栅线图案的相位信息周期规律难以在单幅图像中恢复，而彩色条纹伪随机序列空域编码在对条纹位置编码时充分利用了条纹自身及其周边的色彩信息，在单幅图像中构成像素级别唯一编码，可实现单幅图像动态测量。

附图1为基于彩色编码结构光的高速运动物***姿测量方法原理图，采用高速摄像机1采集高速运动目标物体5以及彩色栅线3的图像，将采集图像传递给图形工作站6，利用由于被测目标物体5而产生的彩色栅线变形部分4产生的相位差，构建目标物三位动态模型。经与先验模型进行匹配，最终求出目标物的位置、姿态信息。首先安装测量装置，将高速摄像机1、投影仪2固定，调整焦距使得距离摄像机600mm的焦平面公共视场大小为1m×1m。在距离摄像机1m的背景平面上投射彩色栅线3。将相机1与图形工作站6相连，准备进行测量。本发明采用带有广角镜头的高速摄像机1拍摄物体运动情况，高速摄像机型号为FASTCAMSA5摄像机，广角镜头型号为AF-S17-35mmf/2.8DIF-ED。拍摄条件如下：高速摄像机帧频为3000fps，图片像素为1024×1024，镜头焦距为17mm，物距为750mm，视场为800mm×800mm。本实施例测量方法的具体步骤如下：

1.彩色栅线编码设计

本实施例选用3原色：红(255，0，0)，绿(0，255，0)，蓝(0，0，255))及其反色：青(0，255，255)，品红(255，0，255)，黄(255，255，0))6种彩色条纹构成3次伪随机序列。每个条纹宽度为一个栅线周期，且要求相邻色彩不同，任意连续三个周期色彩排列具有唯一性。将序列的色彩信息融入栅线图，即每个余弦周期对应一种色彩，其亮度在该色彩对应的有效的RGB通道中变化。图案亮度变化规律如公式(13)所示。

其中：x,y为图案像素坐标，x同时也是竖直栅线的一维坐标，T为周期宽度，avei为平均亮度，r为亮度变化幅值，两者均取255/2。下标C代表有效的颜色通道，每个栅线周期可能在RGB三个颜色通道中的一个或者两个中有效。

2.彩色栅线图像解码

根据各颜色通道亮度大小及差值比例关系将RGB表示模式转换为HIS表示模式。利用色彩信息色调值H进行相展开，完成周期解码，获得周期序列号n；亮度值I代入公式(4)，解算出相主值将周期序列号n、相主值代入公式(1)，计算出每一像素点对应的有效完全相位φ(x,y)。

3.高速运动物体三维动态形貌解算

将解算出的完全相位与初始相位代入公式(7)，实现测量平面内像素点对应高度信息的建立。对所有采集到的一系列动态测量图像进行高度信息重建后，即获得了高速运动物体运动过程中的三维动态形貌。测量原理见图2，投影光线照射到参考面上的一个点A，放上被测物体后对应着成像平面上相同点的投影光线照射到物体表面上的点为D，D点到参考平面的距离为h，移去物体后通过D点的光线照射在参考面上的点为B，投影出瞳P和摄影入瞳C的距离为d，摄影入瞳C与参考面的距离为L，两条光轴在参考平面上的焦点为O。

4.基于先验模型的三维测量点匹配

采用一种基于先验模型的测量点匹配技术，以实现高速运动物***置、姿态参数的精确求解。匹配与位姿求解算法的流程图如图3所示。基于先验模型的三维测量点匹配与位姿求解，分为预估阶段和匹配求解两个阶段。测量数据点集与先验模型数据点集的预校准，通过预校准可以提高匹配算法的可靠性。首先根据尝试错误法反复试验，确定匹配算法中预估环节判断测量数据点集可信度的阈值V与K_u，得到V≥25000,K_u≤1.9，将测量得到的数据点集代入公式(8)、(12)求得V、K_u。按流程图(3)所示，其中，k为拍摄得到图像经三维形貌解算得到的测量点点集的时序编号，k＝1定义为第一组经预估后判定为可信的测量点点集，依次类推。首先按对测量点的可靠性进行两次预评估，进而将测量得到的数据点集与按照预校准时所使用的信息建立的前表面先验模型带入匹配程序，完成三维测量点与先验模型的匹配。根据先验模型的结构参数，从时序图像对应的三维形貌数据点集中获取高速运动物体的相对位置与姿态信息。经图形工作站按照算法流程解算得到位姿信息见下表：

时间/s	偏航角/o	俯仰角/o	滚转角/o	X位移/mm	Y位移/mm	Z位移/mm
							0.00028	5.250	62.112	4.061	110.16	63.29	299.17
0.00052	5.215	62.253	4.212	110.10	63.16	299.13
							0.00076	5.290	61.332	4.092	110.52	63.12	299.05
0.00101	5.302	61.674	4.049	110.78	63.10	299.01
							0.00112	5.276	62.162	4.108	110.32	63.05	298.94
0.00153	5.295	62.116	4.124	110.34	62.95	298.85
							0.00176	5.367	62.321	3.915	110.22	62.82	298.81
0.00203	5.296	61.852	4.110	110.08	62.80	298.76
							0.00225	5.242	61.726	4.102	109.63	62.73	298.74
0.00250	5.265	61.693	4.125	109.61	62.63	298.63
							0.00275	5.211	61.974	4.110	109.49	62.54	298.57
0.00301	5.202	62.774	4.001	109.04	62.43	298.53
							0.00324	5.262	62.633	4.124	109.03	62.41	298.33
0.00351	5.313	61.652	4.026	108.67	62.35	298.21

本发明利用彩色编码结构光投影三维测量技术，结合基于先验模型的匹配运算，实现了在不对高速运动目标物做任何处理的条件下，目标位姿信息的高精度非合作动态测量。弥补了结构光投影三维测量技术应用于动态位姿测量时分辨率低，精度差，测量效率低的缺点；具有设备简单，成本低，操作便利的优点；解决了在大测量视场下，对高速运动目标物进行非合作、高可靠性、高精度、快速测量其运动位姿信息的问题。

Claims (1)

1.一种基于编码结构光的高速运动目标位姿测量方法，采用单目高速摄像机和投影仪，其特征是，测量方法采用彩色投影仪向测量区域投射含有编码信息的彩色栅线，使用高速摄像机连续拍摄经运动物体表面调制而变形的结构光编码栅线图案；经过数字图像解码后，实现成像平面与投影平面的对应点匹配，获取相位变化信息，由***结构参数反算出高速运动物体的三维动态形貌；通过将测量点与基于待测物几何参数建立的先验模型进行匹配，最终准确地测量出高速运动物体的位置、姿态信息；测量方法的具体步骤如下：

1、彩色栅线编码设计

采用一种彩色条纹伪随机序列与灰度栅线相位相结合的彩色栅线空域编码图案，在单幅图像中构成像素级别唯一编码；

2、彩色栅线图像解码

彩色栅线图像解码即求解每一像素点对应的有效完全相位φ(x,y)：包括获取相展开，获取周期序列号n和相主值折叠在0～2π中两部分，如公式(1)所示；

采用由粗到细的分步彩色栅线图像解码技术，在粗识别阶段根据周期边界及色彩信息划分周期区域进行相展开，在细分阶段根据局部区域亮度余弦变化规律进行空域解相，即解算相主值；具体解码过程如下：

1)由RGB表示模式转化为HIS表示模式

根据各颜色通道亮度大小及差值比例关系将RGB表示模式转换为HIS表示模式，将色彩信息与亮度信息分离；

R、G、B分别表示RGB模式下某颜色在三个通道的色调值，色调值H对应彩色栅线图案中的六种颜色，分六种情况计算；用于获取颜色信息，从而确定周期序列号，色调的计算公式如下：

$\left\{\begin{array}{c}\max (R,G,B)=R,\min (R,G,B)=B,H=(G-B)/(R-B)\\ \max (R,G,B)=G,\min (R,G,B)=B,H=2-(R-B)/(G-B)\\ \max (R,G,B)=G,\min (R,G,B)=R,H=2+(B-R)/(G-R)\\ \max (R,G,B)=B,\min (R,G,B)=R,H=4-(G-R)/(B-R)\\ \max (R,G,B)=B,\min (R,G,B)=G,H=4+(R-G)/(B-G)\\ \max (R,G,B)=R,\min (R,G,B)=G,H=6-(B-G)/(R-G)\end{array}\right.---\left(2\right)$

将求得的色调定义为六种色彩，当H值处于0～0.5或者5.5～6区间时为红色，处于0.5～1.5区间时为黄色，处于1.5～2.5区间时为绿色，处于2.5～3.5区间时为青色，处于3.5～4.5区间时为蓝色，处于4.5～5.5区间时为品红色，6个颜色区间分别对应周期序列号0-5；

亮度值I余弦变换规律用于相主值的求解；其计算公式为：

$I=\frac{R+G+B}{3}---\left(3\right)$

2)对彩色栅线图像进行相展开

在彩色栅线编码的伪随机序列中，只要提取出高亮度中心边界和其左右相邻两个周期高亮度中心边界色彩信息，即可确定中心边界对应的周期顺序号，实现周期解码；即解算出完全相位中的周期序列号n值；

3)对彩色栅线图像进行相主值解算

通过等步长相移法对彩色栅线图像进行局域空域解相，即对相主值进行求解；假设连续5个像素对应相同的物体表面反射率r，环境光分量a，条纹调制幅度b和基本畸变相位每两个像素间的相位步长均为2ε；则有：

其中：

反正切值在0～π/2之间；待入测量亮度值进行求解展开至0～2π之间，取两者平均值解出最终根据公式(1)实现完全解相；

3、高速运动物体三维动态形貌解算

通过预先标定好的测量***参数，反算出高速运动物体测量表面高度信息，进而得到三维动态形貌，计算公式如下：

$\begin{array}{cc}AB=pl\·\left({\mathrm{\φ}}_A\right(x,y)-{\mathrm{\φ}}_B(x,y\left)\right)/2\mathrm{\π},& h(x,y)=\frac{AB\·L}{AB+d}\end{array}---\left(7\right)$

投影光线照射到参考面A点，成像平面上对应的像素点其相位为φ_A(x,y)，放上被测物体后，像素点对应的是投影至物体表面D点的光线，该光线投影至参考平面的B点，相位为φ_B(x,y)；d为投影出瞳P和摄影入瞳C的距离，L为摄影入瞳C与参考面的距离，pl为参考面上投影光栅的周期长度；距离AB包含了高度信息h(x,y)，而该距离可由两点间的相位差计算；解算出测量表面的高度信息，最终获得高速运动物体的三维动态形貌；

4、基于先验模型的三维测量点匹配与位姿求解

分为预估阶段和匹配求解两个阶段：

1)预估阶段

①通过测量数据点集与先验模型数据点集的预校准，提高匹配算法的可靠性；在预校准前，首先定义两个指标V和K_u，V表示P的特征矩阵的主要特征值，反应点的分布情况；K_u是表示点集分布集中化程度的一个指标，对起始测量数据点集的可靠度进行预估；

V＝λ_max(Cov(P))(8)

其中

P＝{p_i}(9)

表示将要进行预校准的测量点数据集；

Cov(P)＝E[(p-μ)(p-μ)^T](10)

μ＝E[p](11)

K_u定义为

$K_u=\frac{1}{V^2}\left\{\frac{1}{N_p}\underset{i=1}{\overset{N_p}{\Σ}}{(p_i-\mathrm{\μ})}^4\right\}-3---\left(12\right)$

N_p表示集合P中点的数量；如果有很多点沿着一个特定的方向分布，V将会取大值，并且相应的特征向量表示出点分布的主要方向；当K_u＝0时表明符合正态分布，这样的点分布是可靠的；结合这两个指数，定义具有相对较大的V值和相对较小的K_u值的测量点分布是可靠的；

第一预估阶段将依次循环直至在时序测量点集中找到第一个可信点集，并定义为P₁；对起始测量数据点集(P₁，P₂)的预估结果存在两种可能性：如果P₁和P₂均被认定为可靠点集，就用Cov(P₁)、Cov(P₂)的特征向量和模型点集X与测量点集P₁的中心点，分别对X的姿态与位置进行预校准；如果P₂在第二个预估阶段被认定为不可靠，那么预校准阶段将会采用在第一个时序图像P₁中获取的相对位置与姿态信息作为估计值对先验模型点集X进行预校准；

②先验模型的前表面模型建立；通过预校准时所使用的信息建立摄像机可视部分的先验模型点集代替完整的先验模型点集，有效的提高匹配算法的可靠性与精度；这一局部先验模型称为前表面模型；

2)匹配求解阶段

完成总流程中的预校准与前表面模型的建立后，利用所得数据进行匹配与位姿求解；P^(m)为测量数据点集，X^(m)为位置与姿态经预校准的前表面先验模型数据点集；上标m表示循环次数；其中，最优的平移变换矩阵T^(m)和旋转变换矩阵R^(m)通过基于特征值问题的封闭求解算法直接进行估算，得到表示平移变换矩阵T的相对位置的和表示旋转变换矩阵R的四元数矩阵该算法将一直循环下去，直至达到收敛条件E≤T_E或者循环次数达到预先设定的阈值m≤T_m，最终得到该时刻目标物体的相对位置与相对姿态信息；

通过执行上述计算过程直到k＝k_max，即完成对所有时序图像对应的测量数据点集与先验模型点集的匹配，解算出每一时刻高速运动物体的相对姿态与相对位置进而从时序图象中获取高速运动物体的位姿状态信息。