基于SINS/BDS的抗差組合導航技術研究

溫哲君 陳安升 陳帥 薄煜明

1.南京理工大學，南京210000 2.北京自動化控制設備研究所，北京100854

無人車在城市峽谷環境下，BDS信號因高樓遮擋往往出現斷續、延遲及干擾等異常狀況，慣組也容易因震蕩而造成野值輸出，進而導致導航結果不準確。

捷聯慣性導航系統(SINS)完全自主、刷新率快、短時精度高，可實現全天候的自主三維定位測速，但是誤差會隨時間累計增大[1]。中國北斗衛星導航系統(BDS)能夠在亞太地區提供實時的速度位置和時間信息(PVT)，且精度不隨時間變化[2]。由于BDS和SINS具有很強的互補性，采用SINS/BDS的組合導航模式對SINS誤差進行實時校正，可以獲得較于單個子系統更好的導航性能[3]。

但是SINS/BDS導航系統在無人車的實際應用中也存在著一些問題，例如衛星信號干擾情況下系統的抗干擾性、故障檢測能力差，噪聲突變導致導航精度下降、魯棒性變弱等。本文以此為出發點，提出一種基于SINS/BDS在城市峽谷等復雜干擾環境下可應用于無人車的自適應組合導航抗差算法，可有效提高系統的魯棒性、抗干擾能力及導航精度。

1 基于新息的自適應卡爾曼濾波

采用速度/位置的松組合卡爾曼濾波方式，選取東北天坐標系作為導航坐標系，將IMU和BDS數據進行數據融合。此外，在常規的卡爾曼濾波基礎上引入基于新息的自適應卡爾曼濾波算法[4]，通過調整噪聲R的統計協方差矩陣的大小以實現對新息序列方差的在線調節，并據此調節濾波增益矩陣的計算，大大減小異常噪聲對濾波的污染，有效控制濾波的發散，從而增強系統的魯棒性和穩定性。

1.1 卡爾曼濾波(KF)

常規的卡爾曼濾波器對SINS誤差模型進行建模，采用15維的狀態變量，具體如下：

(1)

根據SINS誤差傳播模型和慣性器件誤差傳播方程可得到系統的狀態方程為：

(2)

其中狀態轉移矩陣F(t)為：

(3)

FN(t)為慣導模型的誤差矩陣，矩陣各項具體見參考文獻[5]；FS(t)、FM(t)為慣性器件的誤差轉換矩陣，具體如下：

(4)

FM(t)=06×6

(5)

系統噪聲驅動矩陣G(t)和系統噪聲矩陣W(t)分別為：

(6)

(7)

對進行系統離散化后，狀態方程和量測方程為：

(8)

常規集中式卡爾曼濾波器主要有以下5個公式構成[6]，分別為：

狀態一步預測：

(9)

狀態一步預測均方誤差陣：

(10)

濾波增益：

(11)

狀態后驗估值：

(12)

后驗均方誤差陣：

(13)

1.2 基于新息的自適應濾波算法

(14)

根據文獻[7]，新息r(t)為零均值的白噪聲過程，有協方差陣：

(15)

所以考慮采用新息序列的方差來衡量新息的變化，新息的理論方差為：

(16)

本文采用一種長度為L的滑動窗口(L一般小于100)對新息的方差進行實時估計，即

(17)

濾波的統計新息的更新方程變為：

(18)

(19)

將上式代入濾波增益矩陣的更新方程(5)中，得到自適應卡爾曼濾波的增益K*為：

(20)

基于新息的方差匹配自適應濾波方法主要解決外部噪聲發生異常變化時系統無法做出正確響應的問題。其可通過在線實時調節噪聲R的協方差大小以達到對新息方差的調整，并因此根據噪聲變化實時動態調整濾波器的增益，可以有效控制濾波的發散。

2 抗差容錯算法設計

當無人車因路況較差導致強烈振動時，IMU也許出現異常數據，代入積分運算后速度、位置將會隨時間累計。為了確保整個系統在慣組短時間數據異常情況下仍可解算出正確的定位導航信息，需要對慣組數據進行檢驗。當無人車在城市峽谷復雜環境下運動時，衛星信號因高樓遮擋或干擾，往往會出現斷續延遲或異常，所以需要對BDS數據的有效性進行判斷[8]。本文分別對IMU/BDS數據分別進行檢驗，并針對異常情況提出處理策略。

2.1 慣組數據異常檢驗

無人車在城市中正常行駛，不會出現劇烈加速、側滑和跳躍的現象，根據慣組的量程和無人車運動環境綜合考慮設置閾值。當慣組的陀螺儀或加速度計輸出數據超出閾值，則當前時刻IMU數據異常。例如，慣組陀螺儀和加速度計某個軸的閾值為ωgate和fgate，對慣組輸出的每一包數據檢驗是否滿足(21)式，若滿足則繼續進行下一級檢驗；否則為異常，進入異常處理策略。

