[发明专利]基于二维相机和三维相机融合的非合作目标位姿测量方法

权利要求书

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下的向量组：no=[no1→,no2→,no3→];]]>

所述非合作目标坐标系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，

Di=h2+Li2+2hLicosα---(1)]]>

式(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；

Si=Diai→---(2)]]>

通过各标记点P_i之间的坐标相减获得向量和向量和单位化后求出两个线性无关的单位列向量和令为所构成的单位列向量；构造出三个标记点P_i在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的向量组：ns=[ns1→,ns2→,ns3→];]]>

4)由向量组no=[no1→,no2→,no3→]]]>和向量组ns=[ns1→,ns2→,ns3→]]]>通过式(3)和式(4)的运算得出旋转矩阵R和平移矩阵T；

R=nsno-1=r00r01r02r10r11r12r20r21r22---(3)]]>

T=Si-RPi=t0t1t2---(4)]]>

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

γ=-arctanr01r00β=arctanr20r00cosγ-r10sinγα=arctanr02sinγ+r12cosγr01sinγ+r11cosγ---(5)]]>

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=L12+L22-2L1L2cosθ1,]]>标记点P₁与P₃之间的距离b=L12+L32-2L1L3cosθ2,]]>标记点P₂与P₃之间的距离c=L22+L32-2L2L3cosθ3;]]>

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

P2=(L12+L22-2L1L2cosθ1,0,0)]]>

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

φ=arccos(a2+b2-c22ab)---(6)]]>

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