戴海發(fā)，卞鴻巍，馬 恒，王榮穎

(海軍工程大學電氣工程學院, 武漢 430033)

0 引言

由于應用場景的復雜性，采用單一方法的傳統(tǒng)導航和定位系統(tǒng)，無法滿足動態(tài)變化的多工況環(huán)境對準確性和可靠性的要求，多傳感器導航定位越來越受到重視。全源定位與導航(All Source Positioning and Navigation, ASPN)是基于多傳感器導航和定位的一種新的導航方式，其目標是為多種導航技術(shù)提供統(tǒng)一的方法和框架[1-6]。本質(zhì)上，ASPN是一種全源的自適應導航方法，該方法能夠?qū)崟r地利用所有可用信息，并根據(jù)承載平臺的動態(tài)變化計算出精確的導航結(jié)果。其研究目標是以最高的準確性提供實時、可靠的導航和定位服務，同時能根據(jù)環(huán)境和任務的需求，在系統(tǒng)中隨時添加新的傳感器或去除舊的傳感器，并能根據(jù)每個傳感器的性能和可用性的變化對組合模式作出自動調(diào)整。

為了實現(xiàn)全源定位與導航的目標，需要解決以下4個問題[7]：1)復雜性，即如何找到必要的專業(yè)知識整合所有的技術(shù)，如何在不侵犯知識產(chǎn)權(quán)的條件下整合來自不同廠家的技術(shù)；如何在不重新設計整個系統(tǒng)的前提下，融入新的技術(shù)和方法。2)背景自適應：即如何根據(jù)變化的工作環(huán)境和載體行為選擇最佳的導航系統(tǒng)配置。3)模糊性，即如何處理多假設問題，包括測量的多值性以及特征匹配點的多值性。4)環(huán)境數(shù)據(jù)處理：即如何收集、分發(fā)、儲存用于識別信號和環(huán)境特征的信息，并定義其原點和空間變量。上述的4個問題，每一個都可以延伸出一個研究方向，本文主要選擇全源信息融合的復雜性問題作為切入點。

針對復雜性的問題，首先需要將不同的導航傳感器進行分類整理，并且統(tǒng)一到相同的理論框架下。目前有不少文獻[8-11]已經(jīng)將當前的主要導航技術(shù)進行了整理和討論，但是這些文獻大多是根據(jù)原理或者測量對象進行分類，種類復雜繁多、缺少統(tǒng)一性，不利于相關(guān)技術(shù)人員全面地掌握這些技術(shù)，并將其應用于全源定位與導航領(lǐng)域。因此，本文提出了一種基于全源定位與導航的信息融合統(tǒng)一理論，借鑒了文獻[12]的思想，將目前主要的導航技術(shù)分為與時間無關(guān)的技術(shù)和與時間相關(guān)的技術(shù)兩大類，從前者可得到測量函數(shù)和測量模型，從后者可得到載體的運動模型。將兩種模型結(jié)合起來，可以得到全源信息融合的基本方程，然后結(jié)合具體應用，選用貝葉斯估計理論框架中的合適算法估計出導航參數(shù)。

本文第1～3節(jié)分別討論了與時間無關(guān)的定位、定姿、測速方法，從數(shù)學的角度將所有的方法分別用統(tǒng)一的位置、姿態(tài)、速度測量函數(shù)來表達，并且討論了部分測量方法的測量誤差。第4節(jié)討論了與時間相關(guān)的航位、航姿推算方法及其典型應用；第5節(jié)，在測量函數(shù)和推算公式的基礎(chǔ)上，構(gòu)建了測量模型和運動模型，然后根據(jù)測量模型和運動模型，建立了全源信息融合的基本模型，并討論了貝葉斯估計理論在全源定位與導航參數(shù)估計中的應用。

1 直接定位公式

定位，即確定載體的空間坐標位置。在本文中，將根據(jù)可識別的外部信息和同一時刻的多個測量值直接確定位置的方式稱為直接定位方式。

1.1 位置函數(shù)

(1)

當?shù)玫蕉鄠€測量值時，可以得到如下測量方程組

