基于强跟踪UPF的重力梯度辅助定位方法

姚剑奇

(中国电子科技集团公司第38研究所，合肥230088)

0　引言

重力辅助导航依靠重力测量的无源性可在校正INS误差的同时，保证水下潜器航行的隐蔽性，具有较高的军用价值[1-2]。

地球重力信息包含绝对重力与重力梯度，重力梯度因具有相关距离短[3]、变化更为剧烈、量测不受厄特弗斯效应影响、独立分量多等优点，可获得更优的匹配定位效果。但因重力梯度变化剧烈，在局部区域，呈现较强的非线性，所以基于递推滤波的重力梯度匹配定位属于非线性最优估计问题。

因目前重力辅助定位精度与卫星定位尚有差距，且水下潜器相对飞行器的机动能力较弱，无法在短时间内完成使系统误差完全可观测的机动模式[4]。根据文献[5]的研究，基于滤波的重力定位系统对惯性器件常值误差的估计能力较弱，且滤波系统采用的INS误差方程存在线性化误差。以上因素使滤波匹配系统存在模型偏差，影响长时间匹配时的定位精度。

本文在给出强跟踪滤波(Strong Trace Filter，STF)一般形式的基础上，将其引入无迹粒子滤波(Unscented Particle Filter，UPF)，形成强跟踪无迹粒子滤波(Strong Trace Unscented Particle Filter，STUPF)辅助定位算法，从而有效应对重力梯度的强非线性分布及系统模型偏差对定位的影响。经仿真验证，STUPF算法的定位精度较高，定位效果稳定且运算量较小。

1　UPF算法原理

设非线性系统满足如下状态空间模型

xk+1=fk(xk)+Γkwk,zk=hk(xk)+vk

(1)

其中，xk∈Rn、zk∈Rm为状态量及量测量；wk、vk为过程及量测噪声；fk(xk)、hk(xk)为状态方程和观测方程；Γk为噪声驱动矩阵。由式(1)的Markov性质得xk的后验概率密度为：

p(x0∶k/z1∶k)=p(zk/xk)p(x0∶k/z1∶k-1)/p(zk/z1∶k-1)

p(x0∶k/z1∶k-1)=p(xk/xk-1)p(x0∶k-1/z1∶k-1)

(2)

PF存在粒子退化及样本贫化问题。UPF[7]是重视重要性密度函数选取的改进PF方法，其在wk～N(0,Qk)、vk～N(0,Rk)且二者不相关的假设下，由无迹Kalman滤波(Unscented Kalman Filter，UKF)获得重要性密度函数所需参数的改进PF方法。因UKF获取的重要性密度函数结合了最新的量测信息且更接近状态真实概率分布，因此UPF可有效缓解粒子退化问题，且所需的粒子数较少，计算量较小。当系统存在模型偏差时，随滤波进行，量测信息对状态估计的作用减弱，滤波精度降低甚至发散。

2　STUPF算法

2.1　算法原理

STF是针对系统模型偏差及状态突变的有效方法，其根据残差信息调节预测协方差阵中的渐消因子，达到调节增益阵，增强滤波系统状态跟踪能力。根据正交性定理[8]，当系统模型匹配，滤波增益最优时，有式(3)近似成立，其中γk为k时刻残差信息。

(3)

(4)

当存在模型偏差时，式(3)不成立，可引入渐消因子使残差序列强制正交，保持滤波器跟踪状态的能力。引入渐消因子后的预测方差阵为：

(5)

将式(5)代入式(3)，并采用文献[8]的近似计算方法可得λk的表达式为：

(6)

(7)

(8)

设Qk为对称正定阵，可得

(9)

由式(7)～式(9)可得Nk、Mk的等价描述：

(10)

对式(5)进行变换后，可得式(11)成立

(11)

式(6)、式(10)及式(11)构成了STF的一般形式。因此可将STF引入UPF中获取重要性密度函数的UKF过程，得到STUPF算法。

2.2　算法步骤

由以上对STUPF所涉及理论的阐述，可得STUPF算法流程如下，其中NS为粒子总数：

初始化：Fori=1…NS

