基于联邦滤波器的SINSGNSDVS水下组合导航研究

基金项目：教育部新世纪优秀人才支持（NCET-06-0877）；国家863计划资助(2007AA0676)作者简介：刘明雍(1971—),男，湖北石首人，教授，博士生导师。主要从事控制理论与应用，水下航行器导航与制导。1基于联邦滤波器的SINS/GNS/DVS水下组合导航研究刘明雍1，周志远1，赵涛2(1.西北工业大学航海学院，西安710072;)（2.中国兵器工业集团第二零二研究所，咸阳712099)摘要：针对现有的水下导航系统不能全面满足AUV导航的需要，为了提高水下航行器组合导航系统长时间远航程的隐蔽性，可靠性及精度，提出了一种基于联邦滤波器的惯性导航系统、地磁导航、多普勒速度声纳的组合导航方案，并采用简化自适应卡尔曼算法对水下航行器组合导航系统进行误差估计，解决了传统卡尔曼滤波容易发散的问题。仿真结果表明，在SINS/GNS/DVS组合导航中采用该算法，提高了定位精度，保证了滤波的快速性，验证了该算法的可行性和正确性。关键词：组合导航；自适应卡尔曼滤波器；地磁导航中图分类号：U666.11文献标志码：AResearchonSINS/DNS/DVSunderwaterintegratednavigationbasedonfederalfilterLiuMing-yong1，ZhouZhi-yuan1，ZhaoTao2（1.CollegeofMarine,NorthwestPolytechnicalUniversity,ShaanxiXi’an,710072)(2.NO.202instituteofChinaordnanceindustrygroupcorporation,ShaanxiXiang-yang,712099)Abstract:ConsideringofthefactthattheexistingunderwaternavigationsystemcannotcompletelystheneedsoftheAUVnavigation.Inordertoenhancethelongrangeanddeepwaterability,reliabilityandaccuracyoftheunderwatervehiclenavigationsystem,inthispaper,weproposeafilteralgorithmbasedontheFederationofinertialnavigationsystemconsistingofDopplerspeedsonar,magneticnavigationmethod.Meanwhile,asimplifiedmethodisadoptedforadaptiveKalmanUnderwaterVehiclenavigationsystemerrorestimatestosolvethedivergenceproblemoftraditionalKalmanfilter.Thesimulationresultsshowthattheproposedmehodcanimporvethepositioningaccuracy.Moreover,thethespeedinessofthefilterisguaranteed.Inaddition,thesimulationresultsalsodemonstratetheeffectivenessandcorrectnessofthealgorithm.Keywords:Integratednavigation;AdaptiveKalmanfilter;GeomagneticNavigation0引言当前水下组合导航定位普遍采用SINS/GPS/DVL方案。由于GPS信号在水下很快衰减，故航行器需定期上爬到近水面,使GPS天线露出水面,来接收GPS的定位信息，对由陀螺罗经和多普勒速度仪组成的自主导航系统进行位置校准。然而随着水下航行器的应用范围、下潜深度不断扩展，特别是军事领域内对其隐蔽性的要求，常规组合方案越来越难以满足需求。作为无源导航技术的地磁导航为实现这一目标提供了新的技术途径。国内外的研究中对地磁导航的研究有很多成果。文献[1]将测量的地磁异常场强度序列与事先存储的地磁异常图实时进行相关匹配，确定飞机在地磁异常图上的经度和纬度；文献[2]中介绍了在对飞行弹体进行弹道控制中,利用地磁探测滚转姿态来实时获取弹体的对地方位,为弹体控制提供方位依据。但是，地磁导航在水下导航方面的应用，所做研究很少。考虑上述问题，本文提出了一种SINS/GNS（地磁）/DVS组合方案，该方法以地磁场模型解算地磁场强度的方式来得到精确位置信息,辅以精确计时进而获得速度信息；以地磁系统获取的速度、位置信息与惯导系统输出的速度、位置信息的差值作为量测值[4],经过卡尔曼滤波,估计导航系统的误差,作为一个子滤波器；并与另一子滤波器SINS/DVL进行信息融合，从而构成联邦滤波器。考虑到水下环境的复杂多变使导航系统特别是地磁导航系统观测噪声的统计特性无法被准确获得，采用改进的自适应滤波算法代替标准卡尔曼滤波，并加以验证。仿真结果表明，在SINS/GNS/DVS组合导航中采用该算法，提高了定位精度，保证了滤波的快速性，验证了该算法的可行性和正确性。1基于联邦滤波的SINS/DNS/DVS组合导航方案设计本文采用的基于联邦卡尔曼滤波的水下航行器SINS/GNS/DVS组合方法如图1所示。该组合方案包括三个滤波器，即主滤波器、子滤波器1和子滤波器2。其中，子滤波器1负责SINS和多普勒速度仪的信息融合，其量测信息主要是指多普勒和SINS形成的速度信息；子滤波器2负责SINS和地磁导航系统融合，其量测信息是地磁导航系统和SINS形成的位置、速度观测信息。主滤波器一方面对各子滤波器进行信息综合与分配，另一方面将系统状态误差的估计值反馈给惯导系统，以校正其累积误差。水下航行器采用该组合方案相比集中滤波器不仅具有实时性好的特点，而且通过信息分配系数的选取可以具有较高的容错能力，便于对各传感器实施故障诊断和隔离。由子滤波器与主滤波器合成的全局估计值gX及其相应的协方差阵gP被放大为1(1)igiP后再反馈到子滤波器以重置子滤波器的估计值，即1ˆˆ,igiiigXXPP（1）惯导INS多普勒DVS主滤波器子滤波器1时间更新最优融合ˆ地磁DNS子滤波器2图1SINS/DNS/DVS联邦滤波器结构图2地磁导航子系统的设计当前的地磁导航技术主要是指地磁匹配定位,即将预先选定的区域地磁场某种地磁场特征值,制成参考图并储存在水下航行器上的计算机中。当航行器通过这些地区时,地磁传感器实时测量地磁场的有关特征值,并构成实时图,实时图与预存的参考图在计算机中进行相关匹配,确定实时图在参考图中的最相似点（匹配点）,从而确定出航行器的精确实时位置。图2为地磁导航与捷联惯导的子系统示意图。地磁传感器数据处理地磁数据库惯性导航地磁图匹配数据融合误差修正图2地磁导航子系统设计原理图本文中所设计的地磁导航的具体实现包括三个步骤：第一步，利用地磁传感器处理后所测得磁偏角和磁倾角作为匹配参数进行图匹配,获取粗位置信息；第二步，由地磁场模型解算地磁场强度,采用逐次迭代逼进地磁场强度测量值的方式以获取精确的位置信息,同时对2个点之间进行精确计时,进而解算出载体的即时速度并由此获得速度信息；第三步，以地磁匹配系统获取的速度、位置信息与惯导系统输出的位置、速度信息的差值作为量测值,经过卡尔曼滤波估计误差，作为联邦滤波器的一个子滤波器,再与惯导主滤器进行数据融合。3组合导航自适应卡尔曼算法对于水下航行器组合导航系统，由于海况的复杂性，系统量测噪声统计特性随实际工作环境而改变，初始的先验值并不能代表实际工作时的噪声情况。虽然经过对捷联式惯性导航系统和地磁匹配系统的大量反复试验，可以获得试验时系统噪声的统计特性，但是实际工作时量测噪声的统计特性仍然是未知的。针对此问题，采用简化的Sage-Husa自适应滤波方法，解决了传统卡尔曼滤波容易发散的问题。。设系统的状态模型和量测模型方程为,11,11kkkkkkkkkkkXXFWZHXV（2）1kW和kV为互不相关的白噪声序列，其方差和均值分别为[]kkEWq，[]TkjkkjEWWQ，[]kkEVr，[]TkjkkjEVVR标准卡尔曼算法描述如下：,1,11ˆˆkkkkkXX（3）,1ˆˆkkkkkXXKZ（4）,1ˆkkkkkZZHX（5）1,1,1ˆ[]TTkkkkkkkkkKPHHPHR（6）,1,11,11ˆ**TTkkkkkkkkPPGQG（7）,1[]kkkkkPIKHP（8）Sage-Husa自适应滤波是在标准卡尔曼滤波的基础上，同时估计过程噪声和量测噪声的统计特性[3]。事实上，组合导航系统中的系统噪声一般具有稳定性，所以本文仅对量测噪声进行估计。111,1ˆˆ(1)()TTkkkkkkkkkkRdRdZZHPH（9）1(1)/(1),01kkdbbb（10）其中，b取0.95，式（3）至式（10）即为简化Sage-Husa自适应滤波算法。4自适应联邦滤波模型建立4．1系统状态方程导航坐标系取当地的地理坐标系，其中XYZ分别指向东北天建立状态方程如下()()()()()tttttXFXGW（11）对于惯导系统，作为参考系统的惯导系统,取姿态误差角（ENU）、速度误差（ENUvvv）位置误差（Lh）陀螺常值漂移（bxbybz）陀螺一阶马尔科夫过程（rxryrz）加速度计零位漂移（xyz）作为系统的状态变量,其中：[IENUENUvvvLhX]Tbxbybzrxryrzxyz[]TIgxgygzbxbybzaxayazWIG和IF的推导过程及具体内容略。对于多普勒子系统，取[]TDVSdxdydzdxdydzvvvKKKX对于地磁子系统，取[]TDNSxmymzmmmmXVVVLh。INS/DVS子系统和INS/DNS子系统状态方程分别为：IIIIIDVSDVSDVSDVSDVSFXGWXFXGWXIIIIIDNSDNSDNSDNSDNSFXGWXFXGWX4．2SINS/DVS量测方程的建立2[]IDVSDVSDVSDVSXZHHVX2333339000(1,1)(1,2)(1,3)(2,1)(2,2)(2,3)(3,1)(3,2)(3,3)UNUENEnnnbxbybznnnnDVSbbxbybznnnbxbybzvvvvvvvvvvvvvvvH0I0CCCHCCCCCCC4．3SINS/DNS量测方程的建立由于利用三轴捷联磁场计测量地磁场获得地磁场在载体坐标系3个坐标轴方向的分量分别为[]TxmymzmBBB以磁偏角和磁倾角匹配获取的粗略位置的经纬度作为循环计算的经纬度初始值00，以高度计测量的高度0h作为循环计算的高度初始值,逐次代入地磁场模型解算地磁场强度[]TxdydzdBBB，当222()()()xmxdymydzmzdBBBBBB最小时,最后一次代入的经纬度和高度即为地磁导航系统输出的位置[]TmmmLh，辅以2个点精确计时则可得到速度[]TDNSxmymzmXVVV。123456mINSDNSmINSDNSmINSDNSDNSExmEINSEDNSNymNINSNDNSUINSUDNSUzmLLVLLVhhVhhZvvVVVvvVVVVVvvV