基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法

付希禹，孫永榮，吳 玲,2，曾慶化，劉建業(yè)

（1. 南京航空航天大學(xué) 自動(dòng)化學(xué)院導(dǎo)航研究中心，南京 211106；2. 南京航空航天大學(xué)金城學(xué)院 機(jī)電工程與自動(dòng)化學(xué)院，南京 211156）

相對(duì)導(dǎo)航是獲取目標(biāo)間相對(duì)關(guān)系的重要手段。傳統(tǒng)的動(dòng)態(tài)相對(duì)定位技術(shù)要求基準(zhǔn)站精確位置已知且固定，導(dǎo)致使用場(chǎng)景受到較大限制[1]?；鶞?zhǔn)站覆蓋范圍有限情況下，如空中加油、交會(huì)對(duì)接、無(wú)人機(jī)編隊(duì)、艦載機(jī)著艦等，差分?jǐn)?shù)據(jù)鏈將不再可靠，傳統(tǒng)相對(duì)定位技術(shù)無(wú)法滿足實(shí)際需求。動(dòng)對(duì)動(dòng)相對(duì)定位技術(shù)能夠通過(guò)差分的方式得到兩個(gè)移動(dòng)目標(biāo)之間相對(duì)位置關(guān)系，克服傳統(tǒng)定位方法中基準(zhǔn)站覆蓋范圍不足的問(wèn)題[2]。北斗三號(hào)全球衛(wèi)星導(dǎo)航系統(tǒng)（BeiDou Satellite Navigation System, BDS）已于2020年正式開(kāi)通，擺脫了以往GPS技術(shù)受制于人的局面。因此，對(duì)于北斗導(dǎo)航系統(tǒng)的研究成為熱門(mén)研究方向。

慣性導(dǎo)航系統(tǒng)（Inertial Navigation System, INS）是目前運(yùn)用最多的自主式導(dǎo)航系統(tǒng)，具有短時(shí)精度高、更新率高、自主性強(qiáng)等特點(diǎn)。因此，BDS和INS組合導(dǎo)航系統(tǒng)可以實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)[3,4]，在北斗衛(wèi)星信號(hào)丟失、可觀測(cè)數(shù)目不足時(shí)，仍可以保持高精度位置輸出。文獻(xiàn)[4]深入分析了單頻多模RTK/MEMS緊組合在提高單頻RTK定位性能方面的優(yōu)勢(shì)，并通過(guò)實(shí)驗(yàn)進(jìn)行了性能驗(yàn)證；文獻(xiàn)[5]在傳統(tǒng)緊組合基礎(chǔ)上，設(shè)計(jì)了一種基于高斯檢驗(yàn)探測(cè)和改善衛(wèi)星導(dǎo)航故障方法，在信號(hào)部分或完全暫停時(shí)，顯示出良好的橋接能力；文獻(xiàn)[6]在GPS/BDS/INS緊組合系統(tǒng)中提出一種改進(jìn)多模型交互式濾波，減少動(dòng)態(tài)模型不確定性帶來(lái)的性能降低，最終實(shí)現(xiàn)分米級(jí)定位精度。文獻(xiàn)[7]提出一種自適應(yīng)魯棒CKF用于GNSS/INS緊組合，抑制模型誤差以提高定位精度；文獻(xiàn)[8]通過(guò)在緊組合中引入RTSS和前后向組合平滑算法，提高相對(duì)及絕對(duì)定位精度，并通過(guò)跑車實(shí)驗(yàn)測(cè)試了性能。復(fù)雜環(huán)境下，衛(wèi)星信號(hào)易受到遮擋，觀測(cè)質(zhì)量不佳，經(jīng)常發(fā)生周跳，整周模糊度浮點(diǎn)精度不高，難以精確固定；而且移動(dòng)基準(zhǔn)站和流動(dòng)站同時(shí)發(fā)生衛(wèi)星信號(hào)丟失時(shí)，由于移動(dòng)基準(zhǔn)站無(wú)法提供位置信息，無(wú)法繼續(xù)維持相對(duì)導(dǎo)航。

本文針對(duì)復(fù)雜場(chǎng)景下，衛(wèi)星觀測(cè)質(zhì)量不佳、信號(hào)丟失時(shí)，BDS定位精度不高、無(wú)法連續(xù)定位的問(wèn)題，結(jié)合BDS和INS系統(tǒng)特點(diǎn)，利用先驗(yàn)信息，提出了一種基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法。

