本发明涉及水下导航领域，尤其涉及一种水下无源组合导航系统建模及信息融合方法。
背景技术：
海洋是人类生存和发展的重要领域，蕴含着丰富的资源。近年来，对海洋资源的开发和利用已经成为各个国家的重要战略目标。由于我国人口众多，人均资源相对匮乏，对海洋进行全面深入的认识以及开发保护具有十分重要的意义。自主式水下航行器属于水下机器人的一种，可以凭借自身的动力系统和导航系统顺利的完成工作任务。水下航行器具有隐蔽性好，机动性强，工作范围广等优点，在海洋资源开发的过程中起到了非常重要的作用。
对于水下航行器而言，导航精度是其重要的技术指标。高精度的导航系统可以持续提供准确的位置信息，使得水下航行器能够长时间的进行水下作业。其中，惯性导航系统作为一种常用的导航设备可以提供载体的位置、速度和姿态等导航信息。惯性导航系统在没有外界信息修正的情况下，定位误差会随着时间累积，难以满足远航程航行要求。提出了采用多普勒计程仪、声呐定位系统、重礼仪等与惯性导航系统相结合的组合导航系统，利用各种导航设备提供的辅助信息对惯导的定位误差进行修正，进而提高了系统的定位精度。随着水下导航技术的发展，将多种传感器的导航信息进行有效融合，进而提高水下航行器的定位精度，也是信息融合技术领域的一个重要的研究方向。
技术实现要素：
本发明目的在于克服上述现有技术的不足，提供了一种水下无源组合导航系统建模及信息融合方法，具体由以下技术方案实现：
所述水下无源组合导航系统建模及信息融合方法，其特征在于具体包括如下步骤：
步骤1)将各子滤波器和主滤波器的初始估计协方差阵设置为组合导航系统初始信息的γi倍，i＝1,2,…,n,m；
步骤2)各子滤波器和主滤波器独立完成时间更新过程，子滤波器分为线性子滤波器与非线性子滤波器，线性子滤波器通过卡尔曼滤波器进行状态变量的时间更新过程，非线性子滤波器通过非线性算法的状态更新过程获得状态；
步骤3)各子滤波器根据最新的量测信息完成量测更新过程，线性子滤波器通过卡尔曼滤波器进行量测更新，而非线性子滤波器通过高斯粒子非线性算法获得更新后的状态分布参数；
步骤4)得到各个子滤波器的局部估计值和主滤波器的估计值后，再进行最优数据融合；
步骤5)将获得的最优估计值，按照指定的信息分配模式设置各子滤波器和主滤波器滤波估计值以及协方差矩阵。
所述水下无源组合导航系统建模及信息融合方法的进一步设计在于，步骤1)中γi满足信息守恒原则，即β1+…+βn+βm＝1，其中γi＝βi-1，βi表示表示信息分配系数。
所述水下无源组合导航系统建模及信息融合方法的进一步设计在于，步骤2)具体包括如下步骤：
步骤2-1)从重要密度函数p(xk|z0:k)采样得到粒子集
步骤2-2)根据式(5)计算各个粒子的权值；
其中，σk表示协方差
步骤2-3)根据式(6)将各粒子的权值归一化；
其中，
步骤2-4)根据式(7)估计后验概率密度p(xk|z0:k)的μk和协方差σk；
式中，m表示样本总数；
根据式(8)计算近似滤波概率分布；
p(xk|z0:k)≈n(xk；μk,σk)(8)
根据式(9)计算重要性密度函数
所述水下无源组合导航系统建模及信息融合方法的进一步设计在于，所述步骤4)中，按照式(1)、式(2)进行最优数据融合：
其中，pg是全局变量状态协方差矩阵、
所述水下无源组合导航系统建模及信息融合方法的进一步设计在于，所述步骤5)中，按照式(3)、式(4)重新设置各个子滤波器和主滤波器的滤波估计值及其协方差矩阵，计算得到：
其中，qi综合协方差矩阵、qii是第i子滤波器协方差矩阵、qm是主滤波器协方差矩阵、q是系统协方差矩阵。
所述水下无源组合导航系统建模及信息融合方法的进一步设计在于，所述步骤2)中，非线性子滤波器，采用改进高斯粒子滤波器。
本发明的优点如下：
本发明具有容易实现，能够为水下无源组合导航可靠性和准确性提供保证。具有容易实现的优点。
附图说明
图1为水下无源组合导航系统结构图。
图2为水下无源组合导航系统建模示意图。
具体实施方式
以下结合附图，对本发明的技术方案进行详细说明。
如图1,本实施例的本发明采用的水下航行器组合导航系统如图所示，主要由捷联惯性导航系统、多普勒计程仪、电子罗盘、声呐定位系统以及信息融合滤波器组成，捷联惯性导航系统、多普勒计程仪、电子罗盘、声呐定位系统分别与信息融合滤波器通信连接。各个导航系统提供相应的导航信息，将这些导航信息利用滤波的方法进行融合，进而得到更高精度的导航信息。
水下航行器系统以捷联惯导系统为主导航系统，测量水下航行器的位置、速度、姿态信息；多普勒计程仪为捷联惯导系统提供速度信息，电子罗盘提供航向信息，声呐定位系统提供位置信息，进行组合，进而提高了组合导航系统的定位精度，能够满足水下航行器高精度导航的需求。
首先建立系统状态方程包括如下环节：
sins状态方程的建立：捷联惯导系统误差模型的分析，取捷联惯导系统的状态变量为：
xsins＝[δlδλδvxδvytxφyφzεxεyεz]t
建立dvl状态方程的建立：取多普勒计程仪的状态变量为：
xdvl＝[δvdδδδc]t
得到多普勒计程仪状态方程为：
建立电子罗盘状态方程：取电子罗盘的状态变量为：
xec＝δψec
则电子罗盘的状态方程可以表示为：
建立声呐定位系统状态方程
在声呐定位系统中，接收机钟差和钟差漂移往往以未知数的形式给出，其误差模型可以表示为：
其中，δt表示接收机的钟差误差；δtr表示接收机钟差的漂移误差；λr表示接收机的相关时间；εr表示白噪声向量。
取钟差误差δt和钟差漂移误差δtr作为长基线定位系统的状态变量，可以得到：
xsn＝[δtδtr]t
则长基线定位系统的状态方程可以表示为：
建立系统量测方程包括如下环节：
建立sins/dvl量测方程：在sins/dvl组合导航系统中，多普勒计程仪可以提供载体的外速度信息，将惯导解算的速度信息与外速度信息作差，将速度差值作为组合导航系统的量测量，则系统的量测方程可以表示为：
建立sins/ec量测方程
在sins/ec组合导航系统中，电子罗盘可以获得载体的航向角信息，将这个航向角信息与惯导估计获得的航向角信息作差，并将航向角差值作为组合导航系统的量测量，其中电子罗盘的航向角可以表示为：ψe＝ψt+δψe。通过惯导解算得到的航向角可以表示为：ψi＝ψt+δψi。并得到sins/ec组合导航系统的量测方程为：
zie＝[ψi-ψe]＝[δψi-δψe]＝[01×6101×3-1]xie+vie
建立sins/sn量测方程：
在sins/sn组合导航中，将非线性伪距方程作为系统的量测量：
令系统的量测量z＝[r1m,r2m,…rim]，则量测方程为：
zil(t)＝h(xil(t))+vil(t)
在信息融合方案中，将惯性导航系统作为主导航系统，将惯导系统输出的速度、位置及姿态信息作为公共参考信息，将多普勒计程仪、电子罗盘、声呐定位系统作为子系统，分别建立各个系统滤波模型。sins/dvl、sins/ec构成线性子系统，可以采用线性卡尔曼滤波器进行滤波估计，而sins/sn构成非线性子系统，需要采用高斯粒子滤波器进行滤波估计。
对于sins/dvl子系统：
sins/dvl组合导航系统的状态方程为：
sins/dvl组合导航系统的量测方程为：
zid＝hidxid+vid
对于sins/ec子系统：
sins/ec子系统状态方程为：
sins/ec子系统量测方程为：
zie＝[01×4101×7-1]xie+vie
对于sins/sn子系统
sins/sn子系统组合导航系统的状态方程为：
sins/sn组合导航系统的量测方程：zis＝h(xis)+vid
在组合导航系统中，采用混合联邦滤波器实现多普勒计程仪、电子罗盘和声呐定位系统与惯导系统的组合。主滤波器和各个子滤波器之间信息交换的过程，结合混合联邦滤波器设计步骤，可以总结基于组合导航系统的混合联邦滤波算法如下：
信息分配：
其中，βi表示信息的分配系数，且
时间更新：
a.线性系统可以按照下式进行时间更新，包括主滤波器
b.非线性子系统根据分配的
量测更新：
a.线性子系统按照下列公式进行量测更新
b.非线性子系统在获得最新量测之后，根据公式
数据融合：
本实施例的具体实施过程为：
步骤1)将各子滤波器和主滤波器的初始估计协方差阵设置为组合导航系统初始信息的γi(i＝1,2,…,n,m)倍。γi满足信息守恒原则，即β1+…+βn+βm＝1，其中
步骤2)，各子滤波器和主滤波器独立完成时间更新过程。其中，线性子滤波器通过卡尔曼滤波器进行状态变量的时间更新过程，非线性子滤波器通过非线性算法的状态更新过程获得状态。非线性子滤波器通过高斯粒子非线性滤波算法进行更新。
该步骤2)具体步骤如下：
步骤2-1)从重要密度函数p(xk|z0:k)采样得到粒子集
步骤2-2)计算各个粒子的权值。
步骤2-3)将各粒子的权值归一化。
步骤2-4)根据下面公式估计后验概率密度p(xk|z0:k)的μk和协方差σk。
式中，m表示样本总数。
由上式计算得到的μk和协方差σk近似滤波概率分布为
p(xk|z0:k)≈n(xk；μk,σk)
对于高斯粒子滤波算法而言，其重要性密度函数
步骤3)各子滤波器根据最新的量测信息完成量测更新过程。其中，线性子滤波器通过卡尔曼滤波器进行量测更新，而非线性子滤波器通过非线性算法获得更新后的状态分布参数。
步骤4)得到各个子滤波器的局部估计值和主滤波器的估计值后，再进行最优数据融合；
最优数据融合按照下列公式计算得到
步骤5)将获得的最优估计值，按照指定的信息分配模式设置各子滤波器和主滤波器滤波估计值以及协方差矩阵。
按照指定的信息分配模式重新设置各个子滤波器和主滤波器的滤波估计值及其协方差矩阵，按照下列公式计算得到：
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。