[发明专利]一种可变焦距柔性化位姿视觉测量方法

摘要

本发明一种可变焦距柔性化位姿视觉测量方法属于计算机视觉测量技术领域，涉及柔性化便捷的可变焦距单目位姿测量方法。该方法通过已知空间局部坐标系坐标的特征点完成运动模型相对于摄像机坐标系的位置和姿态的解算，在摄像机标定和目标位姿测量的过程中，摄像机相对于世界坐标系的位置和摄像机内参数矩阵保持不变，标定完成后直接通过标点参数完成二维图像特征到三维坐标的转换，求解三维目标的位姿。该方法不需要对测量系统进行标定，根据被测物体表面已知相对关系的标记点，实现在变焦距情况下对物体位置、姿态信息的实时测量，不仅增加了系统柔性化，也增加了系统的便捷化。

主权项

一种可变焦距柔性化位姿视觉测量方法，其特征是，测量方法通过已知空间局部坐标系坐标的特征点完成运动模型相对于摄像机坐标系的位置和姿态的解算，在摄像机标定和目标位姿测量的过程中，摄像机相对于世界坐标系的位置和摄像机内参数矩阵保持不变，标定完成后直接通过标点参数完成二维图像特征到三维坐标的转换，求解三维目标的位姿；方法的具体步骤如下：第一步：首先建立被测目标与相机之间的数学模型以模型质心为原点在被测模型上建立局部坐标系，定义模型外轮廓的回转轴线为y轴，通过模型质心与y轴垂直的轴为x轴，然后根据右手定则确定z轴的方向，模型表面所有标记点在模型局部坐标系上的坐标由标记点的加工定位精度保证，摄像机针孔模型描述了三维空间点到二维图像对应点之间的投影关系为：λ_iU_i＝PX_i                    (1)其中,λ_i为与第i个特征点有关的比例因子，P为摄像机投影矩阵，U_i＝(u_i v_i 1)^T为二维图像上特征点的齐次坐标，X_i＝(x_i y_i z_i 1)^T为模型局部坐标系上标记点的齐次坐标，摄像机投影矩阵P写成如下的形式：P＝K[R|t]                    (2)其中,K为摄像机内参数矩阵，描述了三维到二维的投影关系：$K=\left[\begin{array}{ccc}f& s& u_0\\ 0& rf& v_0\\ 0& 0& 1\end{array}\right]---\left(3\right)$f为摄像机焦距，u₀为相机主点的横坐标，v₀为相机主点的纵坐标，S为坐标轴倾斜参数，r表示芯片像素单元的长宽比，R＝(r_ij)³,i,j＝1和T＝(t_x t_y t_z)^T分别表示摄像机坐标系和模型局部坐标系之间的旋转矩阵和平移向量，S为偏斜系数，摄像机主点位于二维图像的中心，设：摄像机芯片为正方形，因此r＝1；主点位于图像的中心；偏斜系数S＝0，则内参数矩阵K可以简化为由焦距f构成的对角阵[f f 1]^T，令w＝1/f，则K＝[1 1 w]^T，将该假设代入公式(1)，将摄像机投影模型为：${\mathrm{\λ}}_i{U}_i=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ {\mathrm{wr}}_{31}& {\mathrm{wr}}_{32}& {\mathrm{wr}}_{33}& {\mathrm{wt}}_z\end{array}\right]X_i---\left(4\right)$将径向畸变引入到投影模型中，且根据一般情况建立图像畸变模型为：$P_u~P_d/(1+{\mathrm{kr}}_d^2)---\left(5\right)$其中，k为畸变系数，P_u＝[u_u v_u 1]^T和P_d＝[u_d v_d 1]^T分别为发生畸变前后的图像点坐标，r_d为点P_d到畸变中心的距离，即畸变半径，假设畸变中心位于二维图像的中心，则r_d²＝u_d²+v_d²，则二维图像上实际成像点的坐标为：$U_i={\[u_i,v_i,1+k(u_d^2+v_d^2)\]}^T---\left(6\right)$将上述镜头畸变模型加入到投影方程中，且根据[u_i]_xu_i＝0，将投影公式(4)两边同时乘以[u_i]x后可以将比例因子λ_i消除，得到投影方程如下所示：$\left[\begin{array}{ccc}0& -1-k(u_i^2+v_i^2)& v_i\\ 1+k(u_i^2+v_i^2)& 0& u_i\\ -v_i& u_i& 0\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ {\mathrm{wr}}_{31}& {\mathrm{wr}}_{32}& {\mathrm{wr}}_{33}& {\mathrm{wt}}_z\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]=0---\left(7\right)$其中，该投影方程描述了三维特征点X_i到包含畸变的二维图像点U_i的投影关系，方程的未知数为两坐标系之间的转换矩阵[P]_3×4的所有元素以及镜头畸变系数k，求出P后根据公式(2)分解出摄像机的内参数矩阵与外参数矩阵，因此，求解量坐标系之间的旋转矩阵与平移向量的过程即为求解摄像机投影矩阵P的过程，第二步：摄像机与被测模型的坐标系转换求取定义公式(7)中摄像机投影矩阵P的每个元素为p_ij，p_ij表示矩阵P的第i行和第j列的元素，将矩阵方程(7)的第三行单独列出，得到如下的形式：‑v_i(p₁₁x_i+p₁₂y_i+p₁₃z_i+p₁₄)+u_i(p₁₂x_i+p₂₂y_i+p₂₃z_i+p₂₄)＝0    (8)该方程为具有8个未知数(p₁₁,p₁₂,p₁₃,p₁₄,p₁₂,p₂₂,p₂₃,p₂₄)的其次线性方程，只要得到4个三维点的精确坐标并在二维图像中找到它们对应的图像坐标，就能利用这4个对应的点将方程写成矩阵的形式：Mv＝0                (9)其中M为一个4×8的系数矩阵，未知数向量v＝[p₁₁,p₁₂,p₁₃,p₁₄,p₁₂,p₂₂,p₂₃,p₂₄]^T，v用上述矩阵方程的标准正交基n_i的线性组合表示，公式如下：$v=\underset{i=1}{\overset{4}{\Σ}}{\mathrm{\α}}_i{n}_i---\left(10\right)$其中α_i为对v进行参数化后得到的新的未知数，令α₄＝1，则投影矩阵P的前两行可以表示为三个未知数α₁，α₂和α₃的线性组合，只要求解出α₁，α₂和α₃，就能将P的前两行元素求出，矩阵方程的第2行可以写成如下的形式：$(1+{\mathrm{kr}}_i^2)(p_{11}{x}_i+p_{12}{y}_i+p_{13}{z}_i+p_{14})-u_i(p_{31}{x}_i+p_{32}{y}_i+p_{33}{z}_i+p_{34})=0---\left(11\right)$结合公式(10),该方程可以写成:A[p₃₁,p₃₂,p₃₃,p₃₄]^T＝B[α₁,α₂,α₃,kα₁,kα₂,kα₃,k,1]^T       (12)其中A、B分别为4×4和4×8的系数矩阵，当X₁，X₂，X₃，X₄不共面时矩阵A可逆，方程(12)写成：[p₃₁,p₃₂,p₃₃,p₃₄]^T＝A^‑1B[α₁,α₂,α₃,kα₁,kα₂,kα₃,k,1]^T       (13)通过公式(13)，将P的第3行用α₁，α₂,α₃和K的线性组合表示，即完成了投影矩阵P的参数化，由于P矩阵的前3列为K和R相乘后的运算结果，且K＝[1 1 w]^T，因此P的前3列具有和旋转矩阵R相同的性质，利用旋转矩阵的正交性质建立关于P矩阵所有元素的约束方程组如下：$\left\{\begin{array}{c}{p}_{11}{p}_{21}+p_{12}{p}_{22}+p_{13}{p}_{23}=0,\\ p_{31}{p}_{11}+p_{32}{p}_{12}+p_{33}{p}_{13}=0,\\ p_{31}{p}_{21}+p_{32}{p}_{22}+p_{33}{p}_{23}=0,\\ p_{11}^2+p_{12}^2+p_{13}^2-p_{21}^2-p_{22}^2-p_{23}^2=0.\end{array}\right.---\left(14\right)$方程组(14)具有4个方程，求解4个未知数(α₁,α₂,α₃,k)，即求出投影矩阵P，进而分解出模型相对于摄像机坐标系的旋转矩阵R₀和平移向量T₀，第三步：摄像机与世界坐标系的坐标系转换关系求取对于共面特征点X_i，假设所有点在Z方向的坐标值为0，即z_i，此时与公式(7)的3行相对应的公式(8)写成：‑v_i(p₁₁x_i+p₁₂y_i+p₁₄)+u_i(p₁₂x_i+p₂₂y_i+p₂₄)＝0      (15)将4个特征点的方程写成矩阵的形式可以得到Mv＝0，且M是4×6的系数矩阵，v＝[p₁₁,p₁₂,p₁₄,p₁₂,p₂₂,p₃₄]^T为未知数向量，v用矩阵M的两个标准正交基向量n₁和n₂表示为：v＝β₁n₁+n₂                  (16)其中，β₁为新定义的未知数,投影矩阵P的前两行都已用未知数及n₁和n₂来代替，当所用的4个特征点共面时，公式(7)的第3行有为：(1+kr_i²)(p₁₁x_i+p₁₂y_i+p₁₄)‑u_i(p₃₁x_i+p₃₂y_i+p₃₄)＝0    (17)利用该公式将投影矩阵P的第3行进行参数化，过程如下：C[p₃₁,p₃₂,p₃₄]^T＝D[β₁,kβ₁,k,1]^T              (18)其中C和D分别为3×3和3×4的系数矩阵，当X₁，X₂，X₃不共线时矩阵C可逆，方程(18)可以写成：[p₃₁,p₃₂,p₃₄]^T＝C^‑1D[β₁,kβ₁,k,1]^T              (19)利用上述公式用β₁和k两个未知数代替P中的所有元素，即完成P的参数化，由于P的前3列具有和旋转矩阵R相同的性质，即旋转矩阵任意两列相互正交且具有相同的模，利用这些性质列出关于P矩阵所有元素的方程，如下：wp₁₁wp₁₂+wp₂₁wp₂₂+p₃₁wp₃₂＝0,               (20)$w^2{p}_{11}^2+w^2{p}_{21}^2+p_{31}^2-w^2{p}_{12}^2-w^2{p}_{22}^2-p_{32}^2=0.---\left(21\right)$结合第4对2维和3维对应点的投影关系可以得到方程：$(1+{\mathrm{kr}}_4^2)(p_{11}{x}_4+p_{12}{y}_4+p_{14})-u_4(p_{31}{x}_4+p_{32}{y}_4+p_{34})=0---\left(22\right)$综合公式(20)，(21)，(22)求解方程中未知数β₁，k和w＝1/f，再结合P的参数化公式反求P矩阵中的所有元素，进而求解出相机相对于世界坐标系的转换矩阵R_C和T_C；第四步：模型运动位姿求解首先建立模型局部坐标系，以模型质心作为局部坐标系原点O_t，沿模型中心轴线指向模型尾部方向为O_tY_t轴正方向，取过原点O_t与O_tY_t轴垂直并与首尾同一组红色标记点的连线相交的方向为O_tX_t轴正方向，根据右手定则定义局部坐标系O_tZ_t轴方向，在模型位姿测量前首先求出摄像机初始位置相对于世界坐标系的转换关系，即旋转矩阵R_C和平移向量T_C，然后通过变焦距位姿测量过程得到模型在运动过程中的每一个时刻模型局部坐标系相对于相机坐标系的平移向量T_O和旋转矩阵R_O，利用以下公式计算模型物局部坐标系与风洞坐标系的转换矩阵：$\left[\begin{array}{c}{x}_t\\ y_t\\ z_t\end{array}\right]=R_o^{-1}{R}_c\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]+R_o^{-1}(T_c-T_o)---\left(23\right)$其中，(x_t,y_t,z_t)为标记点在模型局部坐标系下坐标，(x_w,y_w,z_w)为标记点在风洞坐标系下坐标，并且：$R=R_o^{-1}{R}_o---\left(24\right)$$T=R_o^{-1}(T_c-T_o)={\left[\begin{array}{ccc}{t}_x& t_y& t_z\end{array}\right]}^T---\left(25\right)$R和T分别为模型相对于世界坐标系的旋转矩阵和平移矩阵。