CN102589530A - 基于二维相机和三维相机融合的非合作目标位姿测量方法 - Google Patents

Abstract

一种基于二维相机和三维相机融合的非合作目标位姿测量方法，是将二维相机和三维相机组合成一种相机测量系统，对非合作目标进行相对位姿测量，通过二维相机提供的高分辨率二维图像信息和三维相机提供的灰度、深度及调制信号的幅度信息进行数据融合获得高分辨率三维图像，高效准确地提取特征点信息，计算出非合作目标相对位置和姿态参数。本发明能够有效克服迭代运算繁琐和特征点提取困难等缺点，快速求解满出足精度要求的位姿信息。

Description

基于二维相机和三维相机融合的非合作目标位姿测量方法

技术领域

本发明涉及一种对于非合作目标进行位姿测量的方法，属于三维视觉的目标位姿测量领域。

背景技术

故障航天器的在轨捕获与维修、空间垃圾清除等空间活动已成为航天技术发展需要面对和解决的重要课题，非合作目标的位姿测量技术又是解决这些问题必需的关键技术。与合作目标航天器位姿测量不同，非合作目标本身没有配备目标标识器和交会敏感器，这就使得非合作目标位姿测量具有一定的困难。非合作目标的相对位姿测量研究有着广泛和重要的意义。

现有的一些技术有基于单目视觉的非合作目标位姿测量方法和基于双目视觉的非合作目标位姿测量方法。单目视觉测量算法有相似迭代算法，该算法根据相似三角形原理推导出一种深度迭代机制，通过深度估计和绝对方位问题求解两个步骤对相对位置和姿态参数进行迭代计算；基于双目视觉是对所提取的左、右相机图像的点特征进行3D重构，得到各点在世界坐标系中的坐标，并据此建立目标坐标系，求出其相对于世界坐标系的位置和姿态。

以上方法在具体实现上存在以下不足：

1、多数方法在测量时需要提供非合作目标的详细模型或者结构等特点，这使得方法的适应性和普遍性降低。

2、对于双目立体视觉来说，测量过程中各种误差和特征点匹配错误等因素导致解析解算法可能产生很大的误差，达不到精度要求。

3、迭代求解的方式出现多值结果，带来识别的困难，迭代求解的收敛性和收敛速度将极大影响测量精度，而迭代过程的收敛性和收敛速度主要取决于迭代算法和初值选取。

发明内容

本发明针对上述现有技术所存在的不足，提供一种基于二维相机和三维相机融合的非合作目标位姿测量方法，以克服迭代运算繁琐和特征点提取困难等缺点，快速求解出满足精度要求的位姿信息。

本发明为解决技术问题采用如下技术方案：

本发明基于二维相机和三维相机融合的非合作目标位姿测量方法，按如下步骤进行：

1)将二维相机叠加在三维相机的正上方，组合成相机测量系统，通过标定使得二维相机光心和三维相机光心在同一个竖直线上；由所述二维相机获取非合作目标的二维图像信息，由所述三维相机获取非合作目标的灰度信息、深度信息及调制信号的幅度信息，将所获得的二维图像信息、灰度信息、深度信息及幅度信息通过数据融合方法获得非合作目标三维图像；

2)在所述非合作目标三维图像中选取非合作目标中的三个非共线的特征点作为标记点P_i，i＝1，2，3；根据三维空间坐标计算方法计算出各标记点P_i在非合作目标坐标系O_o-X_oY_oZ_o下的标记点坐标P_i＝(X_i，Y_i，Z_i)，i＝1，2，3，通过标记点P₁和标记点P₂之间的坐标相减获得向量

通过标记点P₁和标记点P₃之间的坐标相减获得向量

对所述向量和向量

分别进行向量单位化后求出两个线性无关的单位列向量

和

令

为

所构成的单位列向量，构造出三个标记点Pi在非合作目标坐标系O_o-X_oY_oZ_o下的向量组： $n_o=[\stackrel{\→}{n_{o1}},\stackrel{\→}{n_{o2}},\stackrel{\→}{n_{o3}}];$

