基于捷联惯性制导和多普勒计程仪的导航方法
技术领域
本发明涉及一种基于捷联惯性制导和多普勒计程仪的导航方法，属于水下及水面航行器的导航及定位跟踪领域。
背景技术
AUV(Autonomous Underwater Vehicle自主水下航行器)是探索和开发利用海洋环境资源的重要工具，也在海洋军事应用方面发挥着重要作用。因其远程性、隐蔽性和自主性，使得AUV的导航问题仍是目前面临的主要技术挑战之一。由于水下环境复杂，常采用组合导航方式对AUV进行导航定位，而组合导航系统滤波器设计是保证导航精度的关键。目前常规AUV的导航定位精度可达到<0.8nmile／h左右，在一定程度上满足了AUV导航定位的精度要求。
SINS(Strapdown Inertial Navigation Systems捷联惯性导航系统)系统具有自主导航能力，不受环境、载体机动及无线电干扰的影响，能够连续的提供载体位置、速度和姿态等导航定位信息，其数据更新频率快，且在短时间内具有较高的相对精度。但是，随着系统工作时间的延长，捷联惯性导航系统的导航误差会随之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿捷联惯性导航系统，以抑制其随时间积累的误差。
DVL(Doppler Velocity Log多普勒计程仪)是广泛应用于水下及水面航行器组合系统的速度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。多普勒系统通过安装于AUV底部的四个换能器信息求得声波频移，进而计算得到载体在载体系的三维速度信息，具有精度高、可靠性、实时性等优点。但是由于水下环境复杂、航位推算系统位置误差随时间累积以及AUV的长航时、隐蔽性要求，基于捷联惯性导航/多普勒的组合系统的常规滤波算法精度难以达到高精度导航定位功能需求。
常规H∞滤波算法对系统统计特性不作任何假设，具有滤波模型简单、鲁棒性好的优点，因此常应用于AUV导航系统。但是常规H∞滤波器跟踪机动过程能力较弱，实时性难以保证。当由于模型不确定性等因素的影响，造成滤波器的状态估计值偏离系统状态时，必然会在输出残差序列的均值与幅值上有所表现，此时若在线调整时变增益矩阵，使得系统估计误差方差最小的同时保持残差序列仍然相互正交，即满足正交性原理，则可迫使滤波器保持对实际系统状态的高精 度跟踪。本发明通过在H∞滤波算法中引入次优渐消因子对过去的数据实施渐消，以减弱老数据对当前滤波估计值的影响，即通过实时调节估计误差方差矩阵以及相应的增益矩阵以满足正交性原理，迫使滤波器对实际系统状态的高精度跟踪，并最大程度地提取输出残差中一切有效信息，得到一种高跟踪性能的滤波算法。该滤波算法具有较强的关于模型不确定性的鲁棒性以及对系统机动过程的跟踪能力，并保持了H∞滤波模型简单的优点，实现滤波器对于AUV机动过程系统状态的跟踪，具有重要的理论意义和工程实用价值。
基于捷联惯性导航/多普勒的AUV组合导航系统将捷联惯性导航、多普勒与本发明所提出的滤波算法的优势结合在一起，可提供载体高精度的实时导航跟踪参数，解决了AUV导航系统易受环境影响以及捷联惯性导航的导航定位误差随时间延续不断累积达不到精度要求的问题。
发明内容
发明目的：本发明的目的在于，克服现有技术的不足，提出了一种用于捷联惯性导航／多普勒的AUV组合导航系统的滤波方法。利用所设计的滤波器将多普勒测速信息引入AUV组合系统，以辅助捷联惯性导航进行导航定位。该方法可以整合多个子导航信息，克服了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维：X=[δVen,δVnn,φe,φn,φu,▿x,▿y,ϵx,ϵy,ϵz]T;]]> W=[wax，way，wgx，wgy，wgz，0，0，0，0，0]T为系统噪声向量；
以捷联惯性导航解算出的速度与多普勒计程仪测得的载体系速度经转换到导航系后两者之差作为系统的量测值，则量测方程表示为：
Z=VSINSen-CbnVDVLeVSINSnn-CbnVDVLn=HX+V]]>
其中，观测向量为![]()
量测噪声向量V＝[wvx，wvy]T，系统量测矩阵为H，将系统状态方程和量测方程离散化可得离散系统滤波方程；利用初值![]()
和P0，根据k时刻的量测Zk就可以递推算到k时刻的状态估计X^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]]>
其中Pk=Γk,k-1Γk,k-1T+Λ(k){Φk,k-1Pk-1Φk,k-1T-Φk,k-1Pk-1HkTLkTRe,k-1HkLkPk-1Φk,k-1T}]]>
其中：Λ(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-1HkTLkTRe,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。
进一步地，所述的捷联惯性测量单元的三轴陀螺仪三轴正交安装，所述的三轴加速度计三轴正交安装。
进一步地，所述的捷联惯性测量单元的三轴陀螺仪为光纤陀螺仪。
进一步地，所述的三轴加速度计为石英挠性加速度计。
进一步地，所述多普勒计程仪的收发器包括发射系统、接收系统和计算补偿电路。
进一步地，所述导航系统还包括显示器，所述显示器与运算器相连接，显示中央处理器运算得到的导航信息。
该导航系统包括：捷联惯性测量单元及捷联惯性测量单元处理模块、多普勒计程仪，安装于AUV上的收发器、换能器、接口单元及中央处理单元。捷联惯性测量单元用于输出惯性测量数据；捷联惯性测量单元处理模块用于接收捷联惯性测量单元输出的数字信号，通过导航积分计算获得载体位置、速度和姿态数据； 多普勒计程仪用于测量载体相对于海底的纵向速度以及横向速度；可安装于AUV底端的换能器用于发射超声波和接收反射回波；安装于AUV底端的收发器包括发射系统、接收系统及计算补偿电路等单元；接口单元用于将收发器送来的航速信号转换为航速，并以数字方式或模拟方式向外部设备输出数据；中央处理单元包括一个运算器和一个滤波模块，能够接收上述捷联惯性测量单元处理模块、多普勒计程仪的导航信息，通过实时滤波计算，利用估计出的系统状态修正捷联惯性测量单元处理模块的参数，得到最终的组合导航信息数据。
本发明可应用于AUV系统，AUV是探索和开发利用海洋环境资源的重要工具，也在海洋军事应用方面发挥着重要作用。因其远程性、隐蔽性和复杂的水下环境，常采用组合导航方式对AUV进行导航定位。捷联惯性导航系统具有自主导航能力，不受环境、载体机动及无线电干扰的影响，能够连续的提供载体位置、速度和姿态等导航定位信息，其数据更新频率快，且在短时间内具有较高的相对精度。但是随着系统工作时间的延长，捷联惯性导航系统的导航误差会随之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿捷联惯性导航系统，以抑制其随时间积累的误差。多普勒是广泛应用于水下及水面航行器组合系统的速度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。具有精度高、可靠性、实时性等优点。但是由于水下环境复杂、航位推算系统位置误差随时间累积以及AUV的长航时、隐蔽性要求，基于捷联惯性导航/多普勒的组合系统的常规滤波算法精度难以达到高精度导航定位功能需求。常规H∞滤波算法对系统统计特性不作任何假设，具有滤波模型简单、鲁棒性好的优点。但是常规H∞滤波器跟踪机动过程能力较弱，实时性难以保证。本发明通过在H∞滤波算法中引入次优渐消因子对过去的数据实施渐消，以减弱老数据对当前滤波估计值的影响，即通过实时调节估计误差方差矩阵以及相应的增益矩阵以满足正交性原理，迫使滤波器保持对实际系统状态的高精度跟踪，并最大程度地提取输出残差中一切有效信息，得到一种高跟踪性能的滤波算法。该滤波算法具有较强的关于模型不确定性的鲁棒性以及对系统机动过程的跟踪能力，并保持了H∞滤波模型简单的优点，实现滤波器对于AUV机动过程系统状态的跟踪。本发明利用该滤波技术将捷联惯性导航导航信息、多普勒计程仪的三维速度信息进行数据融合计算，得到最终的高精度、实时性强，持续性好的导航定位结果。
具体说明如下：
(1)地球几何模型与物理参数
①地球形状描述
惯性导航解算中使用WGS-84参考椭球体作为地球模型，其赤道半径长半轴Re＝6378137.0m，椭球扁率e＝1/298.257223563。子午面曲率半径RN和卯酉圈曲率半径RE可以由下列方程求得：
RN=Re(1-e)2[(1-e)2sin2L+cos2L]32RE=Re[(1-e)2sin2L+cos2L]12]]>
②地球自转角速率(ωie)
在惯性导航系统中，地球自转角速度为：
ωie=15.0411°/h=7.29211585×10-5rad/s
③地球重力加速度(g)
WGS-84参考椭球的重力加速度为：
g=9.7803267714×1+0.00193185138639sin2L1-0.00669437999013sin2L(1+hRe)-2]]>
式中，L表示当地纬度，h表示高度，Re为赤道半径长半轴。
(2)坐标系定义
①地心惯性坐标系(i系)
用oxiyizi表示，原点位于地球中心，oxi与oyi轴在地球赤道平面内，oxi轴指向春分点，ozi轴和地球自转轴重合，oyi与oxi、ozi构成右手直角坐标系。三个坐标轴在惯性空间的指向固定。捷联惯性测量单元的输出以i系为参考基准。
②地球坐标系(e系)
用oxeyeze表示，原点位于地球中心，oze轴和地球自转轴重合，oxe轴沿格林尼治子午面和地球赤道平面的交线，oye轴在赤道平面内，oxe、oye、oze三轴构成右手直角坐标系。地球坐标系与地球固连，e系相对i系转动的角速率即为地球自转角速度ωie。
③地理坐标系(g系)
用oxgygzg表示，原点位于载体重心，一般采用东北天坐标系作为地理坐标系，即oxg轴指向东(E)，oyg轴指向北(N)，ozg轴指向天(U)。
④导航坐标系(n系)
用oxnynzn表示，采用东北天地理坐标系(ENU)作为导航坐标系。
⑤载体坐标系(b系)
用oxbybzb表示，原点一般取捷联惯性测量单元几何中心，oxb轴沿载体横轴向右，oyb轴沿载体纵轴向前，ozb轴沿载体立轴向上。
(3)姿态角与姿态矩阵之间的转换关系
①姿态角定义
航向角：载体纵轴oyb在水平面上的投影与地理子午线北向之间的夹角，称为航向角，计为ψ。航向角数值是以地理北向为起点顺时针方向计算的，其定义域为0～360°。
俯仰角：载体绕横轴oxb转动时，载体纵轴和水平面的夹角，称为俯仰角，记为θ。俯仰角从水平面算起，向上为正，向下为负，其定义域为-90°～+90°。
横摇角：载体纵向对称平面与纵向铅垂平面之间的夹角，称为横摇角，记为γ。横摇角从铅垂平面算起，右倾为正，左倾为负，其定义域为-180°～+180°。
②姿态矩阵
实现载体坐标系b系到地理坐标系(导航系n系)的变换可按下列顺序经三次转动得到，具体顺序为：绕-ozg轴转ψ角，绕ox1轴转θ角，再绕oy2轴转γ角。它们之间的转换关系如下式所示。
oxgygzg→y-ozgox1y1z1→qox1ox2y2z2→goy2oxbybzb]]>
三次转动对应的变换矩阵分别为
Cψ=cosψ-sinψ0sinψcosψ0001]]>
Cθ=1000cosθsinθ0-sincosθ]]>
Cγ=cosγ0-sin010sinγ0cosγ]]>
所以地理坐标系到载体坐标系的转换矩阵为：
Cgb=cosγcosψ+sinγsinψsinθ-cosγsinψ+sinγcosψsinθ-sinγcosθsinψcosθcosψcosθsinθsinγcosψ-cosγsinψsinθ-sinγsinψ-cosγcosψsinθcosγcosθ]]>
以地理坐标系作为导航坐标系，则有捷联惯性导航系统姿态矩阵为
Cbn=(Cgb)T]]>
(③姿态角和姿态矩阵的转换关系
Cbn=(Cgb)T=[Tij]3×3]]>
即可解得姿态矩阵至主值区间姿态角的转换关系为：
![]()
(4)捷联惯性导航系统基本原理
①捷联惯性导航系统微分方程
姿态矩阵微分方程为：![]()
其中，![]()
![]()
为载体角速度，由陀螺仪测得，![]()
且有：
ωien=0ωiecosLωiesinL,ωenn=-VNnRN+hVEnRE+hVEnRE+htanL]]>
式中，Vn=VEnVNnVUnT]]>为载体对地速度在导航系的大小。
载体在导航系内的比力方程为：V·n=fn-(2ωien+ωenn)×Vn+gn]]>
式中![]()
fb为加速度计测得的比力，gn=[0 0 -g]T。
位置更新微分方程：在以地理坐标系作为导航坐标系的系统中，纬度L、经度λ和高度h的微分方程为：
L·=VNnRN+h,λ·=VEn(RE+h)cosL,h·=VUn]]>
②捷联惯性导航系统误差模型
陀螺仪误差模型：εi=εbi+εwi(i=x，y，z)，εbi为随机常数，εwi为白噪声；
加速度计误差模型：![]()
(i=x，y，z)，![]()
为随机常数，![]()
为白噪声；
姿态误差方程为：φ·n=-ωinn×φn+δωien+δωenn-Cbnϵb,φn=φxφyφzT]]>为姿态误差，εb=[εx εy εz]T为陀螺仪误差。
速度误差方程为：
δV·n=-φ×fn-(2δωien+δωenn)×Vn-(2ωien+ωenn)×δVn+Cbn▿b,]]>
δVn=δVenδVnnϵVunT]]>为速度误差，▿b=▿x▿y▿zT]]>为加速度计误差。
位置误差方程为：
δL·=δVnnRN+h-VNn(RN+h)2δh]]>
δλ·=secLRE+hδVen+VEnsecLtanLRE+hδL-VEnsecL(RE+h)2δh]]>
δh·=δVun]]>
(5)滤波算法
①H∞滤波算法介绍
考虑如下基于Krein空间离散状态空间系统模型：
Xk=Φk,k-1Xk-1+Γk,k-1Wk-1Zkyk=HkLkXk+Vk]]>
Xk为tk时刻的待估计系统状态向量；
Φk，k-1为tk-1时刻至tk时刻的一步转移矩阵；
Γk-1为系统噪声驱动阵；
Zk为量测状态向量；
Hk为量测阵；
Wk-1为系统激励噪声序列；
Vk为量测噪声序列；
Xk，k-1为系统状态一步预测；
Pk为估计误差方差阵；
Kk为滤波增益矩阵；
yk是给定的系统状态Xk的线性组合观测量。
其中，Wk和Vk为I2能量有界的噪声，即![]()
对其统计特性不做任何假设。设系统的初始状态为X0，![]()
表示对系统初始状态X0的一个估计，定义初始估计误差方差阵为：![]()
令![]()
表示在给定观测值{Zk}条件下对yk的估计，定义如下的滤波误差ek=y^k-LkXk.]]>
设Tk(Ff)表示将未知干扰![]()
映射至滤波误差{ek}的传递函数。次优H∞估计问题描述：给定正数γ>0，导找次优H∞估计 y^k=Ff(Z0,Z1,...,Zk),]]>使得||Tk(Ff)||∞<γ，即满足
infFfsupX0,W,V∈h2||ek||22||X0-X^0||P0-12+||Wk||22+||Vk||22<γ2]]>
对于给定的γ>0，如果[Φk，k-1，Γk，k-1]是满秩的，则满足条件||Tk(Ff)||∞≤γ的滤波器存在，当且仅当对所有的k，有
Pk-1+HkTHk-γ-2LkTLk>0]]>
其中，Pk满足如下递推Riccati方程
Pk=Φk.k-1Pk-1Φk,k-1T+Γk,k-1Γk,k-1T-Φk,k-1Pk-1HkTLkTRe,k-1HkLkPk-1Φk,k-1T]]>
Re,k=I00-γ2I+HkLkPkHkTLkT]]>
若上述不等式式成立，可得H∞滤波递推算法如下：
y^k=LkX^k]]>
X^k=Φk,k-1X^k-1+Kk(Zk-HkΦk,k-1X^k-1)]]>
Kk=PkHkT(HkPkHkT+I)-1]]>
次优H∞滤波算法在Pk的求解过程中，给定正数γ值通过Re，k进而调整Pk，滤波算法的鲁棒性较好。只要给定初值![]()
和P0，根据k时刻的量测Zk就可以递推算到k时刻的状态估计X^k(k=1,2,3,...).]]>
②改进算法
滤波器状态估计值具有如下的一般结构：![]()
式中X^k,k-1=Φk,k-1X^k-1γk=Zk-HkX^k,k-1,]]>γk表示k时刻的残差向量；
在线选择一个适当的时变增益矩阵Kk，使同时满足如下条件，即正交性原理：
E[(Xk-X^k)(Xk-X^k)T]=min]]>
E[γk+jγkT]=0,k=0,1,2,...,j=1,2,...]]>
当由于模型不确定性等因素的影响，造成滤波器的状态估计值偏离系统状态时，必然会在输出残差序列的均值与幅值上有所表现，此时若在线调整增益阵Kk，使得残差序列仍然保持相互正交，则可迫使滤波器保持对实际系统状态的跟踪。
在H∞滤波算法中引入次优渐消因子对过去的数据实施渐消，以减弱老数据对当前滤波估计值的影响，通过实时调节估计误差方差矩阵Pk以及相应的增益 矩阵Kk来强迫输出残差近似为高斯白噪声，可最大程度地提取输出残差中一切有效信息，即得种跟踪性能较强的滤波算法。
将Pk写成如下形式：
Pk=Γk,k-1Γk,k-1T+Λ(k){Φk,k-1Pk-1Φk,k-1T-Φk,k-1Pk-1HkTLkTRe,k-1HkLkPk-1Φk,k-1T}]]>
其中：Λ(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-1HkTLkTRe,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为弱化因子，引入此弱化因子目的是使状态估计值更加平滑。
该方法以H∞滤波算法为基础，通过实时调节估计误差方差矩阵以及相应的增益矩阵以满足正交性原理，迫使滤波器保持对实际系统状态的高精度跟踪，得到一种高跟踪性能的滤波算法。该滤波算法具有较强的关于模型不确定性的鲁棒性以及对系统机动过程的跟踪能力，并保持了H∞滤波模型简单的优点，实现滤波器对于载体机动过程系统状态的高精度跟踪。
将上述提出的滤波算法应用于AUV组合导航系统，对惯性捷联导航系统导 航信息、多普勒计程仪的三维速度信息进行数据融合，得到高精度、实时性强，持续性好的导航定位结果，有效的提高了组合系统导航定位性能。
本发明与现有技术相比，其有益效果是：该方法可以整合多个子导航传感器信息，优势互补以得到更高精度和更可靠的导航信息。
附图说明
图1为本发明基于捷联惯性制导和多普勒计程仪的导航方法中的基于捷联惯性制导和多普勒计程仪的导航系统组成的主框图；
图2为本发明基于捷联惯性制导和多普勒计程仪的导航方法组合模型框图；
图3为本发明基于捷联惯性制导和多普勒计程仪的导航方法结构原理图；
图4为本发明基于捷联惯性制导和多普勒计程仪的导航方法中滤波融合计算算法流程图；
图5为本发明基于捷联惯性制导和多普勒计程仪的导航方法中的捷联惯性导航和多普勒系统总体流程图；
图6为该为本发明基于捷联惯性制导和多普勒计程仪的导航方法的实施例1的仿真分析效果图。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
实施例：
本实施例的一种基于捷联惯性制导和多普勒计程仪的导航方法，在本实施例中，该导航方法应用于一基于捷联惯性制导和多普勒计程仪的导航系统，该导航系统包括多普勒计程仪、捷联惯性测量单元、捷联惯性测量单元处理模块和中央处理单元；所述的多普勒计程仪包括收发器、四个换能器和接口单元；所述多普勒计程仪通过该接口单元与中央处理单元相连接；所述捷联惯性测量单元包括三轴陀螺和三轴加速度计；所述捷联惯性测量单元与捷联惯性测量单元处理模块相连接；所述捷联惯性测量单元处理模块与中央处理单元相连接；该导航方法包括以下步骤：
(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维：X=[δVen,δVnn,φe,φn,φu,▿x,▿y,ϵx,ϵy,ϵz]T;]]> W=[wax，way，wgx，wgy，wgz，0，0，0，0，0]T为系统噪声向量；
以捷联惯性导航解算出的速度与多普勒计程仪测得的载体系速度经转换到导航系后两者之差作为系统的量测值，则量测方程表示为：
Z=VSINSen-CbnVDVLeVSINSnn-CbnVDVLn=HX+V]]>
其中，观测向量为![]()
量测噪声向量V=[wvx，wvy]T，系统量测矩阵为H，将系统状态方程和量测方程离散化可得离散系统滤波方程；利用初值![]()
和P0，根据k时刻的量测Zk就可以递推算到k时刻的状态估 计X^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]]>
其中Pk=Γk,k-1Γk,k-1T+Λ(k){Φk,k-1Pk-1Φk,k-1T-Φk,k-1Pk-1HkTLkTRe,k-1HkLkPk-1Φk,k-1T}]]>
其中：Λ(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-1HkTLkTRe,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。
本实施例的捷联惯性测量单元的三轴陀螺仪三轴正交安装，所述的三轴加速度计三轴正交安装。
本实施例的捷联惯性测量单元的三轴陀螺仪为光纤陀螺仪。
本实施例的三轴加速度计为硅微加速度计。
本实施例的多普勒计程仪的收发器包括发射系统、接收系统和计算补偿电路。
本实施例的导航系统还包括显示器，所述显示器与运算器相连接，显示中央处理器运算得到的导航信息。
将基于H∞滤波算法的常规导航方法(对比例)、和本实施例的基于惯性捷联 导航和多普勒计程仪的导航方法(实施例)在AUV组合导航系统中的滤波性能进行仿真分析，给出了AUV航迹曲线跟踪对比效果如图6所示，该新的导航方法更接近理论上的预定导航AUV航迹曲线。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。