全源定位与导航的统一理论框架构建

戴海发，卞鸿巍，马 恒，王荣颖

(海军工程大学电气工程学院, 武汉 430033)

0 引言

由于应用场景的复杂性，采用单一方法的传统导航和定位系统，无法满足动态变化的多工况环境对准确性和可靠性的要求，多传感器导航定位越来越受到重视。全源定位与导航(All Source Positioning and Navigation, ASPN)是基于多传感器导航和定位的一种新的导航方式，其目标是为多种导航技术提供统一的方法和框架[1-6]。本质上，ASPN是一种全源的自适应导航方法，该方法能够实时地利用所有可用信息，并根据承载平台的动态变化计算出精确的导航结果。其研究目标是以最高的准确性提供实时、可靠的导航和定位服务，同时能根据环境和任务的需求，在系统中随时添加新的传感器或去除旧的传感器，并能根据每个传感器的性能和可用性的变化对组合模式作出自动调整。

为了实现全源定位与导航的目标，需要解决以下4个问题[7]：1)复杂性，即如何找到必要的专业知识整合所有的技术，如何在不侵犯知识产权的条件下整合来自不同厂家的技术；如何在不重新设计整个系统的前提下，融入新的技术和方法。2)背景自适应：即如何根据变化的工作环境和载体行为选择最佳的导航系统配置。3)模糊性，即如何处理多假设问题，包括测量的多值性以及特征匹配点的多值性。4)环境数据处理：即如何收集、分发、储存用于识别信号和环境特征的信息，并定义其原点和空间变量。上述的4个问题，每一个都可以延伸出一个研究方向，本文主要选择全源信息融合的复杂性问题作为切入点。

针对复杂性的问题，首先需要将不同的导航传感器进行分类整理，并且统一到相同的理论框架下。目前有不少文献[8-11]已经将当前的主要导航技术进行了整理和讨论，但是这些文献大多是根据原理或者测量对象进行分类，种类复杂繁多、缺少统一性，不利于相关技术人员全面地掌握这些技术，并将其应用于全源定位与导航领域。因此，本文提出了一种基于全源定位与导航的信息融合统一理论，借鉴了文献[12]的思想，将目前主要的导航技术分为与时间无关的技术和与时间相关的技术两大类，从前者可得到测量函数和测量模型，从后者可得到载体的运动模型。将两种模型结合起来，可以得到全源信息融合的基本方程，然后结合具体应用，选用贝叶斯估计理论框架中的合适算法估计出导航参数。

本文第1～3节分别讨论了与时间无关的定位、定姿、测速方法，从数学的角度将所有的方法分别用统一的位置、姿态、速度测量函数来表达，并且讨论了部分测量方法的测量误差。第4节讨论了与时间相关的航位、航姿推算方法及其典型应用；第5节，在测量函数和推算公式的基础上，构建了测量模型和运动模型，然后根据测量模型和运动模型，建立了全源信息融合的基本模型，并讨论了贝叶斯估计理论在全源定位与导航参数估计中的应用。

1 直接定位公式

定位，即确定载体的空间坐标位置。在本文中，将根据可识别的外部信息和同一时刻的多个测量值直接确定位置的方式称为直接定位方式。

1.1 位置函数

(1)

当得到多个测量值时，可以得到如下测量方程组

(2)

其中，N为观测量的个数。

根据定位原理的不同，位置函数有不同的表达形式。在直接定位方法中，主要包括基于距离、距离差、方位、水平角与高度角，以及多普勒频移等不同的位置函数。表1所示为几种典型的位置函数及其应用。

1.2 定位方式

由式(1)可知，根据位置函数是否相同可以将定位方式分成单一定位方式和综合定位方式。其中，单一定位方式满足h1=h2=…=hN，即观测量为同一个物理量；如果h1≠h2≠…≠hN，则为综合定位方式，其观测量可以是几个不相同的量。

表1 几种典型的位置函数

注：① arctan表示反正切函数；

1) 单一定位方式

单一定位方式采用同一个物理量的多个观测值，通过联立观测方程求解载体未知的位置矢量[5]。以二维平面定位方式为例，通过测量用户与已知目标的距离，然后以目标为圆心，以该距离为半径作圆，得到圆形的位置线。当获得2条位置线时，用户即位于其交点。不过1对位置圆相交于2个点，可以进一步通过先验信息或者第3个距离测量值确定其中正确的定位点。其观测量为距离，根据位置函数可以得到方程组：

(3)

在这里:

通过最小二乘法解该方程组，可以得到载体的2组位置解，然后根据先验信息或者第3个测量值确定有效解。

2) 综合定位方式

