高度約束MHE算法在偽距單點(diǎn)定位中的應(yīng)用

孫淑光，楊祥源，陳萬通,，張巨聯(lián)，劉慶，任詩雨

( 1. 中國民航大學(xué) 電子信息與自動化學(xué)院, 天津 300300；2. 中國民航大學(xué) 民航航班廣域監(jiān)視與安全管控技術(shù)重點(diǎn)實(shí)驗(yàn)室, 天津300300；3. 上海飛機(jī)設(shè)計(jì)研究院, 上海 201109；4. 上海航天電子技術(shù)研究所, 上海 201109 )

0 引 言

在進(jìn)行偽距單點(diǎn)定位(SPP)時，數(shù)據(jù)往往采用后處理的方式，傳統(tǒng)的精密衛(wèi)星軌道及鐘差產(chǎn)品存在時間延遲的問題，限制了SPP技術(shù)在實(shí)時場景中的應(yīng)用[1]. 近年來，國際全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)服務(wù)(IGS)推出了一些具有高精度的GNSS精密定位產(chǎn)品，可以很大程度上消除衛(wèi)星定位相關(guān)誤差的影響[2].因此，一些相關(guān)研究重新聚焦于SPP技術(shù).

SPP是僅利用一臺接收機(jī)即可實(shí)現(xiàn)高魯棒性的一種定位技術(shù)，而定位精度低仍是限制其實(shí)際應(yīng)用的首要因素. 為進(jìn)一步提高SPP的定位性能，近年來，研究人員發(fā)現(xiàn)一些場景下存在的先驗(yàn)信息可以用來增強(qiáng)單點(diǎn)定位性能.

文獻(xiàn)[3]在SPP觀測模型中加入了先驗(yàn)高度，將其作為準(zhǔn)觀測方程，用于加權(quán)最小二乘(WLS)計(jì)算，實(shí)驗(yàn)結(jié)果表明水平定位精度提高了42.84%. 文獻(xiàn)[4]提出了先驗(yàn)海平面的高度約束精密單點(diǎn)定位(PPP)方法，并將約束方程通過差分處理實(shí)現(xiàn)線性化，實(shí)驗(yàn)結(jié)果表明附加高度約束的定位精度在垂直方向上提高了79%，三維方向上提高了67%. 文獻(xiàn)[5]采用道路幾何模型約束進(jìn)行導(dǎo)航定位，將車輛的狀態(tài)投影在地圖段上，實(shí)驗(yàn)結(jié)果表明約束后的定位精度提高了31.4%.

傳統(tǒng)的SPP解算方法通常采用最小二乘(LS)估計(jì)[6]. 而LS只考慮觀測模型，不涉及系統(tǒng)狀態(tài)模型，故所得定位結(jié)果通常顯得粗糙雜亂. 因此，在一些研究中，研究人員引入了卡爾曼濾波(KF)算法，以實(shí)現(xiàn)KF算法的各種修改形式[7]. 其中，平滑約束卡爾曼濾波方法(SCKF)和無跡卡爾曼濾波方法(UKF)的出發(fā)點(diǎn)是先將非線性約束進(jìn)行線性化，然后將其實(shí)現(xiàn)為完美測量[8-9]. 結(jié)果表明：上述方案所獲得的估計(jì)值僅近似滿足于非線性約束，而且非線性比簡單的線性化過程會具有更好的匹配度[9]. 不同于KF算法，滾動時域估計(jì)(MHE)是把帶約束的估計(jì)問題轉(zhuǎn)化成優(yōu)化問題，同時把施加在系統(tǒng)變量上的約束條件直接表述為優(yōu)化問題中目標(biāo)函數(shù)的約束，并通過在線滾動優(yōu)化進(jìn)行求解[10]，是在約束條件下處理狀態(tài)參數(shù)估計(jì)的最有效方法[11-12]. 同時MHE算法可能會用到KF所不包括的信息量，而且對非線性約束可以直接應(yīng)用，無需線性化.

基于以上考慮，本文將非線性高度約束方程引入到約束MHE濾波算法中進(jìn)行SPP的解算，從而實(shí)現(xiàn)定位精度的提高，獲得更為平滑的定位結(jié)果.

1 SPP數(shù)學(xué)模型

SPP是以偽距觀測為基礎(chǔ)的一種定位方法，其偽距觀測是指對所觀測衛(wèi)星和用戶接收機(jī)之間距離的測量.

1.1 消電離層觀測模型

本文分析的是雙頻的消電離層(IF)的偽距觀測，其觀測方程為[13]