(2)

其中，N為觀測量的個數(shù)。

根據(jù)定位原理的不同，位置函數(shù)有不同的表達形式。在直接定位方法中，主要包括基于距離、距離差、方位、水平角與高度角，以及多普勒頻移等不同的位置函數(shù)。表1所示為幾種典型的位置函數(shù)及其應用。

1.2 定位方式

由式(1)可知，根據(jù)位置函數(shù)是否相同可以將定位方式分成單一定位方式和綜合定位方式。其中，單一定位方式滿足h1=h2=…=hN，即觀測量為同一個物理量；如果h1≠h2≠…≠hN，則為綜合定位方式，其觀測量可以是幾個不相同的量。

表1 幾種典型的位置函數(shù)

注：① arctan表示反正切函數(shù)；

1) 單一定位方式

單一定位方式采用同一個物理量的多個觀測值，通過聯(lián)立觀測方程求解載體未知的位置矢量[5]。以二維平面定位方式為例，通過測量用戶與已知目標的距離，然后以目標為圓心，以該距離為半徑作圓，得到圓形的位置線。當獲得2條位置線時，用戶即位于其交點。不過1對位置圓相交于2個點，可以進一步通過先驗信息或者第3個距離測量值確定其中正確的定位點。其觀測量為距離，根據(jù)位置函數(shù)可以得到方程組：

(3)

在這里:

通過最小二乘法解該方程組，可以得到載體的2組位置解，然后根據(jù)先驗信息或者第3個測量值確定有效解。

2) 綜合定位方式

若f1≠f2≠…≠fN，即幾個觀測量為不同的量，此時無法通過單一的定位技術(shù)求解得到用戶的位置解。但是，仍然可以通過融合這些信息進行定位[13]。

例如，通過測量載體到單一目標的距離d1和方位ψ1，可以得到如下的位置函數(shù)方程組：

(4)

在這里:

同樣的，根據(jù)最小二乘法可以求得二維位置解。

2 直接定姿公式

姿態(tài)描述的是一個坐標系相對于另一個坐標系軸線的方向。在本文中，將根據(jù)同一時刻得到的多個矢量解算得到載體瞬時姿態(tài)的定姿方式，稱為與時間無關(guān)的直接定姿方式。

2.1 姿態(tài)函數(shù)

假設Φβα=(φβα,θβα,ψβα)為載體坐標系α相對于參考坐標系β的姿態(tài)角，hi(Φβα)為Φβα可觀測的姿態(tài)函數(shù)，考慮測量誤差mΦi，實際觀測矢量zΦi滿足如下的姿態(tài)函數(shù)

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

(5)

當?shù)玫蕉鄠€測量矢量時，可以得到如下測量方程組

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

(6)

表2所示為幾種典型的姿態(tài)函數(shù)及其應用。

表2 幾種典型的姿態(tài)函數(shù)

注：① 式中，n為導航坐標系，b為載體坐標系，φnb為滾動角，θnb為俯仰角，ψmb為磁航向角，Bm、γnm為磁通密度的幅值和傾角;

2.2 定姿方式

(7)

(8)

然后根據(jù)下式求解姿態(tài)角：

當觀測矢量大于3個時，可以采用最小二乘法估計出載體余弦矩陣的元素。

3 直接定速公式

速度表示載體坐標系原點相對于參考坐標系原點和坐標軸的變化率。在本文中，將根據(jù)同一個時刻得到的單個或多個測量值解算得到載體瞬時速度解的定/測速方式，稱為與時間無關(guān)的直接定/測速方式。

3.1 速度函數(shù)

(9)

表3所示為幾種典型的速度函數(shù)及其應用。

當?shù)玫蕉鄠€測量值時，可以得到如下測量方程組

(10)

式中：

注：①pd為動壓強，ρ為海水密度，veb為艦船對水速度，vem為水流對地速度;

②B為磁場強度，L為導體長度;

3.2 定速方式

1)單測量值定速

基于單測量值的測速方式僅利用一個測量值就可以直接計算出載體的速度，包括水壓計程儀、電磁計程儀等，但是這種測速方式得到的一般是標量速度。