所述非合作目标坐标系O_o-X_oY_oZ_o建立方法为：以P₁为坐标原点，P₁P₂的连线为X轴，Y轴垂直于X轴且P₃在由X轴和Y轴所确定的平面上，Z轴垂直于由所述X轴和Y轴确定的平面，并与所述X轴和Y轴构成右手系；

3)建立二维相机像平面坐标系O₂-X₂Z₂：以二维相机光轴与二维相机像平面A₂的交点为二维相机像平面坐标系O₂-X₂Z₂的坐标原点O₂，X₂轴与图像像素横方向U平行，Z₂轴与图像像素竖直方向V平行；

建立二维相机坐标系O_s2-X_s2Y_s2Z_s2：以二维相机光心为二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2，Y_s2轴为二维相机光轴方向，X_s2轴与二维相机像平面坐标系O₂-X₂Z₂的X₂轴平行，Z_s2轴与二维相机像平面坐标系O₂-X₂Z₂的Z₂轴平行，并与所述X_s2轴和Y_s2轴构成右手系；

通过各标记点P_i的像点在二维相机像平面坐标系O₂-X₂Z₂下的图像像素坐标(u_i，v_i)按如下方法计算出各标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标S_i：

a、将各标记点P_i的像点在二维相机像平面坐标系O₂-X₂Z₂下的图像像素坐标(u_i，v_i)转化为像点在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标(u_i，v_i，f)，其中f为二维相机焦距；

b、由各标记点P_i的像点在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标(u_i，v_i，f)确定出二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2到各标记点P_i的方向向量，获得

的单位方向向量

c、由三维相机测得各标记点P_i到三维相机光心的距离为L_i，并通过式(1)得出各标记点P_i到二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2的距离D_i，

$D_i=\sqrt{h^2+{L_i}^2+2{\mathrm{hL}}_i\cos \mathrm{\α}}---\left(1\right)$

h为二维相机光心与三维相机光心之间的距离，α为O_s1P′_i与O_s1O_s2的夹角，P′_i为标记点P_i在三维相机像平面A₁上的像点；

d、由式(2)求出各标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标S_i，i＝1，2，3；

$S_i=D_i\stackrel{\→}{a_i}---\left(2\right)$

通过各标记点P_i之间的坐标相减获得向量

和

向量

和

单位化后求出两个线性无关的单位列向量

和令

为

所构成的单位列向量；构造出三个标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的向量组： $n_s=[\stackrel{\→}{n_{s1}},\stackrel{\→}{n_{s2}},\stackrel{\→}{n_{s3}}];$

4)由向量组 $n_o=[\stackrel{\→}{n_{o1}},\stackrel{\→}{n_{o2}},\stackrel{\→}{n_{o3}}]$ 和向量组 $n_s=[\stackrel{\→}{n_{s1}},\stackrel{\→}{n_{s2}},\stackrel{\→}{n_{s3}}]$ 通过式(3)和式(4)的运算得出旋转矩阵R和平移矩阵T；

$R=n_s{n_o}^{-1}=\left(\begin{array}{ccc}{r}_{00}& r_{01}& r_{02}\\ r_{10}& r_{11}& r_{12}\\ r_{20}& r_{21}& r_{22}\end{array}\right)---\left(3\right)$

$T=S_i-{\mathrm{RP}}_i=\left(\begin{array}{c}{t}_0\\ t_1\\ t_2\end{array}\right)---\left(4\right)$

5)将所述旋转矩阵R通过式(5)计算获得绕二维相机坐标系的x轴、y轴和z轴沿逆时针方向的转角α、β和γ；旋转顺序是从x轴到y轴再到z轴；所述转角α、β和γ为非合作目标相对于二维相机坐标系O_s2-X_s2Y_s2Z_s2的三个姿态角；

