衛(wèi)星拒止環(huán)境下無人機(jī)INS/GNSS緊組合定位算法研究

摘 要：無人機(jī)在城市或山區(qū)等復(fù)雜低空環(huán)境下飛行時，很容易進(jìn)入衛(wèi)星拒止區(qū)域，導(dǎo)致接收到的衛(wèi)星信號較少，GNSS接收機(jī)定位精度大幅下降甚至停止工作。針對無人機(jī)在衛(wèi)星拒止環(huán)境下INS/GNSS緊組合的定位性能迅速下降的問題，提出使用零速修正方法對INS/GNSS緊組合定位算法進(jìn)行改進(jìn)；并根據(jù)航路所需導(dǎo)航性能需求，針對長時間處于拒止環(huán)境下可能引起的零速修正重啟問題進(jìn)行了有效約束，保證了無人機(jī)在此環(huán)境下?lián)碛辛己玫膶?dǎo)航性能。仿真實驗證明，相對于傳統(tǒng)卡爾曼濾波，零速修正調(diào)整的卡爾曼濾波顯著降低了定位誤差，其中水平誤差降低了90.39%，高度誤差降低了86.47%，UANP值減少了72.99%，導(dǎo)航定位性能明顯提高。

關(guān)鍵詞：INS/GNSS緊組合；衛(wèi)星拒止環(huán)境；零速修正；卡爾曼濾波；定位誤差；定位性能

中圖分類號：TP39；V249.32 文獻(xiàn)標(biāo)識碼：A 文章編號：2095-1302（2025）01-00-05

0 引 言

隨著物流業(yè)、農(nóng)業(yè)勘探、深度旅游的快速發(fā)展，無人機(jī)的使用范圍越來越廣泛，涵蓋了城市和山區(qū)的物流貨物傳送、復(fù)雜環(huán)境搜索救援及地形勘察信息捕獲與傳輸，成為了物聯(lián)網(wǎng)領(lǐng)域中的重要一環(huán)。而這些復(fù)雜特殊的工作環(huán)境對無人機(jī)的導(dǎo)航定位功能也提出了較高的要求。

現(xiàn)在主流的無人機(jī)導(dǎo)航方法是GNSS系統(tǒng)與INS系統(tǒng)相融合的方法。無人機(jī)體積較小、靈活性較強(qiáng)，在城市或山區(qū)等低空復(fù)雜環(huán)境飛行時，容易進(jìn)入衛(wèi)星拒止區(qū)域。衛(wèi)星拒止區(qū)域是指GNSS信號在傳播過程中會受到自然環(huán)境干擾、建筑物遮蔽或者人為攻擊等影響，造成GNSS定位精度大幅下降甚至停止工作的區(qū)域[1]。在此區(qū)域內(nèi)，接收不到可見星或者可見星數(shù)目較少，GNSS無法單獨(dú)提供導(dǎo)航定位服務(wù)[2]，在INS/GNSS組合導(dǎo)航定位系統(tǒng)中，GNSS對INS的修正開始失效，很難控制低成本慣性導(dǎo)航器件測量誤差的迅速累積[3]，導(dǎo)致組合導(dǎo)航系統(tǒng)的位置誤差逐漸增加。雖然INS/GNSS緊組合系統(tǒng)可以在衛(wèi)星數(shù)目少于4顆時進(jìn)行定位，但隨著時間的增加，由于觀測量不足會導(dǎo)致定位誤差逐漸累計，定位效果也越來越差。針對這種情況，文獻(xiàn)[4]使用神經(jīng)網(wǎng)絡(luò)方法來解決因可見星較少甚至消失而導(dǎo)致的定位誤差發(fā)散問題，但是在一定程度上增加了計算量。文獻(xiàn)[5-6]通過增加外部傳感器獲取額外觀測量，在一定程度上提高了定位精度，取得了較好的誤差補(bǔ)償效果，但同時也增加了系統(tǒng)的成本與無人機(jī)的載重，對無人機(jī)的供電需求提出了挑戰(zhàn)，會在一定程度上影響無人機(jī)的續(xù)航時間及航程。

零速修正（Zero Velocity Update， ZUPT）[7]是提高載體慣性導(dǎo)航精度的一種高效便捷的方法，它利用載體臨時懸停時的速度誤差作為觀測量[8]，增加系統(tǒng)觀測方程，改善組合導(dǎo)航系統(tǒng)濾波效果。本文針對城市或山區(qū)低空復(fù)雜環(huán)境下無人機(jī)受建筑物或山體遮擋、可見星的數(shù)目較少的情況，改進(jìn)了INS/GNSS緊組合導(dǎo)航定位算法。在INS/GNSS組合導(dǎo)航系統(tǒng)模型中加入零速修正約束算法，增加觀測矩陣維數(shù)，限制INS導(dǎo)航誤差的積累，進(jìn)而提高系統(tǒng)的定位精度與穩(wěn)定性。此方法不需要額外的傳感器，且計算方法相對簡單。

