[发明专利]一种带自适应因子的交互式多模型UKF的卫星导航方法

摘要

本发明涉及一种带自适应因子的交互式多模型UKF的卫星导航方法，属于导航控制领域。本发明方法首先利用残差序列的自协方差矩阵作为校正量，设计平滑滤波器将此校正量与陈旧自适应因子融合，得到一个新的自适应因子，并利用此自适应因子实时调整过程噪声协方差矩阵，有效地降低了系统中噪声统计特性未知造成的定位误差。然后，采用交互式多模型算法(IMM)设置模型集M，并根据测量残差实时调整模型概率实现模型间的软切换，降低了系统模型不精确造成的定位误差。本发明方法有效地改善了卫星导航系统中复杂机动载体在模型和噪声统计特性未知的条件下的定位精度。

主权项

一种带自适应因子的交互式多模型UKF的卫星导航方法，其特征在于：具体包括如下步骤：步骤1：建立系统模型；步骤1.1：建立基于全球定位系统GPS的地面载体的状态方程；为全面描述载体的运动状态和钟差漂移频率，用符号X表示全球定位系统GPS状态方程的状态变量，$X={[x,\stackrel{\·}{x},\stackrel{\·\·}{x,}y,\stackrel{\·}{y},\stackrel{\·\·}{y},z,\stackrel{\·}{z},\stackrel{\·\·}{z,}{\mathrm{\δt}}_u,\mathrm{\δ}{\stackrel{\·}{t}}_u]}^T;$其中，x，分别代表WGS‑84坐标系下X_T轴上的位置、速度和加速度；y，分别代表WGS‑84坐标系下Y_T轴上的位置、速度和加速度；z，分别代表WGS‑84坐标系下Z_T轴上的位置、速度和加速度；δt_u和分别代表载体接收机钟差和钟差漂移频率；建立模型集M，模型集M包含描述载体运动状态的当前统计模型CS、常加速度模型CA和常速转弯模型CT，即M＝[CS,CA,CT]；接收机钟差变化率用一阶马尔科夫模型描述，当前统计模型CS的状态方程如公式(1)所示；常加速度模型CA的状态方程如公式(2)所示；常速转弯模型CT的状态方程如公式(3)所示；$X_k^1={\mathrm{\Φ}}_1{X}_{k-1}^1+{\mathrm{\Ψ\ψ}}_{k-1}^1{w}_{k-1}^1---\left(1\right)$$X_k^2={\mathrm{\Φ}}_2{X}_{k-1}^2+{\mathrm{\Ψ}}_{k-1}^2{w}_{k-1}^2---\left(2\right)$$X_k^3={\mathrm{\Φ}}_3{X}_{k-1}^3+{\mathrm{\Ψ}}_{k-1}^3{w}_{k-1}^3---\left(3\right)$其中，分别表示系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻的状态，其中i＝1,2,3，即：表示系统在当前统计模型CS下k时刻的状态；表示系统在常加速度模型CA下k时刻的状态；表示系统在常速转弯模型CT下k时刻的状态；k为正整数；Φ_i为当前统计模型CS、常加速度模型CA和常速转弯模型CT下的状态转移矩阵，Φ_i＝diag(Φ_i,x,Φ_i,y,Φ_i,z,Φ_t,u)，Φ_i，x表示WGS‑84坐标系下X_T轴上的状态转移矩阵；Φ_i,y表示WGS‑84坐标系下Y_T轴上的状态转移矩阵；Φ_i,z表示WGS‑84坐标系下Z_T轴上的状态转移矩阵；Φ_t,u表示描述地面载体接收机钟差和钟差漂移的状态转移矩阵；是系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声协方差矩阵自适应因子；是系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下的过程噪声矩阵；用符号分别表示系统在当前统计模型CS、常加速度模型CA和常速转弯模型CT下(k‑1)时刻的过程噪声协方差矩阵，E[·]表示期望值运算，表示WGS‑84坐标系下描述X_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS‑84坐标系下描述Y_T轴上载体运动状态的过程噪声协方差矩阵；表示WGS‑84坐标系下描述Z_T轴上载体运动状态的过程噪声协方差矩阵；表示描述地面载体接收机钟差和钟差漂移的过程噪声协方差矩阵；步骤1.2：建立基于全球定位系统GPS的地面载体的观测方程；采用伪距作为观测量建立观测方程；在k时刻，接收机观测到的GPS卫星数量用N表示，则建立此时刻CS模型，CA模型和CT模型的伪距观测方程如公式(4)所示；${\mathrm{\ρ}}_n^k=\sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\δt}}_u^k+{\mathrm{\ϵ}}_n^k---\left(4\right)$其中，表示在k时刻观测到的第n颗GPS卫星的伪距，n＝1,2…N，N表示k时刻观测到的GPS卫星的总数；表示k时刻观测到的第n颗GPS卫星在WGS‑84坐标系下X_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS‑84坐标系下Y_T轴上的位置；表示k时刻观测到的第n颗GPS卫星在WGS‑84坐标系下Z_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS‑84坐标系下X_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS‑84坐标系下Y_T轴上的位置；分别表示在当前统计模型CS、常加速度模型CA和常速转弯模型CT下k时刻地面载体在WGS‑84坐标系下Z_T轴上的位置；表示k时刻地面载体接收机钟差；表示k时刻观测到的第n颗GPS卫星的观测误差；为下文描述算法方便，建立CS模型，CA模型和CT模型的观测方程为公式(5)；$y_k^i=h\left(X_k^i\right)+{\mathrm{\ϵ}}_k---\left(5\right)$其中，，为系统的观测量，$h\left(X_k^i\right)=\left[\begin{array}{c}\sqrt{{(x_k^i-x_1^k)}^2+{(y_k^i-y_1^k)}^2+{(z_k^i-z_1^k)}^2}+{\mathrm{\δt}}_u^k\\ \sqrt{{(x_k^i-x_2^k)}^2+{(y_k^i-y_2^k)}^2+{(z_k^i-z_2^k)}^2}+{\mathrm{\δt}}_u^k\\ \·\·\·\\ \sqrt{{(x_k^i-x_n^k)}^2+{(y_k^i-y_n^k)}^2+{(z_k^i-z_n^k)}^2}+{\mathrm{\δt}}_u^k\end{array}\right],$观测方程中的非线性函数，为系统的观测误差；用R_k表示公式(5)的观测误差的协方差矩阵，步骤2：设置CS模型，CA模型和CT模型0时刻的滤波初始值，同时设置当前定位任务的结束时间；在步骤1操作的基础上，分别设置CS模型、CA模型和CT模型的状态的估计初值，用符号和表示；然后设置CS模型、CA模型和CT模型的估计误差协方差矩阵初值，用符号和表示；并设置系统运行的结束时间；步骤3：得到CS模型，CA模型和CT模型在k时刻的滤波初始值；使用步骤2或步骤4得到的CS模型、CA模型和CT模型第(k‑1)时刻的状态的估计初值和以及CS模型、CA模型和CT模型的估计误差协方差矩阵初值和通过公式(6)和公式(7)进行交互混合，分别得到CS模型，CA模型和CT模型中k时刻的状态的估计初值，分别用和表示，以及CS模型，CA模型和CT模型中k时刻的估计误差协方差矩阵初值，分别用和表示；${\hat{X}}_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{\hat{X}}_{i=1}^i{u}_{k-1|k-1}^{i|j}---\left(6\right)$其中，分别表示CS模型，CA模型和CT模型在k时刻滤波的状态初值；j＝1,2,3，分别代表CS模型、CA模型和CT模型；i＝1,2,3，分别表示CS模型，CA模型和CT模型在k‑1时刻状态的最优估计值；表示k‑1时刻模型i和模型j的混合概率，π_ij为模型i到模型j的模型转移概率，其值由人为设定；为模型i在k时刻的概率，的初值由人为设定，并在以后各个时刻由步骤6更新；代表模型j的归一化常数，$P_{k-1}^{0j}=\underset{i=1}{\overset{3}{\mathrm{\Σ}}}{u}_{k-1|k-1}^{i|j}[P_{k-1}^i+({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j}){({\hat{X}}_{k-1}^i-{\hat{X}}_{k-1}^{0j})}^T]---\left(7\right)$其中，i＝1,2,3，分别表示CS模型、CA模型和CT模型在k‑1时刻滤波误差的方差矩阵；分别表示CS模型、CA模型和CT模型在k时刻滤波误差方差矩阵初值；步骤4：对CS模型，CA模型和CT模型进行自适应无迹卡尔曼滤波AUKF，得到各模型在k时刻的滤波值；步骤5：计算模型j在k时刻的过程噪声协方差矩阵阵的自适应矩阵步骤6：在步骤5操作基础上，通过公式(27)至公式(29)对模型概率更新；$u_k^j=\frac{{\mathrm{\Λ}}_k^j{\stackrel{\‾}{c}}_j}{c}---\left(8\right)$其中，为模型j在k时刻的似然函数，可通过公式(28)计算得到；c为归一化常数，可通过公式(29)计算得到；${\mathrm{\Λ}}_k^j\frac{1}{\sqrt{2\mathrm{\π}\left|P_{\mathrm{yy}}^j\right|}}\exp [-0.5{\left({\mathrm{\υ}}_k^j\right)}^T{\left(P_{\mathrm{yy}}^j\right)}^{-1}{\mathrm{\υ}}_k^j]---\left(9\right)$其中exp[·]表示以自然常数e为底的指数函数；$c=\underset{j=1}{\overset{3}{\mathrm{\Σ}}}{\mathrm{\Λ}}_k^j{\stackrel{\‾}{c}}_j---\left(10\right)$为模型j的归一化常数，π_ij为模型i到模型j的转移概率，其值由认为设定，为k‑1时刻的模型概率，在0时刻由人为设定，并在以后每一时刻通过公式(27)更新；步骤7：在步骤6操作基础上，进行信息融合,得到k时刻滤波器的输出；${\hat{X}}_k=\underset{j=1}{\overset{3}{\mathrm{\Σ}}}{u}_k^j{\hat{X}}_k^j---\left(11\right)$$P_k=\underset{j=1}{\overset{3}{\mathrm{\Σ}}}{u}_k^j[P_k^j+({\hat{X}}_k^j-{\hat{X}}_k){({\hat{X}}_k^j-{\hat{X}}_k)}^T]---\left(12\right)$其中为采用交互式多模型自适应UKF算法得到的系统最优估计值，P_k为采用IMM‑AUKF算法得到的状态最优估计值的误差协方差矩阵；步骤8：判断当前时刻是否到达步骤2设置的当前定位任务的结束时间，如果到达，则结束操作；否则，在步骤6操作基础上，将步骤4得到的模型j中k时刻的状态估计值模型j中k时刻的估计误差方差矩阵步骤5得到的模型j在k时刻过程噪声协方差矩阵阵的自适应矩阵以及步骤6得到的模型j的概率作为输入值，重复执行步骤2至步骤7的操作，计算下一时刻滤波器的输出值。