$\left\{\begin{array}{c}\mathrm{\γ}=-\arctan \frac{r_{01}}{r_{00}}\\ \mathrm{\β}=\arctan \frac{r_{20}}{r_{00}\cos \mathrm{\γ}-r_{10}\sin \mathrm{\γ}}\\ \mathrm{\α}=\arctan \frac{r_{02}\sin \mathrm{\γ}+r_{12}\cos \mathrm{\γ}}{r_{01}\sin \mathrm{\γ}+r_{11}\cos \mathrm{\γ}}\end{array}\right.---\left(5\right)$

6)所述平移矩阵T，其中t₀、t₁和t₂分别为非合作目标坐标系O_o-X_oY_oZ_o相对于二维相机坐标系O_s2-X_s2Y_s2Z_s2在x轴、y轴和z轴上的平移量；

本发明基于二维相机和三维相机融合的非合作目标位姿测量方法，在步骤2)中所述三维空间坐标计算方法是：

P₁为非合作目标坐标系O_o-X_oY_oZ_o的坐标原点，在非合作目标坐标系O_o-X_oY_oZ_o下的坐标为P₁＝(0，0，0)，三维相机直接获得各标记点P_i到三维相机光心O_s1的距离L_i，i＝1，2，3，各标记点P_i的像点到三维相机光心O_s1的距离O_s1P′_i为已知，O_s1P′₁和O_s1P′₂之间的夹角为θ₁，O_s1P′₁和O_s1P′₃之间的夹角为θ₂，O_s1P′₂和O_s1P′₃之间的夹角为θ₃，则分别获得标记点P₁与P₂之间的距离 $a=\sqrt{L_1^2+L_2^2-2L_1{L}_2\cos {\mathrm{\θ}}_1},$ 标记点P₁与P₃之间的距离 $b=\sqrt{L_1^2+L_3^2-2L_1{L}_3\cos {\mathrm{\θ}}_2},$ 标记点P₂与P₃之间的距离 $c=\sqrt{L_2^2+L_3^2-2L_2{L}_3\cos {\mathrm{\θ}}_3}.$

则标记点P₂在非合作目标坐标系O_o-X_oY_oZ_o下的坐标

$P_2=(\sqrt{L_1^2+L_2^2-2L_1{L}_2\cos {\mathrm{\θ}}_1},\mathrm{0,0})$

由式(6)求得标记点P₁和P₂的连线与标记点P₁和P₃的连线之间的夹角为φ，

$\mathrm{\φ}=\arccos \left(\frac{a^2+b^2-c^2}{2\mathrm{ab}}\right)---\left(6\right)$

则标记点P₃的横坐标为bcosφ、纵坐标为bsinφ，获得标记点P₃在非合作目标坐标系O_o-X_oY_oZ_o下的坐标为P₃＝(bcosφ，bsinφ，0)。

与现有技术相比，本发明的有益效果体现在：

1、本发明采用的三维相机是通过记录光线在空间中的往返时间，获取场景的三维信息。在特征点提取和边缘检测等过程中不需要进行复杂的计算，具有速度快，计算量小，精度高等特点。

2、本发明采用二维相机和三维相机组合测量模式。通过二维高分辨率图像信息和三维深度等信息融合获得高分辨率三维图像信息，能提高位姿测量的精确性。

3、本发明采用的三维相机的测距成像仪是一种主动式视觉传感器，包含有一个主动调制光源，所以测量时不受外界光照条件限制。

附图说明

图1本发明方法的测量系统图；

图2本发明方法的流程图；

图3本发明方法中非合作目标坐标系、二维相机坐标系、三维相机坐标系，像坐标系之间的关系；

图中标号：1非合作目标；2多自由度旋转平台；3二维相机，4三维相机；5数据处理系统。

具体实施方法

参见图1，本实施例的系统构成包括非合作目标1、多自由度旋转平台2、二维相机3、三维相机4和数据处理系统5。

参见图2，本实施例中基于二维相机和三维相机融合的非合作目标位姿测量方法的工作流程为：

本实施例中基于二维相机和三维相机融合的非合作目标位姿测量方法是按以下步骤进行：

1)将二维相机叠加在三维相机的正上方，组合成相机测量系统，通过标定使得二维相机光心和三维相机光心在同一个竖直线上；由所述二维相机获取非合作目标的二维图像信息，由所述三维相机获取非合作目标的灰度信息、深度信息及调制信号的幅度信息，将所获得的二维图像信息、灰度信息、深度信息及幅度信息通过数据融合方法获得非合作目标三维图像；

