CN104165750B - 立体视觉结合陀螺仪风洞模型位姿测量方法 - Google Patents

Abstract

本发明立体视觉结合陀螺仪风洞模型位姿测量方法属于计算机视觉领域，涉及一种风洞复杂环境投放物模型位姿的高精度测量方法。该测量方法采用立体视觉与陀螺仪相结合的方式实现风洞环境投放物模型位姿的高精度测量；采用陀螺仪测量模型滚转角度信息，利用编码标记点直接显示二进制滚转角度值，左、右高速摄像机进行运动视场内投放物模型编码标记点序列图像采集；经图像处理后，根据各编码标记点位置信息求取投放物模型俯仰、滚转和位置信息，通过编码标志点二进制解码获得模型滚转参数。该方法简单可靠，有效的解决了风洞环境中测量数据传输难的问题。并且高亮度标志点有效抑制了噪声的影响，提高了图像采集质量有利于提高位姿测量精度。

Description

立体视觉结合陀螺仪风洞模型位姿测量方法

技术领域

本发明属于计算机视觉测量领域，涉及一种立体视觉与陀螺仪相结合的投放物模型在风洞测量环境中，进行高速位姿测量的方法。

背景技术

随着航空工业的不断发展，飞行器的设计对风洞环境的实验模拟以及测量精度要求不断增加。并且用于风洞试验物体其位姿参数的精确测量精度将直接影响该投放物模型设计及其气动力分析。针对风洞等恶劣狭小环境，模型体积小、运动范围广、运动速度快、测试角度有限等因素，很难到达其位姿的高精度测量。机器视觉作为新兴技术具有非接触性、灵敏度高、精度高等特点，针对高速物的高精度位姿测量有着十分广阔的应用前景。而在恶劣环境下的基于视觉测量的高速高精度运动位姿测量系统设计将有助于提升航天航空技术的发展。

上海仪表厂的张宇等人发明的专利号为CN 103528571 A“单目立体视觉相对位姿测量方法”发明了基于单目视觉求取运动目标位姿的测量方法。通过变换单目摄像机的位置在不同角度获取含有目标的图像序列。在进行目标检测和场景分析的基础上对相邻两幅图像进行特征匹配，进而进行摄像机自运动的估计和相对位姿的计算。该方法需要摄像机作相对运动，高速运动物体响应速度慢，并且在复杂的风洞环境中，测量背景不断变化进行目标检测困难较大。南京航空航天大学的王云舒等人发明的专利号为CN 103791889 A的“一种利用结构光辅助的单目视觉位姿测量方法”发明了一种单目视觉结合结构光的位姿测量方法。采用十字形结构光与相机同向布置的方式，合理调整测量装置所处位置，使得投射的结构光与物体表面的一条直线有两个交点，后经图像处理求解物体的姿态信息。该方法虽能实现非合作目标的位姿测量，但仅适用于静态或者微位移测量，无法完成物体的高速测量。

发明内容

本发明要解决的技术难题是克服现有技术的缺陷，发明了立体视觉结合陀螺仪风洞模型的位姿测量方法。以陀螺仪输出滚转信息进行编码标志点的编码，通过双目视觉测量系统进行运动目标标记点序列图像采集。经图像处理结合直线拟合手段求取风洞模型俯仰、偏航以及位置参数，并且利用左、右高速摄像机对滚转信息进行解码，最终完成风洞模型位姿六参数的求解。本发明视觉与陀螺仪相结合的风洞环境投放物模型位姿测量方法，由于陀螺仪测量滚转角度精度较高，弥补了直接通过视觉系统测量滚转信息的不足。同时因采用的自发光标记点亮度大，有效的抑制了噪声的影响。标记点形状简单，特征信息提取、匹配、重建速度快、方法简单，实现风洞复杂环境投放模型位姿的高精度快速求取。