1 BDS/INS緊組合動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航模型

本文所提出的基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法為基于動(dòng)對(duì)動(dòng)的雙站相對(duì)導(dǎo)航系統(tǒng)，移動(dòng)基準(zhǔn)站處于運(yùn)動(dòng)狀態(tài)。基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法流程如圖1所示。從算法結(jié)構(gòu)上，優(yōu)先采用基于多普勒的偽距信息平滑優(yōu)化，移動(dòng)基準(zhǔn)站和流動(dòng)站均采用緊組合信息融合方式。移動(dòng)基準(zhǔn)站通過(guò)單點(diǎn)偽距/偽距率的緊組合方式，提供移動(dòng)基準(zhǔn)站位置、速度、姿態(tài)信息。進(jìn)一步地，對(duì)移動(dòng)基準(zhǔn)站與流動(dòng)站同步觀測(cè)，構(gòu)成雙差觀測(cè)方程。觀測(cè)量雙差后消除或削弱大部分誤差，如衛(wèi)星鐘差、接收機(jī)鐘差等，簡(jiǎn)化系統(tǒng)狀態(tài)誤差模型。由慣導(dǎo)預(yù)測(cè)的位置和速度代替衛(wèi)地距和衛(wèi)地距變化率對(duì)偽距、偽距率方程線性化，從而構(gòu)建流動(dòng)站緊組合雙差觀測(cè)方程。移動(dòng)基準(zhǔn)站與流動(dòng)站均進(jìn)行信息融合，通過(guò)緊組合進(jìn)行擴(kuò)展卡爾曼濾波，獲得系統(tǒng)狀態(tài)的最優(yōu)估計(jì)。最后，組合濾波結(jié)果分別對(duì)慣性器件誤差實(shí)時(shí)校正，并解算兩站之間的相對(duì)導(dǎo)航結(jié)果。

圖1 基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法流程Fig.1 Flowchart of kinematic-to-kinematic relative navigation algorithm based on BDS and INS tightly coupled

1.1 緊組合狀態(tài)方程

若不考慮北斗衛(wèi)星導(dǎo)航系統(tǒng)誤差量，則系統(tǒng)狀態(tài)模型由慣導(dǎo)誤差模型及慣性傳感器系統(tǒng)誤差來(lái)描述。因此對(duì)捷聯(lián)慣導(dǎo)構(gòu)建姿態(tài)、速度、位置誤差模型[5,6]。

式中，δ r、δv、ψ分別為位置誤差、速度誤差和姿態(tài)角誤差，?為加速度計(jì)誤差，ε為陀螺漂移，ωie為地球系相對(duì)于慣性系的地球自轉(zhuǎn)角速率在地理系上的投影，ωen為地理系相對(duì)于地球系的地球自轉(zhuǎn)角速率在地理系上的投影。

因此緊組合狀態(tài)方程為：

式中，X(t)為狀態(tài)向量，X=[(δr)T(δv)TψTεT?T]T，F(xiàn)(t)為狀態(tài)轉(zhuǎn)移矩陣，G(t)為系統(tǒng)噪聲驅(qū)動(dòng)矩陣，W(t)為系統(tǒng)噪聲陣。

1.2 緊組合觀測(cè)方程

1.2.1 基于多普勒的偽距信息平滑方法

載波相位平滑偽距是提高偽距觀測(cè)值精度的常用方法，當(dāng)周跳較少或不發(fā)生周跳時(shí)，可有效提高偽距精度。但處于遮擋環(huán)境時(shí)，衛(wèi)星觀測(cè)質(zhì)量差，導(dǎo)航信號(hào)發(fā)生失鎖，平滑器則需不斷重置，影響載波平滑偽距的精度。相關(guān)研究表明，當(dāng)周跳發(fā)生時(shí)，載波相位平滑偽距的精度明顯降低，多普勒無(wú)模糊度影響，因此在復(fù)雜環(huán)境下對(duì)偽距的平滑比載波相位更加準(zhǔn)確[9,10]。

基于Hatch濾波，載波相位平滑偽距算法為[11]：