2)在所述非合作目标三维图像中选取非合作目标中的三个非共线的特征点作为标记点P_i，i＝1，2，3；根据三维空间坐标计算方法计算出各标记点P_i在非合作目标坐标系O_o-X_oY_oZ_o下的标记点坐标P_i＝(X_i，Y_i，Z_i)，i＝1，2，3，通过标记点P₁和标记点P₂之间的坐标相减获得向量通过标记点P₁和标记点P₃之间的坐标相减获得向量

对所述向量

和向量

分别进行向量单位化后求出两个线性无关的单位列向量

和

令为

所构成的单位列向量，构造出三个标记点P_i在非合作目标坐标系O_o-X_oY_oZ_o下的向量组： $n_o=[\stackrel{\→}{n_{o1}},\stackrel{\→}{n_{o2}},\stackrel{\→}{n_{o3}}];$

所述非合作目标坐标系O_o-X_oY_oZ_o建立方法为：以P₁为坐标原点，P₁P₂的连线为X轴，Y轴垂直于X轴且P₃在由X轴和Y轴所确定的平面上，Z轴垂直于由所述X轴和Y轴确定的平面，并与所述X轴和Y轴构成右手系；

3)如图3所示建立二维相机像平面坐标系O₂-X₂Z₂：以二维相机光轴与二维相机像平面A₂的交点为二维相机像平面坐标系O₂-X₂Z₂的坐标原点O₂，X₂轴与图像像素横方向U平行，Z₂轴与图像像素竖直方向V平行；

建立三维相机像平面坐标系O₁-X₁Z₁：以三维相机光轴与三维相机像平面A₁的交点为三维相机像平面坐标系O₁-X₁Z₁的坐标原点O₁，X₁轴与图像像素的横方向U′平行，Z₁轴与图像像素的竖直方向V′平行；

建立二维相机坐标系O_s2-X_s2Y_s2Z_s2：以二维相机光心为二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2，Y_s2轴为二维相机光轴方向，X_s2轴与二维相机像平面坐标系O₂-X₂Z₂的X₂轴平行，Z_s2轴与二维相机像平面坐标系O₂-X₂Z₂的Z₂轴平行，并与所述X_s2轴和Y_s2轴构成右手系；

通过各标记点P_i的像点在二维相机像平面坐标系O₂-X₂Z₂下的图像像素坐标(u_i，v_i)按如下方法计算出各标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标S_i：

a、将各标记点P_i的像点在二维相机像平面坐标系O₂-X₂Z₂下的图像像素坐标(u_i，v_i)转化为像点在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标(u_i，v_i，f)，其中f为二维相机焦距；

b、由各标记点P_i的像点在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标(u_i，v_i，f)确定出二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2到各标记点P_i的方向向量，获得

的单位方向向量

c、由三维相机测得各标记点P_i到三维相机光心的距离为L_i，并通过式(1)得出各标记点P_i到二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2的距离D_i，

$D_i=\sqrt{h^2+{L_i}^2+2{\mathrm{hL}}_i\cos \mathrm{\α}}---\left(1\right)$

h为二维相机光心与三维相机光心之间的距离，α为O_s1P′_i与O_s1O_s2的夹角，P′_i为标记点P_i在三维相机像平面A₁上的像点；

d、由式(2)求出各标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标S_i，i＝1，2，3；

$S_i=D_i\stackrel{\→}{a_i}---\left(2\right)$

通过各标记点P_i之间的坐标相减获得向量

和

向量

和

单位化后求出两个线性无关的单位列向量

和

令

为

所构成的单位列向量；构造出三个标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的向量组： $n_s=[\stackrel{\→}{n_{s1}},\stackrel{\→}{n_{s2}},\stackrel{\→}{n_{s3}}];$

