基于机械臂末端单目视觉的动态目标位置和姿态测量方法

摘要

本发明涉及一种基于机械臂末端单目视觉的动态目标位置和姿态测量方法，属于视觉测量领域。该方法首先进行摄像机标定和手眼标定；然后用摄像机拍摄两幅图像，利用尺度不变特征提取方法，提取图像中目标区域的空间特征点并进行特征点匹配；利用对极几何约束方法，求解两幅图像之间的基础矩阵，得到本质矩阵，进而求解摄像机的旋转变换矩阵和位移变换矩阵；之后对特征点进行三维重构和尺度校正；最后利用重构后的特征点构建目标坐标系，获得目标相对摄像机的位置和姿态。本发明方法采用单目视觉，简化计算过程，使用了手眼标定，可简化摄像机位姿信息测量过程中错误解的剔除。本方法适用于测量静止目标和低动态目标的相对位姿。

说明书

技术领域

本发明涉及一种基于机械臂末端单目视觉的动态目标位置和姿态测量方法，属于视觉 测量领域。

背景技术

近年来，视觉测量技术得到了深入的发展，广泛应用与工业检测、机器人视觉、实时 测量等过程，采用双目视觉和单目视觉是比较常用的方法。相对双目视觉，单目视觉不仅 克服了双目视觉中基线固定的约束，同时将单目摄像机安装在载体末端，可通过载体的运 动对目标不同的特征部位进行测量，且不会发生遮挡现象。

但现有单目视觉测量方法都假设场景静止，即场景中机械臂平台和被测目标都静止不 动，而没有考虑在摄像机移动的同时，测量目标可能存在一定的缓慢运动。因而不能准确 完成动态场景中的测量任务。

经对现有的技术文献检索分析，发现曾庆化等人的发明专利“一种用于室内环境的单 目视觉/惯性全自主导航方法”，申请号为201110273402.5，该专利实现的方法以图像局部 不变特性和对极几何为基础估计摄像机位姿信息，最后将视觉导航信息和惯导信息结合获 得相对精确、可靠的导航信息，并进行特征点3D重构，获得环境信息地图，完成载体的 自主导航。但该方法需要额外的惯性器件获取惯导信息，并且其获取特征点3D信息用来 构建环境信息，而没有对目标位姿进行测量。

发明内容

本发明的目的是提出一种基于机械臂末端单目视觉的动态目标位置和姿态测量方法， 用于测量动态目标的位置和姿态。

本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法，包括以下步 骤：

（1）设摄像机拍摄图像的平面坐标系为（u，v），采用张正友棋盘格标定法，得到摄 像机的内参数矩阵M，$M=\left(\begin{array}{ccc}\frac{f}{\mathrm{dx}}& 0& u_0\\ 0& \frac{f}{\mathrm{dy}}& v_0\\ 0& 0& 1\end{array}\right),$以及摄像机的径向畸变参数k₁和k₂以及切向畸 变参数k₃和k₄，其中，f为摄像机焦距，为摄像机在图像平面坐标系的u轴上的归一 化焦距，为摄像机在图像平面坐标系的v轴上的归一化焦距，（u₀，v₀）为摄相机光轴与 摄像机拍摄图像平面交点的像素坐标；

（2）利用手眼标定法，得到机械臂末端和摄像机之间的旋转变换矩阵，记为以 及机械臂末端和摄像机之间的位移变换矩阵，记为为3×3矩阵，为3×1矩阵；

（3）在目标上设置两个标记点，利用机械臂末端摄像机，在两个不同位置拍摄到该 目标的两幅图像，从两幅图像中区分出目标区域和背景区域，利用尺度不变特征提取方法， 分别从两幅图像中提取出目标区域的空间特征点，该空间特征点包括目标上的两个标记 点；

（4）根据上述空间特征点，对两幅图像中目标区域的空间特征点进行匹配，得到初 始匹配结果，并根据初始匹配结果，对步骤（3）的尺度不变特征提取方法中的参数进行 调整，以获得两幅图像之间有8对以上匹配特征点，匹配特征点中包括目标上的两个标记 点；

（5）根据步骤（4）的两幅图像之间8对以上匹配特征点，利用对极几何约束方法， 求解两幅图像之间的基础矩阵F；

（6）根据上述步骤（1）的摄像机内参数矩阵M和步骤（5）的基础矩阵F，求解摄 像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵E，利用本质矩阵E，求解 摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转变换矩阵和位移变换矩阵 得到四组候选解，具体过程如下：