(21)

其次，根據慣組每個軸的數據輸出進行多項式擬合，從而預測下一次慣組輸出，將預測值乘以比例因子kIMU，與真實輸出值進行對比[9]。

綜合考慮計算量與結果精度選擇進行五階多項式擬合。以慣組的加速度計某個軸為例有：

f(t+τ)=a+bτ+cτ2+dτ3+eτ4+
fτ5(0<τ

(22)

將t、t+T、t+2T、t+3T、t+4T時刻采樣的角速率測量值代入上式可得：

(23)

式中，T為慣組輸出間隔，a、b、c、d、e和f為多項式系數。通過矩陣運算可求出各個擬合系數。將擬合的預測值乘以比例因子，與慣組實際輸出值進行比較，若超出則認為慣組輸出異常，進入異常處理，否則正常。

|f-ffit|

(24)

2.2 慣組數據異常處理策略

若慣組數據通過上述二級檢驗，則認為正常。否則當某級檢驗為異常時，將擬合預測值與上一時刻數據求平均代替當前數據輸出，進而與BDS數據進行信息融合。

(25)

2.3 BDS數據異常檢驗

2.3.1 第一級檢驗

根據衛星導航原理，對BDS數據進行收星數和PDOP、VDOP和HDOP檢驗[10]，若滿足閾值條件則BDS數據正確，繼續進行下一級檢驗；否則為異常進入異常處理。

2.3.2 第二級檢驗

由于城市峽谷環境中BDS信號可能因干擾而存在較大誤差或因高樓遮擋出現斷續延遲等狀況，所以可以利用慣性導航短時精度較高的優勢對BDS數據進行判斷。

當BDS數據更新時，將BDS與SINS解算出的絕對位置與絕對速度進行比較。

(26)

其次，在濾波時刻，將BDS與SINS解算出的位置和速度在濾波周期內的增量進行比較，，若均小于閾值，則認為BDS數據正常；否則為異常。將速度和位置分別檢驗如下：

(27)

式中，PBDS為北斗接收機輸出的位置值，PSINS為SINS解算出的位置值，VBDS為北斗接收機輸出的速度值，VSINS為SINS解算出的速度值，ΔP1為北斗接收機位置RMS誤差，ΔP2為慣性器件位置誤差，ΔP3為組合導航的位置誤差，ΔV1為北斗接收機速度RMS誤差，ΔV2為慣性器件速度誤差，ΔV3為組合導航的速度誤差，Kp和Kv分別為位置比例因子和速度比例因子。

若BDS數據通過上述二級檢驗，則判定BDS數據正常，否則進入BDS數據異常處理流程。

2.4 BDS數據異常處理策略

根據實際情況，本文將BDS數據異常分為長時間異常和短時間異常。

1)BDS數據短時間異常時，根據對系統的建模，正常進行濾波的狀態預測和預測均方誤差的更新，利用狀態預測值對導航結果進行校正，具體如下式所示：

(28)

2)若BDS數據異常時間較長，重新收到有效的BDS數據時，從系統實時性與快速性的角度考慮，用BDS數據重置卡爾曼濾波器，同時重置組合導航位置、速度值。

3)若SINS/BDS數據均異常，SINS和BDS解算出的定位結果勢必存在較大的誤差，組合導航也因此失去意義，系統無法提供精確的導航結果。此時可以采取航位推算(Dead Reckoning，DR)等算法進行導航[11]，本文不做詳細分析。

3 延遲校正補償

3.1 數據傳輸造成時延補償

由于數據的傳輸和接收機軟件的解算需要耗費一定的時間，即從接收機射頻前端收到信號到解算完成傳輸至濾波器存在一定的時間差，且不同硬件平臺傳輸和解算耗時也不同。若選擇BDS數據更新時刻與SINS進行數據融合，會造成二者實時性不匹配，濾波的量測信息有一定誤差。因此，需要對時延造成的誤差進行相應補償。

文中采用的接收機刷新率為1Hz，時統為GPS秒，整秒時刻收到PPS秒脈沖。因為慣性導航短時精度較高，故將上述時間段內的SINS速度/位置增量補償到整秒時刻的BDS速度/位置上，然后再與慣性導航解算結果進行數據融合。

(29)

3.2 濾波造成時間延遲誤差補償

實際應用中，濾波的計算過程需要消耗一定的時間，得到的濾波結果也會存在一定的滯后，如果直接代入反饋校正過程，會造成一定的系統誤差。根據之前所述的濾波方程，狀態轉移矩陣描述了狀態變量隨時間變換的過程[12]，故可以通過使用狀態轉移矩陣來減小濾波計算時間對校正結果的影響，具體過程如圖1：