本发明所采用的技术方案是立体视觉结合陀螺仪风洞模型位姿测量方法，其特征是，该测量方法采用立体视觉与陀螺仪相结合的方式实现风洞环境投放物模型位姿的高精度测量；采用陀螺仪测量模型滚转角度信息，利用编码标记点直接显示二进制滚转角度值，左、右高速摄像机进行运动视场内投放物模型编码标记点序列图像采集；经图像处理后，根据各编码标记点位置信息求取投放物模型俯仰、滚转和位置信息，通过编码标志点二进制解码获得模型滚转参数；投放物模型位姿参数求解具体步骤如下：

1.投放物模型及标志点布局

在投放物模型2上布置标志点，根据投放物模型直径大小，标志点设有数组，每一组含有相同数量的标志点；每组标志点等距按行分布于待测类圆投放物模型圆周表面上，并且各标志点中心连线与投放物模型的轴线平行；标志点分为编码标志点4和常亮标志点5，编码标志点4用于滚转角的编码，常亮标志点5保证在每一滚转角度下都可以进行位姿求解；编码标志点4由安装于投放物模型内部的单轴角速率陀螺仪3控制，利用单轴角速率陀螺仪3测量的滚转角度参数控制各编码标志点4的亮暗以完成编码；编码时，编码标志点4亮记为1，不亮记为0；

2.高速序列图像采集与图像处理

采用已标定好的左、右高速摄像机1、1，进行高速序列图像采集，通过像素坐标系下的坐标与三维空间点的坐标的误差最小化函数对相机参数进行迭代求精；最小化函数方程为：

$min\left(\underset{i=1}{\overset{m}{\Σ}}\underset{j=1}{\overset{m}{\Σ}}\right||m_{i,j}-m(K,Ri,t_i,M_j\left)\right|{|}^2)---\left(1\right)$

式中，m_i,j为像素坐标系下的坐标，m(K,Ri,t_i,M_j)表示三维空间点的坐标经过初始估计的相机参数投影变换到像素坐标系下的坐标；

利用灰度重心法求取投放物模型左右图像标记点中心像面坐标，公式如下：

$\stackrel{\‾}{u}=\frac{\underset{u,v\∈\mathrm{\Ω}}{\Σ}u\×f(u,v)}{\underset{u,v\∈\mathrm{\Ω}}{\Σ}f(u,v)}---\left(2\right)$

$\stackrel{\‾}{v}=\frac{\underset{u,v\∈\mathrm{\Ω}}{\Σ}v\×f(u,v)}{\underset{u,v\∈\mathrm{\Ω}}{\Σ}f(u,v)}---\left(3\right)$

其中：f(u,v)是像面坐标为(u,v)的像素点的灰度值，Ω是目标所在区域的集合，即为整个标志点，是所提取标志点的中心坐标；随后采用改进的八点归一化算法计算左、右高速摄像机的基本矩阵F；基本矩阵满足下面的关系：

m'^TFm＝0 (4)

其中：m_i(u_i,v_i,1)^T，m_i'＝(u_i',v_i',1)^T为三维空间点对应匹配点在左右图像上的像面齐次坐标；假设给定一个对应点的集合{m_i,m_i'|i＝1,2,…,n}，n≥7，当使用n对对应点时，方程(4)可以改写成如下形式：

U_nf＝0 (5)其中：f＝[F₁₁,F₁₂,F₁₃,F₂₁,F₂₂,F₂₃,F₂₄,F₂₅,F₂₆]^T；

$U_n={\left[\begin{array}{ccccccccc}{u}_1{u_1}^{\′}& v_1{u_1}^{\′}& {u_1}^{\′}& u_1{v_1}^{\′}& v_1{v_1}^{\′}& {v_1}^{\′}& u_1& v_1& 1\\ u_2{u_2}^{\′}& v_2{u_2}^{\′}& {u_2}^{\′}& u_2{v_2}^{\′}& v_2{v_2}^{\′}& {v_2}^{\′}& u_2& v_2& 1\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ u_n{u_n}^{\′}& v_n{u_n}^{\′}& {u_n}^{\′}& u_n{v_n}^{\′}& v_n{v_n}^{\′}& {v_n}^{\′}& u_n& v_n& 1\end{array}\right]}_{n\×9}---\left(6\right)$

