一种惯性/视觉里程计/激光雷达的组合导航方法

摘要

本发明属于导航方法，具体涉及一种惯性/视觉里程计/激光雷达的组合导航方法。它包括：(1)状态模型的建立，(2)基于特征信息的视觉里程计测速，(3)量测方程的建立与量测值的获得，(4)卡尔曼滤波，(5)校正系统误差。本发明的效果是：本发明使用机器视觉的自主导航技术，单目相机通过前后帧图像的差异，在已知距离的情况下可以测量出载体的速度；激光雷达可以精确测量出到观测点的距离，然后测出载体的速度，利用测量得到的速度与惯导速度组合导航，最终能在无外界参考信息输入情况下实现高精度导航。

主权项

一种惯性/视觉里程计/激光雷达的组合导航方法，其特征在于，包括下述步骤：(1)惯性/视觉里程计/激光雷达组合导航系统状态模型的建立，如下式$\stackrel{·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$式中：X(t)为上述系统状态向量；W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取，$X\left(t\right)=[{\mathrm{\delta V}}_n,{\mathrm{\delta V}}_u,{\mathrm{\delta V}}_e,\delta L,\delta h,\delta \lambda ,{\phi }_n,{\phi }_u,{\phi }_e,{▿}_x,{▿}_y,{▿}_z,{ϵ}_x,{ϵ}_y,{ϵ}_z,{\mathrm{\delta V}}_{n\_ov},{\mathrm{\delta V}}_{e\_OV}]$δV_n,δV_u,δV_e分别表示捷联惯导系统北向、天向、东向的速度误差；δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差；φ_n,φ_u,φ_e分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角；分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏；ε_x,ε_y,ε_z分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移；δV_{n_ov},δV_{e_OV}分别表示视觉里程计北向、东向的速度误差；(2)基于特征信息的视觉里程计测速a)选取合适的匹配区域选取图像的几何中心位置作为匹配区域；b)提取图像特征信息将当前帧的图像、上一帧的图像中的匹配区域截取出来，对截取后的图像进行SIFT特征点提取，c)特征点匹配用K‑D树快速特征点匹配，将当前帧图像与上一帧图像中的特征点进行匹配，得出一系列的匹配对，d)选择同一特征点计算速度利用激光雷达获在匹配对上获得的不同距离除以时间计算出特征点的速度；e)输出当前帧速度每个特征点都能计算出一个速度，将这些速度进行处理后输出；(3)量测方程的建立与量测值的获得卡尔曼滤波器量测方程形式如下：Z＝HX+V量测值Z为惯导系统和视觉里程计分别给出的速度的差值，实际上是两者误差的差值：$Z=\left[\begin{array}{c}{V}_{n\_imu}-V_{n\_OV}\\ V_{e\_imu}-V_{e\_OV}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\delta V}}_n-{\mathrm{\delta V}}_{n\_OV}\\ {\mathrm{\delta V}}_e-{\mathrm{\delta V}}_{e\_OV}\end{array}\right]+V$式中V为量测噪声，考虑为白噪声，由上式可得H阵如下：$H=\left[\begin{array}{ccccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1\end{array}\right]$(4)卡尔曼滤波根据惯性/视觉里程计/激光雷达组合导航的系统方程和量测方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：${\Phi }_{k,k-1}=I+T_n{F}_{T_n}+T_n{F}_{2T_n}+......+T_n{F}_{{\mathrm{NT}}_n}=I+\underset{i=1}{\overset{N}{\Sigma }}{T}_n{F}_{{\mathrm{iT}}_n}$式中：T_n为导航周期，NT_n为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵，I为单位阵，状态一步预测${\hat{X}}_{k,k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$状态估计${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$滤波增益矩阵$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$一步预测误差方差阵$P_{k,k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^T$估计误差方差阵P_k＝[I‑K_kH_k]P_k,k‑1其中，为一步状态预测值，为状态估计矩阵，Φ_k,k‑1为状态一步转移矩阵，H_k为量测矩阵，Z_k为量测量，K_k为滤波增益矩阵，R_k为观测噪声阵，P_k,k‑1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k‑1为系统噪声驱动阵，Q_k‑1为系统噪声阵；利用步骤(1)和本步骤的方程，可以计算出一系列误差值；(5)校正系统误差用步骤(4)计算出的误差值对系统输出值进行校正。