(12)

滤波过程：k≥1

(2)更新权值：按式(13)迭代更新粒子权值。

(13)

(3)根据有效抽样尺度[5]判断是否重采样。

(14)

3　仿真实验分析

重力梯度滤波匹配系统的状态量取载体的经/纬度位置误差及东/西向速度误差，状态及量测方程如下所示：

Xk+1=Φk+1,kXk+Γkwk

Zk=hk(Xk)+vk

(15)

Φk+1,k=I+TFk,Γk=T(I+TFk/2)

F的具体形式见文献[10]，因篇幅原因，此处不具体给出。Zk取重力梯度向量中的5维独立分量(Txx,Txy,Txz,Tyy,Tyz)，hk(Xk)为Xk对应位置处的梯度量测值。

以某INS的一次真实海事测试数据进行匹配实验，INS数据频率为1帧/s，数据记录时间8h，惯性器件精度：陀螺常值漂移0.01(°)/h，加速度计零位漂移50μg。根据记录数据生成的INS航迹及真实航迹如图1所示。

匹配实验采用的梯度基准图由文献[11]中的方法，由自由空间重力异常数据生成，其Txx、Txy分量如图2所示(分辨率0.5′)。

匹配实验中，设量测误差为标准差为1E的白噪声，滤波周期1min。以标准PF、高斯粒子滤波(Gaussian Particle Filter，GPF)、STUPF进行实验，三种算法粒子数分别为10000、3000、100。每种算法各进行20次独立实验以统计均方根误差(Root Mean Square Error, RMSE)，RMSE定义如下

(16)

RMSE统计数据及各算法单航程平均运行时间MRT如表1所示。

表1　RMSE统计数据及MRT

由表1及图3可知，标准PF运算量最大，匹配精度较差；GPF、STUPF的匹配稳定性近似，但STUPF定位精度略高，且运算量较小，可见STUPF具有一定优势。

4　结论

本文在给出STF一般形式的基础上，将其引入UPF，形成STUPF辅助定位算法，根据残差信息实时调整系统的状态跟踪能力，从而有效应对重力梯度辅助定位中，重力梯度的强非线性分布及系统模型偏差对定位的影响。基于真实INS数据的匹配仿真实验表明，STUPF可获得较好的定位效果，且相对标准PF、GPF算法而言，该算法在定位的精度、稳定性及运算量上具有一定优势。

[1]Bishop G C. Gravitational field maps and navigational errors[J]. IEEE Journal of Oceanic Engineering, 2002, 27(3): 726-737.

[2]崔晨风, 王涛. 重力匹配辅助导航误匹配消除方法研究[J]. 测绘通报, 2013(3):14-17.

[3]Metzger E H, Jircitano A. Inertial navigation performance improvement using gravity gradient matching techniques[J]. Journal of Spacecraft and Rockets, 1976, 13(6): 323-324.

[4]吴美平, 胡小平. 捷联惯导系统误差状态可观性分析[J]. 宇航学报, 2002, 23(2): 54-57.

[5]Prokhorov Y G. Observability of gravimeter-aided inertial navigation systems[J]. Journal of Guidance, Control, and Dynamics, 1995, 18(6): 1416-1419.

[6]Arulampalam M S, Maskell S, Gordon N, et al. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J]. IEEE Transactions on Signal Processing, 2002, 50(2): 174-188.

[7]Cheng Q, Bondon P. A new unscented particle filter[C]// IEEE International Conference on Acoustics, Speech and Signal Processing(ICASSP). IEEE, 2008: 3417-3420.

[8]周东华, 席裕庚, 张钟俊. 一种带多重次优渐消因子的扩展卡尔曼滤波器[J].自动化学报, 1991, 17(6): 689-695.

[9]秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安: 西北工业大学出版社, 1998.

[10]王文晶. 基于重力和环境特征的水下导航定位方法研究[D]. 哈尔滨: 哈尔滨工程大学, 2009: 71-75.

[11]钱东, 刘繁明, 李艳, 等. 导航用重力梯度基准图构建方法的比较研究[J]. 测绘学报, 2011, 40(6): 736-744.