一种惯性里程计组合高精度姿态测量方法
【技术领域】
[0001] 本发明属于导航方法技术领域，具体涉及一种惯性/里程计组合高精度姿态测量 方法，可应用于车载长航时组合导航，复杂地形测绘等领域。
【背景技术】
[0002] 对自主性、可靠性、精度等要求高的车辆导航应用场合，常使用多传感器组合导 航系统，尤其在军用场合，组合导航更是首选方案。惯性导航系统可以完全自主地提供全 面的导航信息，但其误差会随着时间不断积累。里程计作为车载导航系统中十分重要的组 成部分，其特点是成本较低，且误差不随时间积累，可以测得较精确的车速或里程，所以可 以将惯导系统与里程计进行组合，以取得较高精度的测量结果。
[0003] 惯性/里程计组合的常用方法为航位推算，即利用惯导系统进行姿态测量，利用 里程计进行速度或路程测量，并经过刻度系数修正后，借助惯导系统的姿态信息，从载体坐 标系转换到导航坐标系，从而得到载体不随时间发散的、较精确的位置信息。该方法的精 度主要受里程计刻度系数和姿态精度的影响，因此如何提高姿态测量精度，成为影响惯性/ 里程计组合导航精度的主要因素。
[0004] 综上，亟需提出一种基于惯性/里程计的高精度姿态测量方法，将航位推算结果 和惯性导航解算结果进行组合，利用卡尔曼滤波算法对惯导系统姿态误差和陀螺漂移误差 进行估计并修正，从而提高系统姿态测量精度。
【发明内容】
[0005] 本发明要解决的技术问题是提出一种惯性/里程计组合高精度姿态测量方法，对 惯导系统姿态误差和陀螺漂移误差进行估计并修正，提高系统姿态测量精度，进而提高系 统组合导航精度。
[0006] 为了实现这一目的，本发明采取的技术方案是：
[0007] -种惯性里程计组合高精度姿态测量方法，包括以下步骤：
[0008] (1)确定卡尔曼滤波状态方程状态变量
[0009] 卡尔曼滤波状态方程取19个状态变量，分别为：北向速度误差δ Vn、天向速度误 差δ Vu、东向速度误差δ VE、讳度误差知、商度误差δ h、经度误差δ λ、北向失准角φ Ν、天 向失准角、东向失准角ΦΕ、Χ陀螺漂移εχ、γ陀螺漂移e y、Z陀螺漂移εζ、χ加表零偏 Vi.、Y加表零偏％、Z加表零偏1、航位推算纬度误差灸^、航位推算高度误差δ hD、航位 推算经度误差S λ D、里程计刻度系数误差SKD;
[0010] (2)确定系统各误差方程
[0011] a)速度误差方程
[0012]
[0013] 式中：
[0014] VN、Vu、VE-北向、天向、东向速度，单位：m/s ;
[0015] RM-子午圈半径，单位：m ;
[0016] RN-卯酉圈半径，单位：m ;
[0017] h--掼性高度，单位：m;
[0018] ωιβ--地球自转角速率，常数，取7.292115 XlO 5rad/s ;
[0019] φ-纬度，单位：rad ;
[0020] fN、fu、fE-滤波计算用n系下三轴比力分量，单位：m/s2 ;
[0021] Cn、C21、C31、C12、C22、C 32、C13、C23、C33--姿态矩阵< 中的元素；
[0022] b)位置误差方程
[0024] c)姿态角误差方程
[0026] d)陀螺和加速度计误差方程
[0028] e)航位推算位置误差方程
[0030] f)里程计刻度系数误差方程
[0032] 综合式（1)~（6)，可得系统状态方程如下：
[0034] 式中：
[0035] X(t)--系统状态变量，即上述的卡尔曼滤波状态方程19个状态变量；
[0036] F--根据公式（1)~（6)确定的系统矩阵；
[0037] ff(t)--系统噪声；
[0038] (3)确定系统观测方程
[0039] 系统观测量取导航解算位置与航位推算位置之差，则系统观测方程如下：
[0041] 式中：
[0042] Z(t)--观测变量；
[0043] H--观测矩阵；
[0044] V(t)--观测噪声；
[0045] Ψ、\、h--掼性纬度、经度、高度,单位：rad，rad，m ;
[0046] Ψο '' AD>hD-航位推算讳度、经度、高度，单位：rad，rad, m ;
[0047] 将公式（7)和⑶离散化，可得：
[0049] 式中：
[0050] Xk--状态变量；
[0051] --上一滤波周期至当前滤波周期的一步转移矩阵；
[0052] Zk--观测量；
[0053] Hk--观测矩阵；
[0054] Wk--系统激励噪声序列；
[0055] Vk--观测噪声序列；
[0056] 一步转移矩阵〇k,k i可用下式计算：
[0058] 式中：
[0059] I--单位矩阵；
[0060] Tn--导航周期；
[0061] η--相邻两次滤波时刻之间的导航周期个数；
[0062] (4)确定卡尔曼滤波递推方程组
[0063] 卡尔曼滤波递推方程组如下：
[0064] a)状态一步预测
[0066] b)状态估计
[0068] c)滤波增益
[0070] d) -步预测均方误差
[0072] e)估计均方误差
[0074] 式中：
[0075] Xh--当前滤波周期的实时状态估计值；
[0076] --上一滤波周期对当前滤波周期的状态预测值；
[0077] Kk--当前滤波周期的滤波增益阵；
[0078] Pk/kl--上一滤波周期对当前滤波周期的预测误差估计的协方差阵；
[0079] Pk--当前滤波周期的实时误差估计协方差阵；
[0080] Qk!--系统噪声方差阵；
[0081] Rk--观测噪声方差阵；
[0082] (5)卡尔曼滤波递推计算和反馈修正
[0083] 经过卡尔曼滤波递推计算后，估计出北向失准角ΦΝ、东向失准角ΦΕ、X陀螺漂移 ε x、Z陀螺漂移ε ζ ;然后对其进行反馈修正以提高姿态精；修正方法如下：
[0084] a)姿态修正，用Cf表示修正后的姿态矩阵
[0085] 失准角计算公式如下：
[0087] Xk (i)，i = 1~19,表示上述的卡尔曼滤波状态方程19个状态变量中的第i个状 态变量；
[0088] 姿态修正公式如下：
[0090] 式中：c代表cos, s代表sin ;
[0092] b)陀螺漂移修正
[0093] 陀螺漂移修正公式如下：
[
[0095] 将陀螺漂移补偿到载体系下的角速率中，实时参与导航解算：
[
[0097]式中：
表示导航周期内载体坐标系下陀螺角速率输出，单位： rad/s〇
[0099] 本发明提出了一种惯性/里程计组合高精度姿态测量方法，主要测量车辆三维姿 态、三维速度、三维位置等导航信息。本发明技术方案以航位推算结果为基准，进行组合卡 尔曼滤波计算，对水平失准角和水平陀螺漂移进行估计并修正。其中卡尔曼滤波状态方程 取19个状态变量，建立了系统误差模型，利用航位推算结果，对水平失准角和水平陀螺漂 移进行实时在线估计，并进行反馈修正，提高姿态精度，从而有效的提高了系统组合导航精 度。
【具体实施方式】
[0100] 下面结合【具体实施方式】对本发明技术方案进行进一步详细说明。
[0101] -种惯性里程计组合高精度姿态测量方法，包括以下步骤：
[0102] (1)确定卡尔曼滤波状态方程状态变量
[0103] 卡尔曼滤波状态方程取19个状态变量，分别为：北向速度误差δ Vn、天向速度误 差δ Vu、东向速度误差δ VE、讳度误差:辱、商度误差δ h、经度误差δ λ、北向失准角φΝ、天 向失准角、东向失准角ΦΕ、Χ陀螺漂移εχ、γ陀螺漂移e y、Z陀螺漂移εζ、χ加表零偏 ▽ Xi、Y加表零偏\、Z加表零偏兄2、航位推算纬度误差和>〇、航位推算高度误差δ hD、航位 推算经度误差S λ D、里程计刻度系数误差SKD;
[0104] (2)确定系统各误差方程
[0105] g)速度误差方程
[0107] 式中：
[0108] VN、Vu、VE-北向、天向、东向速度，单位：m/s ;
[0109] RM-子午圈半径，单位：m ;
[0110] RN-卯酉圈半径，单位：m;