CN106197263A - 基于机器视觉的多体投放位姿测量方法 - Google Patents

Abstract

本发明基于机器视觉的多体投放位姿测量方法属于计算机视觉测量技术领域，涉及一种适用于多投放物同时投放的位姿测量方法。测量方法先建立各投放物单体特征点先验模型，对各单体进行区分编号。对多视觉测量图像进行处理，对每帧图像中的所有特征点进行运算对特征点进行分组划分刚体，利用先验模型对组内特征点进行比对识别，利用单体中各特征点距离约束识别单体中各特征点。最后，利用坐标转换方式求得各帧图像中各投放物位姿参数。该方法通过刚体标记信息的约束，有效避免了由于视觉测量中各投放物相互干扰而引起的测量中断问题，可同时对多运动目标进行跟踪，有效的解决了多体投放物位姿测量问题。

Description

基于机器视觉的多体投放位姿测量方法

技术领域

本发明属于计算机视觉测量技术领域，涉及一种适用于多投放物同时投放的位姿测量方法。

背景技术

在多种领域中，多体同时投放技术的应用越来越多。由于现代技术的不断进步，对多体投放位姿的测量要求也不断的提高。尤其在高速多体投放中，安全，高效、高精度的获得投放各单体的位姿参数十分重要。针对这种测量需求，视觉测量由于其高频响、高精度成为非接触式测量中的首选。但由于多体同时投放的特殊性，必将导致视觉测量应用过程中单体间特征点干涉问题，这将导致测量无法进行。因此，在存在干涉情况下进行多体位姿测量是现阶段要解决的主要难题。

目前，北京航天航空大学的张弘等人所申请的发明专利公开号为CN101419055，“一种基于视觉的空间三维位姿的测量方法与装置”中提出的测量方法：提取运动物体表面的三个标记点并建立坐标系，通过求解建立坐标系与空间坐标系的位置关系从而得到被测物体相对世界坐标系的位姿信息。但是并不能同时测量多个运动物体的位姿信息，并且当多物体互相遮挡时会影响测量的可达性。南京航天航空大学的李静等人申请的发明专利CN102620656A“一种航天器交会对接相对位姿测量方法”提出将双目视觉测量方法与普吕克直线方程相结合，进行航天器交会对接相对位姿的测量。该方法通过两条异面直线在两坐标系下的相对位置关系解算出两个航天器的相对位姿信息。但是无法测量多个目标的滚转物体的位置姿态信息。

发明内容

本发明要解决的技术难题是克服现有技术的缺陷，发明一种基于机器视觉的多体投放物位姿测量方法，根据刚体特征不变性以及先验模型对各目标体特征点进行识别分组，以区分各目标体特征点，建立图像特征点与目标物实物特征点的对应关系，最终利用坐标转换方式求解各个单体投放物投放过程位姿信息。该方法通过刚体特征点的约束，有效避免了由于视觉测量中各投放物相互干扰而引起的测量中断问题，可同时对多运动目标进行跟踪，有效的解决了多体投放物位姿测量问题。

本发明采用的技术方案是一种基于机器视觉的多体投放物位姿测量方法，其特征是，测量方法首先建立各投放物单体特征点先验模型，对各单体进行区分编号；然后对多视觉测量图像进行处理，对每帧图像中的所有特征点进行运算对特征点进行分组划分刚体，利用先验模型对组内特征点进行比对识别，利用单体中各特征点距离约束识别单体中各特征点；最后利用坐标转换方式求得各帧图像中各投放物位姿参数，该方法的具体提取步骤如下：

第一步：先验模型建立

首先要建立多投放物表面特征点的先验知识模型，每个单体内各特征点的距离信息所组成的向量被称为此单体的先验知识模型；单体的先验知识模型如下式所示：

A_I＝[a_I1 ... a_IN] (1)

其中：A_I为第I个投放物编号，a_I1为第I个投放物中第1个标记的距离先验知识信息，a_IN为第I个投放物中第N个标记的距离先验知识信息；以此建立所有投放物刚体的先验知识模型；单体内先验模型为：

a_Ii＝[L_i1 ... L_in] (2)

