本发明属于导航方法，具体涉及一种惯性/视觉里程计/激光雷达的组合导航方法。
背景技术：
随着飞行器长时间、远程航行的能力不断得到增强，对导航系统的精度和自主性要求也在不断提高。纯惯性导航系统由于其固有的导航误差随时间而积累的缺点，因而不能完全满足实际应用的需要。解决这一问题的途径有两类：第一、提高惯导系统本身的精度。主要依靠采用新材料、新工艺、新技术，提高惯性器件的精度，或研制新型高精度的惯性器件。但需要花费大量的人力财力，且惯性器件精度的提高是有限的。第二、采用组合导航技术。主要是使用惯性系统外部的某些附加导航信息源，用以改善惯性系统的精度，通过软件技术来提高导航精度。然而，可选用的组合信息很多，如果将这些信息全部组合，虽然精度可以达到甚至超过要求，但是计算量巨大，完全无法在实际中使用，如果仅选取部分信息组合，那么选取信息的类型，组合的先后顺序，具体的组合方式都对结果的精度产生巨大影响。现有技术中尚没有一种比较完美的组合方式，能够既兼顾到计算量，又最大程度的减少组合的内容。
技术实现要素：
本发明针对现有技术的缺陷，提供一种惯性/视觉里程计/激光雷达的组合导航方法。
本发明是这样实现的：一种惯性/视觉里程计/激光雷达的组合导航方法，包括下述步骤：
(1)惯性/视觉里程计/激光雷达组合导航系统状态模型的建立，如下式
式中：X(t)为上述系统状态向量；W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取，
X(t)＝[δVn，δVu，δVe，δL，δh，δλ，φn，φu，φe，▽x，▽y，▽z，εx，εy，εz，δVn_ov，δVe_OV]
δVn,δVu,δVe分别表示捷联惯导系统北向、天向、东向的速度误差；
δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差；
φn,φu,φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角；
▽x，▽y，▽z分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏；
εx,εy,εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移；
δVn_ov,δVe_OV分别表示视觉里程计北向、东向的速度误差；
(2)基于特征信息的视觉里程计测速
a)选取合适的匹配区域
选取图像的几何中心位置作为匹配区域；
b)提取图像特征信息
将当前帧的图像、上一帧的图像中的匹配区域截取出来，对截取后的图像进行SIFT特征点提取，
c)特征点匹配
用K-D树快速特征点匹配，将当前帧图像与上一帧图像中的特征点进行匹配，得出一系列的匹配对，
d)选择同一特征点计算速度
利用激光雷达获在匹配对上获得的不同距离除以时间计算出特征点的速度；
e)输出当前帧速度
每个特征点都能计算出一个速度，将这些速度进行处理后输出；
(3)量测方程的建立与量测值的获得
卡尔曼滤波器量测方程形式如下：
Z＝HX+V
量测值Z为惯导系统和视觉里程计分别给出的速度的差值，实际上是两者误差的差值：
式中V为量测噪声，考虑为白噪声，
由上式可得H阵如下：
(4)卡尔曼滤波
根据惯性/视觉里程计/激光雷达组合导航的系统方程和量测方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：
式中：
Tn为导航周期，NTn为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵，I为单位阵，
状态一步预测
状态估计
滤波增益矩阵
一步预测误差方差阵
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵；
利用步骤(1)和本步骤的方程，可以计算出一系列误差值；
(5)校正系统误差
用步骤(4)计算出的误差值对系统输出值进行校正。
如上所述的一种惯性/视觉里程计/激光雷达的组合导航方法，其中，所述的步骤(2)的e)步骤中指的处理是RANSAC方法或平均。
本发明的效果是：本专利使用机器视觉的自主导航技术，单目相机通过前后帧图像的差异，在已知距离的情况下可以测量出载体的速度；激光雷达可以精确测量出到观测点的距离，然后测出载体的速度，利用测量得到的速度与惯导速度组合导航，最终能在无外界参考信息输入情况下实现高精度导航。
具体实施方式
一种惯性/视觉里程计/激光雷达的组合导航方法，包括下述步骤：
(1)惯性/视觉里程计/激光雷达组合导航系统状态模型的建立，如下式
式中：X(t)为上述系统状态向量；W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取。
X(t)＝[δVn,δVu,δVe,δL,δh,δλ,φn,φu,φe,▽x,▽y,▽z,εx,εy,εz,δVn_ov,δVe_OV]
δVn,δVu,δVe分别表示捷联惯导系统北向、天向、东向的速度误差；
δL,δh,δλ分别表示捷联惯导系统的纬度误差、高度误差、经度误差；
φn,φu,φe分别表示捷联惯导系统导航坐标系内北、天、东三个方向的失准角；
▽x,▽y,▽z分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏；
εx,εy,εz分别表示捷联惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移；
δVn_ov,δVe_OV分别表示视觉里程计北向、东向的速度误差；
(2)基于特征信息的视觉里程计测速
a)选取合适的匹配区域
为了提高视觉里程计的测速范围，需要结合惯导输出的速度、姿态信息来选择合适的位置作为匹配区域。一般选取图像的几何中心位置作为匹配区域，区域大小为30*30像素。
b)提取图像特征信息
将当前帧的图像、上一帧的图像中的匹配区域截取出来，对截取后的图像进行SIFT特征点提取。
c)特征点匹配
设定合适的阈值，适用K-D树快速特征点匹配，将当前帧图像与上一帧图像中的特征点进行匹配，得出一系列的匹配对。
d)选择同一特征点计算速度
由视觉里程计的速度计算公式可知，距离和速度的计算是针对同一点而言的，故需要在当前帧图像、上一帧图像中找到同一个特征点。利用激光雷达获得该特征点与相机的距离，然后通过该特征点在当前帧图像与上一帧图像的匹配对中的信息计算出该特征点与相机的相对速度。
e)输出当前帧速度
每个特征点都能计算出一个速度，将这些速度进行一定的处理(如RANSAC方法或平均)，最后输出视觉里程计的测速结果。
(3)量测方程的建立与量测值的获得
卡尔曼滤波器量测方程形式如下：
Z＝HX+V
量测值Z为惯导系统和视觉里程计分别给出的速度的差值，实际上是两者误差的差值：
式中V为量测噪声，考虑为白噪声。
由上式可得H阵如下：
(4)卡尔曼滤波
根据惯性/视觉里程计/激光雷达组合导航的系统方程和量测方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：
式中：
Tn为导航周期，NTn为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵，I为单位阵。
状态一步预测
状态估计
滤波增益矩阵
一步预测误差方差阵
估计误差方差阵
Pk＝[I-KkHk]Pk,k-1
其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，Kk为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵。
(5)校正系统误差
用步骤(4)计算出的误差值对系统输出值进行校正。