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

摘要

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

主权项

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，i=1,2,3在非合作目标坐标系O_o-X_oY_oZ_o下的向量组：$n_o=[{\overrightarrow{n}}_{o1},{\overrightarrow{n}}_{o2},{\overrightarrow{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 \alpha }---\left(1\right)$式(1)中，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{\overrightarrow{a}}_i---\left(2\right)$通过各标记点P_i之间的坐标相减获得向量和向量和单位化后求出两个线性无关的单位列向量和令为所构成的单位列向量；构造出三个标记点P_i，i=1,2,3在二维相机坐标系O_s2-X_s2Y_s2Z_s2下的向量组：4）由向量组$n_o=[{\overrightarrow{n}}_{o1},{\overrightarrow{n}}_{o2},{\overrightarrow{n}}_{o3}]$和向量组$n_s=[{\overrightarrow{n}}_{s1},{\overrightarrow{n}}_{s2},{\overrightarrow{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}\gamma =-\arctan \frac{r_{01}}{r_{00}}\\ \beta =\arctan \frac{r_{20}}{r_{00}\cos \gamma -r_{10}\sin \gamma }\\ \alpha =\arctan \frac{r_{02}\sin \gamma +r_{12}\cos \gamma }{r_{01}\sin \gamma +r_{11}\cos \gamma }\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轴上的平移量。