其中：a_Ii为第I个刚体第i个特征点先验知识模型，L_i1为第i个特征点与单刚体内第1个特征点间距离，L_in为第i个特征点与单刚体内第n个特征点间距离；由此建立所有投放物刚体内先验模型；

第二步：基于时空关系的图像匹配方法

根据光流方法找到同名标记点在不同时刻的图像坐标，并且绘制出同名标记点在一段时间内的运动轨迹；

得到目标物表面标记点的轨迹之后，将左右图像的轨迹进行匹配；匹配的方法如下所示：

1)首先对左图像轨迹起始点在右图像起始点中进行极限约束匹配，找到满足极限约束条件的右图像轨迹；

2)再对左图像轨迹不同时刻与上步中筛选的右图像轨迹同一时刻点进行匹配，直到找到唯一一个与左图像匹配的右图像运动轨迹；

3)最后左右图像两条轨迹中同一时刻的标记点即为匹配点。

第三步：基于刚体不变性的多投放物体间特征区分

求出图像特征点的三维坐标后，比较这些空间点的相对关系，寻找在时间变化情况下空间关系一直不变的特征点，进行分组；具体过程首先建立空间所有点与其他标记点之间距离向量，如下所示：

$G_i=\left[\begin{array}{ccc}{l_{i1}}^1& \mathrm{...}& {l_{in}}^1\\ & .& \\ & .& \\ & .& \\ {l_{i1}}^t& \mathrm{...}& {l_{in}}^t\end{array}\right]---\left(3\right)$

其中：G_i的第t行是空间第i个标记点与其他空间标记点在时刻t下的距离信息向量，l_i1 ¹是第一时刻下第i个点与第一个点的距离，l_in ^t是第t时刻下第i个点与第n个点的距离；以矩阵G_i中在时间序列图像中位置关系一直不变的特征点为一组，从而完成多体分离图像特征点分组；最后利用先验模型方法对各分组内特征点进行组内识别，从而建立图像特征点与目标实物特征点的对应关系；

第四步：单投放物内各特征点识别

将不同被测物体上标记点进行区分之后，要对每个刚体内的每个标记点进行识别，识别方法是首先建立同一个刚体内标记点的距离测量向量，建立方法如下：

g_Jj＝[l_j1 ... l_jm] (4)

其中,l_jm是第J组第个j特征点与第J组第m个特征点的测量值，g_Jj是第J组第j个特征点的距离测量向量；与先验模型a_Ii(i＝1:n)比对如下：

min||l_Jj-a_Ii||＝min{||l_Jj-L_I1||,||l_Jj-L_I2||…||l_Jj-L_In||},i＝1～n (5)

$h(g_{Ii},a_{Ij})=\left[\begin{array}{c}\min \left|\right|l_{I1}-L_{In}\left|\right|\\ \min \left|\right|l_{I2}-L_{In}\left|\right|\\ .\\ .\\ .\\ \min \left|\right|l_{\mathrm{Im}}-L_{\mathrm{Im}}\left|\right|\end{array}\right],i=1~n;j=1~m---\left(6\right)$

其中：h(g_Jj,a_Ii)是第J组第j点与第I组第i点的距离差值，当值最小时两点匹配；目标体G_Jj与目标体A_Ii距离差值：

$H(G_J,A_I)=||\begin{array}{c}M(g_{J1},a_{Ii})\\ M(g_{J2},a_{Ii})\\ .\\ .\\ .\\ M(g_{Jm},a_{Ii})\end{array}||---\left(7\right)$

其中：H(G_J,A_I)是目标体J与目标体I的距离差值，M(g_Jj,a_Ii)是h(g_Jj,a_Ii)的最小值，当H(G_J,A_I)值最小时，两目标体匹配，完成图像与先验模型的匹配识别；

第五步：坐标转换求取位姿参数

用以识别的特征点建立单个投放物局部坐标系O_tX_tY_tZ_t为：世界坐标系中任意点p_ow＝(x_p y_p z_p)^T在局部坐标系O_tX_tY_tZ_t下的坐标为p_ow＝(x_t y_t z_t)^T:

$\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right]=R_{wb}\left[\begin{array}{c}{x}_p-x_0\\ y_p-y_0\\ z_p-z_0\end{array}\right]---\left(8\right)$