（6-1）利用如下公式，根据步骤（5）的基础矩阵F和步骤（1）的摄像机内参数矩 阵M，计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵E：

E＝M^TFM，其中M^T为矩阵M的转置矩阵；

（6-2）本质矩阵E只和旋转变换矩阵与位移变换矩阵有关，式中 为的反对称矩阵，利用特征值分解方法，对上述本质矩阵E进行分解， E＝Udiag(s,s,0)V^Τ，得到四组候选解：

$\left(\begin{array}{cc}{}_c{}^{c}R={\mathrm{UWV}}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZU}}^T\\ {}_c{}^{c}R={\mathrm{UWV}}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZ}}^T{U}^T\\ {}_c{}^{c}R={\mathrm{UW}}^T{V}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZU}}^T\\ {}_c{}^{c}R={\mathrm{UW}}^T{V}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZ}}^T{U}^T\end{array}\right),$其中$W=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right),$$Z=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right);$

（7）根据机械臂的控制参数和上述步骤（2）得到的机械臂末端和摄像机之间的旋转 变换矩阵和位移变换矩阵计算得到摄像机在拍摄目标的两幅图像时的两个不同位 置之间的旋转初值和位移初值根据旋转初值和位移初值得到步骤（6）中 的四组候选解中的正解和具体过程如下：

（7-1）根据机械臂的控制参数得到机械臂末端的旋转变换矩阵和位移变换矩阵根据上述步骤（2）得到的机械臂末端和摄像机之间的旋转变换矩阵和位移变换矩阵 通过下式计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转初值和 位移初值

${}_c{}^c{R}_0={}_e{}^{c}R_e^{e}R_e^c{R}^{-1}$

${}_c{}^c{t}_0={}_e{}^{c}R_e^{e}t+{}_e{}^{c}t-{}_c{}^c{R}_0{}_e{}^{c}t$

（7-2）分别计算上述步骤（6）得到的旋转矩阵候选解与旋转初值之间的旋转矩阵， 得到该旋转矩阵的范数，将与该旋转矩阵范数中最小范数值相对应的候选解作为摄像机旋 转变换的正解

（7-3）分别计算上述步骤（6）得到的位移矩阵候选解与位移初值之间的夹角，将 与该夹角中最小夹角相对应的候选解作为摄像机位移变换的正解

（8）根据上述步骤（7）中得到的摄像机旋转变换矩阵和位移变换矩阵对上 述步骤（4）得到的两幅图像之间的匹配特征点进行特征点三维重构，根据目标上的两个 标记点之间的距离，对摄像机的位移变换和三维重构的特征点进行尺度校正，包括以下步 骤：

（8-1）利用三角测量方法，得到步骤（4）的两幅图像之间所有匹配特征点在摄像机 坐标系中的三维坐标P′；

（8-2）从上述所有匹配特征点的三维坐标中，得到目标上两个标记点之间的计算距 离d；

（8-3）根据目标上的两个标记点之间的物理距离D，得到该物理距离与上述计算距 离之间的比值

（8-4）根据上述比值k，对上述摄像机的位移变换矩阵和特征点在摄像机坐标系 中的三维坐标，根据下式进行尺度校正，得到尺度校正后的摄像机在拍摄目标的两幅图像 时所在的两个不同位置之间的物理位移以及三维重构特征点的物理坐标P：

P＝kP'

（9）构建目标坐标系，求解目标的相对摄像机的位置和姿态，具体过程如下：

（9-1）根据上述步骤（8）得到的三维重构特征点的物理坐标，构建目标坐标系，以 目标上3个特征点为例构建目标坐标系，i，j，k分别为目标坐标系XYZ坐标轴的单位向 量，坐标原点为点A，AB为X轴，以ABC平面中垂直AB的方向为Y轴，然后依据右手法 则得到Z轴，设P_a，P_b，P_c为3特征点在摄像机坐标系下的坐标，各坐标轴的单位向量计算 如下：

$\left(\begin{array}{c}i=\mathrm{norm}\left(\overrightarrow{P_b-P_a}\right),s.t.\left|\right|i\left|\right|=1\\ j=\mathrm{norm}(\overrightarrow{P_c-P_a}-\overrightarrow{P_c-P_a}·i)\\ k=i\times j\end{array},s.t.\left|\right|j\left|\right|=1\right);$

（9-2）依据如下公式计算任意目标特征点在目标坐标系中的坐标_oP_i：

