CN103512559B - 一种弹丸单目视频位姿测量方法及靶标图案 - Google Patents

Abstract

本发明涉及针对弹丸的一种靶标图案，以及应用该图案的一种单目视频位姿测量方法。靶标图案为围绕弹丸外圆周表面的带状区域，沿弹轴方向等分为颜色间隔设置的6个子区域，在子区域的内部设置编码区域，每个子区域的4个顶点作为特征点。应用该靶标图案进行位姿测量，应使弹丸位于摄像机的视场中。为每个子区域设置唯一的三位二进制编码。根据编码识别子区域，再根据特征点在子区域上的位置识别特征点。解算所述弹丸位姿时，首先从摄像机的图像中确定特征点的图像坐标，然后结合对应特征点在弹丸坐标系的坐标值，通过坐标系变换并建立方程组，联合求解得到弹丸的位姿。本发明的方法简单适用范围广。

Description

一种弹丸单目视频位姿测量方法及靶标图案

技术领域

本发明涉及一种针对飞行目标的弹丸靶标图案，以及应用该弹丸靶标图案的一种弹丸的单目视频位姿测量方法。

背景技术

在炮弹弹丸的研制和试验过程中，经常需要获取弹丸的位姿参数，如弹丸的着陆姿态等。但弹丸上通常没有安装精密的位姿测量设备，因而弹丸的位姿测量就成为难题。

于起峰等的《用光测图像确定空间目标俯仰角和偏航角的中轴线法》（国防科技大学学报，2000年22卷(2)：15-19页），提出了中轴线法，该方法先利用弹丸轮廓特征拟合弹丸的中轴线方程，再根据中轴线方程求出弹丸的俯仰角及偏航角，但该方法无法获取弹丸的滚转角，同时由于采用的是双目视频测量方案，测量范围较小。通常，由于弹丸表面光滑、纹理单一，摄像机拍摄的弹丸图像不能提供足够特征信息来解算弹丸的六维位姿参数，因而需要在弹丸表面设计靶标图案。目前靶标图案通常设计在平面上，例如文献“基于平面目标跟踪的无人机三维位姿估计方法”提出的方案中，在地面上设计了多个黑白块特征的靶标图案，实现无人机自主着陆，（参见：Iv′an F，3D pose estimation based on planarobject tracking for UAVs control2010IEEE International Conference on Roboticsand Automation,Alaska，USA：IEEE，May，2010：35-41）。赵锐的“基于单目视觉的物体位姿测量方法研究”，提出了采用发光体光标，利用单摄像机摄像实现了三维空间目标的位置和姿态测量（参见：基于单目视觉的物体位姿测量方法研究，合肥工业大学学位论文，2005）。而有关弹丸表面的靶标图案设计以及相关的测量方法还未见报道。

靶标图案的设计对弹丸位姿测量精度有显著影响，在弹丸的曲面上设计靶标图案主要存在3个难点：第一，弹丸为回转体，应确保在弹丸转动时摄像机始终能在靶标图案上拍摄到足够的特征点；第二，弹丸整体为一个立体曲面，靶标图案的设计应具有满足一定约束（如共面）的特征点；第三，应使靶标图案适应弹丸外形特征。

本发明的弹丸是指通常意义上的弹丸形状，即顶端大致呈锥形、中下端大致呈圆柱形、整体呈回转体结构且弹丸中部没有舵片的弹丸，这种结构在弹丸中最为常见。

对于单目位姿测量系统，根据胡占义等的《关于P4P问题的一点讨论》（参见《自动化学报》，2001年27卷(6)：770-776页），若已知被测目标上4个共面的特征点的图像坐标可得到目标位姿参数的唯一解。因此需要保证摄像机能在弹丸的靶标图案中始终拍摄到4个共面的特征点。

发明内容

为了解决现有平面靶标无法适应弹丸曲面结构的问题，以及提高弹丸的位姿测量精度，本发明的目的是：在弹丸表面设计一种靶标图案，该靶标图案在摄像机的视场中有4个以上共面特征点，并提出与其相对应的弹丸单目视频位姿测量方法。

本发明解决其技术问题所采取的技术方案是：

一种弹丸的靶标图案，以及应用该图案的弹丸单目视频位姿测量方法。

弹丸上的靶标图案设置为围绕弹丸外圆周表面的带状区域，并沿弹轴方向等分为6个子区域；所述子区域设置为两种颜色且相邻子区域的颜色不同，所述的两种颜色与弹丸表面颜色有明显的对比度。

所述子区域的侧边与弹轴共面，所述子区域的上边和下边位于垂直于弹轴的截面圆上。

在所述子区域的内部设置面积较小的区域作为编码区域，所述编码区域的颜色与所在的子区域颜色有明显的对比度；编码区域的形状可以为方形或圆形，或者是其它易于识别的形状；且，相同颜色的子区域内部设置的编码区域数量不同；所述每个子区域的4个顶点作为靶标图案的特征点。

一种应用所述靶标图案的弹丸单目视频位姿测量方法，包括以下步骤：摄像机设置在可调整基座上，朝向要进行位姿测量的带有所述靶标图案的弹丸，并使得弹丸位于摄像机的视场中且摄像机光轴与弹轴的夹角在30°～150°范围内，摄像机能拍摄弹丸侧面；

摄像机的分辨率像素为640×480以上，视场角为25°～45°（相应的等效焦距为43.5mm～81.2mm）；

所述弹丸的表面设置本发明的上述靶标图案，所述每个子区域的4个顶点作为靶标图案的特征点，使所述靶标图案在摄像机的视场中始终有4个以上共面特征点。

拍摄弹丸的图像，并将摄像机采集到的图像数据发送给处理器，解算弹丸的位姿参数；

根据本发明的靶标图案所述子区域的颜色及子区域内包含编码区域的数量，为每个子区域设置唯一的识别编码进行识别。

编码可以采用三位二进制编码，所述的三位二进制编码第一位代表子区域颜色，编码的第二位和第三位代表所述每个子区域内包含的编码区域的数量。根据所述的编码，例如三位二进制编码来识别子区域，再根据所述的特征点在所述子区域上的分布位置识别特征点。

解算所述弹丸位姿时，首先从摄像机的图像中提取并识别所述靶标图案的特征点的图像坐标，然后结合对应特征点在弹丸坐标系的坐标，进行坐标系变换并建立方程组，联合求解得到弹丸的相对位姿；

建立弹丸坐标系、摄像机坐标系和图像坐标系，确定所述的特征点在弹丸坐标系中的坐标；提取所述特征点在图像坐标系中坐标，提取步骤如下：

（1）采用背景差法检测出弹丸成像区域；

（2）在弹丸成像区域，利用阈值法对靶标图案的图像进行分割，并对分割后的图像进行连通区域分析，将包含像素数量最多的连通区作为靶标图案的子区域，根据分割时采用的阈值及该连通区域内包含小连通区域数量来构造子区域编码，以识别该子区域；

（3）提取该连通区域的轮廓，利用Hough变换法从子区域轮廓中提取子区域侧边上的点所形成的直线点，并拟合两条直线；从子区域轮廓中分离子区域上边和下边上的点所形成的曲线点，并拟合椭圆曲线，分别求直线与该椭圆曲线的交点，获得作为靶标图案特征点的子区域顶点在图像坐标系中的坐标；

（4）在识别子区域且获得特征点图像坐标后，根据所述的特征点在所述子区域上的分布位置识别特征点；

所述特征点在弹丸坐标系下的对应坐标为已知量，利用所述特征点在弹丸坐标系中的坐标和其对应的图像坐标系中的坐标，经坐标系变换解算出弹丸相对于摄像机的位姿。

由于靶标图案设计在弹丸的侧面上，所以安放摄像机时应保证摄像机能够拍摄到弹丸的侧面。如果在弹丸的位姿变化幅度较大的场景，例如经常弹丸出现翻转的状态时，则需要两套或者两套以上摄像机和相应的图像处理单元，分别组成两套或者两套以上单目视频测量系统，调整摄像机的位置和朝向，使其在弹丸姿态变化过程中至少有一个摄像机始终能够拍摄到弹丸侧面。

