[发明专利]一种基于标量估计算法的INS/GNSS组合导航方法

摘要

本发明公开了一种基于标量估计算法的INS/GNSS组合导航方法，根据航位推算技术和卫星导航定位技术的不同误差动态特性，基于标量估计算法将两种导航方法结合起来，构建INS/GNSS组合导航系统，方法步骤如下：首先建立和离散化INS/GNSS组合导航系统误差模型；其次基于卡尔曼滤波算法，构建标量估计算法；再基于标量估计算法，引入新的量测矢量Z*；最后应用标量估计算法，将组合导航系统误差模型的状态变量表达成标量估计形式。根据标量估计形式构建INS/GNSS组合导航系统，补偿航行体导航系统误差。本发明能够克服传统卡尔曼滤波因噪声统计特性确定不准确而造成的滤波器发散难题，提高组合导航系统的可靠性，降低组合导航系统的成本，提供一种全新的组合导航方法。

主权项

一种基于标量估计算法的INS/GNSS组合导航方法，其特征在于，方法步骤如下：步骤1.构建INS/GNSS组合导航系统误差模型：为了获取航行体的相对位置、速度和姿态信息，选取东北天地理坐标系作为组合导航系统结算的基本坐标系，其中，东北天坐标系原点为航行体质心，x轴沿参考椭球卯酉圈方向指向东，y轴沿参考椭球子午圈方向指向北，z轴由右手定则决定；在东北天坐标系中，INS/GNSS组合导航系统误差模型简化为如下形式：式中：δV_x为航行体指东方向运动速度误差；δV_y为航行体指北方向运动速度误差；ΔV_x为加速度计沿x轴方向的工具误差；ΔV_y为加速度计沿y轴方向的工具误差；a_x为航行体沿x轴方向的加速度分量；a_y为航行体沿y轴方向的加速度分量；Ф_x为陀螺仪平台绕x轴方向的失准角；Ф_y为陀螺仪平台绕y轴方向的失准角；Ф_z为陀螺仪平台绕z轴方向的失准角；ε_x为陀螺仪x轴方向的漂移；ε_y为陀螺仪y轴方向的漂移；ε_z为陀螺仪z轴方向的漂移；ω_x为陀螺平台沿x轴方向的角速度分量；ω_y为陀螺平台沿y轴方向的角速度分量；ω_z为陀螺平台沿z轴方向的角速度分量；w_x为沿x轴方向的外部干扰；w_y为沿y轴方向的外部干扰；w_z为沿z轴方向的外部干扰；μ为陀螺漂移随机变化的平均频率；g为重力加速度；R为地球半径；为陀螺仪平台所处纬度；表示变量×对时间的微分；将表达式(1)表示为矩阵形式，即组合导航系统误差模型状态方程为：$\stackrel{.}{X}=\mathrm{FX}+W---\left(2\right)$式中：系统状态矢量X＝[δV_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T，系统输入噪声$W={\left[\begin{array}{cccccccc}\mathrm{\Δ}{\stackrel{.}{V}}_x& \mathrm{\Δ}{\stackrel{.}{V}}_y& 0& 0& 0& w_x\left(t\right)& w_y\left(t\right)& w_z\left(t\right)\end{array}\right]}^T$系统状态转移矩阵步骤2.离散化INS/GNSS组合导航系统误差模型：INS/GNSS组合导航系统误差模型的状态方程表达为如下的离散矩阵形式：X_k＝ΦX_k‑1+W_k‑1  (3)式中：组合导航系统误差模型的状态矢量X_k＝[δV_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T，其中，δV_x为航行体指东方向运动速度误差；δV_y为航行体指北方向运动速度误差；Ф_x为陀螺仪平台绕x轴方向的失准角；Ф_y为陀螺仪平台绕y轴方向的失准角；Ф_z为陀螺仪平台绕z轴方向的失准角；ε_x为陀螺仪x轴方向的漂移；ε_y为陀螺仪y轴方向的漂移；ε_z为陀螺仪z轴方向的漂移；k为时间参数，表示当前时刻；组合导航系统误差模型的输入噪声W_k‑1＝[B_x B_y 0 0 0 w_x w_y w_z]^T，其中，B_x为加速度计沿x轴方向的零漂误差；B_y为加速度计沿y轴方向的零漂误差；w_x为沿x轴方向的外部干扰噪声；w_y为沿y轴方向的外部干扰噪声；w_z为沿z轴方向的外部干扰噪声；组合导航系统误差模型的系统状态转移矩阵其中，T为采样时间；μ为陀螺漂移随机变化的平均频率；g为重力加速度；R为地球半径；为陀螺仪平台所处纬度；步骤3.基于卡尔曼滤波算法，构建标量估计算法：离散化后的组合导航系统误差模型如表达式(3)所示，此时，组合导航系统误差测量方程表示为：Z_k＝HX_k+V_k  (4)式中：X_k为系统状态矢量；Z_k为系统测量矢量；V_k为测量噪声；k为时间参数，表示当前时刻；H为系统测量矩阵；此时，经典卡尔曼滤波算法具有以下形式：${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(5\right)$${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1}---\left(6\right)$$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(7\right)$P_k/k‑1＝Φ_k/k‑1P_k‑1Φ_k/k‑1^T+Q_k  (8)P_k＝(I‑K_kH_k)P_k/k‑1  (9)其中，K_k卡尔曼增益矩阵；P_k/k‑1先验估计误差的方差阵；P_k后验估计误差的方差阵；R_k测量噪声的方差阵；Q_k输入噪声的方差阵；k为时间参数，表示当前时刻；基于初始时刻状态变量X₁，结合表达式(3)和(4)，n+1时刻的状态变量X_n+1递推地表示为：$X_{n+1}={\mathrm{\Φ}}^n{X}_1+{\mathrm{\Φ}}^{n-1}{W}_1+{\mathrm{\Φ}}^{n-2}{W}_2+...+W_n={\mathrm{\Φ}}^n{X}_1+\underset{j-=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Φ}}^{j-1}{W}_{n+1-j}---\left(10\right)$其中，j为1‑n之间的自然数；将表达式(10)展开，表达成如下形式：$\left[\begin{array}{c}{x}_{n+1}^1\\ x_{n+1}^2\\ ...\\ x_{n+1}^n\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& ...& {\mathrm{\φ}}_{1n}\\ {\mathrm{\φ}}_{21}& {\mathrm{\φ}}_{22}& ...& {\mathrm{\φ}}_{2n}\\ ...& ...& ...& ...\\ {\mathrm{\φ}}_{n1}& {\mathrm{\φ}}_{n2}& ...& {\mathrm{\φ}}_{\mathrm{nn}}\end{array}\right]\left[\begin{array}{c}{x}_1^1\\ x_1^2\\ ...\\ x_1^n\end{array}\right]+\left[\begin{array}{c}{w}_1^1\\ w_1^2\\ ...\\ w_1^n\end{array}\right]---\left(11\right)$其中：$\left[\begin{array}{cccc}{\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& ...& {\mathrm{\φ}}_{1n}\\ {\mathrm{\φ}}_{21}& {\mathrm{\φ}}_{22}& ...& {\mathrm{\φ}}_{2n}\\ ...& ...& ...& ...\\ {\mathrm{\φ}}_{n1}& {\mathrm{\φ}}_{n2}& ...& {\mathrm{\φ}}_{\mathrm{nn}}\end{array}\right]={\mathrm{\Φ}}^n$φ为矩阵Φⁿ中的各个元素；$\left[\begin{array}{c}{w}_1^1\\ w_1^2\\ ...\\ w_1^n\end{array}\right]=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Φ}}^{j-1}{W}_{n+1-j};$由表达式(11)知，状态矢量中的第i个分变量表达为：$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+{\mathrm{\φ}}_{i1}{x}_1^1+{\mathrm{\φ}}_{i2}{x}_1^2+...+{\mathrm{\φ}}_{\mathrm{in}}{x}_1^n+w_1^i---\left(12\right)$参照表达式(4)，表达式(12)表示成如下形式：$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+({\mathrm{\φ}}_{i1}{z}_1^{*1}+{\mathrm{\φ}}_{i2}{z}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{z}_1^{*n})+({\mathrm{\φ}}_{i1}{v}_1^{*1}+{\mathrm{\φ}}_{i2}{v}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{v}_1^{*n})+w_1^i---\left(13\right)$其中：$Z_1^{*}=\left[\begin{array}{c}{z}_1^{*1}\\ z_1^{*2}\\ ...\\ z_1^{*n}\end{array}\right]={\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_1\\ z_2\\ ...\\ z_n\end{array}\right];$$V_1^{*}=\left[\begin{array}{c}{v}_1^{*1}\\ v_1^{*2}\\ ...\\ v_1^{*n}\end{array}\right]={-\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{v}_1\\ {\mathrm{Hw}}_1+v_2\\ ...\\ {\mathrm{HA}}^{n-2}{w}_1+...+{\mathrm{Hw}}_1+v_n\end{array}\right];$表达式(13)中所示的矢量即为基于初始时刻状态变量X₁所引入的新测量矢量，类似地，基于不同时刻k的状态变量X_k，分别地引入不同时刻k时的新测量矢量$Z_k^{*}=\left[\begin{array}{c}{z}_k^{*1}\\ z_k^{*2}\\ ...\\ z_k^{*n}\end{array}\right]={\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_k\\ z_{k+1}\\ ...\\ z_{k+n-1}\end{array}\right]---\left(14\right)$令${\mathrm{\ξ}}_1^i={\mathrm{\φ}}_{i1}{z}_1^{*1}+{\mathrm{\φ}}_{i2}{z}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{z}_1^{*n}---\left(15\right)$$w_1^{*i}={\mathrm{\φ}}_{i1}{v}_1^{*1}+{\mathrm{\φ}}_{i2}{v}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{v}_1^{*n}+w_1^i---\left(16\right)$此时，表达式(13)表示为：$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+{\mathrm{\ξ}}_1^i+w_1^{*i}---\left(17\right)$其中，为基于初始时刻状态变量X₁所引入测量修正值；为基于初始时刻状态变量X₁所引入噪声修正值；基于经典卡尔曼滤波算法，参照表达式(5)，第i个分变量在k＝1,2,...时刻的最优估计具有如下形式：${\hat{x}}_{\mathrm{nk}+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i+{\mathrm{\ξ}}_k^i+k_k^i(z_{k+1}^{*}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i-{\mathrm{\ξ}}_k^i)---\left(18\right)$相应地，参照表达式(7)，增益系数为：$k_k^i=\frac{p_{k/k-1}^i}{p_{k/k-1}^i+r_k^i}---\left(19\right)$测量噪声方差为：$r_k^i=M\left[{\left(v_k^i\right)}^2\right]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{\left(v_j^i\right)}^2---\left(20\right)$$v_j^i=z_{j+1}^{*i}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(j-1)+1}^i---\left(21\right)$其中，为测量噪声；参照表达式(8)，先验估计误差的方差为：$p_{k/k-1}^i={\mathrm{\φ}}_{\mathrm{ii}}^2{p}_{k-1}^i+M\left[{\left(v_k^i\right)}^2\right]\×{\left(k_{k-1}^i\right)}^2---\left(22\right)$参照表达式(9)，后验估计误差的方差为：$p_k^i=(1-k_k^i)p_{k/k-1}^i---\left(23\right)$步骤4.应用标量估计算法，将组合导航系统误差模型的状态变量表达成标量估计形式对于组合导航系统误差模型来说，其状态变量包括：航行体运动速度误差δV、陀螺仪平台失准角陀螺仪漂移ε，基于标量估计算法，参照表达式(14)，组合导航系统误差测量值表示为：$Z_k^{*}=S^{-1}\left[\begin{array}{c}{z}_k\\ z_{k+1}\\ z_{k+2}\end{array}\right]---\left(24\right)$其中：$S=\left[\begin{array}{ccc}1& 0& 0\\ 1& -\mathrm{gT}& 0\\ 1-\frac{{\mathrm{gT}}^2}{R}& -2\mathrm{gT}& -{\mathrm{gT}}^2\end{array}\right]$为系统能观性矩阵；具体地，组合导航系统误差模型的状态变量写成如下标量形式：$\begin{array}{c}z\left(\mathrm{\δV}\right)=z_k\\ z\left(\mathrm{\Φ}\right)=\frac{1}{\mathrm{gT}}{z}_k-\frac{1}{\mathrm{gT}}{z}_{k+1}\\ z\left(\mathrm{\ϵ}\right)=-\frac{1}{{\mathrm{gT}}^2}{z}_k-\frac{1}{R}{z}_k+\frac{2}{{\mathrm{gT}}^2}{z}_{k+1}-\frac{1}{{\mathrm{gT}}^2}{z}_{k+2}\end{array}---\left(25\right)$参照表达式(25)，测量陀螺平台失准角Ф时，测量噪声的折算方差为：$r^{*}=\frac{2}{g^2{T}^2}r---\left(6\right)$其中：r为测量速度误差δV时噪声的方差；在实际工程应用中，选取组合导航系统东向和北向速度误差作为系统测量值，应用标量估计算法，参照表达式(2)和(25)，得到如下的组合导航系统误差全维度标量估计表达式：其中：z_x,k为组合导航系统东向速度误差测量值；z_y,k为组合导航系统北向速度误差测量值；应用测量得到的航行体东向和北向速度误差，参照表达式(27)，计算和估计出组合导航系统各个方向的速度误差、失准角和陀螺漂移，以此实现组合导航系统误差补偿和校正。