_oP_i＝[i j k](P_i-P_a)

其中，P_i为任意特征点的重构坐标；

（9-3）利用坐标系转换方法或基于点特征定位的方法，根据任意目标特征点在目标坐 标系中的坐标_oP_i和重构坐标P_i，计算目标的相对摄像机的位置和相对旋转矩阵

（9-4）根据上述相对旋转矩阵计算目标的相对姿态，以欧拉角XYZ顺序表示目 标姿态（α,β,γ)，则旋转矩阵表示如下：

$R_{\mathrm{oc}}=R(\alpha ,\beta ,\gamma )=R_x{R}_y{R}_z$

$=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\alpha \right)& \sin \left(\alpha \right)\\ 0& -\sin \left(\alpha \right)& \cos \left(\alpha \right)\end{array}\right)\left(\begin{array}{ccc}\cos \left(\beta \right)& 0& -\sin \left(\beta \right)\\ 0& 1& 0\\ \sin \left(\beta \right)& 0& \cos \left(\beta \right)\end{array}\right)\left(\begin{array}{ccc}\cos \left(\gamma \right)& \sin \left(\gamma \right)& 0\\ -\sin \left(\gamma \right)& \cos \left(\gamma \right)& 0\\ 0& 0& 1\end{array}\right)$

$=\left(\begin{array}{ccc}\cos \left(\beta \right)\cos \left(\gamma \right)& \cos \left(\beta \right)\sin \left(\gamma \right)& -\sin \left(\beta \right)\\ \sin \left(\alpha \right)\sin \left(\beta \right)\cos \left(\gamma \right)-\cos \left(\alpha \right)\sin \left(\gamma \right)& \sin \left(\alpha \right)\sin \left(\beta \right)\sin \left(\gamma \right)+\cos \left(\alpha \right)\cos \left(\gamma \right)& \sin \left(\alpha \right)\cos \left(\beta \right)\\ \cos \left(\alpha \right)\sin \left(\beta \right)\cos \left(\gamma \right)+\sin \left(\alpha \right)\sin \left(\gamma \right)& \cos \left(\alpha \right)\sin \left(\beta \right)\sin \left(\gamma \right)-\sin \left(\alpha \right)\cos \left(\gamma \right)& \cos \left(\alpha \right)\cos \left(\beta \right)\end{array}\right)$

从上述旋转矩阵中，得到目标的相对摄像机的姿态为：

β=asin(-R_oc(1,3))

$\gamma =a\sin \left(\frac{R_{\mathrm{oc}}\left(\mathrm{1,2}\right)}{\cos \left(\beta \right)}\right).$

$a=a\sin \left(\frac{R_{\mathrm{oc}}\left(\mathrm{2,3}\right)}{\cos \left(\beta \right)}\right)$

至此，已得到目标的相对位置和姿态。

本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法，具有以下优 点：

1、本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法，采用单目 视觉，简化计算过程法，动态目标位置和姿态测量所需的硬件，而且克服了双目视觉的不 足。

2、本发明测量方法中，摄像机安装在机械臂末端，可对目标的不同特征部位进行测量， 且不会发生遮挡现象。

3、本发明测量方法中使用了手眼标定，可简化摄像机位姿信息测量过程中错误解的剔 除。

4、本发明测量方法适用于测量静止目标和低动态目标的相对位姿。

附图说明

图1是本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法的流程 框图。

图2是本发明测量方法中使用的手眼标定原理示意图。

图3是本发明测量方法中立体视觉对极几何关系示意图。

图4是本发明测量方法中目标坐标系示意图。

具体实施方式

本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法，其流程框图 如图1所示，包括以下步骤：

（1）设摄像机拍摄图像的平面坐标系为（u，v），采用张正友棋盘格标定法，得到摄 像机的内参数矩阵M，$M=\left(\begin{array}{ccc}\frac{f}{\mathrm{dx}}& 0& u_0\\ 0& \frac{f}{\mathrm{dy}}& v_0\\ 0& 0& 1\end{array}\right),$以及摄像机的径向畸变参数k₁和k₂以及切向畸 变参数k₃和k₄，其中，f为摄像机焦距，为摄像机在图像平面坐标系的u轴上的归一 化焦距，为摄像机在图像平面坐标系的v轴上的归一化焦距，（u₀，v₀）为摄相机光轴与 摄像机拍摄图像平面交点的像素坐标；