式中：Pifj為基于載波頻率fj(j=1,2) 的用戶接收機(jī)到衛(wèi)星i之間的偽距觀測； ρi為接收機(jī)和衛(wèi)星的天線相位中心之間的真實(shí)幾何范圍； dtr和 dti分別為接收機(jī)和衛(wèi)星的時鐘偏移；c為真空中光速；Ti為對流層延遲；bfj和Bfj分別為接收機(jī)和衛(wèi)星的頻率相關(guān)偽距硬件延遲； εi為觀測噪聲和未建模的殘余誤差.

在上述觀測模型中，電離層延遲誤差通過IF組合模型的方式來消除. 為提高SPP的定位精度， dti、dtr、bfj和Bfj項(xiàng)應(yīng)完全消除，在此，利用IGS精密產(chǎn)品消除衛(wèi)星位置和時鐘誤差，利用對流層模型修正對流層延遲誤差. 而對于相對論效應(yīng)、地球自轉(zhuǎn)效應(yīng)、地球固體潮和海洋潮汐引起的誤差，若采用同樣的方式對其修正，反而致使得到的定位結(jié)果精度不高，故在此SPP模型中可被忽略，不作考慮.

此外，衛(wèi)星鐘差項(xiàng)中集成了與衛(wèi)星相關(guān)的硬件延遲，此由精確的衛(wèi)星時鐘誤差乘積進(jìn)行修正，同時與接收機(jī)相關(guān)的硬件延遲被整合到了接收機(jī)時鐘誤差項(xiàng)中，表示為[14]

式中： ζ 為衛(wèi)星鐘差 dti與衛(wèi)星硬件延遲Bpj的整合誤差； μ 為接收機(jī)鐘差 dtr與接收機(jī)的硬件延遲bpj的整合誤差. 當(dāng)上述相關(guān)誤差和噪聲被忽略或校正后，式(1)可以寫為

1.2 SPP參數(shù)估計(jì)

與傳統(tǒng)的迭代算法不同，偽距觀測方程的閉式解方法不需要迭代運(yùn)算. 通過計(jì)算參考衛(wèi)星和其他衛(wèi)星偽距方程之間的線性無關(guān)差，則可將線性化應(yīng)用于式(3).

假設(shè)有n顆(n≥5 )按仰角降序排列得可見衛(wèi)星，則可以獲得關(guān)于矩陣向量形式的未知參數(shù)(x,y,z,μ)的線性觀測公式，記為

式中：r=[x,y,z]T；em=[1,1,···,1]T，m=n-1 ； ν 為 觀測噪聲；l=[ε1/δ1,ε2/δ2,···,εm/δm]T

，且

式中， χi=[αi/δi,βi/δi,γi/δi] ，i=1,2,···,n-1 ，有[15]為后續(xù)應(yīng)用滾動時域估計(jì)算法，采用正交變換的方法來消除式(4)中的接收機(jī)時鐘誤差項(xiàng)μ.假定P∈Rm×m是正交變換矩陣，則有0,···,0)T. 根據(jù)householder 變換矩陣的定義，并通過相應(yīng)代數(shù)計(jì)算，矩陣P可以具體表示為

為消除接收機(jī)時鐘偏移參數(shù) μ ，將式中的轉(zhuǎn)換矩陣P代入式(4)，可以獲得初始正交變換為

特別注意，由于只有第一個等式包含時鐘偏移項(xiàng)μ，故余下的部分可寫為

由此，模型的觀測方程可表述為

式中：y為觀測向量；H為向量r對應(yīng)設(shè)計(jì)矩陣； ε 為誤差修正項(xiàng).

因此，可以使用上述線性方程直接求解接收機(jī)位置參數(shù)解，該方程不包括接收機(jī)時鐘偏移項(xiàng) μ .

為求解式(10)中的未知參數(shù)r，可以采用一般LS. 同時基于LS估計(jì)是以下最小化問題的解決方案，可表示為[16]

■■■■■■■

2 高度約束型MHE算法

2.1 高度約束

在假設(shè)先驗(yàn)高度信息沒有誤差的情況下，具有非線性等式的高度約束方程可表示為

式中：D=diag[1/(a+h),1/(a+h),1/(b+h)] ，a、b分別為WGS-84參考橢球的長半軸和短半軸；h為相對于WGS-84參考橢球的高度[17].

2.2 滾動時域估計(jì)