其中：T_wb＝(x₀ y₀ z₀)为目标体坐标系原点即被测物体质心在世界坐标系中的坐标；R_wb为局部坐标系相对于世界坐标系的旋转矩阵；

$\begin{array}{c}{R}_{wb}=\\ \left[\begin{array}{ccc}\cos \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_Z\right)-\sin \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\sin \left({\mathrm{\θ}}_Z\right)& \cos \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_Z\right)+\sin \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\cos \left({\mathrm{\θ}}_Z\right)& -\sin \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_X\right)\\ -\cos \left({\mathrm{\θ}}_X\right)\sin \left({\mathrm{\θ}}_Z\right)& \cos \left({\mathrm{\θ}}_X\right)\cos \left({\mathrm{\θ}}_Z\right)& \sin \left({\mathrm{\θ}}_X\right)\\ \sin \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_Z\right)+\cos \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\sin \left({\mathrm{\θ}}_Z\right)& \sin \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_Z\right)-\cos \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\cos \left({\mathrm{\θ}}_Z\right)& \cos \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_X\right)\end{array}\right]\end{array}---\left(9\right)$

其中,-θ_Z，-θ_X，-θ_Y为投放物相对于世界坐标系的偏航角，俯仰角和滚转角，求得R_wb，T_wb即单个投放物对于世界坐标系的位姿信息，以此求得所有投放物位姿信息；

本发明的有益效果是在测量过程中，利用刚体的不变性，对多刚体进行刚体间区分，在以区分的刚体中利用各特征点的距离约束对各特征点进行识别，能够有效避免多刚体投放所引起的干涉问题，以实现多体同时投放的高效、高精度的非接触位姿测量。

附图说明

图1为基于机器视觉的多体投放位姿测量方法原理图。其中，1‐多体投放物、2‐单体标记点组、3‐双目视觉测量系统。

图2为位姿测量方法流程图。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。附图1为基于机器视觉的多体投放位姿测量方法原理图。根据刚体特征不变性以及先验模型对各目标体特征点进行识别分组，以区分各目标体特征点，建立图像特征点与目标物实物特征点的对应关系，最终利用坐标转换方式求解各个单体投放物投放过程位姿信息。

第一步：先验模型建立

本实施例采用3刚体同步投放，每个刚体采用3个标记点，因此根据公式(1)建立刚体间特征点先验模型如下所示：

A₁＝[37.76 44.23 25.86]

A₂＝[19.45 49.77 47.95]

A₃＝[34.87 54.31 68.54]

根据公式(2)刚体内先验模型如下所示：

