本发明涉及一种基于自适应采样粒子滤波的水下地形辅助导航方法，属于水下航行器自主导航领域。
背景技术：
目前，水下航行器主导航系统大多是惯性导航系统(ins)。ins通过惯性敏感器件测量加速度和姿态，经过对时间的积分来确定水下航行器的速度、位置以及航向。ins可以独立的利用载体自身设备进行导航，而不与外界发生联系，具有自主、连续、实时地提供导航信息的能力，但长时间工作时会产生较大的位置累计误差，这种误差如果不加以及时的修正，将很可能导致水下航行器的任务失败，甚至危及自身安全。全球定位系统(gps)是一种全方位、全天候、全时段、高精度的卫星导航系统，不存在定位误差随时间积累的问题，可以很好的用来修正ins定位误差。但由于海水对电磁波强烈的吸收作用，水下航行器只能定期的上浮到水面来使用gps进行定位修正。然而这种上浮修正方式对于航行器隐蔽性极其不利，而且当深海作业时，需要更多时间和能源消耗，特别当海面有冰层时，冰层的阻碍更使这种方式无法使用。相对于电磁信号，声信号可以在水下传播较远的距离，因此，在水下航行器无需浮出水面的情况下，可以利用声波发射机和信标(或应答器)来进行导航，即声学导航。声学导航定位系统如长基线、短基线和超短基线定位系统，不存在时间累计误差，定位精度较高，但其需要额外的声呐基阵布放或母船支持，大大降低了水下航行器的机动性和隐蔽性，且其作用区域也有一定的限制，易受人为或自然的干扰。
水下地形作为地球固有属性，可以作为一种很好的辅助导航信息源。当水下航行器航行时，可以利用自身配载的地形探测传感器实时获取水下地形数据信息，将这些数据信息与已存储的水下参考数字地形图进行匹配，可以估计水下航行器的精确位置，以此来修正ins累计误差。这种水下地形辅助导航技术误差不随航行时间或航行距离的增加而积累或发散，且航行器无需升出水面，可以边作业，边利用测量的地形信息进行辅助导航，有效避免了以上gps和声学导航存在的不足。作为一种非传统的导航方法，水下地形辅助导航很好地满足了水下航行器长时间、自主、安全以及高精度的导航要求，具有十分广阔的应用前景。
技术实现要素：
本发明的目的是为了提高水下航行器导航精度及实时性而提供一种基于自适应采样粒子滤波的水下地形辅助导航方法。
本发明的目的是这样实现的：步骤如下：
(1)以惯性导航位置误差为状态，以多波束测深声纳作为水下地形测量传感器，建立水下地形辅助导航的状态空间模型和量测模型；
(2)根据初始位置误差状态分布，产生初始粒子，由状态空间模型进行一步预测粒子更新，得到预测粒子集，并在一步预测粒子更新过程中，采用kld采样技术对粒子数目进行调整；
(3)当有多波束测量值到达时，联合压力深度计测得的深度、惯性导航指示的位置和水下参考数字地图插值函数，由量测模型进行粒子量测更新，得到量测更新后的粒子集及相应的权重；
(4)由量测更新后的粒子集及权重，采用均方误差最小化准则进行航行器位置误差的估计，将估计的误差对惯性导航指示位置进行修正。
本发明还包括这样一些结构特征：
1.步骤(1)中建立的水下地形辅助导航状态空间模型和量测模型为：
δxk＝δxk-1+wk
式中：δxk为k时刻航行器北向和东向位置误差，wk为状态过程噪声；zk为一个n维向量，表示多波束测量水深与压力传感器测量航行器距水面深度之和，n表示多波束一次测量波束个数；
2.步骤(2)具体是：
根据初始状态分布p(δx0)，产生初始粒子
初始化：令新的预测粒子集合为空集sk:＝φ，采样粒子总数n＝0，kld粒子个数nχ＝0，箱子数b＝0；
重采样：从带权值的粒子集
预测更新：由
判断预测粒子是否落入新的箱子，如果不是，则返回重采样步骤；
如果是，则增加箱子数b＝b+1，并计算所需的kld采样粒子数为：
其中，ε为设置的kld门限，δ为标准正态分布分位点；
判断采样粒子数和计算出的kld采样粒子数之间的大小关系，若采样粒子数小于kld采样计算粒子数n＜nχ，则返回到重采样步骤继续进行采样；否则结束循环，得到kld采样粒子集
3.步骤(3)中的量测更新是指结合多波束声纳测量斜距和波束角、压力深度计测量航行器距离水平面的深度、姿态传感器测量横摇和纵摇、惯性导航指示位置和地图插值函数，由量测模型进行量测更新，得到新的粒子集及相应的权值：
首先，由姿态传感器测量横摇和纵摇将多波束声纳测量斜距和波束角转换为载体坐标系下波束脚印的横向距、纵向距和航行器距离水底的高度，然后，加上压力深度计测量深度，构成实测的量测值；然后，由惯性导航指示位置、每个粒子所表示位置误差和姿态传感器测量航向将每个粒子在地图坐标系位置对应的多波束插值水深值求出；最后，利用实测的量测值和每个粒子对应的多波束插值水深值，计算似然函数
4.步骤(4)具体是：
根据最后的粒子集及其相应的归一化权值
与现有技术相比，本发明的有益效果是：本发明所述的一种基于自适应采样粒子滤波的水下地形辅助导航方法，利用多波束测深声纳测量水下地形信息，采用自适应采样粒子滤波对惯性导航位置误差进行估计，从而修正带误差的惯性导航指示位置，相比于其他传统的辅助导航方法，具有安全、自主、高精度、远距离导航的特点，且自适应采样粒子滤波相比于标准粒子滤波能够实时调整粒子数目的大小，有助于提高粒子滤波的有效性和导航的实时性。
附图说明
图1是水下地形辅助导航原理框图；
图2是自适应采样粒子滤波算法流程图；
图3是水下地形参考地形等值线图及仿真真实航迹；
图4是真实轨迹正下方对应的水深变化；
图5a和图5b分别是初始粒子为500自适应采样粒子滤波和标准粒子滤波导航误差和粒子数变化仿真试验结果对比；
图6a和图6b分别是初始粒子为100自适应采样粒子滤波和标准粒子滤波导航误差和粒子数变化仿真试验结果对比。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
结合图1至图6b，本发明的一种基于自适应粒子滤波的水下地形辅助导航方法，包括以下几个步骤：
步骤(1)以惯性导航位置误差为状态，以多波束测深声纳作为水下地形测量传感器，建立水下地形辅助导航的状态空间模型和量测模型；
步骤(2)根据初始位置误差状态分布，产生初始粒子，由状态空间模型进行一步预测粒子更新，得到预测粒子集，并在一步预测粒子更新过程中，采用kld采样技术对粒子数目进行调整；
步骤(3)当有多波束测量值到达时，联合压力深度计测得的深度、惯性导航指示的位置和水下参考数字地图插值函数，由量测模型进行粒子量测更新，得到量测更新后的粒子集及相应的权重；
步骤(4)由量测更新后的粒子集及权重，采用均方误差最小化准则进行航行器位置误差的估计，将估计的误差对惯性导航指示位置进行修正；
惯性导航作为基本的参考导航单元，输出当前时刻带误差的指示位置作为自适应采样粒子滤波器的输入，同时，多波束实时测量水深、压力深度计测量深度、姿态传感器测量姿态和地图插值函数也作为滤波器输入，通过设定合适的滤波参数，滤波输出估计位置误差。每当有新的量测值时，滤波器便可进行一次位置误差估计，其实现的是连续实时导航，如图1所示。
步骤(1)建立水下地形辅助导航状态空间模型和量测模型如下：
δxk＝δxk-1+wk
其中，δxk为k时刻航行器北向和东向位置误差，wk为状态过程噪声。zk为一个n维向量，表示多波束测量水深与压力传感器测量航行器距水面深度之和，n表示多波束一次测量波束个数；
根据上述建立的水下地形辅助导航模型可知，状态模型是线性的，由于地形的高度非线性，量测模型是非线性的，其是一个非线性状态估计问题。采用贝叶斯递推估计方法，得到位置误差状态的后验概率分布为：
p(δxk|z1:k-1)＝∫p(δxk|δxk-1)p(δxk-1z1:k-1)dδxk-1
其中，p(zk|δxk)为似然函数，p(δxk|δxk-1)为一步转移概率分布。
采用最小均方误差估计准则，则位置误差状态估计和协方差估计如下：
步骤(2)根据初始状态分布p(δx0)，产生初始粒子
初始化：令新的预测粒子集合为空集sk:＝φ，采样粒子总数n＝0，kld粒子个数nχ＝0，箱子数b＝0；
重采样：从带权值的粒子集
预测更新：用
判断：预测粒子是否落入新的箱子，如果不是，则返回重采样步骤；如果是，则增加箱子数b＝b+1，并计算所需的kld采样粒子数
其中，ε为设置的kld门限，δ为标准正态分布分位点；
判断采样粒子数和计算出的kld采样粒子数之间的大小，若采样粒子数小于kld采样计算粒子数n＜nχ，则返回到重采样步骤继续进行采样；否则结束循环，得到kld采样粒子集
步骤(3)的量测更新为：结合多波束声纳测量斜距和波束角、压力深度计测量航行器距离水平面的深度、姿态传感器测量横摇和纵摇、惯性导航指示位置和地图插值函数，由量测模型进行量测更新，得到新的粒子集及相应的权值。
首先，由姿态传感器测量横摇和纵摇将多波束声纳测量斜距和波束角转换为载体坐标系下波束脚印的横向距、纵向距以及航行器距离水底的高度，然后，加上压力深度计测量深度，构成实测的量测值；然后，由惯性导航指示位置、每个粒子所表示位置误差和姿态传感器测量航向将每个粒子在地图坐标系位置对应的多波束插值水深值求出；最后，利用实测的量测值和每个粒子对应的多波束插值水深值，计算似然函数
步骤(4)根据最后的粒子集及其相应的归一化权值
对自适应采样粒子滤波和标准粒子滤波在水下地形辅助导航应用进行了仿真试验分析。以船载多波束测深声呐测量某湖泊水下地形作为参考地形图，其水深在10到60米之间，地图网格分辨率为5米。假设航行器定深匀速航行，距离水平面深度5m，航行速度为2m/s，仿真真实轨迹为一条直线，采样间隔0.01s，总的航行时间为600s，图3给出了水下参考地形等值线图及真实轨迹。
采用简化的惯性导航误差模型，假设惯性导航速度误差为常值速度误差，北方向和东方向的速度误差均为常值0.1m/s，初始位置误差设置北向和东向均为50m，采样频率为100hz。多波束测深声纳测量仿真根据设定单ping波束扇面开角、波束数、真实位置以及地形插值函数来确定；采取等角度模式，根据波束开角和波束数确定各波束脚印对应横向距和纵向距，这里假设单ping的波束数127个，开角120度；然后，通过对应的波束脚印点通过双线性插值函数得到一ping的地图插值水深，在其基础上加上高斯白噪声，作为仿真的多波束测深声纳测量水深。图4给出了真实轨迹正下方的水深变化。
由于多波束测量波束脚印会重复在同一个网格中，因此，在进行地形匹配时，并不采用全部的测量水深点，而是采用均匀采样的方法对127个波束进行子采样，选取41个波束作为量测进行地形匹配导航。分别对自适应粒子滤波和标准粒子滤波两种方法进行50次蒙特卡洛(mc)仿真试验，采用位置均方根误差(rmse)来衡量导航精度，其计算如下：
其中m表示mc仿真次数，k表示滤波更新时刻。
结合图5，分析了自适应粒子滤波和标准粒子滤波在初始粒子数为500时的地形辅助导航性能。从图5a可以看出，当初始粒子数为500时，自适应粒子滤波和标准粒子滤波定位导航精度均较高，不会出现滤波发散现象，这是由于500粒子数足够很好地近似状态概率分布，从而自适应采样粒子滤波和标准粒子滤波均有较好的导航精度。但是从图5b可以看出，相比于标准粒子滤波，自适应的粒子滤波所用粒子数在滤波开始收敛时就下降，最终在滤波达到稳态时维持在100粒子数。较少的粒子数意味着对于每次迭代所花费的时间较少，这意味着自适应粒子滤波在保证同样导航性能情况下，使用更少的粒子，具有更好的导航实时性。
结合图6，分析了自适应粒子滤波和标准粒子滤波在初始粒子数100时的水下地形辅助导航误差和粒子数变化。从图6a可以看出，当初始粒子数为100时，标准粒子滤波导航精度较差，会出现发散的现象，自适应粒子滤波的导航精度能够维持很好的精度，且无发散现象。这是由于开始时初始粒子数目太少，不能够很好的近似状态的概率分布，标准的粒子滤波在整个滤波过程中粒子数一直保持不变，所以导致导航精度的下降甚至发散；而自适应采样粒子滤波能够快速的调整粒子数目，使得粒子数目足够近似状态概率分布，从而保证了导航不发散，提高了导航精度。从图6b也可以很明显的看出，在滤波初始时，自适应采样粒子滤波粒子数目迅速调整到较大数目，当滤波开始收敛时，逐渐下降，最终达到稳态维持在100个粒子左右，而标准粒子滤波粒子数一直维持100。因此，这说明自适应粒子滤波相比于标准粒子滤波，更有利于保证导航有效性和实时性。
表1.50次蒙特卡洛仿真试验处理时间
结合表1，分析了不同初始粒子数目情况下，自适应采样粒子滤波和标准粒子滤波50次蒙特卡洛仿真试验处理时间。从表中可以看出，随着初始粒子数目的增加，标准粒子滤波所用时间也成比例地增加，然而，对于自适应采样粒子滤波，其所用处理时间只是轻微的有所增加，这主要是由于自适应粒子滤波始终能够调整以最少的粒子数目来近似概率分布，这大大的降低了处理时间，却损失很少的导航精度。因此，相比于标准粒子滤波，自适应采样粒子滤波能够提高粒子滤波的有效性和导航实时性。
综上，本发明提供了一种基于自适应采样粒子滤波的水下地形辅助导航方法。水下航行器利用配载的多波束测深声纳测量水下地形，采用自适应采样粒子滤波方法将测量地形与水下参考数字地图进行比较，递推地估计水下航行器的位置误差，对惯性导航系统输出位置进行修正。首先，建立基于惯性导航位置误差的状态空间模型和基于多波束测深声纳的量测模型；然后，由初始状态分布和状态空间模型进行一步预测粒子更新，采用kld采样技术根据预测状态分布调整粒子数目，得到预测粒子集；当有多波束测量值到达时，联合压力深度计测量深度、惯性导航指示位置和水下参考数字地图插值函数，由量测模型进行粒子量测更新；最后，由量测更新后的粒子集及权重，采用均方误差最小化准则进行航行器位置误差的估计，将估计的误差对惯性导航指示位置进行修正。本发明能够在保证水下地形辅助导航精度的同时提高导航实时性。