基于捷联惯性制导和多普勒计程仪的优化导航方法

摘要

本发明公开一种基于捷联惯性制导和多普勒计程仪的优化导航方法，其特征在于，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和三轴加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与处理单元相连接；该导航方法包括若干步骤，该方法可以整合多个子导航传感器信息，优势互补以得到更高精度和更可靠的导航信息。

主权项

1.一种基于捷联惯性制导和多普勒计程仪的优化导航方法，其特征在于，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和中央处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与中央处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和三轴加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与中央处理单元相连接；该导航方法包括以下步骤：(1)通过捷联惯性测量单元组件中的三轴陀螺测得三轴角速度信息和加速度计测得三轴加速度信息，捷联惯性测量单元处理模块接收捷联惯性测量单元输出的导航信息，通过导航积分计算获得载体位置、速度和姿态等导航信息；(2)通过多普勒计程仪的收发器中发射电振荡信号，送给换能器；(3)通过多普勒计程仪的四个换能器发射超声波和接收具有频移特性的反射回波；(4)利用收发器中接收系统将换能器送来的回波信号经放大处理后求得多普勒频移并转换为航速模拟信号送给接口单元。设载体航行速度为V，波束发射俯角为θ，声速C≈1500m／s，则单波束频移计算公式为：Δf=2Vf₀cosθ／C，由此可得速度计算公式为：V=ΔfC／(2f₀cosθ)；(5)利用收发器中接口单元将收发器送来的四个航速模拟信号转换为航速，并以数字方式向中央处理单元输出；(6)中央处理单元中的运算器根据多普勒计程仪的接口单元输出的速度信息周期性地计算载体的实时三维载体系速度量；(7)中央处理单元中的滤波模块对接收到的捷联惯性导航导航信息、多普勒计程仪的三维速度信息进行滤波融合计算得到k时刻的惯性捷联测量系统校正量校正后得到最终高精度的导航定位信息；所述的滤波融合计算得到惯性捷联测量系统校正量的方法为：将捷联惯性导航作为参考系统，状态变量X取速度误差姿态角误差(φ_x，φ_y，φ_z)、加速度计随机常值偏置和陀螺随机常值漂移(ε_x，ε_y，ε_z)，共10维：$X={[{\mathrm{\delta V}}_e^n,{\mathrm{\delta V}}_n^n,{\phi }_x,{\phi }_y,{\phi }_z,{▿}_x,{▿}_y,{ϵ}_x,{ϵ}_y,{ϵ}_z]}^T;$W=[w_ax，w_ay，w_gx，w_gy，w_gz，0，0，0，0，0]^T为系统噪声向量；以捷联惯性导航解算出的速度与多普勒计程仪测得的载体系速度经转换到导航系后两者之差作为系统的量测值，则量测方程表示为：$Z=\left[\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right]=\mathrm{HX}+V$其中，观测向量为量测噪声向量V=[w_vx，w_vy]^T，系统量测矩阵为H，将系统状态方程和量测方程离散化可得离散系统滤波方程；利用初值和P₀，根据k时刻的量测Z_k就可以递推算到k时刻的状态估计${\hat{X}}_k(k=\mathrm{1,2,3}...):$${\hat{X}}_k={\Phi }_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\Phi }_{k,k-1}{\hat{X}}_{k-1})$$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$其中，$P_k={\Gamma }_{k,k-1}{\Gamma }_{k,k-1}^T+\Lambda \left(k\right)\left\{\begin{array}{c}{\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T-{\Phi }_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\Phi }_{k,k-1}^T\end{array}\right\}{\Lambda }^T\left(k\right)$由正交性原理，有：$V_{0,k}-{\mathrm{\beta I}}_k-H_k{\Gamma }_k{\Gamma }_k^T{H}_k^T=H_k\Lambda \left(k\right)\left\{\begin{array}{c}{\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T\end{array}-{\Phi }_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\Phi }_{k,k-1}^T\right\}{\Lambda }^T\left(k\right)H_k^T$针对一般情形，H_k行维数较少，有如下的近似算法：$\left\{\begin{array}{c}{\left(H_k^T{H}_k\right)}^{-1}{H}_k^T\approx H_k^T\\ H_k{\left(H_k^T{H}_k\right)}^{-1}\approx H_k\end{array}\right.$于是可以变换得到：$H_k^T\{V_{0,k}-{\mathrm{\beta I}}_k-H_k{\Gamma }_k{\Gamma }_k^T{H}_k^T\}{H}_k=\Lambda \left(k\right)\left\{\begin{array}{c}{\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T-{\Phi }_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\Phi }_{k,k-1}^T\end{array}\right\}{\Lambda }^T\left(k\right)$定义M(k)和N(k)为：$\left\{\begin{array}{c}M\left(k\right)={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T-{\Phi }_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\Phi }_{k,k-1}^T\\ N\left(k\right)=H_k^T\{V_{0,k}-{\mathrm{\beta I}}_k-H_k{\Gamma }_k{\Gamma }_k^T{H}_k^T\}{H}_k\end{array}\right.$式中V_0，k解算方法同上，则代入可得：N(k)=Λ(k)M(k)Λ^T(k)由上式近似解算得到：${\lambda }_{0i,k}=\sqrt{\frac{N_{\mathrm{ii}}\left(k\right)}{M_{\mathrm{ii}}\left(k\right)}}$则渐消矩阵Λ(k)中的各元素为：${\lambda }_i\left(k\right)=\left\{\begin{array}{cc}{\lambda }_{0i,k},& {\lambda }_{0i,k}\ge 1\\ 1,& {\lambda }_{0i,k}\le 1\end{array}\right.$式中的N_ii和M_ii项分别为N(k)和M(k)的主对角线第i个元素；为了提高该算法在实际系统中的适用性，可通过进一步引入调整系数a，人为的调节渐消因子λ_i(k)，即有：${\lambda }_i\left(k\right)=\left\{\begin{array}{cc}a·{\lambda }_{0i,k},& a·{\lambda }_{0i,k}\ge 1\\ 1,& a·{\lambda }_{0i,k}\le 1\end{array}\right.$其中，调整系数a的具体选取根据实际情况确定，可取[0.1，10]，一般取a=5。