式中，M為平滑時(shí)間常數(shù)，為k時(shí)刻平滑后偽距，Pk、φk分別為k時(shí)刻偽距和載波相位，λ為北斗B1頻段波長(zhǎng)。式(3)可實(shí)現(xiàn)載波相位對(duì)偽距平滑，在一定程度上提高偽距精度。多普勒頻移是觀測(cè)文件中的觀測(cè)量，具有良好的觀測(cè)精度，不因載波相位發(fā)生周跳而產(chǎn)生變化。對(duì)多普勒頻移積分是對(duì)多普勒頻移造成載波相位變化量的疊加，其在數(shù)值上表現(xiàn)為載波相位的變化量[12]，因此用多普勒頻移觀測(cè)值代替載波相位差分，即：

式中，T為歷元間隔。將式(4)代入式(3)，則多普勒平滑偽距公式為：

1.2.2 動(dòng)態(tài)對(duì)動(dòng)態(tài)相對(duì)導(dǎo)航觀測(cè)方程

式中，?Δ為雙差算子，P和D為偽距及多普勒，ρ和ρ˙為慣性預(yù)測(cè)衛(wèi)地距及衛(wèi)地距變化率，I和T分別為電離層誤差和對(duì)流層誤差，ε?ΔP和ε?ΔD為噪聲項(xiàng)。站星雙差可消除衛(wèi)星鐘差及接收機(jī)鐘差，短基線情形下，可顯著消除大氣延時(shí)誤差，有效提高觀測(cè)精度[6]。代入衛(wèi)地距在INS預(yù)測(cè)位置(xu0,yu0,zu0)、預(yù)測(cè)速度(x˙u0,y˙u0,z˙u0)一階泰勒展開(kāi)式，忽略大氣延遲，可得動(dòng)對(duì)動(dòng)的偽距、多普勒雙差觀測(cè)方程：

式中，Δl、Δm、Δn為方向矢量差。傳統(tǒng)RTK定位基準(zhǔn)站固定且精確位置已知，移動(dòng)基準(zhǔn)站情形下并不存在此前提。GNSS相對(duì)定位中對(duì)起算點(diǎn)坐標(biāo)的要求并不高，有時(shí)誤差可超過(guò)10 m[2]。對(duì)于緊組合后的移動(dòng)基準(zhǔn)站的輸出，滿足以上要求。

綜合式(8)(9)，需要注意的是，由于狀態(tài)方程是在地理坐標(biāo)系下進(jìn)行建模，進(jìn)一步將狀態(tài)量δ r、δv轉(zhuǎn)換到地心地固坐標(biāo)系（ECEF）下。若雙站同時(shí)觀測(cè)M個(gè)共視衛(wèi)星，則可得到2(M-1)個(gè)方程，聯(lián)立構(gòu)成BDS/INS緊組合動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航觀測(cè)方程：

式中，K為方向向量構(gòu)成的設(shè)計(jì)矩陣。為地理系到地球系的坐標(biāo)系轉(zhuǎn)換矩陣。

1.3 基于擴(kuò)展卡爾曼濾波的衛(wèi)星/慣性信息融合

對(duì)于上述動(dòng)對(duì)動(dòng)量測(cè)信息，衛(wèi)星/慣性量測(cè)信息融合通過(guò)擴(kuò)展卡爾曼濾波（Extended Kalman Filter, EKF）進(jìn)行參數(shù)估計(jì)。優(yōu)先將狀態(tài)方程離散化，隨后進(jìn)入組合導(dǎo)航濾波計(jì)算。濾波方程的系統(tǒng)噪聲協(xié)方差陣通過(guò)對(duì)慣性器件轉(zhuǎn)臺(tái)標(biāo)定后確定。顧及觀測(cè)質(zhì)量，量測(cè)協(xié)方差陣通過(guò)衛(wèi)星高度角進(jìn)行建模：

式中，Kk為增益矩陣，Xk+和Pk+為k時(shí)刻后驗(yàn)狀態(tài)向量和協(xié)方差矩陣，Zk為觀測(cè)矩陣，Hk為量測(cè)矩陣，Rk為量測(cè)噪聲矩陣。通過(guò)如式(12)(13)所示的濾波過(guò)程即可完成單歷元推算。對(duì)于緊組合，當(dāng)出現(xiàn)高樓、樹(shù)木等衛(wèi)星信號(hào)遮擋情況時(shí)，衛(wèi)星數(shù)減少，觀測(cè)方程維數(shù)將下降，但仍可進(jìn)行濾波計(jì)算。數(shù)據(jù)融合后，通過(guò)流動(dòng)站與移動(dòng)基準(zhǔn)站的位置速度作差，進(jìn)一步得到相對(duì)導(dǎo)航結(jié)果。

