全源定位與導(dǎo)航的統(tǒng)一理論框架構(gòu)建

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

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

0 引言

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

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

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

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

1 直接定位公式

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

1.1 位置函數(shù)

(1)

當(dāng)?shù)玫蕉鄠€(gè)測量值時(shí)，可以得到如下測量方程組

(2)

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

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

1.2 定位方式

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

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

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

1) 單一定位方式

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

(3)

在這里:

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

2) 綜合定位方式

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

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

(4)

在這里:

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

2 直接定姿公式

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

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

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

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

(5)

當(dāng)?shù)玫蕉鄠€(gè)測量矢量時(shí)，可以得到如下測量方程組

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

(6)

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

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

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

2.2 定姿方式

(7)

(8)

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

當(dāng)觀測矢量大于3個(gè)時(shí)，可以采用最小二乘法估計(jì)出載體余弦矩陣的元素。

3 直接定速公式

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

3.1 速度函數(shù)

(9)

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

當(dāng)?shù)玫蕉鄠€(gè)測量值時(shí)，可以得到如下測量方程組

(10)

式中：

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

②B為磁場強(qiáng)度，L為導(dǎo)體長度;

3.2 定速方式

1)單測量值定速

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

2)多測量值定速

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

4 航位航速航姿推算公式

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

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

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

(11)

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

(12)

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

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

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

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

(13)

α坐標(biāo)系相對β坐標(biāo)系的速度在γ坐標(biāo)系的投影表示為

(14)

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

(15)

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

(16)

該方法可用于航位推算、慣性導(dǎo)航等方面的應(yīng)用。

(17)

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

5 基于貝葉斯估計(jì)的融合方法

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

5.1 融合模型

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

(18)

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

(19)

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

(20)

5.2 融合方法

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

(21)

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

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

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

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

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

(22)

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

(23)

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

(24)

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

5.3 算例

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

(25)

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

6 結(jié)論

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

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