$\left\{\begin{array}{c}{a}_{11}=\left[\begin{array}{cc}44.23& 25.86\end{array}\right]\\ a_{12}=\left[\begin{array}{cc}37.76& 25.86\end{array}\right]\\ a_{13}=\left[\begin{array}{cc}37.76& 44.23\end{array}\right]\end{array}\right.$

$\left\{\begin{array}{c}{a}_{21}=\left[\begin{array}{cc}49.77& 47.95\end{array}\right]\\ a_{22}=\left[\begin{array}{cc}19.45& 47.95\end{array}\right]\\ a_{23}=\left[\begin{array}{cc}19.45& 49.77\end{array}\right]\end{array}\right.$

$\left\{\begin{array}{c}{a}_{31}=\left[\begin{array}{cc}54.31& 68.54\end{array}\right]\\ a_{32}=\left[\begin{array}{cc}34.87& 68.54\end{array}\right]\\ a_{33}=\left[\begin{array}{cc}34.87& 54.31\end{array}\right]\end{array}\right.$

第二步：多体投放物刚体区分以及单投放物内各特征点识别

对图片中所有信息标记进行处理，求得所有特征点间距离；按先验模型距离数量对所有测量所得特征点距离进行分组，建立距离向量，与单体间先验模型进行对比，取与先验模型向量范数小于阀值的测量特征点组为与先验模型相应的单体，以此对所有测量所得距离向量进行区分，识别各单体标号；

第三步：坐标转换求取位姿参数

根据坐标系转换公式(6)、(7)对视频图像进行处理获得位姿参数部分如下表所示：

目标1位姿参数

时间/s	偏航角/o	俯仰角/o	滚转角/o	X位移/mm	Y位移/mm	Z位移/mm
							0.00023	3.730	58.423	3.289	100.42	63.23	290.08
0.00048	3.850	59.012	3.072	100.16	63.29	289.21
							0.00073	3.815	58.243	3.221	100.10	63.18	289.13
0.00098	3.890	58.342	3.093	100.52	63.12	289.05
							0.00123	3.902	58.678	3.052	100.78	63.10	289.01
0.00148	3.876	59.102	3.103	100.32	63.07	288.94

目标2位姿参数

目标3位姿参数

时间/s	偏航角/o	俯仰角/o	滚转角/o	X位移/mm	Y位移/mm	Z位移/mm
							0.00023	6.45	60.314	7.748	103.31	61.54	238.73
0.00048	6.61	60.341	7.814	103.64	61.38	239.13
							0.00073	6.87	60.643	7.865	103.16	61.19	239.37
0.00098	6.89	60.432	7.972	103.43	61.46	239.43
							0.00123	6.96	60.563	8.003	103.46	61.25	239.58
0.00148	7.13	60.142	7.983	103.32	61.47	239.63

从以上3个表可以看出，本方法可以完成三个物体位置姿态信息的同时测量，并且测量速度快，三个物体之间并无影响。

本发明利用刚体不变性，实现对同时投放的多投放物位姿测量，利用刚体间标记点距离变化特性对单个刚体进行区分，利用刚体内标记点距离约束识别各标记点，这将避免多体同时运动图像中标记点的相互干扰，以实现多体投放物同步位姿测量。

Claims (1)

1.一种基于机器视觉的多体投放物位姿测量方法，其特征是，测量方法首先建立各投放物单体特征点先验模型，对各单体进行区分编号；然后对多视觉测量图像进行处理，对每帧图像中的所有特征点进行运算对特征点进行分组划分刚体，利用先验模型对组内特征点进行比对识别，利用单体中各特征点距离约束识别单体中各特征点；最后利用坐标转换方式求得各帧图像中各投放物位姿参数，该方法的具体提取步骤如下：

第一步：先验模型建立

首先要建立多投放物表面特征点的先验知识模型，每个单体内各特征点的距离信息所组成的向量被称为此单体的先验知识模型；单体的先验知识模型如下式所示：

A_I＝[a_I1 ... a_IN] (1)

其中：A_I为第I个投放物编号，a_I1为第I个投放物中第1个标记的距离先验知识信息，a_IN为第I个投放物中第N个标记的距离先验知识信息；以此建立所有投放物刚体的先验知识模型；单体内先验模型为：

a_Ii＝[L_i1 ... L_in] (2)

其中：a_Ii为第I个刚体第i个特征点先验知识模型，L_i1为第i个特征点与单刚体内第1个特征点间距离，L_in为第i个特征点与单刚体内第n个特征点间距离；由此建立所有投放物刚体内先验模型；

第二步：基于时空关系的图像匹配方法

根据光流方法找到同名标记点在不同时刻的图像坐标，并且绘制出同名标记点在一段时间内的运动轨迹；

得到目标物表面标记点的轨迹之后，将左右图像的轨迹进行匹配；匹配的方法如下所示：

1)首先对左图像轨迹起始点在右图像起始点中进行极限约束匹配，找到满足极限约束条件的右图像轨迹；

2)再对左图像轨迹不同时刻与上步中筛选的右图像轨迹同一时刻点进行匹配，直到找到唯一一个与左图像匹配的右图像运动轨迹；

3)最后左右图像两条轨迹中同一时刻的标记点即为匹配点；

第三步：基于刚体不变性的多投放物体间特征区分

求出图像特征点的三维坐标后，比较这些空间点的相对关系，寻找在时间变化情况下空间关系一直不变的特征点，进行分组；具体过程首先建立空间所有点与其他标记点之间距离向量，如下所示：

$G_i=\left[\begin{array}{ccc}{l_{i1}}^1& ...& {l_{in}}^1\\ \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}\\ \\ \end{array}\\ {l_{i1}}^t& ...& {l_{in}}^t\end{array}\right]---\left(3\right)$