MHE算法按所用信息量的不同可分為全信息MHE和近似MHE. 全信息MHE方法利用了當(dāng)前和過去時刻的所有測量數(shù)據(jù)，測量結(jié)果更為準(zhǔn)確，但另一方面會造成“滾雪球”效應(yīng)，導(dǎo)致問題不可解；近似MHE方法則是為了避免上述問題的出現(xiàn)，通過限制問題的維數(shù)，確保了求解的可行性.

2.3 附加高度約束的滾動時域估計(jì)

在實(shí)際應(yīng)用中，有很多的狀態(tài)方程都可以用來模擬接收機(jī)的狀態(tài)模型. 本文使用如下狀態(tài)方程來模擬接收機(jī)的狀態(tài)模型：

式中：T為連續(xù)兩個歷元的時間間隔；系統(tǒng)噪聲Q為：

式中：qx、qy、qz分別為不同分量上的過程噪聲的功率譜密度；為使觀測方程式(10)中的狀態(tài)向量rk與過程方程式(14)中的狀態(tài)向量Xk相同，將矩陣修改為包含有3個零列，如下所示：

因此，所得SPP觀測方程表示為

式中，R為觀測噪聲. 同理，將高度約束方程的相關(guān)矩陣D補(bǔ)充相應(yīng)零列修改為

相應(yīng)地，高度約束方程則可表示為

因此，高度約束MHE狀態(tài)估計(jì)問題的目標(biāo)函數(shù)構(gòu)造如下：

滿足約束

為限制優(yōu)化問題的維數(shù)，引入了長度為J的固定時域(J=N-M+1 )，將全信息估計(jì)問題的計(jì)算劃分為兩部分：{1 ≤k≤M-1 } 和{M≤k≤N}，采用近似MHE算法將狀態(tài)估計(jì)問題轉(zhuǎn)為如下約束優(yōu)化問題的目標(biāo)函數(shù)

滿足約束

式中： {Xk} 為 {XM,...,XN} ；N-M+1 為 滾 動 時域窗口長度；其維度為為后驗(yàn)估計(jì)協(xié)方差矩陣.

3 實(shí)驗(yàn)結(jié)果與分析

為評估MHE算法在高度約束SPP的性能，下面分別進(jìn)行了靜態(tài)實(shí)驗(yàn)和動態(tài)實(shí)驗(yàn).

3.1 靜態(tài)實(shí)驗(yàn)

本靜態(tài)實(shí)驗(yàn)是基于AREG觀測站(站點(diǎn)位置：阿雷基帕，經(jīng)度為-71.492 9°，緯度為-16.465 4°，海拔高度為2 489.336 m)根據(jù)IGS提供的24 h GNSS觀測數(shù)據(jù)，并以靜態(tài)接收機(jī)的真實(shí)高度作為先驗(yàn)高度值，在MATLAB平臺上進(jìn)行了SPP定位實(shí)驗(yàn). 實(shí)驗(yàn)方案如下：

1)無約束LS的SPP實(shí)驗(yàn)；

2)視窗長度為2 h，高度約束MHE的SPP實(shí)驗(yàn)(MHE-2)；

3)視窗長度為4 h，高度約束MHE的SPP實(shí)驗(yàn)(MHE-4).

表1分別計(jì)算出了不同方案下SPP定位估計(jì)過程中的處理時長. 時間跨度是由MATLAB中的tic和toc函數(shù)計(jì)算，其中tic功能自動記錄啟動時間，toc計(jì)算程序運(yùn)行時的時間差，可見相對LS算法，MHE算法的處理時間較長，且隨MHE視窗大小的增加，程序運(yùn)行時間也會急劇增加.

表1 不同方案的處理時長 s

圖1為SPP在WGS-84直角坐標(biāo)系下的定位估計(jì)誤差. 利用接收機(jī)參考坐標(biāo)與估計(jì)坐標(biāo)之間的差值來計(jì)算誤差值. 考慮到MHE計(jì)算量較大，所以圖1分別給出了WGS-84直角坐標(biāo)系下LS方法和前300個歷元的MHE方法的SPP的估計(jì)誤差結(jié)果. 可以看出，LS定位誤差波動起伏較大，結(jié)果粗糙雜亂；而附加高度約束的MHE方案抗擾動性能更強(qiáng)，定位結(jié)果更為平滑，明顯優(yōu)于傳統(tǒng)無約束LS方案的定位結(jié)果.

圖1 三種方案在X、Y、Z方向上的定位誤差比較

結(jié)合表2，與LS方案相比，MHE-2方案在X、Y、Z三個方向分量上的均方根(RMS)值分別從1.11 m、3.23 m、1.01 m降低到0.21 m、0.91 m、0.23 m，整體定位精度提高了73.03%；同樣，MHE-4方案的RMS值分別降低到0.20 m、0.87 m、0.26 m，整體定位精度提高了81.46%.

