[发明专利]一种基于捷联惯性制导和多普勒计程仪的导航方法

权利要求书

1.一种基于捷联惯性制导和多普勒计程仪的导航方法，其特征在于，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和中央处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与中央处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与中央处理单元相连接；该导航方法包括以下步骤：

(1)通过捷联惯性测量单元组件中的三轴陀螺测得三轴角速度信息和加速度计测得三轴加速度信息，捷联惯性测量单元处理模块接收捷联惯性测量单元输出的导航信息，通过导航积分计算获得载体位置、速度和姿态导航信息；

(2)通过多普勒计程仪的收发器中发射电振荡信号，送给换能器；

(3)通过多普勒计程仪的四个换能器发射超声波和接收具有频移特性的回波信号；

(4)利用收发器中接收系统将换能器送来的回波信号经放大处理后求得多普勒频移并转换为航速模拟信号送给接口单元；设载体航速为V，波束发射俯角为θ，声速C≈1500m/s，则单波束频移计算公式为：Δf＝2Vf₀cosθ/C，由此可得速度计算公式为：V＝ΔfC/(2f₀cosθ)；

(5)利用收发器中接口单元将收发器送来的四个航速模拟信号转换为航速，并以数字方式向中央处理单元输出；

(6)中央处理单元中的运算器根据多普勒计程仪的接口单元输出的速度信息周期性地计算载体的实时三维载体系速度量；

(7)中央处理单元中的滤波模块对接收到的捷联惯性导航信息、多普勒计程仪的三维速度信息进行滤波融合计算得到k时刻的惯性捷联测量系统校正量校正后得到最终高精度的导航定位信息；

所述的滤波融合计算得到惯性捷联测量系统校正量的方法为：

将捷联惯性导航作为参考系统，状态变量X取速度误差姿态角误差(φ_e,φ_n,φ_u)、加速度计随机常值偏置和陀螺随机常值漂移(ε_x,ε_y,ε_z)，共10维：W＝[w_ax,w_ay,w_gx,w_gy,w_gz,0,0,0,0,0]^T为系统噪声向量；

以捷联惯性导航解算出的速度与多普勒计程仪测得的载体系速度经转换到导航系后两者之差作为系统的量测值，则量测方程表示为：

$Z=\left[\begin{array}{c}{V}_{SINSe}^n-C_b^n{V}_{DVLe}\\ V_{SINSn}^n-C_b^n{V}_{DVLn}\end{array}\right]=HX+V,$

V_SINSe是指捷联惯导系统解算的速度在导航坐标系中的东向分量；

V_SINSn是指捷联惯导系统解算的速度在导航坐标系中的北向分量；

V_DVLe是指多普勒计程仪测得的速度在载体系的东向分量；

V_DVLn是指多普勒计程仪测得的速度在载体系的北向分量；

其中，观测向量为量测噪声向量V＝[w_vx,w_vy]^T，系统量测矩阵为H，将系统状态方程和量测方程离散化可得离散系统滤波方程；

利用初值和P₀，根据k时刻的量测Z_k就可以递推算到k时刻的状态估计k＝1，2，3，…：

${\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1})$

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

其中

其中：Λ(k)＝diag[λ₁(k),λ₂(k),…λ_n(k)]，λ_i(k)≥1；i＝1,2,…,n；在次寻优方法中，根据系统的先验知识确定λ₁(k):λ₂(k):…:λ_n(k)的初值为：λ₁(k):λ₂(k):…:λ_n(k)＝α₁:α₂:…:α_n，式中，α_i≥1，i＝1,2,…,n，可得λ_i(k)的近似算法如下：

${\mathrm{\λ}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\α}}_i{\mathrm{\λ}}_{0i,k},& {\mathrm{\α}}_i{\mathrm{\λ}}_{0i,k}\≥1\\ 1,& {\mathrm{\α}}_i{\mathrm{\λ}}_{0i,k}\≤1\end{array}\right.$

其中，

${\mathrm{\λ}}_{0i,k}=\frac{tr\[N\left(k\right)\]}{\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\α}}_i\·M_{ii}\left(k\right)}$

$N\left(k\right)=V_{0,k}-H_k{\mathrm{\Γ}}_k{\mathrm{\Γ}}_k^T{H}_k^T-{\mathrm{\βI}}_k$

$M\left(k\right)=\{{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T-{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}\[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\Φ}}_{k,k-1}^T\}{H}_k^T{H}_k$

tr(A)表示对任意矩阵A的求迹运算，式中的V_0,k由下式解算出：

$V_{0,k}=\left\{\begin{array}{cc}{\mathrm{\γ}}_1{\mathrm{\γ}}_1^T,& k=0\\ \frac{{\mathrm{\ρV}}_{0,k-1}+{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T}{1+\mathrm{\ρ}},& k\≥1\end{array}\right.$

上式中，残差0.95≤ρ≤1为遗忘因子，取ρ＝0.95；β≥1为弱化因子，取β＝1；

其中，V_0,k为计算过程中间量；

K_k为t_k时刻的滤波增益矩阵；

L_k为利用观测值Z_k来估计任意线性组合y_k＝L_kX_k的量测矩阵；

Γ_k为系统噪声驱动阵；

Φ_k,k-1为t_k-1时刻至t_k时刻的一步转移矩阵。