第3期 韩月，等：基于改进粒子滤波的AUV海底地形辅助定位方法 ·555· 状态估计： 方差估计： p= ∑∑wx-x-) 虽然基于重采样技术的P℉算法可避免粒子 退化现象，但重采样会造成高权值粒子被多次复 (a)海底地形测量 制，从而损失粒子集的多样性，即出现“粒子贫 化”现象。为了弥补重采样过程中的多样性损失 一多波束实测地形 问题，对重采样后粒子进行重新分布，在地形辅 0 DTM 助定位中引入辅助采样技术。 20 1.3辅助重采样技术 40 辅助采样是指是在重采样后的父代粒子的基 础上，通过父代粒子的先验PDF进行再次采样。 -8 辅助采样后粒子是融合了父代先验知识的重新分 0400300200100 布的粒子，可有效提高粒子多样性，使得粒子分 北向坐标10(mgid 500 100200300400 东向坐标/10mgid) 布结果更接近于真实PDF。辅助重采样技术的实 现方法如下： (b)匹配定位搜索 1)计算粒子滤波中的有效粒子个数Ner,Nem 图1基于多波束测深的AUV海底地形辅助导航示意 的计算公式为 Fig.1 Schematic diagram of terrain-aided navigation us ing multi-beam sounding (6) =1 12PF地形辅助定位算法 2)若Nem小于设定的重采样阈值Nh,从粒子 P℉算法的本质是将连续积分运算变为随机 集中根据重要性权值重新采样得到新的粒子集 样本集的求和运算，即系统状态的后验PDF可用 {x,i=1,2,…,N,记录其父代粒子及其权值的集 一组在状态空间内随机抽取的加权粒子表示为 合j=1,2…,M,以及每个父代粒子复制 1 2-9 的次数然=1.2…，M。其中M为父代粒子 pXIY)≈ 的个数。 式中N,为抽取加权粒子的个数。PF的关键步骤 3)根据每个父代粒子的分布p(x),重新采样 是通过引入有权值的粒子集{X,i=1,2,…,N,并 次，计算采样后粒子的权值： 根据观测值更新粒子的权重，估计系统的状态。 = p) 研究表明：随着迭代次数增加，粒子权值的方差 pyx) 式中=E(x-),i=1,2,…,即为采样后的均 随之增大，将产生粒子退化现象。为了消除粒子 值，归一化所有权值w=1/N。 退化影响，P℉通常引入重采样技术，复制高权值 粒子，并淘汰低权值粒子，更新权值后的PDF可 2地形辅助定位算法流程 表示为 引入辅助的改进粒子滤波(improved particle p(XIY)≈ w6(X,-X) filter,.PF)算法步骤如下： 1)初始化：t=0,根据p(xo)的分布采样得到 式中w为第i个粒子在1时刻的权值，其计算公 粒子数为N的粒子集1，i=1,2,…,N,粒子的 式为 初始重要性权值为w=1N; w=wPG)p() 2)重要性权值计算：t=t+1,根据先验PDF采 q(xxiy) (5) 样得到x~q(x-,i=1,2,…,N,根据式（⑤计算 粒子滤波的状态估计和方差估计为 重要性权值w100 200 100 200 −80 水深值 −60 /m 北向坐标/10(m·grid −1 ) 东向坐标/10(m·grid −1 ) DTM 多波束实测地形 −40 −20 0 300 400 500 300 400 500 (b) 匹配定位搜索 (a) 海底地形测量 图 1 基于多波束测深的 AUV 海底地形辅助导航示意 Fig. 1 Schematic diagram of terrain-aided navigation us￾ing multi-beam sounding 1.2 PF 地形辅助定位算法 PF 算法的本质是将连续积分运算变为随机 样本集的求和运算，即系统状态的后验 PDF 可用 一组在状态空间内随机抽取的加权粒子表示为 p(Xt |Yt) ≈ 1 Ns ∑Ns i=1 δ(Xt − X i t ) Ns { X i t } ,i = 1,2,··· ,Ns 式中 为抽取加权粒子的个数。PF 的关键步骤 是通过引入有权值的粒子集 ，并 根据观测值更新粒子的权重，估计系统的状态。 研究表明：随着迭代次数增加，粒子权值的方差 随之增大，将产生粒子退化现象。为了消除粒子 退化影响，PF 通常引入重采样技术，复制高权值 粒子，并淘汰低权值粒子，更新权值后的 PDF 可 表示为 p(Xt |Yt) ≈ ∑Ns i=1 w i t δ(Xt − X i t ) w i 式中 t 为第 i 个粒子在 t 时刻的权值，其计算公 式为 w i t = w i t−1 p ( zt |x i t−1 ) p ( x i t |x i t−1 ) q ( x i t |x i t−1 , yt ) (5) 粒子滤波的状态估计和方差估计为 状态估计： x˜t = 1 Nj N j ∑c i=1 w i t x i t 方差估计： pt = ∑Nj j=1 N j ∑c i=1 w i t (x i t − x˜t)(x i t − x˜t) T 虽然基于重采样技术的 PF 算法可避免粒子 退化现象，但重采样会造成高权值粒子被多次复 制，从而损失粒子集的多样性，即出现“粒子贫 化”现象。为了弥补重采样过程中的多样性损失 问题，对重采样后粒子进行重新分布，在地形辅 助定位中引入辅助采样技术。 1.3 辅助重采样技术 辅助采样是指是在重采样后的父代粒子的基 础上，通过父代粒子的先验 PDF 进行再次采样。 辅助采样后粒子是融合了父代先验知识的重新分 布的粒子，可有效提高粒子多样性，使得粒子分 布结果更接近于真实 PDF。辅助重采样技术的实 现方法如下： 1) 计算粒子滤波中的有效粒子个数 Neff ，Neff 的计算公式为 Neff = 1/ ∑Ns i=1 ( w i t )2 (6) Neff Nth { x ∗i t } ,i = 1,2,··· ,N { x j t ,w j t }M j=1 , j = 1,2,··· , M { N j c }M j=1 , j = 1,2,··· , M 2) 若 小于设定的重采样阈值 ，从粒子 集中根据重要性权值重新采样得到新的粒子集 ，记录其父代粒子及其权值的集 合 ，以及每个父代粒子复制 的次数 。其中 M 为父代粒子 的个数。 p(x j i ) N j c 3) 根据每个父代粒子的分布 ，重新采样 次，计算采样后粒子的权值： w j t = p(yt |x i t−1 ) p(yt |x ∗ t ) x ∗ t = E(xt |x i t−1 ) i = 1,2,··· ,N j c w i t = 1/N 式中 ， 即为采样后的均 值，归一化所有权值 。 2 地形辅助定位算法流程 引入辅助的改进粒子滤波 (improved particle filter, IPF) 算法步骤如下： t = 0 p(x0) { x i 0 }N i=1 ,i = 1,2,··· ,N w i 0 = 1/N 1) 初始化： ，根据 的分布采样得到 粒子数为 N 的粒子集 ，粒子的 初始重要性权值为 ； t = t+1 x i t ∼ q ( xt |x i t−1 , yt−1 ) i = 1,2,··· ,N w i t 2) 重要性权值计算： ，根据先验 PDF 采 样得到 ， ，根据式(5)计算 重要性权值 ； 第 3 期 韩月，等：基于改进粒子滤波的 AUV 海底地形辅助定位方法 ·555·