[发明专利]应用于多模卫星导航系统中的快速卡尔曼滤波定位方法

摘要

本发明涉及一种应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，属于导航控制领域。该方法将多模卫星导航的系统模型采用多胞型微分包含系统(Ploytopic Linear Differential Inclusion，PLDIs)模型来描述，从而将非线性的多模卫星导航系统转换为线性定常的误差系统，降低了多模卫星导航系统中定位算法在线编程的复杂性。

主权项

应用于多模卫星导航系统中的快速卡尔曼滤波定位方法，其特征在于：其操作步骤为：步骤1：针对全球定位系统GPS和北斗2号BD‑2卫星导航系统，建立全球定位系统/北斗2号GPS/BD‑2组合的多模卫星导航系统的数学模型，其中选取的坐标系为世界大地坐标系WGS‑84，坐标系的三轴分别用X_T，Y_T和Z_T表示；具体过程为：步骤1.1：建立GPS/BD‑2组合的多模卫星导航系统的状态方程；用符号X表示GPS/BD‑2组合的多模卫星导航系统状态方程的状态变量，$X={[x,\stackrel{\·}{x},\stackrel{\·\·}{x},y,\stackrel{\·}{y},\stackrel{\·\·}{y},z,\stackrel{\·}{z},\stackrel{\·\·}{z},{\mathrm{\δt}}_u^{\mathrm{GPS}},{\stackrel{\·}{\mathrm{\δ}}t}_u^{\mathrm{GPS}},{\mathrm{\δt}}_u^{\mathrm{BD}},{\stackrel{\·}{\mathrm{\δ}}t}_u^{\mathrm{BD}}]}^T,$其中x,分别表示载体WGS‑84坐标系下X_T轴方向上的位置、速度和加速度；y，分别表示载体在WGS‑84坐标系下Y_T轴方向上的位置、速度和加速度；z,分别表示载体在Z_T方向上的位置、速度、和加速度；分别表示GPS接收机钟差和钟差漂移频率；分别表示BD‑2接收机钟差和钟差漂移频率；建立离散状态方程如公式(1)所示；X_k+1＝ΦX_k+U_k+w_k (1)其中，X_k为k时刻GPS/BD‑2组合卫星导航系统的状态；Φ为状态转移矩阵，其具体形式如公式(2)所示；U_k为GPS/BD‑2组合卫星导航系统在k时刻的输入量，如公式(3)所示，U_k在每一采样周期为定值；w_k为系统在k时刻的过程噪声，其协方差矩阵用Q_k表示，计算公式如式(4)所示；Φ＝diag(Φ_x,Φ_y,Φ_z,Φ_t,GPS,Φ_t,BD) (2)其中，${\mathrm{\Φ}}_x={\mathrm{\Φ}}_y={\mathrm{\Φ}}_z=\left[\begin{array}{ccc}1& T& \frac{1}{{\mathrm{\λ}}^2}(-1+\mathrm{\λT}+e^{-\mathrm{\λT}})\\ 0& 1& \frac{1}{\mathrm{\λ}}(1-e^{-\mathrm{\λT}})\\ 0& 0& e^{-\mathrm{\λT}}\end{array}\right],{\mathrm{\Φ}}_{t,\mathrm{GPS}}={\mathrm{\Φ}}_{t,\mathrm{BD}}\left[\begin{array}{cc}1& (1-e^{-{\mathrm{\λ}}_{t}T})/{\mathrm{\λ}}_t\\ 0& e^{-{\mathrm{\λ}}_{t}T}\end{array}\right];$Φ_x为在WGS‑84坐标系中描述载体在X_T轴上的位置、速度和加速度的状态转移矩阵，Φ_y为在WGS‑84坐标系中描述载体在Y_T轴上的位置、速度和加速度的状态转移矩阵，Φ_z为在WGS‑84坐标系中描述载体在Z_T轴上的位置、速度和加速度的状态转移矩阵；Φ_t,GPS为描述GPS接收机钟差和钟差漂移频率的状态转移矩阵，Φ_t,BD为描述BD‑2接收机钟差和钟差漂移频率的状态转移矩阵；λ为一阶时间相关常数的倒数，λ∈(0,1)；λ_t为一阶马尔科夫时间常数倒数，λ_t∈(0,1)；λ和λ_t的值由人为设定，T为系统采样周期；$U_k={\left[\begin{array}{ccccc}{u}_x{\stackrel{\‾}{a}}_{x,k}& u_y{\stackrel{\‾}{a}}_{y,k}& u_z{\stackrel{\‾}{a}}_{z,k}& 0_{1\×2}& 0_{1\×2}\end{array}\right]}^T---\left(3\right)$其中，$u_x=u_y=u_z=\left[\begin{array}{c}\frac{1}{\mathrm{\λ}}(-T+\frac{{\mathrm{\λT}}^2}{2}+\frac{1-e^{-\mathrm{\λT}}}{\mathrm{\λ}})\\ T-\frac{1-e^{-\mathrm{\λT}}}{\mathrm{\λ}}\\ 1-e^{-\mathrm{\λT}}\end{array}\right],$为载体在X_T轴上的加速度均值，为载体在Y_T轴上的加速度均值，为载体在Z_T轴上的加速度均值，和在每一采样周期内为常数；0_1×2表示1行2列的零值矩阵；$Q_k=E\left[w_k{w}_k^T\right]=\mathrm{diag}(Q_{x,k},Q_{y,k},Q_{z.k},Q_{t,\mathrm{gps},k},Q_{t,\mathrm{bd},k})---\left(4\right)$其中E(·)表示求期望；Q_x,k为WGS‑84坐标系下k时刻载体在X_T轴上运动状态的过程噪声协方差矩阵；Q_y,k为WGS‑84坐标系下k时刻载体在Y_T轴上运动状态的过程噪声协方差矩阵；Q_z,k为WGS‑84坐标系下k时刻载体在Z_T轴上运动状态的过程噪声协方差矩阵；Q_t,gps,k为k时刻GPS接收机钟差和钟差漂移频率的过程噪声协方差矩阵，Q_t,bd为k时刻BD‑2接收机钟差和钟差漂移频率的过程噪声协方差矩阵；步骤1.2，建立GPS/BD‑2组合卫星导航系统的观测方程；用符号M表示在k时刻接收机观测到的GPS卫星的个数；用符号N表示在k时刻接收机观测到的BD‑2卫星的个数，则建立k时刻的伪距观测方程如公式(5)和公式(6)所示；${\mathrm{\ρ}}_{m,g}^k=\sqrt{{(x_k-x_{m,g}^k)}^2+{(y_k-y_{m,g}^k)}^2+{(z_k-z_{m,g}^k)}^2}+{\mathrm{\δt}}_{u,k}^{\mathrm{GPS}}+{\mathrm{\ϵ}}_{m,g}^k---\left(5\right)$${\mathrm{\ρ}}_{n,b}^k=\sqrt{{(x_k-x_{n,b}^k)}^2+{(y_k-y_{n,b}^k)}^2+{(z_k-z_{n,b}^k)}^2}+{\mathrm{\δt}}_{u,k}^{\mathrm{BD}}+{\mathrm{\ϵ}}_{n,b}^k---\left(6\right)$其中，为k时刻观测到的第m颗GPS卫星的伪距，m＝1,2...M；为k时刻观测到的第n颗BD‑2卫星的伪距，n＝1,2...N；表示k时刻观测到的第m颗GPS卫星在WGS‑84坐标系下X_T，Y_T和Z_T轴上的位置坐标；表示k时刻观测到的第n颗BD‑2卫星在WGS‑84坐标系下X_T，Y_T和Z_T轴上的位置坐标；[x_k,y_k,z_k]为k时刻待求接收机在WGS‑84坐标系下X_T，Y_T和Z_T轴上的位置坐标；为k时刻GPS接收机钟差，为k时刻BD‑2接收机钟差；为k时刻第m颗GPS卫星的观测噪声，为k时刻第n颗BD‑2卫星的观测噪声；为下文描述算法方便，将公式(5)和公式(6)所示的观测方程简写为公式(7)；y_k＝h(X_k)+ε_k (7)其中y_k为系统的观测量，$y_k={\left[\begin{array}{cccccccc}{\mathrm{\ρ}}_{1,g}^k& {\mathrm{\ρ}}_{2,g}^k& ...& {\mathrm{\ρ}}_{M,g}^k& {\mathrm{\ρ}}_{1,b}^k& {\mathrm{\ρ}}_{2,b}^k& ...& {\mathrm{\ρ}}_{N,b}^k\end{array}\right]}^T;$h(·)为观测方程中的非线性函数；ε_k为系统的观测误差，ε_k的协方差矩阵用符号R_k表示，公式(1)和公式(7)构成了GPS/BD‑2组合卫星导航系统的系统模型，写成公式(8)的形式：$\left\{\begin{array}{c}{X}_{k+1}={\mathrm{\ΦX}}_k+U_k+w_k\\ y_k=h\left(X_k\right)+{\mathrm{\ϵ}}_k\end{array}\right.---\left(8\right)$步骤2：将GPS/BD‑2组合卫星导航的系统方程转换为GPS/BD‑2组合卫星导航系统的误差方程；具体为：用符号表示系统在k‑1时刻的滤波值，多模卫星导航系统在k时刻的误差方程为：$\left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ \mathrm{\Δ}{y}_k=H\left({\hat{X}}_{k-1}\right){\mathrm{\ΔX}}_k+{\mathrm{\ϵ}}_k\end{array}\right.---\left(9\right)$其中，ΔX_k为GPS/BD‑2组合卫星导航系统在k时刻的状态偏差，Δy_k为GPS/BD‑2组合卫星导航系统在k时刻的测量偏差${\mathrm{\Δy}}_k=y_k-h\left({\hat{X}}_k\right);$表示系统在k‑1时刻的雅克比矩阵，即$H\left({\hat{X}}_{k-1}\right)=\frac{\∂h}{\∂X}{|}_{X={\hat{X}}_{k-1}},$表示求函数的偏导；步骤3：将GPS/BD‑2组合卫星导航系统的误差方程(8)通过张量积模型转换的方法转换为多胞型线性微分包含系统模型；具体操作为：步骤3.1：确定GPS/BD‑2组合卫星导航系统状态X中X_TY_TZ_T三坐标轴上位置变量(x,y,z)的值域集合；值域集合用符号Ω_p表示，即Ω_p＝[c_x×d_x]×[c_y×d_y]×[c_z×d_z]，其中[c_x,d_x]为变量x的取值范围，[c_y,d_y]为变量y的取值范围，[c_z,d_z]为变量z的取值范围；Ω_p包含三个维度，分别用符号p_xp_yp_z表示；步骤3.2：将值域集合Ω_p的每一个维度p_n(n＝x,y,z)划分为L个均匀分布的网格，L∈(0,100]，每个网格点用符号p_n,l表示，l＝1,2...L，c_n＜p_n,1＜p_n,2＜...＜p_n,L＜d_n；步骤3.3：计算每个网格点的雅克比矩阵H(p_nl),n＝x,y,z,l＝1,2...L，并将得到的矩阵存入张量空间中；步骤3.4：对张量空间内的每模矩阵执行高阶奇异值分解，在每模矩阵奇异值分解时舍弃零和部分非零每模矩阵奇异值，并将每模矩阵奇异值分解得到的酉矩阵U_j进行标准化转换，，j＝1,2,3，转换后的矩阵用符号表示；经过3次高阶奇异值分解，将张量空间转换为近似张量空间其中为的近似张量空间，为经过高阶奇异值分解后产生的子张量空间；步骤3.5：从子张量空间中提取多胞型线性微分包含系统模型的顶点矩阵H_i，i＝1,2...r，H_i为常值，r为多胞型线性微分包含系统模型总的顶点个数；步骤4：利用步骤3.5得到的顶点矩阵H_i将GPS/BD‑2组合卫星导航系统的误差方程转换为GPS/BD‑2组合卫星导航多胞型线性微分包含系统模型，即转换为r个顶点系统的凸组合形式，如公式(10)所示；$\begin{array}{c}\left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ {\mathrm{\ΔZ}}_k=H_1{\mathrm{\ΔX}}_{k-1}+{\mathrm{\ϵ}}_k\end{array}\right.\\ \left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ {\mathrm{\ΔZ}}_k=H_2{\mathrm{\ΔX}}_{k-1}+{\mathrm{\ϵ}}_k\end{array}\right.\\ .\\ .\\ .\\ \left\{\begin{array}{c}{\mathrm{\ΔX}}_k={\mathrm{\Φ\ΔX}}_{k-1}+w_{k-1}\\ {\mathrm{\ΔZ}}_k=H_r{\mathrm{\ΔX}}_{k-1}+{\mathrm{\ϵ}}_k\end{array}\right.\end{array}---\left(10\right)$步骤5：设置GPS/BD‑2组合卫星导航多胞型线性微分包含系统的在0时刻的滤波初值，其中在多胞型线性微分包含系统中状态在0时刻的初值用符号表示，估计误差协方差矩阵的初值用符号P₀表示，同时设置系统定位结束时间time；步骤6：状态一步预测；根据步骤5或步骤11得到的k‑1时刻滤波值用公式(11)进行状态一步预测得到系统在k时刻的一步预测值其中当k＝1时，通过步骤5获得，当k≥2时，通过步骤11获得；${\hat{X}}_{k,k-1}={\mathrm{\Φ}\hat{X}}_{k-1}+U_{k-1}---\left(11\right)$步骤7：对GPS/BD‑2组合卫星导航多胞型线性微分包含系统模型的每个顶点系统进行卡尔曼滤波，获得每个顶点系统的误差估计值和每个顶点的估计误差协方差矩阵其中i＝1,2...r；具体操作步骤为：步骤7.1：估计误差协方差矩阵一步预测；根据步骤5或步骤11得到的k‑1时刻的估计误差协方差矩阵P_k‑1通过公式(12)进行一步预测，获得系统在k时刻估计误差协方差矩阵的一步预测值P_k,k‑1；其中当k＝1时，P_k‑1通过步骤5获得，当k≥2时，P_k‑1通过步骤11获得；P_k,k‑1＝ΦP_k‑1Φ^T+Q_k‑1 (12)步骤7.2：通过公式(13)计算每个顶点系统在k时刻的滤波增益其中i＝1,2...r；$K_k^i=P_{k-1}{H}_i^T{[H_i{P}_{k,k-1}{H}_i^T+R_k]}^{-1}---\left(13\right)$步骤7.3：通过公式(14)计算系统在k时刻的测量偏差Δy_k；${\mathrm{\Δy}}_k=y_k-{\hat{y}}_k=y_k-h\left({\hat{X}}_{k,k-1}\right)---\left(14\right)$步骤7.4：通过公式(15)计算每个顶点在k时刻估计误差的校正量${\mathrm{\ΔX}}_k^i=K_k^i{\mathrm{\Δy}}_k---\left(15\right)$步骤7.5：通过公式(16)校正每个顶点在k时刻的估计误差协方差矩阵$P_k^i=(I-K_k^i{H}_i)P_{k,k-1}{(I-K_k^i{H}_i)}^T+K_k^i{R}_k{\left(K_k^i\right)}^T---\left(16\right)$步骤8：通过公式(17)计算顶点系统的融合系数α_i；${\mathrm{\α}}_i=\frac{{\left({\mathrm{\σ}}_i^{\left(3\right)}\right)}^{-2}}{\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\left({\mathrm{\σ}}_i^{\left(3\right)}\right)}^{-2}}---\left(17\right)$其中张量空间的3模矩阵的高阶奇异值分解的第i个奇异值，i＝1,2...r；步骤9：在公式(17)的基础上通过公式(18)将每个顶点的估计误差进行融合，得到GPS/BD‑2组合卫星导航多胞型线性微分包含系统模型误差的最优估计值$\mathrm{\Δ}{\hat{X}}_k=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\α}}_i\mathrm{\Δ}{\hat{X}}_k^i---\left(18\right)$步骤10：在公式(17)的基础上通过公式(19)对每个顶点系统的估计误差协方差矩阵融合，获得GPS/BD‑2组合卫星导航系统在k时刻最优的估计误差协方差矩阵$P_k^{-1}=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\α}}_i{\left(P_k^i\right)}^{-1}---\left(19\right)$其中，为P_k的逆矩阵；步骤11：在公式(18)的基础上通过公式(20)求取GPS/BD‑2组合卫星导航系统在k时刻状态的最优估计值${\hat{X}}_k={\hat{X}}_{k/k-1}+{\mathrm{\Δ}\hat{X}}_k---\left(20\right)$步骤12：判断k是否到达设定的系统结束时间time，若未到达设定时间，则返回步骤(6)，重复步骤(6)到步骤(11)的操作；若到达设定时间，则结束操作；经过上述步骤的操作，即可实现对运动载体的实时定位。