ACTA Scientiarum Naturalium Universitatis Pekinensis
A Bayesian Estimation-based Algorithm for Geomagnetic Aided Inertial Navigation
LIU Yuefeng†, ZHENG Peichen
School of Earth and Space Sciences, Peking University, Beijing 100871; † E-mail: yuefengliu@pku.edu.cn
Abstract For existing algorithms are difficult to satisfy the applied requirements, a Bayesian estimation-based algorithm for geomagnetic aided inertial navigation is proposed for aerocraft navigation. The core idea is to convert position estimation to probability estimation. A position correction strategy is also provided in this algorithm. Simulation experiments were performed in which measured data were used. Results show that the algorithm has an average navigation accuracy of 89 meters and spend average 156 seconds for first correction the INS (inertial navigation system). In addition, the performance of the algorithm is not sensitivity to initial estimation errors. Key words Bayesian estimation; geomagnetic aided inertial navigation algorithm; probability estimation
惯性导航技术由于具有隐蔽性, 并能实现全天候三维连续定位, 已经成为各种载体(如飞机、导弹、舰船等)的重要导航定位手段。为了克服惯性导航的系统误差, 基于地形匹配、重力匹配和地磁匹配的辅助惯性导航技术应运而生。其中, 基于地形匹配的辅助导航技术起步较早, 也相对成熟。与地形测量(需要向外辐射信息)相比, 重力匹配和地磁匹配对重力和地磁的测量更加隐蔽, 并且不存在地形匹配在平原和海面等平坦区域高度受限的问题。与重力测量相比, 地磁特征量更加丰富。因此,基于地磁匹配的辅助导航技术日益受到重视, 成为导航领域的研究热点, 其中匹配解算算法的研究成果[1]尤为丰富。
地磁辅助惯性导航系统一般由惯性导航系统(INS)、地磁图数据库、地磁传感器和匹配解算计算机组成, 基本原理是根据惯导系统的位置输出,提取与该位置相关的地磁图数据, 然后将地磁传感器实时测量的地磁特征数据、提取的地磁图数据以及惯性导航系统的系统参量作为匹配解算算法的输入, 通过匹配解算形成导航输出, 并对惯导系统进行校正。有关地形辅助惯性导航系统的匹配解算算法主要有以 TERCOM 算法[2]为代表的相关性匹配算法
[3]和以 SITAN 算法 为代表的滤波算法两大类。由于原理的相似性, 在地磁辅助惯性导航算法研究中,常借鉴上述两类算法原理[47]。TERCOM 算法的基
本原理是, 将载体运动轨迹上的地磁特征测量序列与地磁图进行相似度匹配, 符合极值条件的位置作为输出。由于需要进行序列测量, 因此实时性受到影响, 且容易出现误匹配(虚假定位点)。SITAN 算法的基本原理是基于扩展卡尔曼滤波(Extend Kalman Filtering, EKF)框架, 将局部观测特征线性化(显然与实际情况有很大的出入)以建立观测方程, 并满足卡尔曼滤波对线性系统的要求, 因此会产生额外误差, 使得该算法的估计精度不高, 且容易导致滤波发散。
针对上述不足, 近年来, 基于无迹卡尔曼滤波(Unscented Kalman Filtering, UKF)[811]和粒子滤波[1215]的辅助惯性导航算法成为研究热点。与 EKF 一样, UKF 亦是针对非线性系统的一种次优滤波方法。由于 UKF 采用样本点(称为 σ 点)逼近状态方程的非线性特征, 因而与 EKF 直接将非线性状态方程线性化的方法相比, 理论上具有更高的估计精度。但是, 由于地磁特征空间分布的复杂性, 难以建立观测方程(主要由于地磁特征量与位置为非线性关系), 因而该类算法目前还难以进入实际应用。粒子滤波是一种基于蒙特卡罗方法和递推贝叶斯估计的滤波方法, 针对贝叶斯估计中求解后验概率密度遇到多重积分无法获得解析式的问题, 采用一系列带权随机样本(粒子)对后验概率密度进行近似, 从而得到状态的估计值。该方法目前面临如何确定重要性函数、粒子退化、重采样策略和计算量大等问 题, 是一种具有很好的前景, 但还处于不断完善过程中的针对强非线性系统的滤波方法。
面向飞行器的应用具有以下特点: 1) 由于各种原因(如在飞行航迹上不具备某些区域的地磁数据、某些区域地磁场不适合滤波等), 加上飞行器的速度高, 机动性大, 要求算法能够在较短时间内实现位置修正; 2) 飞行器处于高空, 地磁特征的空间变化比地球表面要平坦许多, 要求算法能够适应这一特点。贝叶斯估计可以有效地解决非线性系统的问题, 但在许多实际应用中存在两方面的困难。一是需要事先了解和确定各种状态参数的概率密度分布函数, 二是遇到多重积分时难以获得解析式。卡尔曼滤波正是在这样的背景下作为贝叶斯估计一个特例提出, 即针对线性系统的, 以最小方差为准则的最优估计。粒子滤波则是为解决难以获得解析式而提出的。针对非线性系统, 如果能够有效地回避或减少上述两方面的困难, 那么采用贝叶斯估计显然比 EKF 方法、UKF 方法和粒子滤波方法更加有效。本文正是基于这种思路, 面向飞行器(如飞机、导弹)实时滤波问题, 提出一种基于贝叶斯估计的地磁辅助惯性导航算法。
1 算法基本思路
本文符号说明见表 1。在任意时刻 t, 得到惯导的输出位置 LINS, 以
LINS为中心形成一个平面矩形区域即为估计区域。如果载体的真实位置落入该区域内, 那么, 通过估计真实位置与 LINS 的相对位置的关系, 就可以实现对真实位置的估计。
进一步, 对估计区域进行网格化细分, 整个区域离散为 2M×2N 个大小相同的网格(图 1), 以 θ(i, j) (i=0, 1, …, 2M−1; j=0, 1, …, 2N−1)表示。假设能够计算出 t 时刻载体真实位置落入所有 θ(i, j)的概率分布 π(θ) ={π[θ(i, j)] (i=0, 1, …, 2M−1; j=0, 1, …, 2N−1)}, 则可以采用期望估计法对载体的真实 LV位置进行估计。
显然, 每个 θ(i, j)的中心点 G(i, j)相对于 LINS 的位置偏移量 Offset(i, j)是已知的。因此, 在任意时刻 t, 根据 LINS可以计算获得 G(i, j)的平面位置以及对应的地磁图基准值 xm(i, j)。
根据 t 时刻地磁实时测量值 x 以及 xm(i, j)(i= 0, 1, …, 2M−1; j=0, 1, …, 2N−1), 对 π(θ)进行一次调整, 调整的方式采用贝叶斯公式(见 1.1 节)。在首次调整之前, 假设π(θ)均匀分布(初始化 π(θ)), 即真实位置落入所有θ(i, j)的概率相同。随着滤波的进行, 地磁测量样本的增加, 以及 π(θ)的迭代计算, π(θ)将不再是均匀分布, 而是在真实位置附近逐步会聚。本文定义概率平面会聚指数 C 对概率的会聚程度进行度量。仿真实验表明, 会聚程度越高,
对真实位置进行估计的精度越高。如果 C 达到预设值, 则根据 π(θ)对位置进行解算, 并对惯导进行位置修正。之后, 重新初始化 π(θ), 开始新一轮π(θ)的迭代调整。在下一次对惯导修正之前, 采用惯导输出作为导航输出。
上述算法思路基于这样一个前提: 在每一轮滤波期内(对应两次惯导位置修正之间的时期), 真实位置和惯导输出 LINS的位置关系相对稳定(即惯导新增的漂移量较小), 否则概率分布会聚到一定程度后便会发散, 从而降低估计精度。这就要求算法的误差收敛速度要比较快。仿真实验表明, 这一点可以做到。
要实现式(4)的计算, 剩下的问题就是如何设定初始的 π(θ), 即 π[θ(i, j)] (i=0, 1, …, 2M−1; j=0, 1, …, 2N−1), 以及如何计算 p[x|θ(i, j)] (i=0, 1, …, 2M−1; j=0, 1, …, 2N−1)。每一次对惯导进行位置修正后, 立即初始化π(θ), 即重新将π(θ)假设为均匀分布, 计算公式为
[ ( i , j)] 1/(2 M 2 N )。(5)式(4)中 p[x|θ(i, j)]的含义是, 如果 t 时刻真实位置落入 θ(i, j), 则测量样本为 x 的概率。本文假设地磁测量误差呈高斯分布, μ 和 σ 分别为均值和标准差。设 t 时刻 θ(i, j)中心对应地磁图的存储值为 xm(i, j), 则根据高斯分布函数可知, p[x|θ(i, j)]的计算公式为
通过式(4)实现 π(θ)的一步计算。
1.2 平面会聚指数与位置修正策略
滤波过程就是一个不断迭代的过程, 即每获取一个新的观测样本, 便根据式(4)对先验概率分布进行调整, 获得后验概率分布, 并作为下一次调整的先验概率分布。通过前面的数学推导可以预测, 随着迭代的不断进行, π(θ)将在真实位置附近会聚。反过来, 如果π(θ)在某个位置附近会聚, 会聚程度越高, 则该位置为载体真实位置的可能性就越大。仿真实验也证明了这一点。
一方面, 通过仿真实验发现, 随着迭代次数的增加, π(θ)由最初的平均分布朝着某个位置会聚。图 2 显示一个仿真案例在滤波过程中概率的平面分布及会聚效果。
为此, 本文定义平面会聚指数 C, 以描述概率分布的平面会聚程度:
式中, C(p)为概率 p 的平面会聚指数; S′(p)为估计区域内一个最小矩形区域的面积, 该矩形区域内所有网格的概率之和大于或等于 p; S为估计区域的总面积。特别地, 当 p=0.5 时, 式(7)表示为
显然, 当 π(θ)均匀分布时, S′=S/2, C=0。S′越小, C越趋近 1, 表示会聚程度越高。图 2给出各时刻的平面会聚指数。
另一方面, 仿真实验表明, 会聚指数与估计误差之间存在良好的相关性(图 3), 会聚指数与时间之间呈现指数关系(图 4)。
基于上述预测以及仿真实验的结果, 本文将平面会聚指数作为当前输出位置精度的度量, 并在航迹规划时预先设定, 作为对惯导进行位置修正的判别条件, 即
式中, C表示当前时刻的会聚指数, Cp表示预设的会聚指数。
1.3 算法流程
算法流程如图 5 所示。滤波器启动后, 设定估计区域大小和网格划分, 并开始执行以下步骤。
1) 根据式(5)设定所有网格的初始先验概率π[θ(i, j)], 将当前时刻设置为 t=t0; 对于任意时刻t=ti, 执行步骤 2~7。
2) 根据惯导输出位置 LINS, 计算每个网格中心点坐标, 并从地磁图中获取相应的地磁存储值。
3) 利用地磁测量值 x 以及步骤 2 的结果, 采用式(6)计算所有网格的条件概率 p[x|θ(i, j)]。
4) 根据所有网格的先验概率 π[θ(i, j)]和条件概
率 p[x|θ(i, j)], 采用式(4)计算所有网格的后验概率π[θ(i, j)|x], 并作为下一时刻的先验概率。
5) 计算概率平面会聚指数 C, 并判断是否满足位置修正条件。6) 如果否, 则 t=ti+1, 返回步骤 2。7) 如果是, 则进行位置解算, 并对惯导进行位置修正, 返回步骤 1。
2仿真分析2.1导航精度分析2.1.1 A区地磁数据
A 区的地磁数据为地面实测数据进行 5 km 高度延拓后获得的地磁异常网格数据, 较符合飞行器导航的实际应用情况。地磁图相关参数: A 区大小为 220 km×190 km; 数据网格大小为 500 m×500 m;异常值变化范围为最大197.04 nt, 最小−147.56 nt;平均梯度(按磁图网格统计)为东西 3.28 nt/km, 南北 2.62 nt/km。
2.1.2 仿真条件和算法参数
仿真模型包括惯导模型和地磁测量误差模型。惯导仿真采用捷联惯导模型, 地磁测量误差模型采用 Box Muller 法构建高斯分布随机数生成器[16]。数据条件包括地磁图、真实地磁场模拟数据和运动轨迹。真实地磁场数据的模拟方法是, 以地磁图网格点数据为骨架, 采用随机分形地形生成技术
[17]中的 Diamond-square 算法 在网格点之间生成随机起伏。运动轨迹采用随机生成的方式。
仿真条件参数如下: 惯导初始位置误差, X 为400 m, Y为 400 m; 初始姿态角误差为 0.01°, 0.01°, 0.01°; 地磁测量误差参数, 均值为 2 nt, 均方差为2 nt; 按照飞行速度 200 m/s, 随机生成 10 段轨迹;滤波周期为 0.5 s, 仿真实验重复次数为 50; 算法参数, 估计区域大小为1600 m×1600 m, 网格大小为50 m×50 m; 位置修正条件为 C=0.985。
2.1.3 结果分析
在上述仿真条件下, 结果显示首次对惯导进行位置修正的时间平均为 156 s, 在首次修正后, 导航输出的平均距离误差为89 m。图 6(a)为 10条轨迹×50 次仿真结果的平均误差曲线。图 6(b)为其中一条轨迹单次仿真的误差曲线, 该曲线反映本文算法的运行模式, 即滤波过程中, 始终以惯导输出作为导航输出。与此同时, 算法不断地进行概率分布的迭代计算, 在会聚指数满足预设条件时, 进行位置 解算, 并对惯导进行位置修正。
2.2 误差收敛性能分析
对于飞行器导航应用而言, 首次位置修正的时长直接决定算法的实用性。由算法原理可知, 算法的误差收敛速度直接影响首次位置修正的时间和精度。本文通过与 SITAN 算法进行比较来分析算法的误差收敛性能。由于滤波过程中的每一步实际上是对概率进行一次调整, 获得的仅为调整后的概率,只有在满足位置修正条件时, 才进行位置解算并修正惯导。为了方便分析收敛性能, 现在对迭代的每一步均进行位置解算, 并生成位置误差曲线, 且不涉及位置修正。
采用与 2.1 节相同的仿真条件, 本文算法参数如下: 估计区域大小为 1600 m×1600 m; 网格大小为 50 m×50 M。SITAN 算法初始估计参数如下: 初始位置估计误差, X 为 400 m; Y为 400 m; 初始速度估计误差, X 为 0 m/s, Y 为 0 m/s; 初始位置估计均方差, X 为 400 m, Y为 400 m, 并由此生成初始估计误差均方阵。上述设置保证了两种算法的初始误差相同。另外, SITAN 算法中的随机地形线性化采用平均取点法。
图 7 为两种算法的误差收敛曲线。结果显示:本文算法在 60, 120 和 180 s 时的误差均值分别为180, 95 和 71 m, 最小为 66 m, 对应时间为 235 s; SITAN 算法在 60, 120 和 180 s 时的误差均值分别为 470, 417 和 370 m, 最小为 244 m, 对应收敛时
间为 452 s。
2.3 初始估计敏感性分析
对于本文算法, 初始位置误差的估计体现在估计区域大小设置上。为此, 进行 3 组仿真实验, 估计区域大小分别设置为 1600 m×1600 m, 2000 m× 2000 m 和 2400 m×2400 m, 其余仿真条件与 2.3 节完全相同。结果显示, 误差收敛性能(图 8)和导航精度(图 9)对估计区域大小的敏感性均不显著。
2.4 地磁数据特征敏感性分析
选择另一区域 B 的地磁数据进行分析。B区的数据为地面实测磁场总强度网格数据, 相关参数:区域大小为 10 km×5 km; 数据网格大小为 100 m × 100 m; 总强度变化范围, 最大值为 53639.25 nt,最小值为 53396.13 nt; 平均梯度(按磁图网格统计)
为东西 17.63 nt/km, 南北 39.60 nt/km。
采用 2.1 节的仿真条件, 结果显示首次位置修正的平均时间为 6.6 s, 导航输出的平均误差为 42 m。可见, 导航区域地磁特征的起伏越大, 地磁图精度越高, 算法的实时性和导航精度越高。
3讨论与结论3.1讨论
估计区域的大小一般采用初始位置误差估计值的 4 倍。尽管本文算法对初始误差估计值的敏感性不显著, 但也不宜过小或过大, 以保证在首次修正惯导的时间内, 真实位置与惯导位置之差始终小于估计区域半边长为原则。
本文算法的前提是, 两次位置修正期间, 惯导输出位置与真实位置的关系相对稳定, 即真实位置总是处于某个网格附近。如果网格设置过小, 那么两次位置修正期间, 真实位置跨越的网格数就会过大, 反而导致会聚指数的发散, 降低估计精度。因此, 网格大小设置宜在对惯导漂移特性进行分析的基础上进行。一般情况下, 以大于两次修正期间惯导新增漂移量为宜。本文算法中概率会聚速度与地磁特征的起伏程度以及地磁图的精度密切相关, 在航迹规划时, 宜对导航区进行仿真分析, 确定合适的会聚指数作为位置修正条件。关于修正策略, 本文仅提出根据当前会聚指数进行判别, 规则较为简单。事实上, 本文算法还有进一步改进的空间, 例如在会聚指数与滤波时长之间进行优化, 以提高算法的鲁棒性。本文不再展开讨论。
本文仿真实验的滤波周期均设置为 0.5 s。实际上, 就目前的地磁仪和惯导系统而言, 可以达到的输出频率均高于上述滤波频率。因此, 在提高滤波频率的条件下, 有望进一步缩短首次位置修正所需时间, 以满足飞行器辅助导航的实时性需求。
3.2 结论
面向飞行器辅助导航的实际特点, 本文提出一种基于贝叶斯估计的地磁辅助惯性导航算法, 研制了软件仿真系统, 并基于实测数据进行了仿真实验,结论如下: 1) 提出算法的数学模型, 可行性得到仿真实验的验证; 2) 在实时性和导航精度方面, 比现有算法有较大改进; 3) 算法性能对初始估计敏感性不显著, 能够为实际应用带来方便; 4) 提出的位置修正策略是可行的, 但还有进一步优化的空间。