基于激光测距与摄像机视觉融合的高精度位姿解算方法

摘要

本发明公开了一种基于激光测距与摄像机视觉融合的高精度位姿解算方法，包括：S1、进行基于合作目标的NLSM的摄像机视觉位姿测量算法线性建模；S2、进行基于多点激光测距仪的线性建模；S3、进行基于激光测距与摄像机的融合线性建模。综合利用了激光测距高精度的测距信息与摄像机高精度的角分辨率信息，有效提高了目标的位置姿态测量精度，且解决了融合了基于合作目标的激光测距与摄像机视觉的位置姿态测量算法的问题。

主权项

一种基于激光测距与摄像机视觉融合的高精度位姿解算方法，其特征在于，包括以下步骤：S1、进行基于合作目标的NLSM的摄像机视觉位姿测量算法线性建模，摄像机的成像模型：$\left\{\begin{array}{c}{u}_i=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ v_i=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$其中，(u_i，v_i)为第i个合作目标在摄像机平面坐标系中的坐标，(x_ci，y_ci，z_ci)为对应的合作目标在像空间坐标系中的坐标，像空间坐标系与目标坐标系的坐标转换为：$\left[\begin{array}{c}{x}_{\mathrm{ci}}\\ y_{\mathrm{ci}}\\ z_{\mathrm{ci}}\end{array}\right]=M_{\mathrm{ct}}\left[\begin{array}{c}{x}_{\mathrm{ti}}\\ x_{\mathrm{ti}}\\ x_{\mathrm{ti}}\end{array}\right]+\left[\begin{array}{c}{t}_{\mathrm{xc}}\\ t_{\mathrm{yc}}\\ t_{\mathrm{zc}}\end{array}\right],i=\mathrm{1,2,3}$其中，(x_ti，y_ti，z_ti)为合作目标在目标坐标系中的坐标，(t_xc，t_yc，t_zc)为目标坐标系原点在像空间坐标系中的坐标，M_ct为目标坐标系到像空间坐标系的转换矩阵，(θ₁,θ₂,θ₃)为相应的旋转角，M_ct表示为：$M_{\mathrm{ct}}=\left[\begin{array}{ccc}\cos {\theta }_2\cos {\theta }_3-\sin {\theta }_1\sin {\theta }_2\sin {\theta }_3& \cos {\theta }_2\sin {\theta }_3+\sin {\theta }_1\sin {\theta }_2\cos {{\theta }_{}}_3& -\cos {\theta }_1\sin {\theta }_2\\ -\cos {\theta }_1\sin {\theta }_3& \cos {\theta }_1\cos {\theta }_3& \sin {\theta }_1\\ \sin {\theta }_2\cos {\theta }_3+\cos {\theta }_2\sin {\theta }_1\sin {\theta }_3& \sin {\theta }_2\sin {\theta }_3-\sin {\theta }_1\cos {\theta }_2\cos {\theta }_3& \cos {\theta }_1\cos {\theta }_2\end{array}\right]$令$\left\{\begin{array}{c}{F}_{2i-1}\left(\chi \right)=f\frac{z_{\mathrm{ci}}}{x_{\mathrm{ci}}}\\ F_{2i}\left(\chi \right)=f\frac{y_{\mathrm{ci}}}{x_{\mathrm{ci}}}\end{array}\right.i=\mathrm{1,2,3}$其中，χ＝(t_xc,t_yc,t_zc,θ₁,θ₂,θ₃)，根据NLSM原理，得到：minV^TV＝min(Bε‑L)^T(Bε‑L)其中，χ⁰为χ的初值估计，ε为χ⁰的改正值，符号“T”表示矩阵转置，矩阵B为函数F(χ)的一阶偏导数，表示为：$B=\frac{\partial F}{\partial \chi }{|}_{{\chi }^0}$矩阵L中的元素l_i表示为：$\left\{\begin{array}{c}{l}_{2i-1}=u_i-F_{2i-1}\left({\chi }^0\right)\\ l_{2i}=v_i-F_{2i}\left({\chi }^0\right)\end{array}\right.i=\mathrm{1,2,3}$S2、进行基于多点激光测距仪的线性建模：激光测距仪的安装位置P点在像空间坐标系中的坐标为(x_pc，y_pc，z_pc)，P与合作目标之间的激光测距距离为D_i，i＝1，2，3；$D_i\left(\chi \right)={(x_{\mathrm{cp}}-x_{\mathrm{ci}})}^2+{(y_{\mathrm{cp}}-y_{\mathrm{ci}})}^2+{(z_{\mathrm{cp}}-z_{\mathrm{ci}})}^2-d_i^2,i=\mathrm{1,2,3}$对其进行线性化，得到：$D_i\left(\chi \right)=D_i\left({\chi }^0\right)+\frac{\partial D_i}{\partial \chi }ϵ,i=\mathrm{1,2,3}$改写为矩阵形式，则有：D(χ)＝D(χ⁰)+Cε其中，C为条件方程D(χ)的一阶偏导数，为：$C=\frac{\partial D}{\partial \chi }{|}_{{\chi }^0}$S3、进行基于激光测距与摄像机的融合线性建模：将激光测距作为摄像机视觉测量算法的强制性约束条件，则有：D(χ⁰)+Cε＝0相应的目标函数为：$\min \left[{(\underset{\mathrm{6,6}}{B}\underset{\mathrm{6,1}}{ϵ}-\underset{\mathrm{6,1}}{l})}^T\right(\underset{\mathrm{6,6}}{B}\underset{\mathrm{6,1}}{ϵ}-\underset{\mathrm{6,1}}{l})+2{(\underset{\mathrm{3,1}}{D}+\underset{\mathrm{3,6}}{C}\underset{\mathrm{6,1}}{ϵ})}^T\underset{\mathrm{3,1}}{\lambda }]$对相应的λ进行求解：λ＝[C(B^TB)^‑1C^T]^‑1[D+C(B^TB)^‑1(B^TL)]对相应的ε进行求解：ε＝(B^TB)^‑1(B^TL‑C^Tλ)相应的位置姿态参数χ表示为：χ^(k+1)＝χ^(k)+ε式中，χ^(k)为第k次的计算结果。