1 INS/GNSS緊組合系統(tǒng)模型

INS/GNSS緊組合導(dǎo)航系統(tǒng)通過IMU的輸出進(jìn)行慣導(dǎo)解算，得到位置、速度、姿態(tài)角等信息，并利用GNSS接收機(jī)提供原始數(shù)據(jù)偽距和偽距率。為了得到位置、速度、姿態(tài)的最佳估計值，系統(tǒng)通常采用卡爾曼濾波算法進(jìn)行時間更新和觀測更新[9]。

1.1 系統(tǒng)誤差模型

緊組合系統(tǒng)的狀態(tài)模型為：

（1）

系統(tǒng)采用間接法來估計導(dǎo)航參數(shù)，即以系統(tǒng)輸出的導(dǎo)航參數(shù)的誤差作為狀態(tài)變量[10]，狀態(tài)變量X由GNSS的誤差狀態(tài)和INS的誤差狀態(tài)共同組成，表達(dá)式為：

（2）

式中：XI表示慣導(dǎo)系統(tǒng)的誤差狀態(tài)，由位置誤差δL、δλ、δh，速度誤差δv，姿態(tài)角誤差?，以及陀螺儀常值漂移和加速度常值漂移組成[11]，各個誤差狀態(tài)對應(yīng)的數(shù)學(xué)模型可參考文獻(xiàn)[12]；XG表示GNSS的誤差狀態(tài)，由接收機(jī)鐘差bclk和接收機(jī)鐘漂dclk組成，狀態(tài)方程為：

（3）

式中：Tclk表示相關(guān)時間，與GNSS接收機(jī)有關(guān)；ωb、ωd表示白噪聲。

1.2 量測模型

緊組合系統(tǒng)采用INS的位置、速度與GNSS接收機(jī)得到的可見星位置、速度推算出慣導(dǎo)偽距和偽距率，將其與GNSS接收機(jī)直接得到的偽距和偽距率之差作為量測信息。量測模型如式（4）所示：

X （4）

式中：、分別為偽距差、偽距率差的量測矩陣；、為偽距差、偽距率差的量測噪聲矩陣。

可觀測度是衡量系統(tǒng)能否準(zhǔn)確估計其狀態(tài)的一種可能性指標(biāo)[13]。卡爾曼濾波的有效性在很大程度上依賴于系統(tǒng)的可觀測度[14]。在緊組合系統(tǒng)中，可見星的數(shù)目較少時，量測矩陣的維數(shù)較少，會導(dǎo)致導(dǎo)航結(jié)果的某些維度不可觀測，卡爾曼濾波會使不可觀測維度產(chǎn)生虛假的濾波增益，導(dǎo)致導(dǎo)航性能下降甚至發(fā)散[15]。使用零速修正方程可以增加速度誤差觀測量，提高系統(tǒng)可觀測度。可觀測度的具體分析可參考文獻(xiàn)[13]。

2 零速修正算法

當(dāng)無人機(jī)進(jìn)入衛(wèi)星拒止區(qū)域，導(dǎo)航系統(tǒng)檢測到可見星數(shù)目突然減少時，會啟動初次零速修正流程。該流程首先通過零速檢測技術(shù)來判斷無人機(jī)是否處于靜止?fàn)顟B(tài)，一旦確認(rèn)無人機(jī)為靜止，隨即啟動零速修正。初次零速修正之后，無人機(jī)繼續(xù)飛行，如果無人機(jī)長時間處于衛(wèi)星拒止環(huán)境下，INS/GNSS系統(tǒng)難以得到GNSS信號的有效修正。隨著時間的增加，系統(tǒng)誤差依然會緩慢增加，為避免積累誤差過大，必要時需要再次啟動零速修正，但零速修正太頻繁會影響任務(wù)執(zhí)行，若零速修正間隔時間太久又會導(dǎo)致定位誤差過大。因此，本文引入無人機(jī)實際導(dǎo)航性能（UAV Actual Navigation Performance， UANP）的概念[16]，用于監(jiān)控?zé)o人機(jī)運(yùn)行過程中導(dǎo)航系統(tǒng)的精度，基于水平位置精度的具體要求，提出了零速修正重啟的約束條件，確保組合導(dǎo)航系統(tǒng)在滿足精度要求的前提下盡可能減少零速修正的重啟次數(shù)。