表2 SPP定位誤差RMS值

3.2 動態(tài)實(shí)驗(yàn)

為了進(jìn)一步評估附加高度約束MHE的SPP定位性能，通過實(shí)時采集的動態(tài)GPS衛(wèi)星觀測數(shù)據(jù)進(jìn)行了動態(tài)SPP定位實(shí)驗(yàn).

如圖2所示本實(shí)驗(yàn)數(shù)據(jù)于2021年6月2日采集于武漢的中國科學(xué)院測量與地球物理研究所導(dǎo)航樓頂，從GPS時間(CPST)的10:37:00開始，觀測時長約5 min. 實(shí)驗(yàn)過程中的動態(tài)接收機(jī)沿著自身圓形軌道勻速轉(zhuǎn)動. 此外，在距離該數(shù)據(jù)采集地點(diǎn)約4.5 m的地方設(shè)有一個基準(zhǔn)站(精確坐標(biāo)已知)同時進(jìn)行觀測獲取衛(wèi)星觀測數(shù)據(jù)，然后用RTKLIB軟件的載波相位差模式進(jìn)行解算動態(tài)接收機(jī)的位置坐標(biāo)，以獲得接收機(jī)的精確定位數(shù)據(jù). 圖3為實(shí)驗(yàn)中動態(tài)接收機(jī)的運(yùn)動軌跡坐標(biāo).

圖2 動態(tài)數(shù)據(jù)收集站點(diǎn)

圖3 動態(tài)接收機(jī)運(yùn)動軌跡

本次動態(tài)實(shí)驗(yàn)中，先驗(yàn)高度值被設(shè)置為所有歷元的接收機(jī)定位的平均高度. 為了與靜態(tài)實(shí)驗(yàn)相一致，并且較小視窗尺寸的MHE方法的定位精度的提高對實(shí)際應(yīng)用也更有意義，因此，實(shí)驗(yàn)中MHE的視窗尺寸也設(shè)置為2和4.

圖4為動態(tài)接收機(jī)做勻速圓周運(yùn)動時，LS方法和前300個歷元的MHE方法的SPP定位誤差結(jié)果. 結(jié)果表明：與無約束LS相比，MHE兩種方案的結(jié)果具有較小的波動，且MHE-4方案的結(jié)果優(yōu)于MHE-2方案. 同時也不難發(fā)現(xiàn)，隨時間推移，動態(tài)接收站工作趨于穩(wěn)定及MHE-4方案觀測數(shù)據(jù)信息的增加，其定位結(jié)果在250 s后相較與LS和MHE-2結(jié)果存在明顯偏離，定位結(jié)果更為精確.

圖4 三種方案在X、Y、Z方向上的定位誤差比較

表3給出了前300個歷元SPP定位的RMS誤差. 由表3可知，與LS方案相比，MHE-2方案在X、Y、Z三個方向分量上的RMS值分別從3.81 m、7.31 m、4.14 m降低到2.53 m、4.70 m、3.11 m，整體定位精度提高了33.0%； MHE-4方案的RMS值分別降低到1.95 m、3.23 m、2.26 m，整體定位精度提高了52.4%.與MHE-2方案相比，MHE-4方案的整體定位精度提高了19.4%.

表3 SPP定位誤差RMS值

4 結(jié) 論

SPP技術(shù)是一項(xiàng)重要的定位技術(shù)，可為一些特定的應(yīng)用場景提供高魯棒性的定位. 本文從國內(nèi)外研究的先驗(yàn)信息可以用來增強(qiáng)單點(diǎn)定位性能的基礎(chǔ)上，提出了附加高度約束的MHE算法來進(jìn)一步提高SPP定位精度，并進(jìn)行了靜態(tài)和動態(tài)實(shí)驗(yàn)分析. 結(jié)果表明：相對于傳統(tǒng)的LS方法，附加高度約束的MHE算法的單點(diǎn)定位結(jié)果更加準(zhǔn)確和平滑，驗(yàn)證了該方法的有效性、可行性，可適用于更為廣泛的應(yīng)用場景.

但同時MHE算法的估計(jì)精度與運(yùn)行效率互相矛盾，兩者不可兼得，其中運(yùn)行效率主要與視窗的大小有關(guān). 在實(shí)際應(yīng)用過程中，需要根據(jù)應(yīng)用場景的不同，在定位估計(jì)精度和計(jì)算工作量之間折衷考慮視窗參數(shù)大小.