全源定位與導航的統一理論框架構建

戴海發，卞鴻巍，馬 恒，王榮穎

(海軍工程大學電氣工程學院, 武漢 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個方面的問題。針對復雜性的問題，本文提出了一種基于全源定位與導航的信息融合統一理論。

通過本文的討論，導航技術中的大多數測量技術都可以用統一的數學工具來表達，使得導航理論更加的簡潔、易于掌握。同時通過這些數學表達式，可以從中尋求新的信息融合方法和最優估計算法，為下一步提高全源導航系統的可靠性、準確性、信息豐富性以及智能化奠定一定的基礎。