本发明的方法，相比于陀螺等传统的弹丸位姿测量方法具有不接触弹丸、测量误差不累积的优点。而且由于采用单目视频测量系统，相比于现有技术中的中轴线法系统结构更简单、测量范围大且能够测量出弹丸的滚转角。通过采用本发明的靶标图案，将弹丸的表面分成6个子区域，并且每个子区域内的特征点都满足共面性约束，克服了现有平面靶标无法适应弹丸曲面结构的不足。本发明的靶标图案能用于各种具有回转体结构的弹丸及导弹，不受弹丸口径或其它尺寸限制，适用范围很广。

以下结合附图，对本发明的具体实施方式进行详细的描述后，本发明的其他特征和优点将会更加清楚地显现。

附图说明

图1是本发明的弹丸的单目视频位姿测量方法的系统构成示意图；

图2是本发明的单目弹丸成像模型示意图；

图3是本发明的弹丸截面单目成像模型示意图；

图4是本发明的弹丸子区域划分示意图；

图5是本发明的弹丸特征点共面设计示意图；

图6是本发明的靶标图案的特征点分布及子扇区编码设计图；

图7是本发明的靶标图案结构图；

图8是本发明的弹丸的轴线与摄像机光轴夹角为30°时的靶标图案；

图9是本发明的弹丸单目视频位姿测量系统的各个坐标系定义；

图10是本发明的标准坐标系变换图；

图11是本发明的理想坐标系变换图。

具体实施方式

下面结合附图和一个典型的具体实施方式，对本发明弹丸的靶标图案的技术方案，以及一种弹丸的单目视频位姿测量方法进行详细说明。

参见图6，7，8所示，一种弹丸的靶标图案，设计为围绕弹丸的外圆周表面的带状区域，并且所述的靶标图案沿弹轴方向等分为6个子区域5；相邻的子区域的颜色间隔设置为两种颜色，所述的两种颜色与弹丸表面颜色有明显的对比度。靶标图案位于弹丸外表面，且尽量设置在弹丸横截面中直径较大的部分。

弹丸的6个子区域5分别设置为与弹丸本身的表面颜色对比度明显的另外两种颜色。参见图7所示，由于本具体实施方式中的弹丸为灰色，选用黑色和白色来设计靶标图案。也可以根据具体情况，使用红色、黑色或者蓝色等颜色。

子区域的侧边8与弹轴共面，子区域的上边9和下边10位于垂直于弹轴的截面圆上。

为识别各个子区域5，在相同颜色的子区域内部设置0～2个编码区域7。编码区域7为面积较小的区域，与子区域5颜色有明显的对比度。本具体实施方式中，在黑色子区域中的编码区域选用白色，白色子区域中的编码区域选用黑色。

编码区域7的形状可以为方形或圆形，也可以采用其它易于识别的形状。

结合编码区域7的数量及所在子区域5的颜色给每个子区域5设计唯一的三位二进制编码。靶标图案的特征点6为子区域的4个顶点。

所述子区域的侧边与弹轴共面，所述子区域的上边和下边位于垂直于弹轴的截面圆上。

在所述子区域的内部设置面积较小的区域作为编码区域，所述编码区域的颜色与所在的子区域颜色有明显的对比度；编码区域的形状可以为方形或圆形，或者是其它易于识别的形状；所述相同颜色的子区域内部设置的编码区域数量不同，本实施例中的编码区域数量为0～2个。

所述子区域的4个顶点作为靶标图案的特征点。

参见图1所示，是本发明的应用上述靶标图案的弹丸单目视频位姿测量方法的系统构成示意图。包括摄像机1和具有上述靶标图案的弹丸3，摄像机1设置在可调基座2上。调整基座，使摄像机1朝向弹丸3，所述的弹丸3位于摄像机1的视场中。由摄像机采集到的图像数据发送到处理器中解算弹丸的位姿参数。

所述摄像机的分辨率像素为768×576，视场角为30°，等效焦距为67.2mm。

首先详细描述如何解决靶标图案的局部遮挡问题。

在弹丸位姿变化过程中，摄像机只能拍摄到弹丸部分表面。为保证靶标图案成像中始终有足够的特征信息，用靶标图案将弹丸表面划分成一定数量的子区域，使弹丸在运动过程中至少有一个完整的子区域能被摄像机拍摄到；再在每个子区域内设计一定数量的特征点，就可以保证靶标图案的成像中始终有足够的特征信息。结合图2，详细描述划分子区域的过程。

参见图2建立弹丸单目成像模型，分析子区域划分数量。θ表示摄像机光轴o_cz_c与弹丸轴线间的夹角，l_p表示弹丸长度，l_I表示弹丸的成像长度。用l_p′表示弹丸在图像平面4的投影长度，它们之间关系为：

l′_p=l_psinθ,l_I∝l_p′ （1）

当θ∈(0,30°)∪(150°,180°)时，l_p′<0.5l_p，l_I也小于它在θ=90°时长度的一半，此时弹丸成像的形变量较大，设计在弹丸表面的靶标图案成像形变也较大，导致特征点无法提取。因此仅针对θ∈[30°,150°]范围内设计标记，使标记在θ的这个范围内结构清晰。（这个范围内靶标图案成像形变相对较小）。

在图2中用面x_co_cz_c横切弹丸，得到的弹丸截面近似为椭圆形。参见图3，建立该弹丸横截面的单目视觉模型。

L表示摄像机光心o_c到椭圆圆心o_o的距离；C点为摄像机光轴o_cz_c与椭圆的交点，弧线弧线是两段完全相同的弧线；线o_cA、o_cB与椭圆分别相切与A、B两点，两点间弧线能被摄像机拍摄到。该弧线对应的圆心角为φ，φ对应的弧线均能被摄像机拍摄到。

将椭圆分段并使每段椭圆弧线对应的圆心角大小相同；合理的设计分段数量，保证无论弹丸如何绕弹轴转动，始终有一段完整的弧线落入角φ对应的弧线中，则应符合以下条件：

表示每段椭圆弧线对应的圆心角。在此条件下，角φ对应的弧线中始终有一段以上完整的弧线；若该条件不满足，例如弧线弧线对应圆心角为

∠Fo_oC=∠Eo_oC>0.5φ （3）

弧线弧线均没能完整的落入在成像窗口中。

计算φ以确定椭圆划分份数量。图2中椭圆短轴a和长轴b长度分别为

a=R,b=R/sinθ （4）

式中R为弹丸半径。参见图2，相切点B的坐标(x_b,z_b)满足

$\left\{\begin{array}{c}\frac{x_b^2}{a^2}+\frac{{(z_b-L)}^2}{b^2}=1\\ x_b={\mathrm{kz}}_b\end{array}\right.---\left(5\right)$

式中k为切线o_cB的斜率。由于只有一个交点，求解得到B点的坐标

$k=\frac{a}{\sqrt{L^2-b^2}},x_b=\frac{a}{d_o}\sqrt{L^2-b^2}$

进而求得φ

$\mathrm{\&phi;}=2\arctan \left(\frac{x_b}{L-z_b}\right)=2\arctan \left(\frac{a}{b^2}\sqrt{L^2-b^2}\right)=2\arctan \left(\frac{\sqrt{{\sin }^2\mathrm{\&theta;}(L^2{\sin }^2\mathrm{\&theta;}-R^2)}}{R}\right)$

sinθ越大，φ越大。当θ为30°或150°时，sinθ最小，因此

${\mathrm{\&phi;}}_{\min }=2\arctan \left(\frac{\sqrt{L^2-{4R}^2)}}{4R}\right)$

在实际单目视频测量系统中，通常满足L≥10R，所以有φ>135.5°。为满足式（2），椭圆分段数应n≥6。在图3中取一点D使弧为其中一段弧，过D点作椭圆切线交o_cz_c于点G

l′_D≈l_Dsin∠CGD,l_I∝l′_D （6）

式中，l_D表示D点处的1°角对应的弧线，这里近似为近似直线，l′_D表示该弧线在像平面的投影长度。

取θ=30°，当n=6时，l′_D≈0.27l_D，此时相同长度的弧线，在D点处的成像长度约为C点处四分之一；n取值越大，D点处弧线成像形变就越小，也就越有利于设计在弹丸上的特征点的提取；但随着n的增大，靶标图案分块数增加，靶标图案的设计复杂度也会增加，同时靶标图案上特征点之间距离减小，这会降低了系统对姿态角的分辨力能力。参见图4，因而选取n=6，即将椭圆分成6段弧线，相应的将弹丸所有截面均按圆心角等分成6段弧线，即将弹丸表面等分成6个子区域5’。