（2）利用手眼标定法，得到机械臂末端和摄像机之间的旋转变换矩阵，记为以 及机械臂末端和摄像机之间的位移变换矩阵，记为为3×3矩阵，为3×1矩阵；

图2是手眼标定基本原理示意图。如图2所示：C_obj表示定标物坐标系，C_c1和C_e1表 示机械臂运动前的摄像机坐标系和机械臂末端坐标系，C_c2和C_e2表示机械臂运动后的摄像 机坐标系和机械臂末端坐标系；A、B、C、D、X分别表示其连接的两个坐标系之间的相对 方位，为4×4矩阵，包括两个坐标系之间的旋转矩阵R_i和平移向量t_i，下标i表示矩阵名 称，以X为例进行说明，则$X=\left(\begin{array}{cc}{R}_2& t_x\\ 0& 1\end{array}\right),$由图3可得到如下关系：

CX＝XD

展开后可得到：

R_cR_x＝R_xR_d

R_ct_x+t_c＝R_xt_d+t_x

标定时，首先控制机械臂运动，确保该运动不是纯平移，得到A和B，从控制器参数 中得到D，根据上式得到一组约束方程；控制机械臂再次运动，确保此次运动和上次运动 的旋转轴不互相平行，且此次运动也不是纯平移，同理，根据上式得到一组约束方程；根 据这两组约束方程，求解R_x和t_x，即为机械臂末端和摄像机之间的旋转变换矩阵和位 移变换矩阵从而完成了手眼标定。

（3）在目标上设置两个标记点，利用机械臂末端摄像机，在两个不同位置拍摄到该 目标的两幅图像，从两幅图像中区分出目标区域和背景区域，利用尺度不变特征提取方法， 分别从两幅图像中提取出目标区域的空间特征点，该空间特征点包括目标上的两个标记 点；

（4）根据上述空间特征点，对两幅图像中目标区域的空间特征点进行匹配，得到初 始匹配结果，并根据初始匹配结果，对步骤（3）的尺度不变特征提取方法中的参数进行 调整，以获得两幅图像之间有8对以上匹配特征点，匹配特征点中包括目标上的两个标记 点；

（5）根据步骤（4）的两幅图像之间8对以上匹配特征点，利用对极几何约束方法， 求解两幅图像之间的基础矩阵F；

图3表示立体视觉中的对极几何约束关系。如图3所示：基线为连接两摄像机光心O(O') 的直线，对极点e(e')为基线与像平面的交点，对极平面为过基线与特定点P的平面，极 线是对极平面与像平面的交线，p(p')为P在成像平面上的投影，投影点和其对应的极线 满足关系l'＝Fp，空间中任意点在两图像平面上的成像点像素坐标满足如下的对极几何约 束关系，即极线几何约束：

p'Fp＝0

基础矩阵F的秩为2，自由度为7，可根据8点算法，由步骤（4）得到的8对以上的 特征点对，计算基础矩阵F。

（6）根据上述步骤（1）的摄像机内参数矩阵M和步骤（5）的基础矩阵F，求解摄 像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵E，利用本质矩阵E，求解 摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转变换矩阵和位移变换矩阵 得到四组候选解，具体过程如下：

（6-1）利用如下公式，根据步骤（5）的基础矩阵F和步骤（1）的摄像机内参数矩 阵M，计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵E：

E＝M^TFM，其中M^T为矩阵M的转置矩阵；

（6-2）本质矩阵E只和旋转变换矩阵与位移变换矩阵有关，式中 为的反对称矩阵，利用特征值分解方法，对上述本质矩阵E进行分解， E＝Udiag(s,s,0)V^Τ，得到四组候选解：

$\left(\begin{array}{cc}{}_c{}^{c}R={\mathrm{UWV}}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZU}}^T\\ {}_c{}^{c}R={\mathrm{UWV}}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZ}}^T{U}^T\\ {}_c{}^{c}R={\mathrm{UW}}^T{V}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZU}}^T\\ {}_c{}^{c}R={\mathrm{UW}}^T{V}^T& {\left[{}_c{}^{c}t\right]}_{\times }={\mathrm{UZ}}^T{U}^T\end{array}\right),$其中$W=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right),$$Z=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right);$

（7）根据机械臂的控制参数和上述步骤（2）得到的机械臂末端和摄像机之间的旋转 变换矩阵和位移变换矩阵计算得到摄像机在拍摄目标的两幅图像时的两个不同位 置之间的旋转初值和位移初值根据旋转初值和位移初值得到步骤（6）中 的四组候选解中的正解和具体过程如下：