2 實(shí)驗(yàn)驗(yàn)證與分析

為驗(yàn)證本文算法有效性，搭建了如圖2所示BDS/INS的動(dòng)態(tài)對(duì)動(dòng)態(tài)實(shí)驗(yàn)平臺(tái)，基于該平臺(tái)開(kāi)展車載實(shí)驗(yàn)。實(shí)驗(yàn)地點(diǎn)為位于江蘇省南京市寧丹大道的城市環(huán)境，城市道路較為開(kāi)闊，有效數(shù)據(jù)采集時(shí)間約為30 min，運(yùn)動(dòng)軌跡如圖3中黃色曲線所示。

圖2 車載實(shí)驗(yàn)平臺(tái)及場(chǎng)景Fig.2 Experiment platform and scene

圖3 實(shí)驗(yàn)軌跡圖Fig.3 Trajectory of experiment

實(shí)驗(yàn)平臺(tái)慣性設(shè)備為荷蘭XSENS公司出品的MTI-G-700型慣性傳感器，標(biāo)稱陀螺儀零偏穩(wěn)定性為10 o/h，加速度計(jì)精度40 μg。GNSS接收機(jī)為NovAtel OEM6板卡授時(shí)導(dǎo)航接收機(jī)，GNSS天線吸附在車頂。衛(wèi)星/慣性采樣頻率分別為200 Hz和1 Hz。兩部車載測(cè)量系統(tǒng)分別標(biāo)記為移動(dòng)基準(zhǔn)站和流動(dòng)站，經(jīng)測(cè)量衛(wèi)星/慣性空間桿臂誤差lb=[-0.2,0,0.14]Tm 并校正。移動(dòng)基準(zhǔn)站及流動(dòng)站速度及其基線長(zhǎng)度變化如圖4-5所示。

圖4 移動(dòng)基準(zhǔn)站速度及基線長(zhǎng)Fig.4 Velocity and baseline length of moving base station

圖5 流動(dòng)站速度及基線長(zhǎng)Fig.5 Velocity and baseline length of rover station

行駛過(guò)程中兩車保持動(dòng)態(tài)對(duì)動(dòng)態(tài)運(yùn)動(dòng)狀態(tài)，衛(wèi)星截止高度角為15 o，可見(jiàn)衛(wèi)星數(shù)量及PDOP變化情況如圖6所示。從圖中可以看出，實(shí)驗(yàn)過(guò)程中，截止高度角為15 o情況下，可見(jiàn)北斗衛(wèi)星數(shù)量保持在15顆左右，平均衛(wèi)星數(shù)量14.6顆，平均PDOP 1.60；部分歷元出現(xiàn)樹(shù)木遮擋，衛(wèi)星數(shù)量下降至11-13顆左右，整體觀測(cè)質(zhì)量較好。

圖6 可見(jiàn)衛(wèi)星數(shù)量及PDOPFig.6 Satellite visibility and PDOP

為對(duì)本文算法進(jìn)行分析，車載系統(tǒng)同時(shí)搭載了千尋網(wǎng)絡(luò)RTK，并將定位及測(cè)速結(jié)果作為參考基準(zhǔn)，整周模糊度正確固定時(shí)，可輸出cm級(jí)定位結(jié)果。圖7-8為采用兩種相對(duì)導(dǎo)航方案統(tǒng)計(jì)得到的相對(duì)導(dǎo)航結(jié)果，藍(lán)色標(biāo)識(shí)為基于偽距/偽距率差分的衛(wèi)星相對(duì)導(dǎo)航（方案1，SRN），紅色標(biāo)識(shí)為本文BDS/INS相對(duì)導(dǎo)航算法（方案2，TCRN）。

圖7 SRN/TCRN相對(duì)位置誤差Fig.7 SRN/TCRN relative position errors

圖8 SRN/TCRN相對(duì)速度誤差Fig.8 SRN/TCRN relative velocity errors

進(jìn)一步地，對(duì)上述兩種方案的E/N/U三個(gè)方向的相對(duì)位置、相對(duì)速度誤差平均值、均方根誤差（Root Mean Square, RMS）、最大值、最小值進(jìn)行了統(tǒng)計(jì)分析，如表1所示。