在图4中，无论弹丸如何绕弹轴旋转，总有一个完整的子区域5’出现在摄像机视场中。若再在每个子区域5’内设计一定数量的特征点就可以使靶标图案成像中始终有足够的特征信息。

其次，在每个子区域内设计4个特征点，并保证这4个共面，下面详细描述如何保证靶标图案上的特征点共面性：

参见图5，为保证特征点共面，将特征点分别设计在两个垂直于弹轴的截面圆的圆弧上。面ABC与面A′B′C′是垂直于弹轴AA′的截面，为保证特征点B、C、B′、C′共面，应使线BB′、线CC′分别与弹轴AA′共面。证明如下：

若两截面圆半径相同，则有BB′//AA′,CC′//AA′。因此标记点B,C,B′,C′共面。

若两截面圆半径不相同，由于线BB′、线CC′均与弹轴AA′共面，BB′延长线与弹轴AA′交于D，CC′延长线与弹轴AA′交于D′。根据三角形相似性有

$\frac{A^{\&prime;}{B}^{\&prime;}}{\mathrm{AB}}=\frac{A^{\&prime;}D}{\mathrm{AD}},\frac{A^{\&prime;}{C}^{\&prime;}}{\mathrm{AC}}=\frac{A^{\&prime;}{D}^{\&prime;}}{{\mathrm{AD}}^{\&prime;}}---\left(7\right)$

由于

AB=AC=R₁,A′B′=A′C′=R₂ （8）

式中R₁、R₂分别为两个截面圆的半径。进而

$\frac{A^{\&prime;}D}{\mathrm{AD}}=\frac{A^{\&prime;}{D}^{\&prime;}}{{\mathrm{AD}}^{\&prime;}}$ 即 $\frac{A^{\&prime;}D}{{\mathrm{AA}}^{\&prime;}+A^{\&prime;}D}=\frac{A^{\&prime;}{D}^{\&prime;}}{{\mathrm{AA}}^{\&prime;}+A^{\&prime;}{D}^{\&prime;}}---\left(9\right)$

可得

A′D′=A′D （10）

即D′、D两点重合，即线BB′、线CC′相交于D(D′)。两相交直线必然共面，所以特征点B、C、B′、C′共面。证毕。

参见图6，由于图4中带状子区域5，的侧边与弹轴共面，根据上文分析用两个垂直于弹轴的横截面横切弹丸，仅保留两横截面中间部分的6个子区域，其它部分恢复弹丸原来颜色（不设计靶标图案），并将这些子区域的顶点作为靶标图案的特征点6。

保留的子区域应尽量位于弹丸中直径较大的部分，靶标图案沿弹轴方向的宽度应足够大。

在解决靶标图案局部遮挡问题和特征点位置设计好之后，得到靶标图案的大致结构：

为使靶标图案上的特征点满足共面约束，用两个垂直于弹轴的横截面将图4中的子区域5’分成三段，保留中间部分以形成围绕弹丸外圆周的带状的靶标图案部分，靶标图案围绕弹丸外表面等分为6个子区域5，每个子区域5的4个顶点作为特征点6。子区域5的侧边8与弹轴共面，子区域5的上边9和下边10位于垂直于弹丸纵轴的截面圆上。根据本发明的靶标图案，无论弹丸如何旋转，总有一个完整的子区域5出现在摄像机视场中。

根据所述编码区域的数量及子区域的颜色为每个子区域设置唯一的三位二进制编码。

下面详细描述靶标图案的编码设计：

为完成特征点识别，应使靶标图案的每个特征点均具有唯一可识别性。参见图7，根据子区域颜色及包含编码区域7数量给每个子区域设计唯一编码。本具体实施例的各子区域编码见下表。

根据相邻顺序依次将子区域编号为子区域一、子区域二…，子区域六，其中子区域一、三、五设计为黑色，子区域二、四、六设计为白色；子区域一、四内没有编码区域，子区域三、六中含有1个编码区域，子区域二、五中含有2个编码区域。编码的设计方式为：第1位代表子区域颜色，黑色为0，白色为1；编码的第2、3位代表编码区域的数量，没有编码区域为00，含1个编码区域为01，含2个编码区域7为10。

根据子区域的颜色和包含编码区域的数量首先识别子区域，在识别子区域后，根据特征点在子区域内的位置，如以弹丸顶部朝上为准，根据特征点在子区域左下方还是在右上方来识别特征点。

参见图8，当弹丸的轴线与摄像机轴线夹角为30°时，弹丸成像的形变量在设计范围内为最大。图8中在靶标形变量最大时，靶标图案的结构很清楚，各部分没有粘连，能够提取到特征点。

如图8所示，图像中有完整的子区域一，子区域一上有4个共面的特征点，特征点11～14分别记为P0～P3，利用这些点能够解算弹丸的位姿参数。

以下以图8的靶标图像为例，结合本发明的靶标图案，详细描述应用该靶标图案的一种弹丸的单目视频位姿测量方法，对解算弹丸位姿的过程进行详细描述。下文中，特征点提取过程是本申请的发明点，而位姿解算原理和公式采用的是本领域公知的技术，本申请结合靶标图案进行弹丸位姿的解算。

参见图9，首先定义系统的各个坐标系。以子区域一为例，说明任意子区域上的4个特征点在各个坐标系中的坐标值为：

（1）建立弹丸坐标系O-XYZ。本例中该坐标原点在弹丸底面中心，设子区域一上第i个特征点P_i在该坐标系的坐标为W_i=(X_i,Y_i,Z_i)^T,i=0,1,2,3，由于靶标的设计参数均已知，所以特征点在该坐标系下的坐标均为已知量；

（2）建立摄像机坐标系O_c-X_cY_cZ_c。该坐标系原点在摄像机光心，设子区域一上第i个特征点P_i在该标系的坐标为特征点在该坐标系下的坐标为未知量；

（3）建立图像像素坐标系o_I-uv，以图像平面左上角为原点，u、v轴分别沿图像的横轴和纵轴。设子区域一上第i个特征点Pi在该坐标系的坐标为分别为(u_i,v_i)^T,i=0,1,2,3，特征点在该坐标系下的坐标通过提取得到。图像坐标系为o-xy，以光轴与图像平面的交点为原点。设子区域一上第i个特征点P_i在图像坐标系中的坐标为(x_i,y_i)^T,i=0,1,2,3。

解算时首先要提取特征点在图像像素坐标系中的坐标(u_i,v_i)^T,i=0,1,2,3，提取步骤如下：

（1）采用背景差法检测出弹丸成像区域。在测量过程中摄像机静止不动，先拍摄一幅背景图像；利用后续的弹丸测试图像减去背景图像，差分图像中不为0的区域即为弹丸区域成像；

（2）在弹丸成像区域，根据靶标灰度值选择合适的阈值对图像进行分割。分别选择不同阈值分割黑色区域和白色区域，如分割黑色区域，则可以设置较小的阈值，阈值可以选择为T1=20；分割白色区域，则可设置较大的阈值，阈值可以选择为T2=200。对分割后的图像进行连通区域分析，将包含像素数量最多的连通区作为靶标图案的子区域，根据分割时采用的阈值及该连通区域内包含小连通区域数量来构造子区域编码，以识别该子区域；

（3）利用先膨胀再减去膨胀前图像的形态学方法提取该连通区域的轮廓，利用Hough变换法从轮廓中分离直线点，这里的直线点指子区域侧边上的点，并拟合两条直线，其中，拟合直线的方法属于本领域内的公知技术；从子区域轮廓中分离曲线点，这里曲线点指子区域上边和下边上的点，并拟合两个椭圆曲线，分别求两直线与两椭圆曲线的交点，共有四个交点，排除掉明显错误的两个交点，即可获得子区域顶点坐标。其中，拟合曲线的方法属于本领域内的公知技术；

（4）根据子区域的颜色、内含编码区域的数量得到该子区域的编码，根据编码即可完成子区域识别。再根据特征点在子区域上的分布位置识别特征点，例如以弹丸朝上为准，根据特征点的位置是在子区域的左下方还是右下方，识别特征点。