其中：G_i的第t行是空间第i个标记点与其他空间标记点在时刻t下的距离信息向量，l_i1 ¹是第一时刻下第i个点与第一个点的距离，l_in ^t是第t时刻下第i个点与第n个点的距离；以矩阵G_i中在时间序列图像中位置关系一直不变的特征点为一组，从而完成多体分离图像特征点分组；最后利用先验模型方法对各分组内特征点进行组内识别，从而建立图像特征点与目标实物特征点的对应关系；

第四步：单投放物内各特征点识别

将不同被测物体上标记点进行区分之后，要对每个刚体内的每个标记点进行识别，识别方法是首先建立同一个刚体内标记点的距离测量向量，建立方法如下：

g_Jj＝[l_j1 ... l_jm] (4)

其中l_in是第J组第个j特征点与第J组第m个特征点的测量值，g_Jj是第J组第j个特征点的距离测量向量；与先验模型a_Ii(i＝1:n)比对如下：

min||l_Jj-a_Ii||＝min{l_Jj-L_I1||，||l_Jj-L_I2||...||l_Jj-L_In||}，i＝1～n (5)

$h(g_{Ii},a_{Ij})=\left[\begin{array}{c}\min \left|\right|l_{I1}-L_{In}\left|\right|\\ \min \left|\right|l_{I2}-L_{In}\left|\right|\\ \begin{array}{c}.\\ .\\ .\end{array}\\ \min \left|\right|l_{\mathrm{Im}}-L_{In}\left|\right|\end{array}\right],i=1~n;j=1~m---\left(6\right)$

其中：h(g_Jj，a_Ii)是第J组第j点与第I组第i点的距离差值，当值最小时两点匹配；目标体G_Jj与目标体A_Ii距离差值：

$H(G_J,A_I)=\left|\right|\begin{array}{c}M\left(g_{J1},a_{Ii}\right)\\ M\left(g_{J2},a_{Ii}\right)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ M\left(g_{Jm},a_{Ii}\right)\end{array}\left|\right|---\left(7\right)$

其中：H(G_J，A_I)是目标体J与目标体I的距离差值，M(g_Jj，a_Ii)是h(g_Jj，a_Ii)的最小值，当H(G_J，A_I)值最小时，两目标体匹配，完成图像与先验模型的匹配识别；

第五步：坐标转换求取位姿参数

用以识别的特征点建立单个投放物局部坐标系O_tX_tY_tZ_t为：世界坐标系中任意点p_ow＝(x_p y_p z_p)^T在局部坐标系O_tX_tY_tZ_t下的坐标为p_ow＝(x_t y_t z_t)^T:

$\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right]=R_{wb}\left[\begin{array}{c}{x}_p-x_0\\ y_p-y_0\\ z_p-z_0\end{array}\right]---\left(8\right)$

其中：T_wb＝(x₀ y₀ z₀)为目标体坐标系原点即被测物体质心在世界坐标系中的坐标；R_wb为局部坐标系相对于世界坐标系的旋转矩阵；

$\begin{array}{c}{R}_{wb}=\\ \left[\begin{array}{ccc}\cos \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_Z\right)-\sin \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\sin \left({\mathrm{\θ}}_Z\right)& \cos \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_Z\right)+\sin \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\cos \left({\mathrm{\θ}}_Z\right)& -\sin \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_X\right)\\ -\cos \left({\mathrm{\θ}}_X\right)\sin \left({\mathrm{\θ}}_Z\right)& \cos \left({\mathrm{\θ}}_X\right)\cos \left({\mathrm{\θ}}_Z\right)& \sin \left({\mathrm{\θ}}_X\right)\\ \sin \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_Z\right)+\cos \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\sin \left({\mathrm{\θ}}_Z\right)& \sin \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_Z\right)-\cos \left({\mathrm{\θ}}_Y\right)\sin \left({\mathrm{\θ}}_X\right)\cos \left({\mathrm{\θ}}_Z\right)& \cos \left({\mathrm{\θ}}_Y\right)\cos \left({\mathrm{\θ}}_X\right)\end{array}\right]\end{array}---\left(9\right)$

其中，-θ_Z，-θ_X，-θ_Y为投放物相对于世界坐标系的偏航角，俯仰角和滚转角，求得R_wb，T_wb即单个投放物对于世界坐标系的位姿信息，以此求得所有投放物位姿信息。