2.1 零速檢測

無人機(jī)零速狀態(tài)的判定直接影響INS/GNSS組合系統(tǒng)零速修正的效果。由于陀螺儀和加速度計受到常值漂移和隨機(jī)噪聲等影響，即使是在載體靜止的狀態(tài)下，它們各自的輸出值也不會為零，從而導(dǎo)致導(dǎo)航系統(tǒng)計算出無人機(jī)的速度也幾乎不為零。因此，需要有效的檢測方法來判斷無人機(jī)是否處于零速狀態(tài)。

無人機(jī)的零速狀態(tài)一般利用慣性傳感器的測量參數(shù)來判定，文獻(xiàn)[7]給出了4種檢測方法，包括加速度閾值檢測法、角速度閾值檢測法、加速度方差檢測法和加速度角速度組合檢測法，4種方法的效率差別不大。為使零速的判斷更加準(zhǔn)確，盡量減少誤判和漏判[17]的情況，本文采用加速度閾值檢測法和速度閾值檢測法[18]冗余的零速判定規(guī)則，其算法模型如下：

（5）

式中：ak為加速度計k時刻的輸出值；N為窗口大小；v為組合后的速度輸出值。

2.2 ZUPT算法模型

當(dāng)無人機(jī)的實際速度在東、北、上3個方向上都為0時得到的速度觀測量為ZUPT的觀測量[19]，其算法模型如下：

（6）

零速修正階段的量測矩陣如下：

（7）

將式（6）與式（7）合并得到下式：

（8）

由此可知，零速修正時系統(tǒng)整體的觀測量是由速度誤差觀測量與偽距差、偽距率差共同組成的，這樣能夠增加系統(tǒng)的可觀測度。

2.3 零速修正重啟的約束條件

經(jīng)過首次零速修正之后，保證了無人機(jī)后續(xù)飛行中的定位誤差在一個相對安全的范圍內(nèi)，計算導(dǎo)航系統(tǒng)誤差（Navigation System Error， NSE）作為UANP的判斷閾值。飛行過程中，一直監(jiān)測導(dǎo)航系統(tǒng)的UANP，UANP一旦超出NSE，重啟零速修正。

2.3.1 NSE的計算

無人機(jī)在航路飛行時，偏離預(yù)期航路的總的系統(tǒng)誤差（Total System Error， TSE）主要由3部分組成：路徑定義誤差（Path Definition Error， PDE）、飛行控制誤差（Flight Control Error， FCE）和導(dǎo)航系統(tǒng)誤差（NSE），如圖1所示。路徑定義誤差（PDE）在計算中可以忽略不計[20]。飛行控制誤差（FCE）能夠反映無人機(jī)的控制精度，受地空通信傳輸?shù)臅r間延遲、無人機(jī)的飛行技術(shù)以及飛機(jī)周圍大氣擾動的影響，一般可將其限定在一定的范圍內(nèi)。導(dǎo)航系統(tǒng)誤差（NSE）與無人機(jī)所使用的導(dǎo)航系統(tǒng)有關(guān)。

若無人機(jī)在已知寬度的航路上飛行，根據(jù)FCE的大小，即可計算導(dǎo)航系統(tǒng)誤差，公式如下：

（9）

式中：β是緩沖值，用來增加余度；引入系數(shù)ε是為了得到更安全的NSE，并將ε · NSE作為UANP的上限。

2.3.2 UANP的計算

將東向定位誤差表示為δx，北向定位誤差表示為δy，則導(dǎo)航系統(tǒng)的定位誤差E表示為：

（10）

誤差的協(xié)方差矩陣P可以表示為：

（11）

緯度誤差δL、經(jīng)度誤差δλ與東向誤差δx、北向誤差δy的關(guān)系如下所示：

（12）

式中：Re為地球半徑。由此，式（11）可以表示為：

（13）

根據(jù)航路所需導(dǎo)航性能要求，INS/GNSS導(dǎo)航系統(tǒng)95%概率下的水平定位誤差為UANP，表達(dá)式為：

（14）

2.3.3 緩沖值β及系數(shù)ε的選取

緩沖值β的選擇與導(dǎo)航系統(tǒng)的固有誤差有關(guān)，取可見星數(shù)目良好時導(dǎo)航系統(tǒng)的UANP值作為緩沖值。系數(shù)的選擇與導(dǎo)航系統(tǒng)的誤差變化率、減速到0的時間及路寬有關(guān)，旨在對導(dǎo)航系統(tǒng)誤差進(jìn)行縮放，得到更加安全的NSE作為UANP的判斷閾值，一般取值1～2。整體算法流程如圖2所示，啟動零速修正之后，如果衛(wèi)星一直處于拒止環(huán)境下，導(dǎo)航定位系統(tǒng)將持續(xù)循環(huán)飛行、減速、零速修正的過程。