通过以上步骤得到了子区域一上的特征点在图像坐标系下的坐标(u_i,v_i)^T,i=0,1,2,3，而这些特征点在弹丸坐标系下的对应坐标W_i=(X_i,Y_i,Z_i)^T,i=0,1,2,3为已知量。位姿解算算法利用这4个共面特征点的弹丸坐标和它们对应的图像像素坐标来求解出弹丸相对于摄像机的相对位姿。解算过程属于本领域的已知技术，具体的解算步骤如下：

利用式（11）将特征点在图像像素坐标系o-uv中的坐标(u_i,v_i)^T转化为图像坐标系坐标(x_i,y_i)^T。其中，dx和dy为像素坐标系中像素在x轴和y轴方向上的距离，(u₀,v₀)为摄像机主点坐标，这些均属于摄像机内参数，摄像机内参数通过摄像机标定得到。摄像机标定是本领域公知的技术。

$\left\{\begin{array}{c}{x}_i=u_i\mathrm{dx}-u_0\mathrm{dx}\\ y_i=v_i\mathrm{dy}-v_0\mathrm{dy}\end{array}\right.---\left(11\right)$

根据透视投影模型，特征点的图像坐标系坐标与其对应摄像机坐标间的关系为：

$\left\{\begin{array}{c}{x}_i=f\frac{X_i^c}{Z_i^c}\\ y_i=f\frac{Y_i^c}{Z_i^c}\end{array}\right.---\left(12\right)$

式中f为摄像机焦距，也通过摄像机标定得到。

根据坐标系变换关系，有M表示摄像机坐标系与弹丸坐标系间的相对坐标变换。

$\left[\begin{array}{c}{X}_i^c\\ Y_i^c\\ Z_i^c\\ 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ r_{31}& r_{32}& r_{33}& t_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]---\left(13\right)$

式中弹丸的位姿参数R,t定义如下：

$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],t=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$

其中R,t分别为旋转矩阵和位移向量。特征点的图像坐标系坐标与弹丸坐标之间的关系为

$\left\{\begin{array}{c}\frac{x_i}{f}=\frac{r_{11}{X}_i+r_{12}{Y}_i+r_{13}{Z}_i+t_x}{r_{31}{X}_i+r_{32}{Y}_i+r_{33}{Z}_i+t_z}\\ \frac{y_i}{f}=\frac{r_{21}{X}_i+r_{22}{Y}_i+r_{23}{Z}_i+t_y}{r_{31}{X}_i+r_{32}{Y}_i+r_{33}{Z}_i+t_z}\end{array}\right.---\left(14\right)$

可将M矩阵做如下分解：

M=S^-1CT （15）

式（15）中将求解M矩阵转换为分别求解矩阵S^-1，C，T。三个矩阵的定义如下：

（1）参见图10，对弹丸坐标系进行变换，使得子区域一上的4个特征点P0，P1，P2，P3的位置在新坐标系中，P0为新坐标系的原点，P1在新坐标系的X轴的正轴上，P2位于新坐标系XY平面的第一区间内，这个变换用T表示，称新坐标系为标准坐标系。

（2）参见图11，对摄像机坐标系进行变换，使得P0像点p0为图像坐标系原点，P1像点p1在图像坐标系x轴的正轴上，此变换用S表示，称新摄像机坐标系为理想坐标系，所获得的图像为理想图像。此时的摄像机焦点位置不变，正视于标准坐标系α的原点，且图像坐标系的x轴与标准坐标系的X轴平行。

（3）将标准坐标系与理想坐标系间的相对变换用矩阵C表示，求出C。

下面利用公知技术求解各个变换矩阵：

（1）求T

参见图10，由上面思路，将弹丸坐标系经过平移、旋转变换成为标准坐标系，使得4个特征点的位置变换到标准坐标系指定的位置。

经过平移变换将P0点平移到新坐标系原点，坐标变换用T_o表示，则 $W_i^o={(X_i^o,Y_i^o,Z_i^o,1)}^T=T_o{W}_i$ 为平移后坐标值。

$T_o=\left[\begin{array}{cccc}1& 0& 0& -X_o\\ 0& 1& 0& -Y_o\\ 0& 0& 0& -Z_o\\ 0& 0& 0& 1\end{array}\right]$

先将平移后弹丸坐标系绕Z轴旋转θ角，使P1的坐标值在新坐标轴X轴的正半轴上，变换用R_θ表示， $W_i^{\mathrm{\&theta;}}={(X_i^{\mathrm{\&theta;}},Y_i^{\mathrm{\&theta;}},Z_i^{\mathrm{\&theta;}},1)}^T=R_{\mathrm{\&theta;}}{W}_i^o.$

$R_{\mathrm{\&theta;}}=\left[\begin{array}{cccc}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}& 0& 0\\ -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

要保证P1在X轴的正半轴，应有则可求得

$\mathrm{\&theta;}={\tan }^{-1}\left(\frac{Y_1^o}{X_1^o}\right)---\left(15\right)$

如果 $X_1^o<0,$ 则θ=θ+π，使得 $X_1^{\mathrm{\&theta;}}>0;$ 若 $X_1^o=Y_1^o=0,$ 则θ=0。

同理，再绕变换后的弹丸坐标系Y轴旋转β角，使P1点的Z轴坐标为0。变换用R_β表示， $W_i^{\mathrm{\&beta;}}={(X_i^{\mathrm{\&beta;}},Y_i^{\mathrm{\&beta;}},Z_i^{\mathrm{\&beta;}},1)}^T=R_{\mathrm{\&beta;}}{W}_i^{\mathrm{\&theta;}}$

$R_{\mathrm{\&theta;}}=\left[\begin{array}{cccc}\cos \mathrm{\&beta;}& 0& -\sin \mathrm{\&beta;}& 0\\ 0& 1& 0& 0\\ \sin \mathrm{\&beta;}& 0& \cos \mathrm{\&beta;}& 0\\ 0& 0& 0& 1\end{array}\right]$

要使P1在XY平面上，应有可以求得

$\mathrm{\&beta;}={\tan }^{-1}\left(\frac{-Z_1^{\mathrm{\&theta;}}}{X_1^{\mathrm{\&theta;}}}\right)---\left(16\right)$

同理，再绕变换后的弹丸坐标系X轴旋转α角，使P2在XY平面第一象限，变换用R_α表示， $W_i^{\mathrm{\&alpha;}}={(X_i^{\mathrm{\&alpha;}},Y_i^{\mathrm{\&alpha;}},Z_i^{\mathrm{\&alpha;}},1)}^T=R_{\mathrm{\&alpha;}}{W}_i^{\mathrm{\&beta;}}$

$R_{\mathrm{\&theta;}}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \mathrm{\&alpha;}& -\sin \mathrm{\&alpha;}& 0\\ 0& -\sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& 0\\ 0& 0& 0& 1\end{array}\right]$

使P2在XY平面第一象限，应有可以求得

$\mathrm{\&alpha;}={\tan }^{-1}\left(\frac{Z_2^{\mathrm{\&beta;}}}{Y_2^{\mathrm{\&beta;}}}\right)---\left(17\right)$

如果则α=α+π，以保证P2在XY平面第一象限。

用X_i,Y_i,Z_i代替上式（15～17）中变量，则可以得到

$\left\{\begin{array}{c}\mathrm{\&theta;}={\tan }^{-1}\left(\frac{Y_1-Y_0}{X_1-X_0}\right)\\ \mathrm{\&beta;}={\tan }^{-1}\left\{\frac{-(Z_1-Z_0)}{\sqrt{{(X_1-X_0)}^2+{(Y_1-Y_0)}^2}}\right\}\\ \mathrm{\&alpha;}={\tan }^{-1}\left\{\frac{[(X_2-X_0)\cos \mathrm{\&theta;}+(Y_2-Y_0)\sin \mathrm{\&theta;}]\sin \mathrm{\&beta;}+(Z_2-Z_0)\cos \mathrm{\&beta;}}{-(X_2-X_0)\sin \mathrm{\&theta;}+(Y_2-Y_0)\cos \mathrm{\&theta;}}\right\}\end{array}\right.---\left(18\right)$

其中，如果X₁<X₀，则θ=θ+π，如果X₁=X₀且Y₁=Y₀，则θ=0；如果-(X₂-X₀)sinθ+(Y₂-Y₀)cosθ<0，则α=α+π。

则矩阵T如（19）式所示。此时，特征点P0在新物体坐标系的原点，P1在X轴正轴上，P2在XY平面内。

$T=\left[\begin{array}{cccc}{T}_{11}& T_{12}& T_{13}& T_{14}\\ T_{21}& T_{22}& T_{23}& T_{24}\\ T_{31}& T_{32}& T_{33}& T_{34}\\ 0& 0& 0& 1\end{array}\right]---\left(19\right)$

其中：

$\left\{\begin{array}{c}{T}_{11}=\cos \mathrm{\&beta;}\cos \mathrm{\&theta;},T_{12}=\cos \mathrm{\&beta;}\sin \mathrm{\&theta;},T_{13}=-\sin \mathrm{\&beta;},\\ T_{21}=-\cos \mathrm{\&alpha;}\sin \mathrm{\&theta;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&theta;},T_{22}=\cos \mathrm{\&alpha;}\cos \mathrm{\&theta;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&theta;},T_{23}=\sin \mathrm{\&alpha;}\cos \mathrm{\&beta;},\\ T_{31}=\sin \mathrm{\&alpha;}\sin \mathrm{\&theta;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&theta;},T_{32}=-\sin \mathrm{\&alpha;}\cos \mathrm{\&theta;}+\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&theta;},T_{33}=\cos \mathrm{\&beta;}\cos \mathrm{\&theta;},\\ T_{i4}=-X_0{T}_{i1}-Y_0{T}_{i2}-Z_0{T}_{i3},i=\mathrm{1,2,3}\end{array}\right.$

则有：

${(X_i^{\mathrm{\&alpha;}},Y_i^{\mathrm{\&alpha;}},Z_i^{\mathrm{\&alpha;}},1)}^T=T\&CenterDot;{(X_i,Y_i,Z_i,1)}^T,i=\mathrm{0,1,2,3}$

（2）求S

参见图11，从原摄像机坐标系转换到理想坐标系的变换为S。对原摄像机坐标系进行旋转，使得P0成像点p0为图像坐标系原点，P1像点p1在图像坐标系的新坐标系X轴的正半轴。

对弹丸作T变换，并不改变特征点在图像中坐标。因此特征点在弹丸坐标系与标准坐标系所成的像是完全相同的。因此

$\left\{\begin{array}{c}{x}_i=x_i^{\mathrm{\&alpha;}}=\frac{f{X}_i^c}{Z_i^c}\\ y_i=y_i^{\mathrm{\&alpha;}}=\frac{f{Y}_i^c}{Z_i^c}\end{array}\right.---\left(20\right)$

式（20）中，f为摄像机焦距，指用标准坐标系α表示特征点在图像坐标中的坐标。

首先，绕摄像机坐标系Y_c轴旋转φ角，使p0位于图像坐标系的y轴上，变换用R_φ表示，则有 $W_i^{\mathrm{\&phi;}}={(X_i^{\mathrm{\&phi;}},Y_i^{\mathrm{\&phi;}},Z_i^{\mathrm{\&phi;}},1)}^T=R_{\mathrm{\&phi;}}{W}_i^c.$ 其中

$R_{\mathrm{\&phi;}}=\left[\begin{array}{cccc}\cos \mathrm{\&phi;}& 0& -\sin \mathrm{\&phi;}& 0\\ 0& 1& 0& 0\\ \sin \mathrm{\&phi;}& 0& \cos \mathrm{\&phi;}& 0\\ 0& 0& 0& 1\end{array}\right]$

特征点在新坐标系下仍满足透视投影关系，即：

$\left\{\begin{array}{c}{x}_i^{\mathrm{\&phi;}}=\frac{f{X}_i^{\mathrm{\&phi;}}}{Z_i^c}\\ y_i^{\mathrm{\&phi;}}=\frac{f{Y}_i^{\mathrm{\&phi;}}}{Z_i^c}\end{array}\right.$

p0位于图像坐标系的y轴上，应使则根据旋转矩阵R_φ有 $X_0^c\cos \mathrm{\&phi;}-Z_0^c\sin \mathrm{\&phi;}=0.$

$\mathrm{\&phi;}={\tan }^{-1}\left(\frac{X_0^c}{Z_0^c}\right)={\tan }^{-1}\left(\frac{x_0^{\mathrm{\&alpha;}}}{f}\right),0\&le;\left|\mathrm{\&phi;}\right|\&le;\frac{\mathrm{\&pi;}}{2}---\left(21\right)$

同理，绕摄像机坐标系X_c轴旋转ω角，使得p0点位于图像坐标系原点，变换用R_ω表示，则有 $W_i^{\mathrm{\&omega;}}={(X_i^{\mathrm{\&omega;}},Y_i^{\mathrm{\&omega;}},Z_i^{\mathrm{\&omega;}},1)}^T=R_{\mathrm{\&omega;}}{W}_i^{\mathrm{\&phi;}}.$

$R_{\mathrm{\&omega;}}\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \mathrm{\&omega;}& \sin \mathrm{\&omega;}& 0\\ 0& -\sin \mathrm{\&omega;}& \cos \mathrm{\&omega;}& 0\\ 0& 0& 0& 1\end{array}\right]$

特征点在新坐标系下仍满足透视投影关系，即：

$\left\{\begin{array}{c}{x}_i^{\mathrm{\&omega;}}=\frac{f{X}_i^{\mathrm{\&omega;}}}{Z_i^{\mathrm{\&omega;}}}\\ y_i^{\mathrm{\&omega;}}=\frac{f{Y}_i^{\mathrm{\&omega;}}}{Z_i^{\mathrm{\&omega;}}}\end{array}\right.$

使得p0点位于图像坐标系原点，应有所以

$\mathrm{\&omega;}={\tan }^{-1}(-\frac{Y_0^{\mathrm{\&phi;}}}{Z_0^{\mathrm{\&phi;}}})={\tan }^{-1}\left(\frac{-y_0^{\mathrm{\&alpha;}}\cos \mathrm{\&phi;}}{f}\right),0\&le;\left|\mathrm{\&omega;}\right|\&le;\frac{\mathrm{\&pi;}}{2}---\left(22\right)$

同理，绕摄像机坐标系Z_c轴旋转ρ角，使p1点位于图像坐标系x轴正半轴，变换用R_ρ表示，则有 $W_i^{\mathrm{\&rho;}}={(X_i^{\mathrm{\&rho;}},Y_i^{\mathrm{\&rho;}},Z_i^{\mathrm{\&rho;}},1)}^T=R_{\mathrm{\&phi;}}{W}_i^{\mathrm{\&omega;}}$

$R_{\mathrm{\&rho;}}=\left[\begin{array}{cccc}\cos \mathrm{\&rho;}& \sin \mathrm{\&rho;}& 0& 0\\ -\sin \mathrm{\&rho;}& \cos \mathrm{\&rho;}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

特征点在新坐标系下仍满足透视投影关系，即：

$\left\{\begin{array}{c}{x}_i^{\mathrm{\&rho;}}=\frac{{\mathrm{fX}}_i^{\mathrm{\&rho;}}}{Z_i^{\mathrm{\&rho;}}}\\ y_i^{\mathrm{\&rho;}}=\frac{{\mathrm{fY}}_i^{\mathrm{\&rho;}}}{Z_i^{\mathrm{\&rho;}}}\end{array}\right.$

P1点位于图像坐标系x轴正半轴，应 $y_1^{\mathrm{\&rho;}}=0.$ $Y_1^{\mathrm{\&rho;}}=-X_1^{\mathrm{\&omega;}}\sin \mathrm{\&rho;}+Y_1^{\mathrm{\&omega;}}\cos \mathrm{\&rho;}=0$

$\mathrm{\&rho;}={\tan }^{-1}\left(\frac{Y_1^{\mathrm{\&omega;}}}{X_1^{\mathrm{\&omega;}}}\right)={\tan }^{-1}\left(\frac{y_1^{\mathrm{\&alpha;}}\cos \mathrm{\&omega;}+x_1^{\mathrm{\&alpha;}}\sin \mathrm{\&phi;}\sin \mathrm{\&omega;}+f\cos \mathrm{\&phi;}\sin \mathrm{\&omega;}}{x_1^{\mathrm{\&alpha;}}\cos \mathrm{\&phi;}-f\sin \mathrm{\&phi;}}\right)---\left(23\right)$

综上式（21～23）可以得到三个旋转角：

$\left\{\begin{array}{c}\mathrm{\&phi;}={\tan }^{-1}\left(\frac{x_0^{\mathrm{\&alpha;}}}{f}\right),0\&le;\left|\mathrm{\&phi;}\right|\&le;\frac{\mathrm{\&pi;}}{2}\\ \mathrm{\&omega;}={\tan }^{-1}\left(\frac{-y_0^{\mathrm{\&alpha;}}\cos \mathrm{\&phi;}}{f}\right),0\&le;\left|\mathrm{\&omega;}\right|\&le;\frac{\mathrm{\&pi;}}{2}\\ \mathrm{\&rho;}={\tan }^{-1}\left(\frac{y_1^{\mathrm{\&alpha;}}\cos \mathrm{\&omega;}+x_1^{\mathrm{\&alpha;}}\sin \mathrm{\&phi;}\sin \mathrm{\&omega;}+f\cos \mathrm{\&phi;}\sin \mathrm{\&omega;}}{x_1^{\mathrm{\&alpha;}}\cos \mathrm{\&phi;}-f\sin \mathrm{\&phi;}}\right)\end{array}\right.---\left(24\right)$

式（24）中，为标准坐标系中特征点在原图像坐标系中的坐标。事实上，它等于未变换之前的图像坐标系坐标。

进过以上变换将摄像机坐标系变为理想坐标系，两坐标系间的旋转矩阵为S=R_ρR_ωR_φ，经S变换后，各特征点在理想图像中的坐标为：

$\left\{\begin{array}{c}{x}_i^{\mathrm{\&rho;}}=f\frac{X_i^{\mathrm{\&rho;}}}{Z_i^{\mathrm{\&rho;}}}=f\frac{S_{11}{X}_i^c+S_{12}{Y}_i^c+S_{13}{Z}_i^c}{S_{31}{X}_i^c+S_{32}{Y}_i^c+S_{33}{Z}_i^c}=f\frac{S_{11}{x}_i^{\mathrm{\&alpha;}}+S_{12}{y}_i^{\mathrm{\&alpha;}}+S_{13}f}{S_{31}{x}_i^{\mathrm{\&alpha;}}+S_{32}{y}_i^{\mathrm{\&alpha;}}+S_{33}f}\\ y_i^{\mathrm{\&rho;}}=f\frac{Y_i^{\mathrm{\&rho;}}}{Z_i^{\mathrm{\&rho;}}}=f\frac{S_{21}{X}_i^c+S_{22}{Y}_i^c+S_{23}{Z}_i^c}{S_{31}{X}_i^c+S_{32}{Y}_i^c+S_{33}{Z}_i^c}=f\frac{S_{21}{x}_i^{\mathrm{\&alpha;}}+S_{22}{y}_i^{\mathrm{\&alpha;}}+S_{33}f}{S_{31}{x}_i^{\mathrm{\&alpha;}}+S_{32}{y}_i^{\mathrm{\&alpha;}}+S_{33}f}\end{array}\right.---\left(25\right)$

为了最后求解M，得出矩阵为

$S^{-1}=\left[\begin{array}{cccc}{S}_{11}& S_{12}& S_{13}& 0\\ S_{21}& S_{22}& S_{23}& 0\\ S_{31}& S_{32}& S_{33}& 0\\ 0& 0& 0& 1\end{array}\right]---\left(26\right)$

其中：

$\left\{\begin{array}{c}{S}_{11}=\cos \mathrm{\&phi;}\cos \mathrm{\&rho;}+\sin \mathrm{\&phi;}\sin \mathrm{\&omega;}\sin \mathrm{\&rho;},S_{12}=-\cos \mathrm{\&phi;}\sin \mathrm{\&rho;}+\sin \mathrm{\&phi;}\sin \mathrm{\&omega;}\cos \mathrm{\&rho;},S_{13}=\sin \mathrm{\&phi;}\cos \mathrm{\&omega;}\\ S_{21}=\cos \mathrm{\&omega;}\sin \mathrm{\&rho;},S_{22}=\cos \mathrm{\&omega;}\cos \mathrm{\&rho;},S_{23}=-\sin \mathrm{\&omega;}\\ S_{31}=-\sin \mathrm{\&phi;}\cos \mathrm{\&rho;}+\cos \mathrm{\&phi;}\sin \mathrm{\&omega;}\sin \mathrm{\&rho;},S_{32}=\sin \mathrm{\&phi;}\sin \mathrm{\&rho;}+\cos \mathrm{\&phi;}\sin \mathrm{\&omega;}\cos \mathrm{\&rho;},S_{33}=\cos \mathrm{\&phi;}\cos \mathrm{\&omega;}\end{array}\right.$

（3）求C

矩阵C表示标准坐标系与理想坐标系间的相对变换，即根据透视投影模型

$\left\{\begin{array}{c}{x}_i^{\mathrm{\&rho;}}=f\frac{X_i^{\mathrm{\&rho;}}}{Z_i^{\mathrm{\&rho;}}}=f\frac{C_{11}{X}_i^{\mathrm{\&alpha;}}+C_{12}{X}_i^{\mathrm{\&alpha;}}+C_{13}{Z}_i^{\mathrm{\&alpha;}}+C_{14}}{C_{31}{X}_i^{\mathrm{\&alpha;}}+C_{32}{Y}_i^{\mathrm{\&alpha;}}+C_{33}{Z}_i^{\mathrm{\&alpha;}}+C_{34}}\\ y_i^{\mathrm{\&rho;}}=f\frac{X_i^{\mathrm{\&rho;}}}{Z_i^{\mathrm{\&rho;}}}=f\frac{C_{21}{X}_i^{\mathrm{\&alpha;}}+C_{22}{Y}_i^{\mathrm{\&alpha;}}+C_{23}{Z}_i^{\mathrm{\&alpha;}}+C_{24}}{C_{31}{X}_i^{\mathrm{\&alpha;}}+C_{32}{Y}_i^{\mathrm{\&alpha;}}+C_{33}{Z}_i^{\mathrm{\&alpha;}}+C_{34}}\end{array}\right.$

进过T变换和S变换后，有以下关系：

$\left\{\begin{array}{c}{X}_0^{\mathrm{\&alpha;}}=Y_0^{\mathrm{\&alpha;}}=Z_0^{\mathrm{\&alpha;}}=Y_1^{\mathrm{\&alpha;}}=Z_1^{\mathrm{\&alpha;}}=Z_2^{\mathrm{\&alpha;}}=0\\ x_0^{\mathrm{\&rho;}}=x_1^{\mathrm{\&rho;}}=y_0^{\mathrm{\&rho;}}=0\end{array}\right.---\left(27\right)$

由 $x_0^{\mathrm{\&rho;}}=0,$ 可知 $X_0^{\mathrm{\&rho;}}=0.$ 有

$C_{11}{X}_0^{\mathrm{\&alpha;}}+C_{12}{Y}_0^{\mathrm{\&alpha;}}+C_{13}{Z}_0^{\mathrm{\&alpha;}}+C_{14}=0$

由于都等于0，所以 $C_{14}=0.$

同理，所以C24=0。

$x_1^{\mathrm{\&rho;}}=f\frac{X_1^{\mathrm{\&rho;}}}{Z_1^{\mathrm{\&rho;}}}=f\frac{C_{11}{X}_1^{\mathrm{\&alpha;}}+C_{12}{Y}_1^{\mathrm{\&alpha;}}+C_{13}{Z}_1^{\mathrm{\&alpha;}}+C_{14}}{C_{31}{X}_1^{\mathrm{\&alpha;}}+C_{32}{Y}_1^{\mathrm{\&alpha;}}+C_{33}{Z}_1^{\mathrm{\&alpha;}}+C_{34}}=f\frac{C_{11}{X}_1^{\mathrm{\&alpha;}}+C_{14}}{C_{31}{X}_1^{\mathrm{\&alpha;}}+C_{34}}$

整理得到：

$f{X}_1^{\mathrm{\&alpha;}}{C}_{11}-x_1^{\mathrm{\&rho;}}{X}_1^{\mathrm{\&alpha;}}{C}_{31}-x_1^{\mathrm{\&rho;}}{C}_{34}=0---\left(28\right)$

由于 $y_1^{\mathrm{\&rho;}}=0,$ 可知 $Y_1^{\mathrm{\&rho;}}=0$

$C_{21}{X}_1^{\mathrm{\&alpha;}}+C_{22}{Y}_1^{\mathrm{\&alpha;}}+C_{23}{Z}_1^{\mathrm{\&alpha;}}+C_{24}=0$

由于C₂₄都等于0，所以C₂₁=0

$x_2^{\mathrm{\&rho;}}=f\frac{X_2^{\mathrm{\&rho;}}}{Z_2^{\mathrm{\&rho;}}}=f\frac{C_{11}{X}_2^{\mathrm{\&alpha;}}+C_{12}{Y}_2^{\mathrm{\&alpha;}}+C_{13}{Z}_2^{\mathrm{\&alpha;}}+C_{14}}{C_{31}{X}_2^{\mathrm{\&alpha;}}+C_{32}{Y}_2^{\mathrm{\&alpha;}}+C_{33}{Z}_2^{\mathrm{\&alpha;}}+C_{34}}=f\frac{C_{11}{X}_1^{\mathrm{\&alpha;}}+C_{12}{Y}_2^{\mathrm{\&alpha;}}}{C_{31}{X}_2^{\mathrm{\&alpha;}}+C_{32}{Y}_2^{\mathrm{\&alpha;}}+C_{34}}$

$f{X}_2^{\mathrm{\&alpha;}}{C}_{11}+f{Y}_2^{\mathrm{\&alpha;}}{C}_{12}-x_2^{\mathrm{\&rho;}}{X}_2^{\mathrm{\&alpha;}}{C}_{31}-x_2^{\mathrm{\&rho;}}{Y}_2^{\mathrm{\&alpha;}}{C}_{32}-x_2^{\mathrm{\&rho;}}{C}_{34}=0---\left(29\right)$

同理，由可以构造方程

$f{Y}_2^{\mathrm{\&alpha;}}{C}_{22}-y_2^{\mathrm{\&rho;}}{X}_2^{\mathrm{\&alpha;}}{C}_{31}-y_2^{\mathrm{\&rho;}}{Y}_2^{\mathrm{\&alpha;}}{C}_{32}-y_2^{\mathrm{\&rho;}}{C}_{34}=0---\left(30\right)$

由单位正交矩阵的性质可得：

$\left\{\begin{array}{c}{C}_{11}^2+C_{31}^2=1\\ C_{12}^2+C_{22}^2+C_{32}^2=1\\ C_{11}{C}_2+C_{32}{C}_{31}=0\end{array}\right.---\left(31\right)$

要求C的封闭解，仅用三个点无法实现，必须引入第四个点P3的信息，由于P0、P1、P2、P3共面，有可以构造出两个方程。

$x_3^{\mathrm{\&rho;}}=f\frac{X_3^{\mathrm{\&rho;}}}{Z_3^{\mathrm{\&rho;}}}=f\frac{C_{11}{X}_3^{\mathrm{\&alpha;}}+C_{12}{Y}_3^{\mathrm{\&alpha;}}+C_{13}{Z}_3^{\mathrm{\&alpha;}}+C_{14}}{C_{31}{X}_3^{\mathrm{\&alpha;}}+C_{32}{Y}_3^{\mathrm{\&alpha;}}+C_{33}{Z}_3^{\mathrm{\&alpha;}}+C_{34}}=f\frac{C_{11}{X}_3^{\mathrm{\&alpha;}}+C_{12}{Y}_3^{\mathrm{\&alpha;}}}{C_{31}{X}_3^{\mathrm{\&alpha;}}+C_{32}{Y}_3^{\mathrm{\&alpha;}}+C_{34}}$

$f{X}_3^{\mathrm{\&alpha;}}{C}_{11}+f{Y}_3^{\mathrm{\&alpha;}}{C}_{12}-x_3^{\mathrm{\&rho;}}{X}_3^{\mathrm{\&alpha;}}{C}_{31}-x_3^{\mathrm{\&rho;}}{Y}_3^{\mathrm{\&alpha;}}{C}_{32}-x_3^{\mathrm{\&rho;}}{C}_{34}=0---\left(32\right)$

由可以构造

$f{Y}_3^{\mathrm{\&alpha;}}{C}_{22}-y_3^{\mathrm{\&rho;}}{X}_3^{\mathrm{\&alpha;}}{C}_{31}-y_3^{\mathrm{\&rho;}}{Y}_3^{\mathrm{\&alpha;}}{C}_{32}-y_3^{\mathrm{\&rho;}}{C}_{34}=0---\left(33\right)$

联立式（28～33）求解，得出如下的结果：

C₁₄=C₂₄=C₂₁=0 （34）

$r=\frac{f{X}_1^{\mathrm{\&alpha;}}{B}_3-x_1^{\mathrm{\&rho;}}{B}_1}{x_1^{\mathrm{\&rho;}}(X_1^{\mathrm{\&alpha;}}{B}_3-B_2)},C_{11}=\&PlusMinus;\sqrt{\frac{1}{1+r^2}},C_{31}=r{C}_{11}---\left(35\right)$

$C_{34}=\frac{(f{X}_1^{\mathrm{\&alpha;}}{C}_{11}-x_1^{\mathrm{\&rho;}}{X}_1^{\mathrm{\&alpha;}}{C}_{31})}{x_1^{\mathrm{\&rho;}}}=\frac{C_{11}(f{X}_1^{\mathrm{\&alpha;}}-r{x}_1^{\mathrm{\&rho;}}{X}_1^{\mathrm{\&alpha;}})}{x_1^{\mathrm{\&rho;}}}---\left(36\right)$

其中： $\left\{\begin{array}{c}{B}_1=f{Y}_2^{\mathrm{\&alpha;}}{Y}_3^{\mathrm{\&alpha;}}(y_2^{\mathrm{\&rho;}}-y_3^{\mathrm{\&rho;}})(X_2^{\mathrm{\&alpha;}}{Y}_3^{\mathrm{\&alpha;}}-X_3^{\mathrm{\&alpha;}}{Y}_2^{\mathrm{\&alpha;}})\\ B_2=Y_2^{\mathrm{\&alpha;}}{Y}_3^{\mathrm{\&alpha;}}(y_2^{\mathrm{\&rho;}}{x}_3^{\mathrm{\&rho;}}-y_3^{\mathrm{\&rho;}}{x}_2^{\mathrm{\&rho;}})(X_2^{\mathrm{\&alpha;}}{Y}_3^{\mathrm{\&alpha;}}-X_3^{\mathrm{\&alpha;}}{Y}_2^{\mathrm{\&alpha;}})\\ B_3=Y_2^{\mathrm{\&alpha;}}{Y}_3^{\mathrm{\&alpha;}}(y_2^{\mathrm{\&rho;}}{x}_3^{\mathrm{\&rho;}}-y_3^{\mathrm{\&rho;}}{x}_2^{\mathrm{\&rho;}})(Y_3^{\mathrm{\&alpha;}}-Y_2^{\mathrm{\&alpha;}})\end{array}\right.$

由于C₃₄表示由摄像机焦点到标准坐标系原点的距离，因此C₃₄>0。故C₁₁的符号选择为：如果 $(f{X}_1^{\mathrm{\&alpha;}}-r{x}_1^{\mathrm{\&rho;}}{X}_1^{\mathrm{\&alpha;}})/x_1^{\mathrm{\&rho;}}>0,$ 则C₁₁>0；如果 $(f{X}_1^{\mathrm{\&alpha;}}-r{x}_1^{\mathrm{\&rho;}}{X}_1^{\mathrm{\&alpha;}})/x_1^{\mathrm{\&rho;}}<0,$ 则C₁₁<0。同时有：

$\left\{\begin{array}{c}{C}_{32}=\frac{(Y_2^{\mathrm{\&alpha;}}{X}_3^{\mathrm{\&alpha;}}{y}_3^{\mathrm{\&rho;}}-X_2^{\mathrm{\&alpha;}}{Y}_3^{\mathrm{\&alpha;}}{y}_2^{\mathrm{\&rho;}})C_{31}+(Y_2^{\mathrm{\&alpha;}}{y}_3^{\mathrm{\&rho;}}-Y_3^{\mathrm{\&alpha;}}{y}_2^{\mathrm{\&rho;}})C_{34}}{Y_2^{\mathrm{\&alpha;}}{Y}_3^{\mathrm{\&alpha;}}(y_2^{\mathrm{\&rho;}}-y_3^{\mathrm{\&rho;}})}\\ C_{22}=\frac{X_3^{\mathrm{\&alpha;}}{y}_3^{\mathrm{\&rho;}}{C}_{31}+Y_3^{\mathrm{\&alpha;}}{y}_3^{\mathrm{\&rho;}}{C}_{32}+y_3^{\mathrm{\&rho;}}{C}_{34}}{f{Y}_3^{\mathrm{\&alpha;}}},\\ 31C_{12}=\frac{X_2^{\mathrm{\&alpha;}}{x}_2^{\mathrm{\&rho;}}{C}_{31}+Y_2^{\mathrm{\&alpha;}}{x}_2^{\mathrm{\&rho;}}{C}_{32}+x_2^{\mathrm{\&rho;}}{C}_{34}-f{X}_2^{\mathrm{\&alpha;}}{C}_{11}}{f{Y}_3^{\mathrm{\&alpha;}}}\end{array}\right.---\left(37\right)$

根据C为单位正交矩阵，有如下等式成立：

$\left\{\begin{array}{c}{C}_{13}=C_{21}{C}_{32}-C_{31}{C}_{22}\\ C_{23}=C_{12}{C}_{31}-C_{11}{C}_{32}\\ C_{33}=C_{11}{C}_{22}-C_{12}{C}_{21}\end{array}\right.---\left(38\right)$

至此C的所有元素都确定来了。

这样，三个变换矩阵T、S、C均可确定下来，由式（15）求出M，旋转矩阵R和偏移向量t也可确定。M表示弹丸与摄像机坐标系之间的位姿变换，它里面的旋转因子R应满足正交性。而M的求解过程中，特征点坐标值存在误差，易导致M不满足正交性，因此还要此的基础上对矩阵的旋转子R进行优化。

由于 $W_i^c=M{W}_i$

$M=\left[\begin{array}{cccc}{m}_1& m_2& m_3& m_{10}\\ m_4& m_5& m_6& m_{11}\\ m_7& m_8& m_9& m_{12}\\ 0& 0& 0& 1\end{array}\right]$

整理得到：

$\left[\begin{array}{cccc}{X}_1^c-m_{10}& X_2^c-m_{10}& X_3^c-m_{10}& X_4^c-m_{10}\\ Y_1^c-m_{11}& Y_2^c-m_{11}& Y_3^c-m_{11}& Y_4^c-m_{11}\\ Z_1^c-m_{12}& Z_2^c-m_{12}& Z_3^c-m_{12}& Z_4^c-m_{12}\end{array}\right]=R\left[\begin{array}{cccc}{X}_1& X_2& X_3& X_4\\ Y_1& Y_2& Y_3& Y_4\\ Z_1& Z_2& Z_3& Z_4\end{array}\right]$

将上式简写为D=RE，优化使min||RE-D||，其中，E=[E₁ E₂ E₃ E₄]，D=[D₁ D₂ D₃ D₄]，约束条件为R是正交矩阵。

定义一个4×4的旋转矩阵B

$B=\underset{i=1}{\overset{4}{\mathrm{\&Sigma;}}}{B}_i^T{B}_i$

其中 $B=\left[\begin{array}{cc}0& {(E_i-D_i)}^T\\ (D_i-E_i)& [D_i-E_i]\&times;\end{array}\right],$ [·]×表示 $\left[(x,y,z)\right]\&times;=\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]$

用r=(r₀,r₁,r₂,r₃)^T表示与矩阵B的最小特征值所对应的特征向量。则旋转矩阵R的解为

$R=\left[\begin{array}{ccc}{r_0}^2+{r_1}^2-{r_2}^2-{r_3}^2& 2(r_1{r}_2-r_0{r}_3)& 2(r_1{r}_3+r_0{r}_2)\\ 2(r_1{r}_2+r_0{r}_3)& {r_0}^2-{r_1}^2+{r_2}^2-{r_3}^2& 2(r_2{r}_3-r_0{r}_1)\\ 2(r_1{r}_3-r_0{r}_2)& 2(r_2{r}_3+r_0{r}_1)& {r_0}^2-{r_1}^2-{r_2}^2+{r_3}^2\end{array}\right]$

接下来要进行的是利用光束平差迭代的方法，对旋转矩阵R和平移向量t的结果进行进一步处理，可以得到更准确的解。光束平差法是本领域公知的技术，这里不再详细描述。

应当认识到，以上描述只是本发明的一个特定实施例，本发明并不仅仅局限于以上图示或描述的特定的结构，权利要求将覆盖本发明的实质精神及范围内的所有变化方案。

Claims (3)

1.一种应用靶标图案的弹丸单目视频位姿测量方法，其特征在于包括以下步骤：

靶标图案设置为围绕弹丸外圆周表面的带状区域，并沿弹轴方向等分为6个子区域；所述子区域设置为两种颜色且相邻子区域的颜色不同，所述的两种颜色与弹丸表面颜色均有明显对比度；

所述子区域的侧边与弹轴共面，所述子区域的上边和下边位于垂直于弹轴的截面圆上；

所述子区域的内部设置面积较小的区域作为编码区域，所述编码区域的颜色与其所在的子区域颜色有明显的对比度；所述编码区域的形状为方形或圆形，或者是其它易于识别的形状；

且，相同颜色的子区域内部设置的编码区域数量不同；

所述每个子区域的4个顶点作为靶标图案的特征点；

摄像机设置在可调整基座上，使摄像机朝向带有所述靶标图案的弹丸且使摄像机光轴与弹轴的夹角在30°～150°范围内，摄像机能拍摄到弹丸侧面且能在所述靶标图案上拍摄到4个以上共面特征点；

所述摄像机的分辨率像素为640×480以上，视场角为25°～45°；

所述摄像机拍摄弹丸的图像，并将摄像机采集到的图像数据发送给处理器，用于解算弹丸的位姿参数；

根据所述靶标图案的子区域的颜色、子区域内包含编码区域的数量，为所述每个子区域设置唯一的识别编码；

根据所述的识别编码来识别子区域，再根据所述的特征点在所述子区域上的分布位置识别特征点；

解算所述弹丸位姿时，首先从摄像机的图像中提取并识别所述靶标图案的特征点的图像坐标，然后结合对应特征点在弹丸坐标系的坐标，进行坐标系变换并建立方程组，联合求解得到弹丸的相对位姿；

建立弹丸坐标系、摄像机坐标系和图像坐标系，确定所述的特征点在弹丸坐标系中的坐标；提取所述特征点在图像坐标系中坐标，提取步骤如下：

(1)采用背景差法检测出弹丸成像区域；

(2)在弹丸成像区域，利用阈值法对靶标图案的图像进行分割，并对分割后的图像进行连通区域分析，将包含像素数量最多的连通区作为靶标图案的子区域，根据分割时采用的阈值及该连通区域内包含小连通区域数量来构造子区域编码，以识别该子区域；

(3)提取该连通区域的轮廓，利用Hough变换法从子区域轮廓中提取子区域侧边上的点所形成的直线点，并拟合两条直线；从子区域轮廓中分离子区域上边和下边上的点所形成的曲线点，并拟合椭圆曲线，分别求直线与该椭圆曲线的交点，获得作为靶标图案特征点的子区域顶点在图像坐标系中的坐标；

(4)在识别子区域且获得特征点图像坐标后，根据所述的特征点在所述子区域上的分布位置识别特征点；

所述特征点在弹丸坐标系下的对应坐标为已知量，利用所述特征点在弹丸坐标系中的坐标和其对应的图像坐标系中的坐标，经坐标系变换解算出弹丸相对于摄像机的位姿。

2.根据权利要求1所述的应用靶标图案的弹丸单目视频位姿测量方法，其特征在于，为所述每个子区域设置唯一的三位二进制编码；所述的三位二进制编码第一位代表子区域颜色，第二位和第三位代表所述每个子区域内包含的编码区域的数量。

3.根据权利要求1或2所述的应用靶标图案的弹丸单目视频位姿测量方法，其特征在于，在弹丸的位姿变化幅度较大的场合，设置两套或者两套以上的摄像机和相应的图像处理单元，分别组成两套或者两套以上单目视频测量系统；并调整摄像机的位置和朝向，使其在弹丸姿态变化过程中至少有一个摄像机能拍摄到弹丸侧面。