4)由向量组 $n_o=[\stackrel{\→}{n_{o1}},\stackrel{\→}{n_{o2}},\stackrel{\→}{n_{o3}}]$ 和向量组 $n_s=[\stackrel{\→}{n_{s1}},\stackrel{\→}{n_{s2}},\stackrel{\→}{n_{s3}}]$ 通过式(3)和式(4)的运算得出旋转矩阵R和平移矩阵T；

$R=n_s{n_o}^{-1}=\left(\begin{array}{ccc}{r}_{00}& r_{01}& r_{02}\\ r_{10}& r_{11}& r_{12}\\ r_{20}& r_{21}& r_{22}\end{array}\right)---\left(3\right)$

$T=S_i-{\mathrm{RP}}_i=\left(\begin{array}{c}{t}_0\\ t_1\\ t_2\end{array}\right)---\left(4\right)$

5)将所述旋转矩阵R通过式(5)计算获得绕二维相机坐标系的x轴、y轴和z轴沿逆时针方向的转角α、β和γ；旋转顺序是从x轴到y轴再到z轴；所述转角α、β和γ为非合作目标相对于二维相机坐标系O_s2-X_s2Y_s2Z_s2的三个姿态角；

$\left\{\begin{array}{c}\mathrm{\γ}=-\arctan \frac{r_{01}}{r_{00}}\\ \mathrm{\β}=\arctan \frac{r_{20}}{r_{00}\cos \mathrm{\γ}-r_{10}\sin \mathrm{\γ}}\\ \mathrm{\α}=\arctan \frac{r_{02}\sin \mathrm{\γ}+r_{12}\cos \mathrm{\γ}}{r_{01}\sin \mathrm{\γ}+r_{11}\cos \mathrm{\γ}}\end{array}\right.---\left(5\right)$

6)所述平移矩阵T，其中t₀、t₁和t₂分别为非合作目标坐标系O_o-X_oY_oZ_o相对于二维相机坐标系O_s2-X_s2Y_s2Z_s2在x轴、y轴和z轴上的平移量；

具体实施中，步骤1)中数据融合方法是引用The 3D reconstruction of PMD camera andhigh resolution color camera论文上的方法，文章在2011第三届IEEE信号处理系统国际会议上发表，论文编号：S0318，论文作者：胡良梅、张雅琼、张旭东等。

步骤2)中所述三维空间坐标计算方法是：P₁为非合作目标坐标系O_o-X_oY_oZ_o的坐标原点，在非合作目标坐标系O_o-X_oY_oZ_o下的坐标为P₁＝(0，0，0)，三维相机直接获得各标记点P_i到三维相机光心O_s1的距离L_i，i＝1，2，3，各标记点P_i的像点到三维相机光心O_s1的距离O_s1P′_i为已知，O_s1P′₁和O_s1P′₂之间的夹角为θ₁，O_s1P′₁和O_s1P′₃之间的夹角为θ₂，O_s1P′₂和O_s1P′₃之间的夹角为θ₃，则分别获得标记点P₁与P₂之间的距离 $a=\sqrt{L_1^2+L_2^2-2L_1{L}_2\cos {\mathrm{\θ}}_1},$ 标记点P₁与P₃之间的距离 $b=\sqrt{L_1^2+L_3^2-2L_1{L}_3\cos {\mathrm{\θ}}_2},$ 标记点P₂与P₃之间的距离

$c=\sqrt{L_2^2+L_3^2-2L_2{L}_3\cos {\mathrm{\θ}}_3}.$

则标记点P₂在非合作目标坐标系O_o-X_oY_oZ_o下的坐标

$P_2=(\sqrt{L_1^2+L_2^2-2L_1{L}_2\cos {\mathrm{\θ}}_1},\mathrm{0,0})$

由式(6)求得标记点P₁和P₂的连线与标记点P₁和P₃的连线之间的夹角为φ，

$\mathrm{\φ}=\arccos \left(\frac{a^2+b^2-c^2}{2\mathrm{ab}}\right)---\left(6\right)$

则标记点P₃的横坐标为bcosφ、纵坐标为bsinφ，获得标记点P₃在非合作目标坐标系O_o-X_oY_oZ_o下的坐标为P₃＝(bcosφ，bsinφ，0)。

Claims (2)