3 仿真實驗及分析

3.1 零速檢測有效性驗證

在表1所列的仿真參數(shù)下進(jìn)行了946次的零速檢測實驗，其中速度的閾值設(shè)為0.99 m/s，加速度的閾值設(shè)為4×10-4 m/s2，窗口N設(shè)為20。將加速度檢測、速度檢測以及二者結(jié)合的零速判斷方法進(jìn)行對比，結(jié)果見表2。

從表2可以看出，相對于單獨(dú)的加速度閾值判斷和單獨(dú)的速度判斷，加速度檢測與速度檢測冗余的零速判斷方法準(zhǔn)確率更高。實驗過程中發(fā)現(xiàn)，在勻速狀態(tài)下，加速度判斷法容易將其誤判為靜止?fàn)顟B(tài)，而速度判斷恰好彌補(bǔ)了這一缺陷。速度判斷法容易在零速狀態(tài)前后出現(xiàn)誤判，而加速度判斷則不會出現(xiàn)這種情況。同時也發(fā)現(xiàn)零速狀態(tài)前后，無人機(jī)的加速度絕對值越大越有利于零速狀態(tài)的判斷。

3.2 INS/GNSS組合導(dǎo)航結(jié)果分析

利用飛行軌跡發(fā)生器仿真無人機(jī)的800 s飛行軌跡，如圖3所示。假設(shè)無人機(jī)在寬度為68 m的樓宇間飛行，F(xiàn)CE設(shè)為常值4.2 m；在上述仿真條件下系數(shù)ε與緩沖值β分別取值為1.2、12.2；經(jīng)過計算，NSE為14.67。

在800 s的仿真時間里，可見星數(shù)目如圖4所示。700 s時無人機(jī)進(jìn)入衛(wèi)星拒止環(huán)境，可見星數(shù)目減為3顆，無人機(jī)啟動零速修正開始減速，零速的狀態(tài)持續(xù)10 s；711 s時再次啟動飛行，飛行過程中一直檢測UANP值，系統(tǒng)在739 s時檢測到了UANP值大于設(shè)定的閾值，無人機(jī)重啟零速修正，到達(dá)零速狀態(tài)后持續(xù)10 s，再次啟動飛行，直到760 s離開拒止環(huán)境，可見星數(shù)目恢復(fù)到4顆以上。UANP值以及真實水平誤差對比如圖5所示。為便于觀察分析，只取600～800 s的實驗結(jié)果進(jìn)行分析，如圖6～圖8所示。

觀察圖6～圖8可以看出，無人機(jī)進(jìn)入衛(wèi)星拒止環(huán)境后，無ZUPT緊組合的UANP值及水平誤差、高度誤差都逐漸增加，且真實水平誤差逐漸超出UANP值，濾波逐漸失效；添加零速修正后，UANP值及誤差水平、高度誤差都明顯減小，且真實誤差始終小于UANP值，說明濾波一直有效，UANP值一直沒有超出設(shè)定的上限ε · NSE。對衛(wèi)星拒止的時間內(nèi)兩種方法下的各項數(shù)據(jù)進(jìn)行對比，具體見表3。

由表3可知，進(jìn)入衛(wèi)星拒止區(qū)域的60 s內(nèi)，相比于無ZUPT的緊組合，ZUPT緊組合的UANP值降低了72.99%，水平誤差降低了90.39%，高度誤差降低了86.47%。由此可知，ZUPT緊組合相比于無ZUPT緊組合的定位效果更好，提高了導(dǎo)航定位性能。

4 結(jié) 語

本文針對無人機(jī)在低空復(fù)雜環(huán)境下飛行時，衛(wèi)星拒止的情況下INS/GNSS緊組合定位誤差發(fā)散的問題，使用零速修正來增加觀測量，解決了定位誤差隨時間增加而增大的問題，提高了導(dǎo)航定位精度。針對長時間拒止環(huán)境下可能引起的零速修正重啟過于頻繁的問題，通過引入無人機(jī)實際導(dǎo)航性能和其他條件獲得再次啟動零速修正的閾值，對導(dǎo)航系統(tǒng)誤差增長進(jìn)行有效約束，保證無人機(jī)在此環(huán)境下具有良好的導(dǎo)航性能。仿真實驗結(jié)果表明，本文提出的方法能夠有效抑制衛(wèi)星拒止情況下定位誤差的增加，同時提高緊組合系統(tǒng)的導(dǎo)航定位性能。