表1 SRN/TCRN相對(duì)導(dǎo)航誤差表Tab.1 SRN/TCRN relative navigation errors

綜合上述結(jié)果可以發(fā)現(xiàn)，基于偽距/偽距率差分的相對(duì)導(dǎo)航由于消除或削弱了星鐘誤差、大氣延遲的影響，相對(duì)位置及相對(duì)速度精度達(dá)分米級(jí)。同時(shí)，由于慣性參與信息融合濾波，基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法中有效抑制了偽距差分算法中包含的噪聲，相對(duì)位置、相對(duì)速度誤差曲線更加平滑，性能得到顯著改善。具體而言，以RMS誤差為例，相對(duì)位置E、N、U方向誤差分別從0.162 m、0.188 m、0.742 m降至0.127 m、0.161 m、0.678 m，本文所提出的算法在相對(duì)位置精度分別提升了21.6%、14.4%和8.63%；相對(duì)速度E、N、U方向誤差分別從0.099 m/s、0.140 m/s、0.115 m/s降至0.080 m/s、0.123 m/s、0.039 m/s，相對(duì)速度精度分別提升了19.2%、12.1%、66.2%。通過(guò)BDS和INS數(shù)據(jù)融合，相對(duì)導(dǎo)航系統(tǒng)可實(shí)現(xiàn)優(yōu)勢(shì)互補(bǔ)，這一結(jié)果驗(yàn)證了緊組合在相對(duì)導(dǎo)航上的精度優(yōu)勢(shì)。

為進(jìn)一步分析動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法在衛(wèi)星觀測(cè)質(zhì)量不佳、信號(hào)丟失時(shí)的性能，歷元時(shí)間內(nèi)分別設(shè)置5 s、10 s、20 s、30 s衛(wèi)星中斷，模擬信號(hào)失鎖中斷場(chǎng)景，以驗(yàn)證所提方法有效性。表2反映了在信號(hào)中斷時(shí)，本文方法的RMS誤差。

表2 BDS中斷條件下導(dǎo)航性能Tab.2 Navigation performance in adverse BDS condition

顯然，BDS信號(hào)中斷觀測(cè)條件下，由于無(wú)法獲得觀測(cè)信息進(jìn)行差分解算，依賴衛(wèi)星的相對(duì)定位、測(cè)速算法將不再可用，相對(duì)位置及相對(duì)速度信息無(wú)法獲得，這對(duì)應(yīng)用于動(dòng)對(duì)動(dòng)的場(chǎng)景，如空中加油，將產(chǎn)生極大的風(fēng)險(xiǎn)。通過(guò)分析可知，緊組合出現(xiàn)了隨時(shí)間發(fā)散的特性，但由于慣導(dǎo)具有短時(shí)精度高的特性，可保持一定程度的相對(duì)導(dǎo)航能力，保證系統(tǒng)連續(xù)的相對(duì)導(dǎo)航信息輸出，但限于IMU精度，其能力有一定上限。從上述實(shí)驗(yàn)分析中可以看出，本文算法在以上場(chǎng)景中，能較好的保持相對(duì)導(dǎo)航能力，有效提升了動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航的穩(wěn)定性和可靠性。

3 結(jié) 論

為改善衛(wèi)星觀測(cè)質(zhì)量不佳及信號(hào)丟失時(shí)，BDS定位精度不高、無(wú)法連續(xù)定位的問(wèn)題，結(jié)合BDS和INS系統(tǒng)特點(diǎn)，提出了一種基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法。文中分析構(gòu)建了捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型；研究了多普勒平滑偽距算法；構(gòu)建了相對(duì)導(dǎo)航觀測(cè)模型，并通過(guò)緊組合EKF濾波數(shù)據(jù)融合，最終解算獲得相對(duì)導(dǎo)航結(jié)果。通過(guò)車載實(shí)測(cè)實(shí)驗(yàn)，結(jié)果表明，相比于衛(wèi)星導(dǎo)航算法，緊組合在相對(duì)導(dǎo)航上具有一定的精度優(yōu)勢(shì)。當(dāng)衛(wèi)星信號(hào)受到遮擋中斷時(shí)，基于BDS/INS緊組合的動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航算法能夠繼續(xù)保持相對(duì)導(dǎo)航能力，從而有效提升了動(dòng)對(duì)動(dòng)相對(duì)導(dǎo)航的穩(wěn)定性和可靠性。