2)多測量值定速

基于多個測量值進行測速的方式，利用同一時刻的多個傳感器測量值，聯(lián)立方程組解算載體的矢量速度。這種定速方式包括多普勒計程儀、基于聲相關(guān)的測速等。

4 航位航速航姿推算公式

在本文中，將通過測量位置變化量，或者測量速度進行積分并與原位置疊加，從而得到當前位置的定位方式，稱為與時間相關(guān)的航位推算方式。同理將通過測量速度變化量，或者測量加速度進行積分并與原速度疊加的測速方式，稱為與時間相關(guān)的航速推算方式；將通過測量姿態(tài)變化量，或者測量角速度進行積分并與原姿態(tài)疊加，從而得到當前姿態(tài)的定姿方式，稱為與時間相關(guān)的航姿推算方式。

4.1 角速度與姿態(tài)推算

載體旋轉(zhuǎn)運動公式為

(11)

對式(11)積分,并假設在解算步長內(nèi)角速率為常值，可簡化得到:

(12)

式中，τ為解算步長，t為解算時刻，exp(·)表示矩陣指數(shù)運算。

將余弦矩陣轉(zhuǎn)化為歐拉角，即可解得載體的姿態(tài)角。

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

將加速度定義為一個坐標系的原點位置相對于另一個坐標系原點和坐標軸的二次時間導數(shù)，可理解為在參考坐標系中作用在單位質(zhì)量物體上的力。載體坐標系α相對于參考坐標系β的加速度在γ坐標系的投影可表示為

(13)

α坐標系相對β坐標系的速度在γ坐標系的投影表示為

(14)

根據(jù)式(13)、式(14)，可以通過加速度的積分并疊加原來的速度，得到載體的速度

(15)

同樣的，載體的位置可以通過速度的積分并疊加原來的位置得到

(16)

該方法可用于航位推算、慣性導航等方面的應用。

(17)

根據(jù)式(17)，可以由連續(xù)時刻的位置矢量求微分得到載體的速度。該方法可用于衛(wèi)導測速、里程計測速等方面的應用。

5 基于貝葉斯估計的融合方法

傳統(tǒng)的定位導航一般單獨使用直接定位中的一個方法，同時獲得多個相同屬性的測量值，然后解算出載體的位置、姿態(tài)或速度。為了提高定位導航的精度，一般會將直接定位解和航位推算解結(jié)合起來，采用濾波估計的方法得到精度更高的定位導航解。但是這種方法存在一定的缺陷，即在一些苛刻的環(huán)境下，可能無法同時得到相同屬性的多個測量值，例如在城市的街區(qū)可觀測的衛(wèi)星可能少于3個，此時將導致衛(wèi)星導航系統(tǒng)無法進行定位。然而，根據(jù)全源導航定位的思想，盡管某些定位技術(shù)無法正常使用，但是仍然可以利用一切可利用的信息進行定位和導航。例如，可以采用綜合定位的方式，通過結(jié)合幾種不同的直接定位技術(shù)和觀測值解算出載體的導航解。更進一步，即使綜合了所有可用的直接定位的信息，也無法求解出載體的導航解，仍然可以通過結(jié)合航位推算方法，估計出載體的導航解。按照這一思路，就可能產(chǎn)生許多新的導航解算方法。下面，本文將推導全源定位與導航系統(tǒng)的融合模型，并討論基于貝葉斯估計的融合方法在該問題上的應用。

5.1 融合模型

結(jié)合式(2)、式(5)、式(9)，可以得到導航參數(shù)測量方程：

(18)

結(jié)合式(11)、式(13)、式(14)，可以得到載體運動的狀態(tài)方程：

(19)

聯(lián)立式(18)、式(19)并且離散化，得到導航參數(shù)的估計方程：

(20)

5.2 融合方法

定義狀態(tài)量序列Xk=[x0,x1,…,xk]，觀測量序列Zk=[z0,z1,…,zk]。記p(xk|Zk)表示xk的條件概率密度函數(shù)。根據(jù)貝葉斯準則，得到

