[发明专利]一种惯导/里程计自主组合导航方法

权利要求书

1.一种惯导/里程计自主组合导航方法，其特征在于：包括以下步骤：

步骤(1)

进行惯导系统初始对准，得到惯导系统初始姿态；惯导系统初始对准结束后转入惯导/里程计组合导航状态，对卡尔曼滤波器参数进行初始化；

步骤(2)

进行惯性导航计算，同时采集里程计脉冲信号，解算惯导速度、位置、姿态信息，里程计姿态阵，并将里程计脉冲信息转换为位移信息，计算里程计航位推算位移增量；同时计算离散状态转移矩阵和量测矩阵；

步骤(3)

导航计算每到卡尔曼滤波周期时刻，采用基于位移误差的求和测量，计算滤波观测量；

步骤(4)

进行里程计信息故障检测；

步骤(5)

进行卡尔曼滤波；

步骤(6)

对惯导姿态阵、速度、位置误差进行修正，对里程计姿态阵进行修正；对里程计刻度系数进行修正。

2.根据权利要求1所述的惯导/里程计自主组合导航方法，其特征在于：本方法的坐标系定义如下：

n：导航坐标系oxyz，北天东地理坐标系，x轴指北，y轴指天，z轴指东； 

n′：计算导航坐标系o′x′y′z′，x′轴指北，y′轴指天，z′轴指东；

b：惯导载体系o″x″y″z″，与陀螺坐标系重合，前上右坐标系，x″轴指向前，y″轴朝上，z″轴指右；

m：里程计坐标系o″′x″′y″′z″′，前上右坐标系，x″′轴指向前，y″′轴朝上，z″′轴指右。

3.根据权利要求2所述的惯导/里程计自主组合导航方法，其特征在于：

步骤(1)中，惯导系统初始对准方法采用静基座对准或动基座对准方法。

4.根据权利要求2或3所述的惯导/里程计自主组合导航方法，其特征在于：步骤(2)中，

设里程计位移输出在里程计坐标系m中表示为：

ΔS^m＝[ΔS_x 0 0]^T    (1)

则里程计位移在导航坐标系n中表示为：

里程计导航坐标系n位移误差方程：

(3)

可得：

式中：ΔS^m：理想里程计坐标系m输出位移；ΔS_x：理想里程计输出载体前向位移；ΔSⁿ：理想里程计坐标系m输出位移在导航坐标系n中的表示； 实际里程计坐标系m输出位移在导航坐标系n投影； 惯导载体系b到计算导航坐标系n′的计算姿态转移矩阵； 实际 里程计坐标系m输出位移； 理想导航坐标系n到计算导航坐标系n′的姿态转移矩阵； 里程计坐标系m到理想导航坐标系n的姿态转移矩阵； 计算里程计坐标系m到计算导航坐标系n′的姿态转移矩阵； 惯导载体系b到里程计坐标系m的姿态转移矩阵；δK_D：里程计刻度系数误差；φ＝[φ_N φ_U φ_E]^T：惯导失准角；里程计坐标系m位移误差在导航坐标系n中的投影分量；φ_a＝[φ_ax φ_ay φ_az]^T：里程计坐标系m与惯导载体系b之间x轴安装误差角，y轴安装误差角，z轴安装偏差角；

设计如下惯导/里程计组合导航系统误差模型：

将以上连续系统误差模型进行离散化得到式(6)所示模型：

其中，离散状态转移矩阵为：

I_15×15为15维单位阵，T_n为离散周期；

ω_ie：地球自转角速率；

V_N，V_U，V_E：惯导北向、天向、东向速度；

f_N，f_U，f_E：惯导北向、天向、东向加速度；

λ，L，h：惯导经度、纬度、高度；

R_M，R_N：地球子午面、卯酉面半径；

δV_N，δV_U，δV_E：惯导北向、天向、东向速度误差；

φ_N，φ_U，φ_E：惯导北向、天向、东向失准角；

惯导经度、高度、纬度误差；

加速度计零位误差；

ε_x，ε_y，ε_z：陀螺常值漂移；

φ_ay：里程计载体系与惯导载体系之间y轴安装误差角； 

量测矩阵为：

其中，T_s为里程计采样周期。