本文的局限性在于：首先，零速檢測方法中的滑動窗口大小以及各閾值取值是根據(jù)實驗經(jīng)驗得出的，不同仿真條件下取值可能不同。其次，設(shè)定無人機(jī)減速到0的時間為1 s，而實際無人機(jī)減速時間是不定的。還需要說明的是，本文設(shè)置的應(yīng)用場景不適用于固定翼無人機(jī)，因為大多數(shù)固定翼無人機(jī)不可在空中懸停，達(dá)不到零速修正的條件。

注：本文通訊作者為徐延枝。

參考文獻(xiàn)

[1] 院魯剛. RNP運(yùn)行下的BDS/GPS/DME組合導(dǎo)航研究[D].天津：中國民航大學(xué)，2023.

[2]高小鵬.基于高精度MEMS慣性器件的多傳感器融合定位系統(tǒng)的設(shè)計與研究[J].物聯(lián)網(wǎng)技術(shù)，2021，11（7）：99-103.

[3]彭宏偉，梁列全，溫梓呈，等.基于MI地磁傳感器的低成本慣性組合導(dǎo)航裝置設(shè)計[J].物聯(lián)網(wǎng)技術(shù)，2022，12（6）：5-8.

[4] DAI H F， BIAN H W， WANG R Y， et al. An INS/GNSS integrated navigation in GNSS denied environment using recurrent neural network [J]. Defence technology，" 2020， 16（2）： 334-340.

[5]侯秀麗，徐寶國，周穎，等.基于GPS/INS/磁力計多傳感器融合的連續(xù)定位[J].傳感技術(shù)學(xué)報，2020，33（9）：1320-1326.

[6] YU Z， JIANG J， YAN P， et al. A lightweight odometry network for GNSS/INS integration during GNSS outages[J]. Applied soft computing， 2024， 151： 111143.

[7] SKOG I， HANDEL P， NILSSON J O， et al. Zero-velocity detection：an algorithm evaluation[J]. IEEE transactions on bio-medical engineering， 2010， 57（11）： 2657-2666.

[8] 王雪麗. 小型無人機(jī)輔助定位系統(tǒng)研究[D].柳州：廣西科技大學(xué)，2019.

[9] 孟祥羽.基于自適應(yīng)Kalman濾波的INS/GNSS組合導(dǎo)航研究[D].長春：吉林大學(xué)，2023.

[10]王可，劉立剛，周斌，等.基于SINS/GNSS/輪速的列車組合導(dǎo)航算法研究[J].系統(tǒng)仿真學(xué)報，2021，33（1）：189-195.

[11]宋麗君，李，付強(qiáng)文，等.基于格網(wǎng)框架的極區(qū)SINS/GPS組合導(dǎo)航算法研究[J].系統(tǒng)仿真學(xué)報，2019，31（8）：1683-1691.

[12]王新龍.慣性導(dǎo)航基礎(chǔ)[M]. 2版.西安：西北工業(yè)大學(xué)出版社，2019.

[13]帥平，陳定昌，江涌. GPS/SINS組合導(dǎo)航系統(tǒng)狀態(tài)的可觀測度分析方法[J].宇航學(xué)報，2004（2）：219-224.

[14] GOSHEN-MESKIN D， BAR-ITZHACK I Y. Observability analysis of piece-wise constant systems—Part I： theory [J]. IEEE transactions on aerospace and electronic systems， 1992， 28（4）： 1056-1067.

[15] 楊元喜.自適應(yīng)動態(tài)導(dǎo)航定位[M].北京：測繪出版社，2006.

[16] SUN R， ZHANG Y C， YE B J. A required navigation performance based approach to monitor the approach to monitor the accuracy and integrity performance of UAVs for delivery applications [D]. Washington Yotto Otchieng， 2018.

[17]賈錚洋. 基于GNSS/MIMU的室內(nèi)外無縫定位算法研究[D].鄭州：戰(zhàn)略支援部隊信息工程大學(xué)， 2018.

[18]常樂，章紅平，高周正，等.零速修正在GNSS/INS緊組合導(dǎo)航中的應(yīng)用研究[J].大地測量與地球動力學(xué)，2016，36（10）：879-883.

[19] 汪天生，李擎.改進(jìn)的零速修正算法在行人自主導(dǎo)航中的應(yīng)用[J].系統(tǒng)仿真學(xué)報，2018，30（11）：4359-4366.

[20]孫淑光.飛機(jī)導(dǎo)航系統(tǒng)[M].北京：科學(xué)出版社，2023.