(21)

式中，p(xk|Zk-1)的數(shù)學意義為系統(tǒng)從0時刻開始到k-1時刻為止，獲得系統(tǒng)觀測量序列Zk=[z0,z1,…,zk]時，下一時刻的系統(tǒng)狀態(tài)將為xk的概率。這是根據(jù)以往的觀測量事先預測下一時刻的系統(tǒng)狀態(tài)，因此稱為先驗概率密度函數(shù)。

p(xk|Zk)的數(shù)學意義為系統(tǒng)從0時刻開始到k時刻為止，獲得系統(tǒng)觀測量序列Zk=[z0,z1,…,zk]時，當前k時刻的系統(tǒng)狀態(tài)為xk的概率，因為系統(tǒng)觀測量Zk實際上是系統(tǒng)狀態(tài)xk的外在表現(xiàn)結(jié)果，所以這實際上是根據(jù)結(jié)果來分析原因，或者說根據(jù)已知系統(tǒng)輸出來判斷系統(tǒng)輸入的概率，因此稱為后驗概率密度函數(shù)。

p(zk|xk)稱為似然概率密度函數(shù)(Likelihood)，表示傳感器的模型。

在此，將遞推貝葉斯估計表述如下：

假定k-1時刻的后驗概率密度p(xk-1|Zk-1)已知，通過時間更新，可求得k時刻的先驗概率密度函數(shù)：

(22)

在k時刻獲得新的觀測信息zk后，進行量測更新，后驗概率密度函數(shù)計算公式如下

(23)

實際上，隨機濾波問題可以描述如下：已知初始概率密度p(x0)、p(xk|xk-1)、p(zk|xk)，濾波的目的就是從開始0時刻到當前k時刻以來，在所有的量測中估計當前系統(tǒng)的最佳狀態(tài)。本質(zhì)上，這是在估計后驗概率密度p(xk|Zk)或p(Xk|Zk)。隨機系統(tǒng)狀態(tài)濾波估計問題本質(zhì)上是一個求逆問題，即已知不同離散時刻測量到的測量值Zk=[z0,z1,…,zk]，和已知狀態(tài)空間映射函數(shù)f:RNx?RNx以及量測空間映射函數(shù)h:RNx?RNy的條件下，計算系統(tǒng)狀態(tài)的最優(yōu)或者次優(yōu)解。即

(24)

隨機系統(tǒng)狀態(tài)濾波估計問題可以由貝葉斯估計形式所描述，是一個十分廣泛的問題。對于靜態(tài)系統(tǒng)，可以采用權(quán)值平均法、最小二乘法、高斯牛頓法；如果先驗信息已知，可以采用MLE、MMSE等方法。在動態(tài)系統(tǒng)中，Kalman濾波器可用于線性估計；對于非線性估計，可以采用EKF、UKF、GSF等方法[15-16]；對于非高斯噪聲的條件，可以采用Markov鏈式蒙特卡羅方法[17]、PF[18]等方法；此外文獻[19-23]提出了一種基于因子圖的信息融合方法，該方法支持即插即用、易于擴展且支持非線性估計，被廣泛應用于全源導航與定位系統(tǒng)中。

5.3 算例

假設測量誤差服從高斯分布，那么式(24)可以簡化為如下的非線性最小二乘問題：

(25)

由式(25)可知，并不需要得到完整的位置、姿態(tài)速度的測量信息，而只需要知道與這些狀態(tài)量相關(guān)的部分測量信息，就可以利用高斯牛頓法等非線性優(yōu)化方法，將這些信息融合起來得到最優(yōu)的導航狀態(tài)估計。

6 結(jié)論

全源定位與導航是導航定位技術(shù)的發(fā)展趨勢，同時也面臨許多挑戰(zhàn)。為了實現(xiàn)全源導航的目標，需要解決復雜性、背景自適應、模糊性、環(huán)境數(shù)據(jù)處理等4個方面的問題。針對復雜性的問題，本文提出了一種基于全源定位與導航的信息融合統(tǒng)一理論。

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