一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法

摘要

本发明提供一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，该方法利用IMU测量得到的数据与INS的初始数据进行捷联惯导实时解算，建立GPS/INS速度位置组合滤波系统方程，并根据GPS/INS双系统测量互差分序列，统计计算GPS测量噪声协方差估计值，进行自适应的卡尔曼滤波解算。本发明实现了GPS测量噪声的实时跟踪，以及滤波增益矩阵的自适应调节，提高了组合导航系统定位精度。

主权项

1.一种基于GPS/INS组合导航系统不同测量特性的自适应滤波方法，包括：步骤一：利用IMU惯性测量单元测量得到沿载体轴相对于惯性空间的角速率和加速度分量信息，与INS惯性导航系统的初始位置、速度、姿态及初始姿态矩阵信息，进行捷联惯导实时解算，得到INS位置、速度、姿态的输出值；步骤二：根据INS的平台误差角方程、速度误差方程、位置误差方程以及陀螺、加速度计误差模型，并根据INS与GPS的位置差、速度差，分别建立GPS/INS组合导航系统卡尔曼滤波器的状态方程与观测方程；步骤三：通过连续系统离散化，建立离散型卡尔曼滤波器的递推方程；其特征在于：还包括下述步骤：步骤四：根据GPS数据采集频率，计算GPS/INS双系统测量互差分序列；（1）INS测量差分序列为：$\mathrm{\Delta INS}\left(k\right)=\left\{\begin{array}{cc}{Z}_{\mathrm{INS}}\left(k\right)-Z_{\mathrm{INS}}(k-1)& k\le t_0\\ {\hat{Z}}_{\mathrm{INS}}\left(k\right)-Z_{\mathrm{GPS}/\mathrm{INS}}(k-1)& k>t_0\end{array}\right.$其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；t₀为惯性可信时间阈值，根据惯导系统测量精度而确定；Z_INS(k)为k时刻捷联惯导实时解算的位置、速度；Z_GPS/INS(k-1)为k-1时刻的组合导航系统输出的位置、速度；通过如下方式得到：以Z_GPS/INS(k-1)为k-1时刻的初值，采用k-1~k时间段内IMU测量数据，通过捷联惯导解算步骤，进行k-1~k时间段的惯性递推运算，得到k时刻的位置、速度，即为（2）GPS测量差分序列为：ΔGPS(k)＝Z_GPS(k)-Z_GPS(k-1)    k＝1,2,3,...其中，Z_GPS(k)为k时刻GPS输出的位置、速度；（3）GPS/INS双系统测量互差分序列为C(k)：C(k)＝ΔINS(k)-ΔGPS(k)k＝1，2,3,...；步骤五：设定时间阈值T，若组合导航系统滤波解算时刻k大于时间阈值T则直接进行步骤六；若组合导航系统滤波解算时刻k不大于时间阈值T，则对步骤四中得到的GPS/INS双系统测量互差分序列进行小样本统计的可信度判别：1）对GPS/INS双系统测量互差分序列，进行无重叠时间段的开窗统计，得到GPS/INS双系统测量的方差值序列D(k)：其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；N为无重叠统计窗的窗口宽度，C(i)为GPS/INS的双系统测量互差分序列在i时刻的值；2）对1）中得到的GPS/INS双系统测量的方差值序列D(k)，进行小样本统计的可信度判别，该判别条件为：$\max \left(\right|D\left(j\right)-\frac{1}{k-N+1}\underset{i=N}{\overset{k}{\Sigma }}D\left(i\right)\left|\right)<0.4*\frac{1}{k-N+1}\underset{i=N}{\overset{k}{\Sigma }}D\left(i\right)$k≥3N,j＝{k-2N,k-N,k}其中，max为求最大值的函数；若组合导航系统滤波解算时刻k小于或等于时间阈值T时已满足上述可信度判别条件，则执行步骤六，且进入步骤六时的GPS测量噪声协方差估计值为：${\hat{R}}_k=\frac{1}{2}D\left(k\right)$k≤T若组合导航系统滤波解算时刻k等于时间阈值T时仍不满足所述的可信度判别条件，则执行步骤六，且进入步骤六时GPS测量噪声协方差估计值为：${\hat{R}}_k=\frac{1}{k-1}\underset{i=1}{\overset{k}{\Sigma }}{[C\left(i\right)-\frac{1}{k}\underset{i=1}{\overset{k}{\Sigma }}C\left(i\right)]}^2/2$k＝T；若组合导航系统滤波解算时刻k小于时间阈值T时不满足上述可信度判别条件，则等待下一时刻的统计结果；步骤六：根据GPS/INS双系统测量互差分序列，采用连续滑动窗口法计算GPS测量噪声协方差估计值，将其代入步骤三所述的卡尔曼滤波器的递推方程，进行自适应的卡尔曼滤波解算；①将步骤四中的GPS/INS的双系统测量互差分序列C作为统计样本，计算该统计样本在连续滑动窗内的均值和方差，计算公式依次为：$\left\{\begin{array}{c}E\left(k\right)=\frac{1}{M}\underset{i=k-M+1}{\overset{k}{\Sigma }}C\left(i\right)\\ {\sigma }^2\left(k\right)=\frac{1}{M-1}\underset{i=k-M+1}{\overset{k}{\Sigma }}{[C\left(i\right)-E\left(k\right)]}^2\end{array}\right.,k\ge M$其中，k为组合导航系统滤波解算时刻，即GPS的数据采集时刻；M为连续滑动窗的窗口宽度，C(i)为GPS/INS的双系统测量互差分序列在i时刻的值，E(k)为k时刻的均值，σ(k)为k时刻的标准差，σ²(k)为k时刻的方差；②针对连续滑动窗内的GPS/INS的双系统测量互差分序列，进行野值点判别，该判别条件为：|C(i)-E(k)|＞α·σ(k)    k-M+1≤i≤k其中，α为可调剔除因子，若C(i)满足上述野值点判别条件，则将C(i)置为均值E(k)，得到新的统计样本，再执行步骤①，重新计算该统计样本在连续滑动窗内的均值和方差，最后执行步骤③；否则直接执行步骤③；③基于统计样本在连续滑动窗内的均值和方差，计算GPS测量噪声协方差的初步估值${\hat{R}}_{\mathrm{GPS}}\left(k\right)=\frac{1}{2}{\sigma }^2\left(k\right)$④根据该GPS测量噪声协方差的初步估值计算GPS测量噪声协方差估计值，计算公式为：$\left\{\begin{array}{c}{d}_k=(1-b)/(1-b^{k+1})\\ {\hat{R}}_k=(1-d_k){\hat{R}}_{k-1}+d_k{\hat{R}}_{\mathrm{GPS}}\left(k\right)\end{array}\right.,0<b<1$其中，b为遗忘因子，0＜b<1；d_k为k时刻的权系数，为k时刻的GPS测量噪声协方差估计值；为k-1时刻的GPS测量噪声协方差估计值；⑤将GPS测量噪声协方差估计值代入步骤三中卡尔曼滤波器的递推方程，实时调节滤波增益矩阵K_k，进行自适应卡尔曼滤波解算：$\left\{\begin{array}{c}{\hat{X}}_{k|k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(Z_k-H_k{\hat{X}}_{k|k-1})\\ K_k=P_{k|k-1}{H^T}_k{(H_k{P}_{k|k-1}{H^T}_k+{\hat{R}}_k)}^{-1}\\ P_{k|k-1}={\Phi }_{k,k-1}{P}_{k-1}{{\Phi }^T}_{k,k-1}+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T\\ P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{\hat{R}}_k{K^T}_k\end{array}\right.$其中，X_k为k时刻的状态向量，即被估计向量，为状态X_k-1的卡尔曼滤波估计值，为利用得到的对X_k的一步预测，Φ_k，k-1为k-1到k时刻的状态转移矩阵，Γ_k-1为系统噪声矩阵，K_k为k时刻的滤波增益矩阵，P_k|k-1为一步预测均方误差矩阵，R_k为观测噪声协方差矩阵，Q_k-1为k-1时刻系统噪声协方差矩阵，P_k为估计均方误差矩阵，H_k为k时刻的量测矩阵，Z_k为k时刻的观测向量，I为单位矩阵。