（7-1）根据机械臂的控制参数得到机械臂末端的旋转变换矩阵和位移变换矩阵根据上述步骤（2）得到的机械臂末端和摄像机之间的旋转变换矩阵和位移变换矩阵 通过下式计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转初值和 位移初值

${}_c{}^c{R}_0={}_e{}^{c}R_e^{e}R_e^c{R}^{-1}$

${}_c{}^c{t}_0={}_e{}^{c}R_e^{e}t+{}_e{}^{c}t-{}_c{}^c{R}_0{}_e{}^{c}t$

（7-2）分别计算上述步骤（6）得到的旋转矩阵候选解与旋转初值之间的旋转矩阵， 得到该旋转矩阵的范数，将与该旋转矩阵范数中最小范数值相对应的候选解作为摄像机旋 转变换的正解

（7-3）分别计算上述步骤（6）得到的位移矩阵候选解与位移初值之间的夹角，将 与该夹角中最小夹角相对应的候选解作为摄像机位移变换的正解

（8）根据上述步骤（7）中得到的摄像机旋转变换矩阵和位移变换矩阵对上 述步骤（4）得到的两幅图像之间的匹配特征点进行特征点三维重构，根据目标上的两个 标记点之间的距离，对摄像机的位移变换和三维重构的特征点进行尺度校正，包括以下步 骤：

（8-1）利用三角测量方法，得到步骤（4）的两幅图像之间所有匹配特征点在摄像机 坐标系中的三维坐标P′；

空间中任意点在摄像机坐标系下的投影坐标可由如下公式计算：

$Z_{c1}\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=M_{3\times 4}^1\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)=\begin{array}{c}\left(\begin{array}{cccc}{m}_{11}^1& m_{12}^1& m_{13}^1& m_{14}^1\\ m_{21}^1& m_{22}^1& m_{23}^1& m_{24}^1\\ m_{31}^1& m_{32}^1& m_{33}^1& m_{34}^1\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)\end{array}$

$Z_{c2}\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)=M_{3\times 4}^2\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)=\begin{array}{c}\left(\begin{array}{cccc}{m}_{11}^2& m_{12}^2& m_{13}^2& m_{14}^2\\ m_{21}^2& m_{22}^2& m_{23}^2& m_{24}^2\\ m_{31}^2& m_{32}^2& m_{33}^2& m_{34}^2\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)\end{array}$

其中，[u₁,v₁,1]^T和[u₂,v₂,1]^T分别为摄像机拍摄的两幅图像中对应点的像素齐次坐标， [X,Y,Z,1]^T为空间点在世界坐标系下的齐次坐标，设世界坐标系与摄像机运动前的摄像机 坐标系重合，则$M_{3\times 4}^1=M\left(\begin{array}{cc}I& 0\end{array}\right),$将上式展开，消除Z_c1和Z_c2，可得 到关于X，Y，Z的四个线性方程：

$\left(\begin{array}{c}(u_1{m}_{31}^1-m_{11}^1)X+\left(u_1{m}_{32}^1-m_{12}^1\right)Y+(u_1{m}_{33}^1-m_{13}^1)Z=m_{14}^1-u_1{m}_{34}^1\\ (v_1{m}_{31}^1-m_{21}^1)X+(v_1{m}_{32}^1-m_{22}^1)Y+(v_1{m}_{33}^1-m_{23}^1)Z=m_{24}^1-v_1{m}_{34}^1\\ (u_2{m}_{31}^2-m_{11}^2)X+(u_2{m}_{32}^2-m_{12}^2)Y+(u_2{m}_{33}^2-m_{13}^2)Z=m_{14}^2-u_2{m}_{34}^2\\ (v_2{m}_{31}^2-m_{21}^2)X+(v_2{m}_{32}^2-m_{22}^2)Y+(v_2{m}_{33}^2-m_{23}^2)Z=m_{24}^2-v_2{m}_{34}^2\end{array}\right)$

上述方程组有3个变量4个方程，由于已经假设对应像素点和摄像机坐标原点的直线 一定相交，即方程组必定有唯一解，实际上，由于数据总有噪声，可采用最小二乘法求解 X，Y，Z，从而完成特征点的三维重构。

（8-2）从上述所有匹配特征点的三维坐标中，得到目标上两个标记点之间的计算距 离d；