1.一种基于二维相机和三维相机融合的非合作目标位姿测量方法，其特征按如下步骤进行：

1)将二维相机叠加在三维相机的正上方，组合成相机测量系统，通过标定使得二维相机光心和三维相机光心在同一个竖直线上；由所述二维相机获取非合作目标的二维图像信息，由所述三维相机获取非合作目标的灰度信息、深度信息及调制信号的幅度信息，将所获得的二维图像信息、灰度信息、深度信息及幅度信息通过数据融合方法获得非合作目标三维图像；

2)在所述非合作目标三维图像中选取非合作目标中的三个非共线的特征点作为标记点P_i，i＝1，2，3；根据三维空间坐标计算方法计算出各标记点P_i在非合作目标坐标系O_o-X_oY_oZ_o下的标记点坐标P_i＝(X_i，Y_i，Z_i)，i＝1，2，3，通过标记点P₁和标记点P₂之间的坐标相减获得向量通过标记点P₁和标记点P₃之间的坐标相减获得向量

对所述向量和向量

分别进行向量单位化后求出两个线性无关的单位列向量

和

令

为

所构成的单位列向量，构造出三个标记点P_i在非合作目标坐标系O_o-X_oY_oZ_o下的向量组： $n_o=[\stackrel{\→}{n_{o1}},\stackrel{\→}{n_{o2}},\stackrel{\→}{n_{o3}}];$

所述非合作目标坐标系O_o-X_oY_oZ_o建立方法为：以P₁为坐标原点，P₁P₂的连线为X轴，Y轴垂直于X轴且P₃在由X轴和Y轴所确定的平面上，Z轴垂直于由所述X轴和Y轴确定的平面，并与所述X轴和Y轴构成右手系；

3)建立二维相机像平面坐标系O₂-X₂Z₂：以二维相机光轴与二维相机像平面A₂的交点为二维相机像平面坐标系O₂-X₂Z₂的坐标原点O₂，X₂轴与图像像素横方向U平行，Z₂轴与图像像素竖直方向V平行；

建立二维相机坐标系O_s2-X_s2Y_s2Z_s2：以二维相机光心为二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2，Y_s2轴为二维相机光轴方向，X_s2轴与二维相机像平面坐标系O₂-X₂Z₂的X₂轴平行，Z_s2轴与二维相机像平面坐标系O₂-X₂Z₂的Z₂轴平行，并与所述X_s2轴和Y_s2轴构成右手系；

通过各标记点P_i的像点在二维相机像平面坐标系O₂-X₂Z₂下的图像像素坐标(u_i，v_i)按如下方法计算出各标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标S_i：

a、将各标记点P_i的像点在二维相机像平面坐标系O₂-X₂Z₂下的图像像素坐标(u_i，v_i)转化为像点在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标(u_i，v_i，f)，其中f为二维相机焦距；

b、由各标记点P_i的像点在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标(u_i，v_i，f)确定出二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2到各标记点P_i的方向向量，获得

的单位方向向量

c、由三维相机测得各标记点P_i到三维相机光心的距离为L_i，并通过式(1)得出各标记点P_i到二维相机坐标系O_s2-X_s2Y_s2Z_s2的坐标原点O_s2的距离D_i，

$D_i=\sqrt{h^2+{L_i}^2+2{\mathrm{hL}}_i\cos \mathrm{\α}}---\left(1\right)$

式(1)中，h为二维相机光心与三维相机光心之间的距离，α为O_s1P′_i与O_s1O_s2的夹角，P′_i为标记点P_i在三维相机像平面A₁上的像点；

d、由式(2)求出各标记点Pi在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的坐标S_i，i＝1，2，3；

$S_i=D_i\stackrel{\→}{a_i}---\left(2\right)$

通过各标记点P_i之间的坐标相减获得向量

和向量

和

单位化后求出两个线性无关的单位列向量

和

令

为所构成的单位列向量；构造出三个标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的向量组： $n_s=[\stackrel{\→}{n_{s1}},\stackrel{\→}{n_{s2}},\stackrel{\→}{n_{s3}}];$

