自适应闭环H∞滤波器对北斗/捷联导航系统的修正方法

摘要

本发明公开了一种基于自适应闭环H_∞滤波器的对北斗双星/捷联惯导组合导航系统进行修正的方法，包括下列步骤：利用SINS/BDNS组合导航系统中的惯性测量组件测量载体的角速度信号和线加速度信号；北斗接收机利用北斗卫星信息，获得载体的速度和位置；导航计算机根据获得的信号进行捷联惯性算；构建自适应闭环H_∞滤波器对组合导航系统进行修正。本发明的方法具有如下优点：(1)组合导航系统滤波参数设置简单，可变的参数只有滤波器阀值γ和自适应调整系数σ；(2)该方法对系统噪声不敏感，滤波系统鲁棒性强；(3)经实验证明，该方法滤波精度高，修正后的组合导航结果往往优于同等条件下常规Kalman滤波的组合导航结果。

主权项

1、一种自适应闭环H∞滤波器对北斗/捷联导航系统的修正方法，包括下列步骤：(1)利用SINS/BDNS组合导航系统中的惯性测量组件测量载体的角速度信号和线加速度信号，再输出这两种信号传输给导航计算机；(2)北斗接收机利用北斗卫星信息，获得载体的速度和位置，再输出到导航计算机；(3)导航计算机根据步骤(1)所获得的信号进行捷联惯性算：首先利用角速度信号获得姿态矩阵，并从中提取出姿态角和方位角；然后利用线加速度信号获得载体在地理坐标系中的速度和位置；最后输出SINS解算下的载体姿态、方位、速度和位置，其特征在于，还包括下列步骤：(4)建立组合滤波模型：以捷联惯导三个平台误差角、三个速度误差、三个位置误差这9个最基本的导航误差量构成状态向量X，以步骤(3)中SINS和步骤(2)中BDNS各自输出的位置、速度差值构成量测值Z，得到滤波动态方程组：式(1)中，A(t)、G(t)分别是系统状态矩阵和状态噪声阵，V_I(t)、V_p为系统噪声向量和量测噪声向量，H(t)为系统量测矩阵；系统状态向量表示为：X＝[φ_E φ_N φ_U δv_E δv_N δv_U δL δλ δh]^T系统噪声向量表示为：V_I＝[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T量测向量表示为：Z＝[L_B-L_I λ_B-λ_I h_B-h_I v_EB-v_EI v_NB-v_NI v_UB-v_UI]^T量测矩阵表示为：$H=\left[\begin{array}{ccc}{0}_{3\times 3}& 0_{3\times 3}& \mathrm{diag}\left[\begin{array}{ccc}{R}_M& R_N\cos L& 1\end{array}\right]\\ 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\end{array}\right]$量测噪声表示为：$V_p={\left[\begin{array}{cccccc}{N}_N& N_E& N_h& {ϵ}_{v_E}& {ϵ}_{v_N}& {ϵ}_{v_U}\end{array}\right]}^T$其中φ_E、φ_N、φ_U分别为捷联系统数学平台误差角；δv_E、δv_N、δv_U为捷联惯导的速度误差；δL、δλ、δh为捷联惯导的位置误差；下标E、N、U表示东北天坐标系；ω_gx、ω_gy、ω_gz分别表示陀螺噪声；ω_ax、ω_ay、ω_az表示加速度计噪声；R_M，R_N分别为所处椭球子午圈和卯酉圈上的曲率半径；L_B，λ_B，h_B为北斗接收机输出的经纬高；L_I，λ_I，h_I为惯导输出的经纬高；v_EB，v_NB，v_UB为北斗接收机输出的速度；v_EI，v_NI，v_UI为惯导输出的速度；N_E、N_N、N_h为北斗接收机输出的位置噪声；和为北斗接收机的速度噪声；(5)构建常规H_∞滤波器：将(1)式离散化后，如下式所示：$\left.\begin{array}{c}{\hat{x}}_{k,k-1}={\Phi }_{k,k-1}{\hat{x}}_{k-1,k-1}\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\gamma }^{-2}I\\ {\hat{x}}_{k,k}={\hat{x}}_{k,k-1}+K_k[y_k-C_k{\hat{x}}_{k,k-1}]\\ K_k=[{\Gamma }_k{D}_k^T+{\Phi }_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\Phi }_{k,k-1}-K_k{C}_k)P_{k,k-1}^{-1}{({\Phi }_{k,k-1}-K_k{C}_k)}^{-1}+({\Gamma }_k-K_k{D}_k){({\Gamma }_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(2\right)$其中$\left.\begin{array}{c}{x}_{k+1}={\Phi }_{k+1,k}{x}_k+{\Gamma }_k{w}_k\\ y_k=C_k{x}_k+D_k{w}_k\end{array}\right\}$${\Phi }_{k+1,k}=\underset{i=0}{\overset{n}{\Sigma }}\frac{A_k^n}{n!}{(t_k-t_{k-1})}^n,$${\Gamma }_k=T\underset{i=0}{\overset{n}{\Sigma }}\frac{A_k^{n-1}}{n!}{(t_k-t_{k-1})}^{n-1}{B}_k$B_k＝[G_k0_9×4]，C_k＝H_k，D_k＝[0_9×4I]，$w_k=\left[\begin{array}{c}{V}_{\mathrm{Ik}}\\ V_p\end{array}\right]$其中，γ为H_∞滤波器的调节阀值，取大于1的数；(6)改进步骤(5)所得的常规H_∞滤波器，得到自适应闭环H_∞滤波器：在式(2)中分别引入闭环修正和遗忘因子λ，构成自适应闭环H_∞滤波器，如下所示：$\left.\begin{array}{c}{\hat{x}}_{k,k-1}\equiv 0\\ P_{k,k-1}=P_{k-1,k-1}^{-1}-{\gamma }^{-2}I\\ {\hat{x}}_{k,k}=K_k{y}_k\\ {\hat{x}}_{k,k}^{*}=\lambda {\hat{x}}_{k,k}=\lambda K_k{y}_k=\left(\lambda K_k\right)y_k=K_k^{*}{y}_k\\ K_k=[{\Gamma }_k{D}_k^T+{\Phi }_{k,k-1}{P}_{k,k-1}^{-1}{C}_k^T]{[I+C_k{P}_{k,k-1}^{-1}{C}_k^T]}^{-1}\\ P_{k,k}=({\Phi }_{k,k-1}-k_k{C}_k)P_{k,k-1}^{-1}{({\Phi }_{k,k-1}-K_k{C}_k)}^{-1}+({\Gamma }_k-K_k{D}_k){({\Gamma }_k-K_k{D}_k)}^{-1}\end{array}\right\}---\left(3\right)$其中，遗忘因子λ_k利用滤波系统相邻时刻量测值y_k-1、y_k的残差变化来计算，如下所示：${\lambda }_k=(1-\frac{{\left|\right|y_{k-1}\left|\right|}_2}{{\left|\right|y_k\left|\right|}_2})\sigma ---\left(4\right)$其中σ是调整系数；至本步骤结束，即可获得捷联惯导的误差值(7)对北斗双星/捷联惯导组合导航系统进行修正，获得修正后的组合导航结果，该步骤过程如下：利用反馈修正通道，将滤波估计出的误差去修正捷联惯导系统对应参数，得到最终的组合导航结果，其中，速度、位置的修正式(5)所示：$\left.\begin{array}{c}{v}_E^{\prime }=v_E-\delta v_E\\ v_N^{\prime }=v_N-{\mathrm{\delta v}}_N\\ v_U^{\prime }=v_U-{\mathrm{\delta v}}_U\\ L^{\prime }=L-\mathrm{\delta L}\\ {\lambda }^{\prime }=\lambda -\mathrm{\delta \lambda }\\ h^{\prime }=h-\mathrm{\delta h}\end{array}\right\}---\left(5\right)$上式中v′_E，v′_N，v′_U为修正后的速度，L′，λ′，h′为修正后的位置，v_E，v_N，v_U，L，λ，h对应的为捷联惯导系统的速度和位置；对姿态角的修正如下：首先根据式(6)用估计出的平台误差角φ_E，φ_N，φ_U对原有的姿态矩阵C_n^c进行修正更新，获得新的姿态矩阵C_n^b，然后再从C_n^b求解出横滚角γ、俯仰角θ和航向角ψ，如(7)式所示：$C_n^b=\left[\begin{array}{ccc}1& {\phi }_U& -{\phi }_N\\ -{\phi }_U& 1& {\phi }_E\\ {\phi }_N& -{\phi }_E& 1\end{array}\right]C_n^c---\left(6\right)$$\left.\begin{array}{c}\gamma ={\mathrm{tg}}^{-1}(-C_n^b\left(\mathrm{1,3}\right)/C_n^b\left(\mathrm{3,3}\right))\\ \theta ={\sin }^{-1}\left(C_n^b\left(\mathrm{2,3}\right)\right)\\ \psi ={\mathrm{tg}}^{-1}(C_n^b\left(\mathrm{2,1}\right)/C_n^b\left(\mathrm{2,2}\right))\end{array}\right\}---\left(7\right)$其中C_n^b(i，j)，i＝1，2，3，j＝1，2，3表示姿态矩阵的元素；最后，将修正后的组合导航结果，包括姿态、速度和位置输出到监控系统，既完成对北斗双星/捷联惯导组合导航系统的修正。