若f1≠f2≠…≠fN，即几个观测量为不同的量，此时无法通过单一的定位技术求解得到用户的位置解。但是，仍然可以通过融合这些信息进行定位[13]。

例如，通过测量载体到单一目标的距离d1和方位ψ1，可以得到如下的位置函数方程组：

(4)

在这里:

同样的，根据最小二乘法可以求得二维位置解。

2 直接定姿公式

姿态描述的是一个坐标系相对于另一个坐标系轴线的方向。在本文中，将根据同一时刻得到的多个矢量解算得到载体瞬时姿态的定姿方式，称为与时间无关的直接定姿方式。

2.1 姿态函数

假设Φβα=(φβα,θβα,ψβα)为载体坐标系α相对于参考坐标系β的姿态角，hi(Φβα)为Φβα可观测的姿态函数，考虑测量误差mΦi，实际观测矢量zΦi满足如下的姿态函数

zΦi=hi(Φβα)+mΦi,i=1,2，…,N

(5)

当得到多个测量矢量时，可以得到如下测量方程组

zΦ=hΦ(Φβα)+mΦ

(6)

表2所示为几种典型的姿态函数及其应用。

表2 几种典型的姿态函数

注：① 式中，n为导航坐标系，b为载体坐标系，φnb为滚动角，θnb为俯仰角，ψmb为磁航向角，Bm、γnm为磁通密度的幅值和倾角;

2.2 定姿方式

(7)

(8)

然后根据下式求解姿态角：

当观测矢量大于3个时，可以采用最小二乘法估计出载体余弦矩阵的元素。

3 直接定速公式

速度表示载体坐标系原点相对于参考坐标系原点和坐标轴的变化率。在本文中，将根据同一个时刻得到的单个或多个测量值解算得到载体瞬时速度解的定/测速方式，称为与时间无关的直接定/测速方式。

3.1 速度函数

(9)

表3所示为几种典型的速度函数及其应用。

当得到多个测量值时，可以得到如下测量方程组

(10)

式中：

注：①pd为动压强，ρ为海水密度，veb为舰船对水速度，vem为水流对地速度;

②B为磁场强度，L为导体长度;

3.2 定速方式

1)单测量值定速

基于单测量值的测速方式仅利用一个测量值就可以直接计算出载体的速度，包括水压计程仪、电磁计程仪等，但是这种测速方式得到的一般是标量速度。

2)多测量值定速

基于多个测量值进行测速的方式，利用同一时刻的多个传感器测量值，联立方程组解算载体的矢量速度。这种定速方式包括多普勒计程仪、基于声相关的测速等。

4 航位航速航姿推算公式

在本文中，将通过测量位置变化量，或者测量速度进行积分并与原位置叠加，从而得到当前位置的定位方式，称为与时间相关的航位推算方式。同理将通过测量速度变化量，或者测量加速度进行积分并与原速度叠加的测速方式，称为与时间相关的航速推算方式；将通过测量姿态变化量，或者测量角速度进行积分并与原姿态叠加，从而得到当前姿态的定姿方式，称为与时间相关的航姿推算方式。

4.1 角速度与姿态推算

载体旋转运动公式为

(11)

对式(11)积分,并假设在解算步长内角速率为常值，可简化得到:

(12)

式中，τ为解算步长，t为解算时刻，exp(·)表示矩阵指数运算。

将余弦矩阵转化为欧拉角，即可解得载体的姿态角。

4.2 加速度与速度、位置推算

将加速度定义为一个坐标系的原点位置相对于另一个坐标系原点和坐标轴的二次时间导数，可理解为在参考坐标系中作用在单位质量物体上的力。载体坐标系α相对于参考坐标系β的加速度在γ坐标系的投影可表示为

(13)

α坐标系相对β坐标系的速度在γ坐标系的投影表示为

(14)

根据式(13)、式(14)，可以通过加速度的积分并叠加原来的速度，得到载体的速度

(15)

同样的，载体的位置可以通过速度的积分并叠加原来的位置得到

(16)

该方法可用于航位推算、惯性导航等方面的应用。

(17)

根据式(17)，可以由连续时刻的位置矢量求微分得到载体的速度。该方法可用于卫导测速、里程计测速等方面的应用。

5 基于贝叶斯估计的融合方法