利用狀態轉移矩陣將計算得到的濾波結果遞推到當前時間，然后再完成對狀態量的校正過程。系統整體解算流程為：在整秒tk時刻進行濾波得到系統狀態量為Xtk，讀取當前時間并計算時間延遲td得到離散化后的狀態轉移矩陣Φtk+td|tk。利用tk+td時刻的狀態轉移矩陣Φtk+td|tk將tk時刻的狀態變量估計Xtk遞推至tk+td時刻，得到當前時刻狀態變量的最優估計Xtk+td，再對系統校正。

4 試驗與分析

為了驗證本文所述系統和算法的抗差容錯性能和導航精度，進行了半實物仿真試驗。試驗以DSP/FPGA為硬件平臺，利用軌跡發生器產生一條軌跡模擬無人車的運動。將慣組置于三軸轉臺上實時模擬載體角度變化，仿真管理接口子系統將慣組發送的實時角速度與軌跡發生器生成的比力信息進行拼包后發送給硬件平臺。試驗軌跡如圖2。

圖1 狀態轉移校正流程

圖2 半實物試驗軌跡圖

試驗中慣組刷新率為200Hz，陀螺儀的零偏和零偏穩定性均為1(°)/h，設置加速度計的零偏和零偏穩定性為0.5mg和0.15mg，衛星接收機的速誤差為0.15m/s(RMS)，位置誤差為10m(RMS)，刷新率為1Hz。半實物仿真試驗實物圖如圖3。

圖3 半實物試驗實物圖

考慮無人車的實際運動環境和慣組的實際量程，慣組數據異常檢驗閾值fgate和ωgate設置為8m/s2和25(°)/s，比例因子kIMU設為1.5；BDS數據異常檢驗中，收星數閾值為5，DOP值閾值為5，比例因子Kp和Kv均取2。

半實物試驗過程中，將本文所述的自適應抗差卡爾曼濾波算法與常規卡爾曼濾波進行比較，并設置了2種檢驗場景：1)無人車經過隧道、城市峽谷等場景時的丟星狀態；2)外界噪聲突變引起的濾波不穩定進而導致導航精度下降。

1)在BDS數據中100s～150s時間段設置DOP值過大，這時BDS數據無法通過前文所述的二級檢驗，從而進入丟星狀態。以東北天坐標系的北向誤差進行說明，試驗結果如圖4～5。

圖4 位置誤差曲線

圖5 速度誤差曲線

在丟星的50s中，常規組合導航算法失效，只能依靠純慣性導航(實線1)，位置誤差為13.08(RMSE)，速度誤差為1.16m/s(RMSE)。而本文所述的抗差導航算法(虛線2)根據慣性系統的建模對系統進行實時修正，位置誤差為6.84m(RMSE)，速度誤差為0.15m/s(RMSE)。從統計結果可以看出，本文所述的算法可在衛星失效情況下大大提升導航的精度。

2)在BDS數據中100s～150s時間段設置位置、速度噪聲增大為正常情況的5倍。以東北天坐標系的北向誤差進行說明，試驗結果如圖6～7。

圖6 位置誤差曲線

圖7 速度誤差曲線

在噪聲異常的50s中，常規的組合導航算法(實線1)由于無法敏感外界噪聲變化，結果出現較大的跳變，呈發散趨勢，其中位置誤差為8.64(RMSE)，速度誤差為0.41m/s(RMSE)。本文所述的自適應抗差導航算法(虛線2)可對噪聲變化進行統計并實時調節濾波中的增益陣，減小量測量的置信度，其中位置誤差為5.01m(RMSE)，速度誤差為0.089m/s(RMSE)。從統計結果可以看出，本文所述的算法可在噪聲情況下大大提升導航的精度，增強系統的魯棒性。

5 結論

對于常規的集中式卡爾曼濾波器(KF)在衛星或慣性數據異常時，導航結果變差甚至濾波發散，導致系統崩潰的問題，設計了一種基于SINS/BDS的自適應抗差容錯導航系統(SINS/BDS-based adaptive fault-tolerant navigation system)，對SINS和BDS數據進行有效性檢驗，并針對異常值進行剔除，做出相應處理措施。同時引入基于新息的自適應算法以減小噪聲導致的系統異常。由于無人車在城市峽谷中行駛時IMU/BDS導航源易受干擾，故其可采用本文所述的自適應抗差導航算法，保證導航精度的同時提高了系統的魯棒性和穩定性。最后通過半實物試驗驗證了整個系統的導航精度和可靠性，信息融合抗差算法設計合理。