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

摘要

本发明公开一种基于捷联惯性制导和多普勒计程仪的导航方法，其特征在于，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和中央处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与中央处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和三轴加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与中央处理单元相连接；该导航方法包括若干步骤，它克服了AUV导航系统易受环境影响以及捷联惯性导航系统的导航定位误差随时间延续不断累积达不到精度要求的缺陷。

主权项

一种基于捷联惯性制导和多普勒计程仪的导航方法，其特征在于，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和中央处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与中央处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与中央处理单元相连接；该导航方法包括以下步骤：(1)通过捷联惯性测量单元组件中的三轴陀螺测得三轴角速度信息和加速度计测得三轴加速度信息，捷联惯性测量单元处理模块接收捷联惯性测量单元输出的导航信息，通过导航积分计算获得载体位置、速度和姿态导航信息；(2)通过多普勒计程仪的收发器中发射电振荡信号，送给换能器；(3)通过多普勒计程仪的四个换能器发射超声波和接收具有频移特性的回波信号；(4)利用收发器中接收系统将换能器送来的回波信号经放大处理后求得多普勒频移并转换为航速模拟信号送给接口单元；设载体航速为V，波束发射俯角为θ，声速C≈1500m/s，则单波束频移计算公式为：Δf＝2Vf0cosθ/C，由此可得速度计算公式为：V＝ΔfC/(2f0cosθ)；(5)利用收发器中接口单元将收发器送来的四个航速模拟信号转换为航速，并以数字方式向中央处理单元输出；(6)中央处理单元中的运算器根据多普勒计程仪的接口单元输出的速度信息周期性地计算载体的实时三维载体系速度量；(7)中央处理单元中的滤波模块对接收到的捷联惯性导航信息、多普勒计程仪的三维速度信息进行滤波融合计算得到k时刻的惯性捷联测量系统校正量校正后得到最终高精度的导航定位信息；所述的滤波融合计算得到惯性捷联测量系统校正量的方法为：将捷联惯性导航作为参考系统，状态变量X取速度误差姿态角误差(φe,φn,φu)、加速度计随机常值偏置和陀螺随机常值漂移(εx,εy,εz)，共10维：W＝[wax,way,wgx,wgy,wgz,0,0,0,0,0]T为系统噪声向量；以捷联惯性导航解算出的速度与多普勒计程仪测得的载体系速度经转换到导航系后两者之差作为系统的量测值，则量测方程表示为：Z=VSINSen-CbnVDVLeVSINSnn-CbnVDVLn=HX+V,]]>VSINSe是指捷联惯导系统解算的速度在导航坐标系中的东向分量；VSINSn是指捷联惯导系统解算的速度在导航坐标系中的北向分量；VDVLe是指多普勒计程仪测得的速度在载体系的东向分量；VDVLn是指多普勒计程仪测得的速度在载体系的北向分量；其中，观测向量为量测噪声向量V＝[wvx,wvy]T，系统量测矩阵为H，将系统状态方程和量测方程离散化可得离散系统滤波方程；利用初值和P0，根据k时刻的量测Zk就可以递推算到k时刻的状态估计k＝1，2，3，…：X^k=Φk,k-1X^k-1+Kk(Zk-HkΦk,k-1X^k-1)]]>Kk=PkHkT(HkPkHkT+I)-1]]>其中其中：Λ(k)＝diag[λ1(k),λ2(k),…λn(k)]，λi(k)≥1；i＝1,2,…,n；在次寻优方法中，根据系统的先验知识确定λ1(k):λ2(k):…:λn(k)的初值为：λ1(k):λ2(k):…:λn(k)＝α1:α2:…:αn，式中，αi≥1，i＝1,2,…,n，可得λi(k)的近似算法如下：λi(k)=αiλ0i,k,αiλ0i,k≥11,αiλ0i,k≤1]]>其中，λ0i,k=tr[N(k)]Σi=1nαi·Mii(k)]]>N(k)=V0,k-HkΓkΓkTHkT-βIk]]>M(k)={Φk,k-1Pk-1Φk,k-1T-Φk,k-1Pk-1[HkTLkT]Re,k-1HkLkPk-1Φk,k-1T}HkTHk]]>tr(A)表示对任意矩阵A的求迹运算，式中的V0,k由下式解算出：V0,k=γ1γ1T,k=0ρV0,k-1+γkγkT1+ρ,k≥1]]>上式中，残差0.95≤ρ≤1为遗忘因子，取ρ＝0.95；β≥1为弱化因子，取β＝1；其中，V0,k为计算过程中间量；Kk为tk时刻的滤波增益矩阵；Lk为利用观测值Zk来估计任意线性组合yk＝LkXk的量测矩阵；Γk为系统噪声驱动阵；Φk,k‑1为tk‑1时刻至tk时刻的一步转移矩阵。