脉冲数与里程计脉冲数，采样周期为 0. 01s。对准完成后车辆机动10分钟，保存原始脉冲数。
[005引计算机动时间（10分钟）内惯组累积的位移增量，具体如下：
其中at为采样周期间隔，< 为惯组速度，N为机动时间内惯 导更新次数。
[0053] 计算机动时间内里程计中间变量
[0054]
[00对其中，K。,为里程计脉冲当量，N康示各采样间隔内里程计输出脉冲数，q：(/)隶示 各个采样间隔内的姿态余弦矩阵。
[0056] 计算得到包含俯仰安装偏角及航向安装偏角的矢量：
[0057]
[0058] 从中分离得到俯仰安装偏角和航向安装偏角；
[0059]
[0060] 上述步骤S2中，根据状态向量X建立状态微分方程乂 =F的X+W的， 其中，F(t)为状态转移矩阵；w(t)为系统噪声，其中t表示该物理量随时间变化； w(f) = [-(C:0'Kf(C:(0P:.)T0m]t，C:(/)为第i个采样周期内的姿态余弦矩阵，切为巧 螺随机噪声，^^:为加速度计随机噪声；乂 =[(於脚叩' (SP)T柄叩' (Pt)T耗^了了 4为姿态误差状态向量，4 =[&E *U]T，&E为东向姿态误差，为北向姿态误差， 为天向姿态误差；5v"为速度误差状态向量，5户&玄7，为东向速度 误差，为北向速度误差，^咕为天向速度误差；sP为位置误差状态向量，5P=[ 5L5入5h]T，5L为绅度误差、5A为经度误差、5h为高度误差；苗为巧螺漂移误差向量， 4 =[马马e_!]T，e:为X巧螺误差，￡；；为y巧螺误差，马为Z巧螺误差；K为加速度计偏 置误差向量，K=[>? 6ft]，巧为X加速度计误差，巧为y加速度计误差，巧为Z加 速度计误差；C"d为里程计误差相关量，C"d= [5K。。，5ae，5aJ，5K"d为里程计刻度 系数误差，5ae为俯仰安装偏角误差，5a4为航向安装偏角误差。
[0061] 给出状态向量X各项的微分方程，其中姿态误差微分方程如下：
[0062]
[006引其中，<为地球自转角速度与载体运动引起的角速度之和。
[0064] Ml、Mg计算公式如下：
[0065]
[0066] 其中Rm为地球子午圈半径、RW为地球卯酉圈半径，h为车载的高度。
[0069] 速度误差微分方程为：
[0070]
[00川其中，馬为比力在导航系下的投影，Ms、M4计算公式如下：
[0072]
[0073] 其中vD为惯组速度，却:为地球自转角速度在导航系下的投影，为载体运动引 起的角速度在导航系下的投影。
[0074] 位置误差微分方程为：
[00巧]
[0076] 上式中Ms、Me计算公式如下：
[0077]
[0078] 得到状态微分方程；
[0079]
[0080] 得到状态转移矩阵F(t)表示如下：
[0081]
[0082] 系统噪声状态向量
其中诗为巧螺随机噪声， 为巧螺随机噪声，C；：.为惯组坐标系与导航坐标系之间的方向余弦矩阵。
[0083] 上述步骤S3中，根据里程计位移增量与惯组位移增量构造量测量，建立系统量测 方程。
[0084] 首先计算惯组在采样周期内导航系下的位移增量（可优选梯形构造法）为：
[0085]
[008引上式中AT为惯组采样周期，在采样周期内里程计增量ASf在导航系下的位移增 量为：
[0087]
[0088] 累积预设时间（如1秒）内惯导位移增量与里程计位移增量，并将二者之差作为 量测值，即；
[0089]
[0090] 建立量测方程：
[0091] Z=H(t)X+v(t)
[009引上式中量测矩阵H(t)为：
[0093]
[0094] 其中i表示采样周期序号，且有：
[0095]
[0096] 上述步骤S4中，对连续状态微分方程离散化，建立卡尔曼滤波器，根据状态方程 和量测方程设定系统噪声矩阵Qk和量测噪声矩阵Rk初值，设定滤波初值X。和滤波状态误 差初值P。。根据惯组数据和里程计数据实时进行导航计算，将量测值输入卡尔曼滤波器，通 过滤波估计后得到各个误差状态的估计值，利用估计值修正导航参数，W修正后导航参数 作为下一次导航初值继续进行导航解算，直至导航过程结束。
[0097] 首先对连续状态微分方程离散化，得到离散形式的状态方程：
[0098] Xk+i=巫k+i,品+Wk
[0099] 其中；
[0100]
[0101] 上式中T表示卡尔曼滤波离散化周期，该里优选设置为0. 1秒。设置卡尔曼滤波 初始值，包括滤波初值X。和滤波状态误差初值P。，W及系统噪声矩阵Qk和量测噪声矩阵Rk 初值：
[0102] X〇= 0
[0103] 滤波状态误差初值P。为对角矩阵，对角线元素如下：
[0104]
[0105] 噪声矩阵Qk为对角矩阵，对角线元素如下：
[0106]
[010引噪声矩阵Rk为对角矩阵，对角线元素如下
[0109]
[0110] 在离散状态方程基础i建立卡!^曼滤波器，推导^态向量更新计算公式，如下: 阳…]a)计算状态一步预测式-…
[0112]
[011引b)计算一步预测均方差阵Pk/k_i
[0114]
[0115] C)计算滤波增益矩阵Kk
[0116]
[0117]d)计算状态最优估计?义，
[011 引
[0119] e)计算状态估计均方差阵Pk
[0120] Pk= (I-KkHk)Pk/k-i
[0121] 根据惯组数据和里程计数据实时进行导航计算，得到姿态矩阵C；：、速度V、位置 (Z，义h)，将量测值ZabJt入卡尔曼滤波器，通过滤波估计后得到各个误差状态的估计值， 包括姿态误差估计值、速度误差估计值而^"、位置误差估计值.、里程计刻度系 数误差及安装偏角误差估计值式，利用估计值修正导航参数，修正方法如下；
[0128] W反馈校正后的导航参数作为下一次导航初值，继续进行导航解算，直至导航结 束。
[0129] 本领域的技术人员容易理解，W上所述仅为本发明的较佳实施例而已，并不用W 限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含 在本发明的保护范围之内。
【主权项】
1. 一种激光惯组与里程计组合导航方法，其特征在于，所述方法包括以下步骤： 51、 采集正常工作状态下第i个采样周期内里程计脉冲数Ni,求解惯组与里程计之间的 俯仰安装偏角α 0和航向安装偏角α ,，所迓为矢量第j个分量，j = 1，2, 3 ;q： (〇为第i个采样周期内惯组坐标系与导航坐标系之间的方向余弦矩阵， ％预设 时间内惯组导航解算的位移增量，Ktjd为里程计脉冲当量； 52、 建立状态方程X = + 其中，F为状态转移矩阵，w为系统噪声，Φ为惯导姿态误差，δ vn为惯导速度误 差，S P为惯导位置误差，< 为陀螺漂移误差；为加速度计偏置误差，里程计误差相关量Ld= [sKmi, δ α 0，δ α ψ]，其中δ Ktjd为里程计刻度系数误差，δ α 0为俯仰安装偏角误 差，δ α ψ为航向安装偏角误差； 53、 建立量测方程ζ = ΗΧ+ν，其中V为量测噪声；量测量 累积预 设采样周期内惯导位移增量与里程计位移增量，并将二者之差作为量测量ζ，所述Δ#/?为 惯组在第i个采样周期内导航系下的位移增量，为第i个采样周期内里程计增量ASf 在导航系下的位移投影；I为单位矩阵， 54、 利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误 差和器件参数误差，完成组合导航。2. -种激光惯组与里程计组合导航系统，其特征在于，所述系统包括： 第一子模块，用于采集正常工作状态下第i个采样周期内里程计脉冲数Ni,求解惯组与 里程计之间的俯仰安装偏角a e和航向安装偏角α ψ，所述为矢量Δαρ第j个分量，j = 1，2, 3 ; C；： (〇为第i个采样周期内惯组坐标系与导航坐标系之间的方向余弦矩阵，为预设 时间内惯组导航解算的位移增量，Ktjd为里程计脉冲当量；第二子模块，用于建立状态方程X = + w，其中，F为状态转移矩阵，w为系统噪声，Φ为惯导姿态误差，δ vn为惯导速度误 差，SP为惯导位置误差，< 为陀螺漂移误差；为加速度计偏置误差，里程计误差相关量 Ld= [sKmi, δ α 0，δ α ψ]，其中δ Ktjd为里程计刻度系数误差，δ α 0为俯仰安装偏角误 差，δ α ψ为航向安装偏角误差； 第三子模块，用于建立量测方程ζ = ΗΧ+ν，其中V为量测噪声；量测量累积预设采样周期内惯导位移增量与里程计位移增量，并 将二者之差作为量测量ζ，所述为惯组在第i个采样周期内导航系下的位移 增量，为第i个采样周期内里程计增量Δ#在导航系下的位移投影；量测矩阵第四子模块，用于利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导 航系统参数误差和器件参数误差，完成组合导航。
【专利摘要】本发明公开了一种激光惯组与里程计组合导航方法及系统，方法包括步骤：S1、采集正常工作状态下N个采样周期内里程计脉冲数，求解惯组与里程计之间的俯仰安装偏角和航向安装偏角；S2、建立状态方程；S3、建立量测方程，其中量测量为惯组在第i个采样周期内导航系下的位移增量，为第i个采样周期内里程计增量在导航系下的位移投影；S4、利用状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。本发明还提供实现上述方法的系统。实施本发明能有效修正组合导航误差量，大幅度提高组合导航精度，尤其适用于惯组处于大角度斜置的情况。
【IPC分类】G01C21/20, G01C25/00
【公开号】CN104977004
【申请号】CN201510408912
【发明人】李旦, 刘明, 牟方佳, 杨道安, 胡华峰
【申请人】湖北航天技术研究院总体设计所
【公开日】2015年10月14日
【申请日】2015年7月13日