4)由向量组 $n_o=[\stackrel{\→}{n_{o1}},\stackrel{\→}{n_{o2}},\stackrel{\→}{n_{o3}}]$ 和向量组 $n_s=[\stackrel{\→}{n_{s1}},\stackrel{\→}{n_{s2}},\stackrel{\→}{n_{s3}}]$ 通过式(3)和式(4)的运算得出旋转矩阵R和平移矩阵T；

$R=n_s{n_o}^{-1}=\left(\begin{array}{ccc}{r}_{00}& r_{01}& r_{02}\\ r_{10}& r_{11}& r_{12}\\ r_{20}& r_{21}& r_{22}\end{array}\right)---\left(3\right)$

$T=S_i-{\mathrm{RP}}_i=\left(\begin{array}{c}{t}_0\\ t_1\\ t_2\end{array}\right)---\left(4\right)$

5)将所述旋转矩阵R通过式(5)计算获得绕二维相机坐标系的x轴、y轴和z轴沿逆时针方向的转角α、β和γ；旋转顺序是从x轴到y轴再到z轴；所述转角α、β和γ为非合作目标相对于二维相机坐标系O_s2-X_s2Y_s2Z_s2的三个姿态角；

$\left\{\begin{array}{c}\mathrm{\γ}=-\arctan \frac{r_{01}}{r_{00}}\\ \mathrm{\β}=\arctan \frac{r_{20}}{r_{00}\cos \mathrm{\γ}-r_{10}\sin \mathrm{\γ}}\\ \mathrm{\α}=\arctan \frac{r_{02}\sin \mathrm{\γ}+r_{12}\cos \mathrm{\γ}}{r_{01}\sin \mathrm{\γ}+r_{11}\cos \mathrm{\γ}}\end{array}\right.---\left(5\right)$

6)所述平移矩阵T，其中t₀、t₁和t₂分别为非合作目标坐标系O_o-X_oY_oZ_o相对于二维相机坐标系O_s2-X_s2Y_s2Z_s2在x轴、y轴和z轴上的平移量。

2.根据权利要求1所述的基于二维相机和三维相机融合的非合作目标位姿测量方法，其特征是步骤2)中所述三维空间坐标计算方法是：

P₁为非合作目标坐标系O_o-X_oY_oZ_o的坐标原点，在非合作目标坐标系O_o-X_oY_oZ_o下的坐标为P₁＝(0，0，0)，三维相机直接获得各标记点P_i到三维相机光心O_s1的距离L_i，i＝1，2，3，各标记点P_i的像点到三维相机光心O_s1的距离O_s1P′_i为已知，O_s1P′₁和O_s1P′₂之间的夹角为θ₁，O_s1P′₁和O_s1P′₃之间的夹角为θ₂，O_s1P′₂和O_s1P′₃之间的夹角为θ₃，则分别获得标记点P₁与P₂之间的距离 $a=\sqrt{L_1^2+L_2^2-2L_1{L}_2\cos {\mathrm{\θ}}_1},$ 标记点P₁与P₃之间的距离 $b=\sqrt{L_1^2+L_3^2-2L_1{L}_3\cos {\mathrm{\θ}}_2},$ 标记点P₂与P₃之间的距离 $c=\sqrt{L_2^2+L_3^2-2L_2{L}_3\cos {\mathrm{\θ}}_3};$

则标记点P₂在非合作目标坐标系O_o-X_oY_oZ_o下的坐标

$P_2=(\sqrt{L_1^2+L_2^2-2L_1{L}_2\cos {\mathrm{\θ}}_1},\mathrm{0,0})$

由式(6)求得标记点P₁和P₂的连线与标记点P₁和P₃的连线之间的夹角为φ，

$\mathrm{\φ}=\arccos \left(\frac{a^2+b^2-c^2}{2\mathrm{ab}}\right)---\left(6\right)$

则标记点P₃的横坐标为bcosφ、纵坐标为bsinφ，获得标记点P₃在非合作目标坐标系O_o-X_oY_oZ_o下的坐标为P₃＝(bcosφ，bsinφ，0)。

Images