（8-3）根据目标上的两个标记点之间的物理距离D，得到该物理距离与上述计算距 离之间的比值

（8-4）根据上述比值k，对上述摄像机的位移变换矩阵和特征点在摄像机坐标系 中的三维坐标，根据下式进行尺度校正，得到尺度校正后的摄像机在拍摄目标的两幅图像 时所在的两个不同位置之间的物理位移以及三维重构特征点的物理坐标P：

P＝kP'

（9）构建目标坐标系，求解目标的相对摄像机的位置和姿态，具体过程如下：

（9-1）根据上述步骤（8）得到的三维重构特征点的物理坐标，构建目标坐标系，

本发明的一个实施例中，以目标上3个特征点为例构建目标坐标系，如图4所示：A， B，C为目标上的3个特征点，i，j，k分别为目标坐标系XYZ坐标轴的单位向量，坐标原 点为点A，AB为X轴，以ABC平面中垂直AB的方向为Y轴，然后依据右手法则得到Z轴， 设P_a，P_b，P_c为3特征点在摄像机坐标系下的坐标，各坐标轴的单位向量计算如下：

$\left(\begin{array}{c}i=\mathrm{norm}\left(\overrightarrow{P_b-P_a}\right),s.t.\left|\right|i\left|\right|=1\\ j=\mathrm{norm}(\overrightarrow{P_c-P_a}-\overrightarrow{P_c-P_a}·i)\\ k=i\times j\end{array},s.t.\left|\right|j\left|\right|=1\right);$

（9-2）依据如下公式计算任意目标特征点在目标坐标系中的坐标_oP_i：

_oP_i＝[i j k](P_i-P_a)

其中，P_i为任意特征点的重构坐标；

（9-3）利用坐标系转换方法或基于点特征定位的方法，根据任意目标特征点在目标坐 标系中的坐标_oP_i和重构坐标P_i，计算目标的相对摄像机的位置和相对旋转矩阵

（9-4）根据上述相对旋转矩阵计算目标的相对姿态，本发明的一个实施实例中， 以欧拉角XYZ顺序表示目标姿态（α,β,γ)，则旋转矩阵表示如下：

$R_{\mathrm{oc}}=R(\alpha ,\beta ,\gamma )=R_x{R}_y{R}_z$

$=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\alpha \right)& \sin \left(\alpha \right)\\ 0& -\sin \left(\alpha \right)& \cos \left(\alpha \right)\end{array}\right)\left(\begin{array}{ccc}\cos \left(\beta \right)& 0& -\sin \left(\beta \right)\\ 0& 1& 0\\ \sin \left(\beta \right)& 0& \cos \left(\beta \right)\end{array}\right)\left(\begin{array}{ccc}\cos \left(\gamma \right)& \sin \left(\gamma \right)& 0\\ -\sin \left(\gamma \right)& \cos \left(\gamma \right)& 0\\ 0& 0& 1\end{array}\right)$

$=\left(\begin{array}{ccc}\cos \left(\beta \right)\cos \left(\gamma \right)& \cos \left(\beta \right)\sin \left(\gamma \right)& -\sin \left(\beta \right)\\ \sin \left(\alpha \right)\sin \left(\beta \right)-\cos \left(\gamma \right)-\cos \left(\alpha \right)\sin \left(\gamma \right)& \sin \left(\alpha \right)\sin \left(\beta \right)\sin \left(\gamma \right)+\cos \left(\alpha \right)\cos \left(\gamma \right)& \sin \left(\alpha \right)\cos \left(\beta \right)\\ \cos \left(\alpha \right)\sin \left(\beta \right)\cos \left(\gamma \right)+\sin \left(\alpha \right)\sin \left(\gamma \right)& \cos \left(\alpha \right)\sin \left(\beta \right)\sin \left(\gamma \right)-\sin \left(\alpha \right)\cos \left(\gamma \right)& \cos \left(\alpha \right)\cos \left(\beta \right)\end{array}\right)$

从上述旋转矩阵中，得到目标的相对摄像机的姿态为：

β=asin(-R_oc(1,3))

$\gamma =a\sin \left(\frac{R_{\mathrm{oc}}\left(\mathrm{1,2}\right)}{\cos \left(\beta \right)}\right).$

$a=a\sin \left(\frac{R_{\mathrm{oc}}\left(\mathrm{2,3}\right)}{\cos \left(\beta \right)}\right)$

至此，已得到目标的相对位置和姿态。