本发明属于陆用车辆导航技术领域，尤其是一种里程计辅助车载动态对准方法。
背景技术：
陆用车辆导航系统一般由惯性测量单元和里程计构成。惯性测量单元由三只陀螺仪和三只加速度计构成，敏感载体角增量和速度增量；里程计测量车辆行驶的位移增量。惯性测量单元构成惯性导航系统，装订初始位置完成对准后就可以进行导航解算。惯性导航系统又可以与里程计结合构成车辆航位推算系统，通过惯导系统的姿态航向分解车辆里程计测量的位移增量，从而完成车辆的定位解算功能。由于惯性导航系统的初始对准一般是在载车静止状态下完成的，因此，如何在车辆行驶过程中完成惯导系统的初始对准功能是目前迫切需要解决的问题。
技术实现要素：
本发明的目地在于克服现有技术的不足，提出一种里程计辅助车载动态对准方法，解决导航系统初始启动过程中车辆必须静止不动的问题。
本发明解决其技术问题是采取以下技术方案实现的：
一种里程计辅助车载动态对准方法，包括以下步骤：
步骤1、在惯导系统中，进行惯导解算输出姿态航向、速度和位置信息并更新；
步骤2、利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成里程计航位推算；
步骤3、以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，利用卡尔曼滤波器对惯导系统的姿态角误差进行估计和校准。
所述步骤1采用三只陀螺仪和三只加速度构成惯性导航系统进行惯导解算，具体方法为：通过采集陀螺仪的输出角速度数据并扣除地球自转角速度以及牵连角速度，计算得到姿态角；加速度计测量的加速度通过方向余弦阵进行坐标变换到地理坐标系的北向、东向、天向轴上，积分地理坐标系上的加速度并扣除重力和有害加速度的影响得到地理坐标系的北向、东向和天向速度；利用地理坐标系的北向、东向速度，并考虑地球的曲率半径计算得到经度和纬度。
所述步骤3惯导系统误差模型包括：姿态角误差、速度误差、位置误差、陀螺常值误差和加表常值误差；所述里程计行位推算的误差模型包括：里程计定位误差、俯仰安装偏角误差、方位安装偏角误差和里程当量误差。
所述利用卡尔曼滤波器对惯导系统的姿态角误差进行估计和校准的方法包括以下步骤：
(1)卡尔曼滤波一步预测步骤：
(2)当滤波时间到时进行滤波更新步骤；
(3)当系统计时器到达修正时刻，以修正后的姿态四元数、速度、位置为初始值进入自主导航步骤。
所述步骤(1)包括状态转移阵的计算、输入噪声方差阵的计算和状态预测与误差方差预测的计算。
所述步骤(2)包括量测计算、滤波增益计算、状态估计更新和误差方差更新。
所述步骤(3)包括姿态四元数修正、速度修正和位置修正过程。
本发明的优点和积极效果是：
1、本发明以航位推算系统的位置信息与惯导系统的位置信息进行比较，利用卡尔曼滤波器对惯导系统的姿态航向误差进行实时估计与校准，可以在车辆行驶过程中完成惯导系统的初始对准，避免了在导航系统初始启动过程中车辆必须静止不动的限制，提高了导航系统的快速性和灵活性，更加高效地满足了用户车辆的行驶和导航需求。
2、本发明利用载车里程计提供的位移增量信息与惯性测量组件提供的陀螺加表数据，基于惯导系统误差模型和里程计航位推算误差模型，通过卡尔曼滤波器进行深度融合，在车辆行进过程中，完成姿态航向的精确对准与位置误差的校准功能的组合导航技术可简化车载导航系统的启动过程，缩短了反应时间，提高了载车机动性能，可解决车载导航系统在车辆行驶过程中的初始对准问题。
附图说明
图1是本发明的导航解算流程图；
图2是本发明的惯导里程计组合导航系统导航流程图。
具体实施方式
以下结合附图对本发明实施例做进一步详述。
一种里程计辅助车载动态对准方法，在包括惯性测量单元和里程计车载导航系统内实现，其利用惯性测量单元的角增量和速度增量输出进行惯性导航解算；利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成航位推算；再利用卡尔曼滤波器，以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，对惯导系统的姿态角误差进行估计和校准。具体包括如下步骤：
步骤1、在惯性导航系统中，进行惯导解算输出姿态航向、速度和位置信息并更新。
在本步骤中，在三只陀螺仪和三只加速度构成惯性导航系统中，进行捷联惯导解算。通过采集陀螺的输出角速度数据并扣除地球自转角速度以及牵连角速度，计算得到姿态角。加速度计测量的加速度通过方向余弦阵进行坐标变换到地理坐标系的北向、东向、天向轴上，积分地理坐标系上的加速度并扣除重力和有害加速度的影响得到地理坐标系的北向、东向和天向速度。利用地理坐标系的北向、东向速度，并考虑地球的曲率半径计算得到经度和纬度。导航解算流程如图1所示，具体方法为：
记初始姿态角对应的方向余弦矩阵对应的姿态四元数记为将转成姿态四元数形式：
k＝0，1，2，…时，利用四元数更新算法计算tk+1时刻的姿态四元数
其中
Δθ＝[Δθx Δθy Δθz]T为(tk,tk+1]采样周期内的陀螺输出角增量，单位：rad；
为(tk,tk+1]更新周期内n系相对于i系转过的角度。
为e系相对于i系的自转角速率在n系的投影，为n系相对于e系的角速率在n系上的投影，由以下两式计算：
式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果。最后进行四元数归一化处理
得到更新后的姿态四元数
加速度计输出的速度增量Δv经由坐标变换得地理坐标系上的速度分量
其中，Δv＝[Δvx Δvy Δvz]T为加速度计输出的速度增量。
地理坐标系上的加速度分量为
fn＝Δvn/h
其中，记fn＝[fE fN fU]T。
速度更新初始时刻，vn(0)＝[0 0 0]T；
k＝0，1，2，…时，在一个速度更新周期(tk,tk+1]内，速度更新为
其中，
δA为有害加速度，计算如下：
初始时刻，L(0)、λ(0)和S(0)为装订的纬度、经度和高程；
k＝0，1，2，…时，在一个速度更新周期(tk,tk+1]内，位置更新为
步骤2、利用里程计的车辆位移增量输出与惯导解算的姿态角进行里程分解，完成里程计航位推算。
记车轮接触的地平面内的右前上坐标系为车体系m系。则在该系内里程计速度表示为：
m系与惯组的b系之间的安装误差角为俯仰安装误差角αθ和方位安装误差角αψ，有如下关系：
则导航系内的速度为：
其中，
则数字递推的位置更新算法如下：
其中RN为当地地球子午圈曲率半径，RE为当地地球卯酉圈曲率半径，h为航位推算的更新周期5ms。
步骤3、利用卡尔曼滤波器，以惯导系统的误差模型和航位推算的误差模型作为误差状态的传播方程，对惯导系统的姿态角误差进行估计和校准。
在本步骤中，选取了21维状态变量，分别是惯导系统误差状态(包括姿态角误差φ3、速度误差δvn、位置误差δp、陀螺常值误差ε和加表常值误差▽)和里程计定位系统误差状态(里程计定位误差δpodo、俯仰安装偏角误差δαθ、方位安装偏角误差δαψ和里程当量误差δKodo)
惯导里程计组合导航的误差状态转移矩阵如下所示，包括惯导误差方程和里程计定位误差方程两部分：
其中惯导系统误差方程为
里程计定位误差方程为
FOdo＝[FOdo1 03×12 FOdo2 FOdo3]
如图2所示，惯导里程计组合导航系统的卡尔曼滤波器一步预测包括状态转移阵Φk,k-1的计算、输入噪声方差阵的计算、状态预测与误差方差预测Pk,k-1的计算三个阶段。下面分别进行说明
(1)状态转移阵Φk,k-1的计算
记(tk-1,tk]为一个预测周期，h＝tk-tk-1。预测周期h一般较短，状态转移阵计算如下
其中，F＝FINS/Odo。
(2)输入噪声方差阵的计算
连续系统的系统噪声即三个陀螺和三个加速度计向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为
Qq＝G(t)Q(t)GT(t)
其中，Q(t)为常量，G(t)为噪声输入矩阵，重写如下：
Q＝diag[(εx)2 (εy)2 (εz)2 (▽x)2 (▽y)2 (▽z)2]
得到连续系统的输入噪声方差Qq后计算Kalman滤波的输入噪声方差如下：
(3)状态预测与误差方差预测Pk,k-1的计算
初始时刻当k＝0时，对和P0进行初始化；
k＝0，1，2，…时，递推计算
当滤波更新周期未到时，不进行滤波更新，继续进行预测，令
Pk＝Pk,k-1
当滤波更新周期到时，Pk按下一节的滤波更新计算。
当滤波时间到时进行滤波更新，滤波更新周期一般为外参考信息更新周期，滤波更新周期设为1s，分为四步计算：
(1)量测计算
按下式计算量测值：
下标s表示捷联惯导系统解算输出，下标r表示参考基准输出。
Ls、λs为解算纬度、经度，单位：rad；
Ss为解算高度，单位：m；
Lr、λr为装订纬度、经度，单位rad；
Sr为参考高度，单位：m。
(2)滤波增益计算
按下式计算滤波增益Kk：
其中，
Pk,k-1为误差方差预测计算得；
Hk＝[06×3 I6×6 06×6]；
Rk＝diag[(Rve)2 (Rvn)2 (Rvu)2 (Rlat)2 (Rlon)2 (Rh)2]。
(3)状态估计更新
按下式计算状态估计
其中，为状态预测计算得。
(4)误差方差更新
按下式计算误差方差Pk：
当系统计时器到达修正时刻，以修正后的姿态四元数、速度、位置为初始值进入自主导航，假设tk时刻为修正时间点：
姿态四元数修正时刻，利用Kalman滤波估计得到失准角φE、φN、φU，对应的第1、2、3个元素，修正角记为φc＝[φE φN φU]T，采用四元数法修正tk时刻的姿态四元数
其中，
通过以上步骤，实现在车辆行驶过程中完成惯导系统的初始对准功能。
需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。