[发明专利]一种惯性里程计组合高精度姿态测量方法

摘要

本发明属于导航方法技术领域，具体涉及一种惯性/里程计组合高精度姿态测量方法，可应用于车载长航时组合导航，复杂地形测绘等领域。本发明技术方案以航位推算结果为基准，进行组合卡尔曼滤波计算，对水平失准角和水平陀螺漂移进行估计并修正。其中卡尔曼滤波状态方程取19个状态变量，建立了系统误差模型，利用航位推算结果，对水平失准角和水平陀螺漂移进行实时在线估计，并进行反馈修正，提高姿态精度，从而有效的提高了系统组合导航精度。

主权项

一种惯性里程计组合高精度姿态测量方法，其特征在于，包括以下步骤：(1)确定卡尔曼滤波状态方程状态变量卡尔曼滤波状态方程取19个状态变量，分别为：北向速度误差δV_N、天向速度误差δV_U、东向速度误差δV_E、纬度误差高度误差δh、经度误差δλ、北向失准角φ_N、天向失准角φ_U、东向失准角φ_E、X陀螺漂移ε_x、Y陀螺漂移ε_y、Z陀螺漂移ε_z、X加表零偏Y加表零偏Z加表零偏航位推算纬度误差航位推算高度误差δh_D、航位推算经度误差δλ_D、里程计刻度系数误差δK_D；(2)确定系统各误差方程a)速度误差方程式中：V_N、V_U、V_E——北向、天向、东向速度，单位：m/s；RM——子午圈半径，单位：m；RN——卯酉圈半径，单位：m；h——惯性高度，单位：m；ω_ie——地球自转角速率，常数，取7.292115×10^‑5rad/s；——纬度，单位：rad；f_N、f_U、f_E——滤波计算用n系下三轴比力分量，单位：m/s²；C₁₁、C₂₁、C₃₁、C₁₂、C₂₂、C₃₂、C₁₃、C₂₃、C₃₃——姿态矩阵中的元素；b)位置误差方程c)姿态角误差方程d)陀螺和加速度计误差方程$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_i=0\\ {\stackrel{\·}{\▿}}_i=0\end{array}\right.(i=x,y,z)\·\·\·\left(4\right)$e)航位推算位置误差方程f)里程计刻度系数误差方程$\mathrm{\δ}{\stackrel{\·}{K}}_D=0\·\·\·\left(6\right)$综合式(1)～(6)，可得系统状态方程如下：$\stackrel{\·}{X}\left(t\right)=\mathrm{FX}\left(t\right)+W\left(t\right)\·\·\·\left(7\right)$式中：X(t)——系统状态变量，即上述的卡尔曼滤波状态方程19个状态变量；F——根据公式(1)～(6)确定的系统矩阵；W(t)——系统噪声；(3)确定系统观测方程系统观测量取导航解算位置与航位推算位置之差，则系统观测方程如下：式中：Z(t)——观测变量；H——观测矩阵；V(t)——观测噪声；λ、h——惯性纬度、经度、高度，单位：rad，rad，m；λ_D、h_D——航位推算纬度、经度、高度，单位：rad，rad，m；将公式(7)和(8)离散化，可得：$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.\·\·\·\left(9\right)$式中：X_k——状态变量；Φ_k,k‑1——上一滤波周期至当前滤波周期的一步转移矩阵；Z_k——观测量；H_k——观测矩阵；W_k——系统激励噪声序列；V_k——观测噪声序列；一步转移矩阵Φ_k,k‑1可用下式计算：${\mathrm{\Φ}}_{k,k-1}=I+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}F{T}_n\·\·\·\left(10\right)$式中：I——单位矩阵；T_n——导航周期；n——相邻两次滤波时刻之间的导航周期个数；(4)确定卡尔曼滤波递推方程组卡尔曼滤波递推方程组如下：a)状态一步预测${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\·\·\·\left(11\right)$b)状态估计${\hat{X}}_{k/k-1}{=\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\·\·\·\left(12\right)$c)滤波增益$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\·\·\·\left(13\right)$d)一步预测均方误差$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}\·\·\·\left(14\right)$e)估计均方误差$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T\·\·\·\left(15\right)$式中：——当前滤波周期的实时状态估计值；——上一滤波周期对当前滤波周期的状态预测值；K_k——当前滤波周期的滤波增益阵；P_k/k‑1——上一滤波周期对当前滤波周期的预测误差估计的协方差阵；P_k——当前滤波周期的实时误差估计协方差阵；Q_k‑1——系统噪声方差阵；R_k——观测噪声方差阵；(5)卡尔曼滤波递推计算和反馈修正经过卡尔曼滤波递推计算后，估计出北向失准角φ_N、东向失准角φ_E、X陀螺漂移ε_x、Z陀螺漂移ε_z；然后对其进行反馈修正以提高姿态精度。