[发明专利]一种惯性/视觉里程计/激光雷达的组合导航方法

权利要求书

1.一种惯性/视觉里程计/激光雷达的组合导航方法，其特征在于，包括下述步骤：

(1)惯性/视觉里程计/激光雷达组合导航系统状态模型的建立，如下式

式中：X(t)为上述系统状态向量；W(t)为系统白噪声；系数矩阵F(t)和G(t)根据误差方程求取，

δ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_0V}分别表示视觉里程计北向、东向的速度误差；

(2)基于特征信息的视觉里程计测速

a)选取合适的匹配区域

选取图像的几何中心位置作为匹配区域；

b)提取图像特征信息

将当前帧的图像、上一帧的图像中的匹配区域截取出来，对截取后的图像进行SIFT特征点提取，

c)特征点匹配

用K-D树快速特征点匹配，将当前帧图像与上一帧图像中的特征点进行匹配，得出一系列的匹配对，

d)选择同一特征点计算速度

利用激光雷达获在匹配对上获得的不同距离除以时间计算出特征点的速度；

e)输出当前帧速度

每个特征点都能计算出一个速度，将这些速度进行处理后输出；

(3)量测方程的建立与量测值的获得

卡尔曼滤波器量测方程形式如下：

Z＝HX+V

量测值Z为惯导系统和视觉里程计分别给出的速度的差值，实际上是两者误差的差值：

式中V为量测噪声，即为白噪声，

由上式可得H阵如下：

(4)卡尔曼滤波

根据惯性/视觉里程计/激光雷达组合导航的系统方程和量测方程，计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：

式中：

T_n为导航周期，NT_n为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵，I为单位阵，

状态一步预测

状态估计

滤波增益矩阵

一步预测误差方差阵

估计误差方差阵

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)计算出的误差值对系统输出值进行校正。

2.如权利要求1所述的一种惯性/视觉里程计/激光雷达的组合导航方法，其特征在于：所述的步骤(2)的e)步骤中指的处理是RANSAC方法或平均。