传统的定位导航一般单独使用直接定位中的一个方法，同时获得多个相同属性的测量值，然后解算出载体的位置、姿态或速度。为了提高定位导航的精度，一般会将直接定位解和航位推算解结合起来，采用滤波估计的方法得到精度更高的定位导航解。但是这种方法存在一定的缺陷，即在一些苛刻的环境下，可能无法同时得到相同属性的多个测量值，例如在城市的街区可观测的卫星可能少于3个，此时将导致卫星导航系统无法进行定位。然而，根据全源导航定位的思想，尽管某些定位技术无法正常使用，但是仍然可以利用一切可利用的信息进行定位和导航。例如，可以采用综合定位的方式，通过结合几种不同的直接定位技术和观测值解算出载体的导航解。更进一步，即使综合了所有可用的直接定位的信息，也无法求解出载体的导航解，仍然可以通过结合航位推算方法，估计出载体的导航解。按照这一思路，就可能产生许多新的导航解算方法。下面，本文将推导全源定位与导航系统的融合模型，并讨论基于贝叶斯估计的融合方法在该问题上的应用。

5.1 融合模型

结合式(2)、式(5)、式(9)，可以得到导航参数测量方程：

(18)

结合式(11)、式(13)、式(14)，可以得到载体运动的状态方程：

(19)

联立式(18)、式(19)并且离散化，得到导航参数的估计方程：

(20)

5.2 融合方法

定义状态量序列Xk=[x0,x1,…,xk]，观测量序列Zk=[z0,z1,…,zk]。记p(xk|Zk)表示xk的条件概率密度函数。根据贝叶斯准则，得到

(21)

式中，p(xk|Zk-1)的数学意义为系统从0时刻开始到k-1时刻为止，获得系统观测量序列Zk=[z0,z1,…,zk]时，下一时刻的系统状态将为xk的概率。这是根据以往的观测量事先预测下一时刻的系统状态，因此称为先验概率密度函数。

p(xk|Zk)的数学意义为系统从0时刻开始到k时刻为止，获得系统观测量序列Zk=[z0,z1,…,zk]时，当前k时刻的系统状态为xk的概率，因为系统观测量Zk实际上是系统状态xk的外在表现结果，所以这实际上是根据结果来分析原因，或者说根据已知系统输出来判断系统输入的概率，因此称为后验概率密度函数。

p(zk|xk)称为似然概率密度函数(Likelihood)，表示传感器的模型。

在此，将递推贝叶斯估计表述如下：

假定k-1时刻的后验概率密度p(xk-1|Zk-1)已知，通过时间更新，可求得k时刻的先验概率密度函数：

(22)

在k时刻获得新的观测信息zk后，进行量测更新，后验概率密度函数计算公式如下

(23)

实际上，随机滤波问题可以描述如下：已知初始概率密度p(x0)、p(xk|xk-1)、p(zk|xk)，滤波的目的就是从开始0时刻到当前k时刻以来，在所有的量测中估计当前系统的最佳状态。本质上，这是在估计后验概率密度p(xk|Zk)或p(Xk|Zk)。随机系统状态滤波估计问题本质上是一个求逆问题，即已知不同离散时刻测量到的测量值Zk=[z0,z1,…,zk]，和已知状态空间映射函数f:RNx⟹RNx以及量测空间映射函数h:RNx⟹RNy的条件下，计算系统状态的最优或者次优解。即

(24)

随机系统状态滤波估计问题可以由贝叶斯估计形式所描述，是一个十分广泛的问题。对于静态系统，可以采用权值平均法、最小二乘法、高斯牛顿法；如果先验信息已知，可以采用MLE、MMSE等方法。在动态系统中，Kalman滤波器可用于线性估计；对于非线性估计，可以采用EKF、UKF、GSF等方法[15-16]；对于非高斯噪声的条件，可以采用Markov链式蒙特卡罗方法[17]、PF[18]等方法；此外文献[19-23]提出了一种基于因子图的信息融合方法，该方法支持即插即用、易于扩展且支持非线性估计，被广泛应用于全源导航与定位系统中。

5.3 算例

假设测量误差服从高斯分布，那么式(24)可以简化为如下的非线性最小二乘问题：

(25)

由式(25)可知，并不需要得到完整的位置、姿态速度的测量信息，而只需要知道与这些状态量相关的部分测量信息，就可以利用高斯牛顿法等非线性优化方法，将这些信息融合起来得到最优的导航状态估计。

6 结论

全源定位与导航是导航定位技术的发展趋势，同时也面临许多挑战。为了实现全源导航的目标，需要解决复杂性、背景自适应、模糊性、环境数据处理等4个方面的问题。针对复杂性的问题，本文提出了一种基于全源定位与导航的信息融合统一理论。

通过本文的讨论，导航技术中的大多数测量技术都可以用统一的数学工具来表达，使得导航理论更加的简洁、易于掌握。同时通过这些数学表达式，可以从中寻求新的信息融合方法和最优估计算法，为下一步提高全源导航系统的可靠性、准确性、信息丰富性以及智能化奠定一定的基础。