方程(4)是一个齐次方程组，并且，F在相差一个比例因子的情况下，应该有8个独立变量；但是，因为F还满足det(F)＝0，因此，在F的9个未知变量中只有7个独立变量；如果，给定n≥8组匹配点，F矩阵即可确定；

在求得F矩阵后，利用左、右高速摄像机获取的数字图像之间极线约束关系进行标志点中心匹配；并对左右图像对应匹配点进行三维重建以得到标志点中心在世界坐标系下的三维坐标值，重建公式如下：

$\left\{\begin{array}{c}{x}_i=\frac{{\mathrm{zX}}_i^{\′}}{f_1}\\ y_i=\frac{{\mathrm{zY}}_i^{\′}}{f_1}\\ z_i=\frac{f_1(f_2{t}_y-Y_{i^{\′}}^{\′}{t}_z)}{Y_1(r_7{X}_i^{\′}+r_8{Y}_i^{\′}+r_9{f}_1)-f_2(r_4{X}_i^{\′}+r_5{Y}_i^{\′}+r_6{f}_1)}\end{array}\right.---\left(7\right)$

其中：x_i'，x_i′′分别为对应匹配标志点中心的像面坐标；x_i'＝(X_i',Y_i')，X_i'，Y_i'分别为x_i'在像面坐标系下的横、纵坐标；x_i′′＝(X_i′′,Y_i′′)，X_i′′，Y_i′′分别为x_i‘'在像面坐标系下的横、纵坐标；(x_i,y_i,z_i)为由两对应匹配点x_i'、x_i‘'重建获得的空间标记点的三维坐标；f₁、f₂分别对应左、右高速摄像机的焦距；为从左高速摄像机变换到右高速摄像机的旋转矩阵，[t_x t_y t_z]是右摄像机相对于左摄像机的平移矩阵；

3.位姿求解俯仰角、偏航角及位置信息求解

在完成图像标志点序列图像处理后，基于重建信息进行直线拟合完成投放物模型的俯仰角、偏航角与位置参数的求解；

设通过空间直线拟合得到的直线方程为：

$\frac{x-x_0}{m}=\frac{y-y_0}{n}=\frac{z-z_0}{p}---\left(8\right)$

其中，(x₀,y₀,z₀)为空间直线上一点；(m,n,p)为空间直线的方向数；本发明利用最小二乘法对重建标志点进行直线拟合，完成投放物模型俯仰角、偏航角以及位置的求解。最小二乘法拟合直线过程为：将公式(8)改写为一般形式：

$\left\{\begin{array}{c}x=\frac{m}{p}(z-z_0)+x_0=az+b\\ y=\frac{n}{p}(z-z_0)+y_0=cz+d\end{array}\right.---\left(9\right)$

令

利用重建的标志点中心的三维坐标分别对上面两个方程进行数据拟合，使得

$\left\{\begin{array}{c}{Q}_x=\underset{i=1}{\overset{m}{\Σ}}{\[x_i-({\mathrm{az}}_i-b)\]}^2\\ Q_y=\underset{i=1}{\overset{m}{\Σ}}{\[y_i-({\mathrm{cz}}_i-d)\]}^2\end{array}\right.---\left(10\right)$

最小，得到最优参数[a b c d]，进而得到直线的拟合参数[m n p x₀ y₀ z₀]；然后求直线与平面的夹角以及直线中点相对于坐标系中的位置获得投放物模型俯仰角、偏航角与位置信息；具体如下：

取任意平面的法向量为(A,B,C)，则空间直线与该平面的夹角公式为：

令XOY坐标平面的法向量为(0,0,1)；则所求空间直线与XOY坐标平面的夹角为：

$sin\mathrm{\α}=\frac{\left|p\right|}{\sqrt{m^2+n^2+p^2}}---\left(12\right)$

α即为所求投放物模型的俯仰角；

令YOZ坐标平面的法向量为(1,0,0)；则所求空间直线与YOZ坐标平面的夹角为：

$sin\mathrm{\β}=\frac{\left|m\right|}{\sqrt{m^2+n^2+p^2}}---\left(13\right)$

β为所求投放物模型的偏航角；拟合直线的中点O(x₁,y₁,z₁)代表投放物模型的位置信息；

2)投放物模型滚转角度求解

设根据求解要求被测投放物模型表面有M+2个标志点，其中M个编码标志点4用于滚转角度编码，每个编码标志点在编码过程中用二进制1表示亮，0表示不亮，另外2个标志点为常亮标志点5；为便于识别两个常亮标志点5分别位于模型的头尾两侧，各相邻标志点之间的距离为d；设在t时刻有N(N≤M)个编码标志点4变亮，利用左、右高速摄像机经特征提取、匹配与重建得到标志点中心的三维信息(x₁,y₁,z₁)，(x₂,y₂,z₂)…(x_n,y_n,z_n)，然后按顺序约束依次计算各重建的编码标志点与模型尾部最近的常亮标志点的实际距离记为D_i(i＝1，2…且i≤N)；计算

$\left\{\begin{array}{c}{D}_i=\sqrt{{(x_i-x^{\′})}^2+{(y_i-y^{\′})}^2+{(z_i-z^{\′})}^2}\\ \frac{D_1}{d}=n_1\\ \begin{array}{cc}\frac{D_i-D_{i-1}}{d}=n_{ii}& (i=\mathrm{2...})\end{array}\end{array}\right.---\left(14\right)$

则在第一个编码点之前有n₁-1个编码标志点不亮记为0；在第i和第i-1个亮编码标志点之间有n_i-1个不亮编码标志点，记为0，按此种方式，并用二进制1和0按顺序对编码标志点进行解码；最后将二进制换算成十进制得到投放物模型的滚转信息；

本发明的有益效果是，利用视觉结合陀螺仪的测量方式完成了风洞漆黑狭小封闭的复杂环境下高速投放风洞模型位姿的快速高精度测量。利用陀螺仪测量滚转角度精度较高的特点，弥补了直接通过视觉系统测量滚转信息的不足。同时因采用的自发光标记点亮度大，有效的抑制了噪声的影响，并且标记点形状简单，特征信息提取、匹配、重建速度快、方法简单，实现了风洞复杂环境投放模型位姿的高精度快速求取。

附图说明

图1为一种立体视觉结合陀螺仪风洞模型的位姿测量系统示意图。其中，1-左高速摄像机，1’-右高速摄像机，2-投放物模型，3-单轴角速率陀螺仪，4-编码标志点，5-常亮标志点，6-试验台，d-各相邻标志点之间的距离。

图2为风洞模型的位姿高精度测量方法流程图。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。附图1为本发明采用的一种立体视觉结合陀螺仪风洞模型的位姿测量系统示意图。以投放物模型2的滚转角在0-45°范围内为例，由安放于投放物模型内部的单轴角速率陀螺仪3来进行测量，并以此为信息，对精密安装于模型表面的编码标志点4进行编码。在标定的基础上，同步触发安装在试验台6上的左、右高速摄像机1、1’获取投放物模型2与编码标志点的序列图像信息。随后将采集的数字图像传送至图像处理终端得到编码标志点中心的重建信息。通过直线拟合以求取投放物模型的俯仰角、偏航角以及位置信息；通过两高速摄像机对编码标志点信息进行解码，以得到投放物模型的滚转信息；最终求取投放物模型的位姿参数。具体位姿测量方法的步骤如下：

1.标志点布局与测量系统

如附图1所示，将单自由度角速率陀螺仪3精确安装于投放物模型2内部以进行滚转信息测量。将标记点按布局精确安装于模型表面，使标志点中心按行排列，并且中心连线与投放物模型的轴线平行。将附图1中所示的标定好的左、右高速摄像机1、1’固定于试验台6上，调整两摄像机焦距保证在视场内投放物模型具有测量可达性。因本发明具体实施例测量模型滚转角可变范围为0-45°，只要一个编码组并且每一编码组布置六个编码标志点4和两个常亮点5即可满足测量要求。

本发明采用的单轴角速率陀螺仪型号为CS-ARS-09，用它进行滚转信息求取与编码。使用型号为FASTCAM SAX左、右高速摄像机1、1’配合广角镜头Nikon1735，在3000fps情况下对被测物的下落过程进行拍摄。

2.高速序列图像采集与图像处理

利用图像处理终端对标志点中心进行提取、匹配与重建，首先利用灰度重心法求取目标标记点中心，随后采用改进的八点归一化算法利用公式(1)、(2)、(3)、(4)、(5)、(6)计算出描述左、右摄像机相互关系的基本矩阵F：

$F=\left[\begin{array}{ccc}0.000000100446521& 0.000034176438243& 0.005530198347258\\ 0.000000685914458& 0.000002386954124& -0.00625384125924\\ 0.000382645247728& 0.005243956784158& -0.35986214523686\end{array}\right]$

再利用左、右高速摄像机获取的数字图像之间极线约束关系进行标志点中心匹配后，根据公式(7)对左右图像对应匹配点进行三维重建以得到标志点中心在世界坐标系下的三维坐标值。

3.位姿求解

1)俯仰角、偏航角及位置信息求解

根据重建的三维空间点利用最小二乘法通过公式(8)、(9)、(10)拟合直线，由直线与平面的夹角公式(11)进一步利用公式(12)和(13)，完成投放物模型俯仰角、偏航角；由直线中心的三维坐标求解投放物模型的位置信息，结果如表2所示。

表2俯仰角、偏航角与位置信息求解结果

(2)滚转角求解

根据标志点的重建信息利用公式(14)计算附图一中本实施例此时刻亮或暗标志点的数量与位置，此时时间为0.00225s，有三个亮编码标志点和三个暗编码标志点，以二进制1和0的形式对应解码得到二进制结果为101010。将二进制换算成十进制得到当前滚转信息，滚转角为：420，最终完成位姿信息六参数的求解。

本发明采用由双目高速视觉与陀螺仪组成的高速测量系统风洞模型的快速高精度测量。利用陀螺仪测量滚转信息解决了视觉求取滚转信息精度差的问题，利用双目测量系统进行滚转信息识别解决了风度复杂漆黑环境数据传输难问题。本发明采用了高亮度标志点，有效的抑制了噪声，提高了图像采集质量并且标志点布局简单，图像处理以及识别简单迅速，在完成测量要求的基础上，大大降低了计算量。

Claims (1)

1.一种立体视觉结合陀螺仪风洞模型位姿测量方法，其特征是，该测量方法采用立体视觉与陀螺仪相结合的方式实现风洞环境投放物模型位姿的高精度测量；采用陀螺仪测量模型滚转角度信息，利用编码标记点直接显示二进制滚转角度值，左、右高速摄像机进行运动视场内投放物模型编码标记点序列图像采集；经图像处理后，根据各编码标记点位置信息求取投放物模型俯仰、滚转和位置信息，通过编码标志点二进制解码获得模型滚转参数；投放物模型位姿参数求解具体步骤如下：

(1)投放物模型及标志点布局

在投放物模型(2)上布置标志点，根据投放物模型直径大小，标志点设有数组，每一组含有相同数量的标志点；每组标志点等距按行分布于待测类圆投放物模型圆周表面上，并且各标志点中心连线与投放物模型的轴线平行；标志点分为编码标志点(4)和常亮标志点(5)，编码标志点(4)用于滚转角的编码，常亮标志点(5)保证在每一滚转角度下都可以进行位姿求解；编码标志点(4)由安装于投放物模型内部的单轴角速率陀螺仪(3)控制，利用单轴角速率陀螺仪(3)测量的滚转角度参数控制各编码标志点(4)的亮暗以完成编码；编码时，编码标志点(4)亮记为1，不亮记为0；

(2)高速序列图像采集与图像处理

采用已标定好的左高速摄像机(1)、右高速摄像机(1’)进行高速序列图像采集，通过像素坐标系下的坐标与三维空间点的坐标的误差最小化函数对相机参数进行迭代求精；最小化函数方程为：

$min\left(\underset{i=1}{\overset{m}{\Σ}}\underset{j=1}{\overset{m}{\Σ}}\right||m_{i,j}-m(K,Ri,t_i,M_j\left)\right|{|}^2)---\left(1\right)$

式中，m_i,j为像素坐标系下的坐标，m(K,Ri,t_i,M_j)表示三维空间点的坐标经过初始估计的相机参数投影变换到像素坐标系下的坐标；

利用灰度重心法求取投放物模型左右图像标记点中心像面坐标，公式如下：

$\stackrel{\‾}{u}=\frac{\underset{u,v\∈\mathrm{\Ω}}{\Σ}u\×f(u,v)}{\underset{u,v\∈\mathrm{\Ω}}{\Σ}f(u,v)}---\left(2\right)$

$\stackrel{\‾}{v}=\frac{\underset{u,v\∈\mathrm{\Ω}}{\Σ}v\×f(u,v)}{\underset{u,v\∈\mathrm{\Ω}}{\Σ}f(u,v)}---\left(3\right)$

其中：f(u,v)是像面坐标为(u,v)的像素点的灰度值，Ω是目标所在区域的集合，即为整个标志点，是所提取标志点的中心坐标；随后采用改进的八点归一化算法计算左、右高速摄像机的基本矩阵F；基本矩阵满足下面的关系：

m'^TFm＝0 (4)

其中：m_i(u_i,v_i,1)^T，m_i'＝(u_i',v_i',1)^T为三维空间点对应匹配点在左右图像上的像面齐次坐标；假设给定一个对应点的集合{m_i,m_i'|i＝1,2,…,n}，n≥7，当使用n对对应点时，方程(4)可以改写成如下形式：

U_nf＝0 (5)

其中：f＝[F₁₁,F₁₂,F₁₃,F₂₁,F₂₂,F₂₃,F₂₄,F₂₅,F₂₆]^T；

$U_n={\left[\begin{array}{ccccccccc}{u}_1{u_1}^{\′}& v_1{u_1}^{\′}& {u_1}^{\′}& u_1{v_1}^{\′}& v_1{v_1}^{\′}& {v_1}^{\′}& u_1& v_1& 1\\ u_2{u_2}^{\′}& v_2{u_2}^{\′}& {u_2}^{\′}& u_2{v_2}^{\′}& v_2{v_2}^{\′}& {v_2}^{\′}& u_2& v_2& 1\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .\\ u_n{u_n}^{\′}& v_n{u_n}^{\′}& {u_n}^{\′}& u_n{v_n}^{\′}& v_n{v_n}^{\′}& {v_n}^{\′}& u_n& v_n& 1\end{array}\right]}_{n\×9}---\left(6\right)$

方程(4)是一个齐次方程组，并且，F在相差一个比例因子的情况下，应该有8个独立变量；但是，因为F还满足det(F)＝0，因此，在F的9个未知变量中只有7个独立变量；如果，给定n≥8组匹配点，F矩阵即可确定；

在求得F矩阵后，利用左、右高速摄像机获取的数字图像之间极线约束关系进行标志点中心匹配；并对左右图像对应匹配点进行三维重建以得到标志点中心在世界坐标系下的三维坐标值，重建公式如下：

$\left\{\begin{array}{c}{x}_i=\frac{{{\mathrm{zX}}_i}^{\′}}{f_1}\\ y_i=\frac{{{\mathrm{zY}}_i}^{\′}}{f_1}\\ z_i=\frac{f_1(f_2{t}_y-{Y_{i^{\′}}}^{\′}{t}_z)}{Y_1(r_7{X_i}^{\′}+r_8{Y_i}^{\′}+r_9{f}_1)-f_2(r_4{X_i}^{\′}+r_5{Y_i}^{\′}+r_6{f}_1)}\end{array}\right.---\left(7\right)$

其中：x_i'，x_i′′分别为对应匹配标志点中心的像面坐标；x_i'＝(X_i',Y_i')，X_i'，Y_i'分别为x_i'在像面坐标系下的横、纵坐标；x_i′′＝(X_i′′,Y_i′′)，X_i′′，Y_i′′分别为x_i‘'在像面坐标系下的横、纵坐标；(x_i,y_i,z_i)为由两对应匹配点x_i'、x_i‘'重建获得的空间标记点的三维坐标；f₁、f₂分别对应左、右高速摄像机的焦距；为从左高速摄像机变换到右高速摄像机的旋转矩阵，[t_x t_y t_z]是右摄像机相对于左摄像机的平移矩阵；

(3)位姿求解俯仰角、偏航角及位置信息求解

在完成图像标志点序列图像处理后，基于重建信息进行直线拟合完成投放物模型的俯仰角、偏航角与位置参数的求解；

设通过空间直线拟合得到的直线方程为：

$\frac{x-x_0}{m}=\frac{y-y_0}{n}=\frac{z-z_0}{p}---\left(8\right)$

其中，(x₀,y₀,z₀)为空间直线上一点；(m,n,p)为空间直线的方向数；将公式(8)改写为一般形式：

$\left\{\begin{array}{c}x=\frac{m}{p}(z-z_0)+x_0=az+b\\ y=\frac{n}{p}(z-z_0)+y_0=cz+d\end{array}\right.---\left(9\right)$

令

利用重建的标志点中心的三维坐标分别对上面两个方程进行数据拟合，使得

$\left\{\begin{array}{c}{Q}_x=\underset{i=1}{\overset{m}{\Σ}}{\[x_i-({\mathrm{az}}_i-b)\]}^2\\ Q_y=\underset{i=1}{\overset{m}{\Σ}}{\[y_i-({\mathrm{cz}}_i-d)\]}^2\end{array}\right.---\left(10\right)$

最小，得到最优参数[a b c d]，进而得到直线的拟合参数[m n p x₀ y₀ z₀]；然后求直线与平面的夹角以及直线中点相对于坐标系中的位置获得投放物模型俯仰角、偏航角与位置信息；具体如下：

取任意平面的法向量为(A,B,C)，则空间直线与该平面的夹角公式为：

令XOY坐标平面的法向量为(0,0,1)；则所求空间直线与XOY坐标平面的夹角为：

$sin\mathrm{\α}=\frac{\left|p\right|}{\sqrt{m^2+n^2+p^2}}---\left(12\right)$

α即为所求投放物模型的俯仰角；

令YOZ坐标平面的法向量为(1,0,0)；则所求空间直线与YOZ坐标平面的夹角为：

$sin\mathrm{\β}=\frac{\left|m\right|}{\sqrt{m^2+n^2+p^2}}---\left(13\right)$

β为所求投放物模型的偏航角；拟合直线的中点O(x₁,y₁,z₁)代表投放物模型的位置信息；

投放物模型滚转角度求解

设根据求解要求被测投放物模型表面有M+2个标志点，其中M个编码标志点(4)用于滚转角度编码，每个编码标志点在编码过程中用二进制1表示亮，0表示不亮，另外2个标志点为常亮标志点(5)；为便于识别两个常亮标志点(5)分别位于模型的头尾两侧，各相邻标志点之间的距离为d；设在t时刻有N(N≤M)个编码标志点(4)变亮，利用左、右高速摄像机经特征提取、匹配与重建得到标志点中心的三维信息(x₁,y₁,z₁)，(x₂,y₂,z₂)…(x_n,y_n,z_n)，然后按顺序约束依次计算各重建的编码标志点与模型尾部最近的常亮标志点的实际距离记为D_i(i＝1，2…且i≤N)；计算

$\left\{\begin{array}{c}{D}_i=\sqrt{{(x_i-x^{\′})}^2+{(y_i-y^{\′})}^2+{(z_i-z^{\′})}^2}\\ \frac{D_1}{d}=n_1\\ \frac{D_i-D_{i-1}}{d}=n_i,(i=2...)\end{array}\right.---\left(14\right)$

则在第一个编码点之前有n₁-1个编码标志点不亮记为0；在第i和第i-1个亮编码标志点之间有n_i-1个不亮编码标志点，记为0，按此种方式，并用二进制1和0按顺序对编码标志点进行解码；最后将二进制换算成十进制得到投放物模型的滚转信息。