溫哲君 陳安升 陳帥 薄煜明

1.南京理工大學(xué)，南京210000 2.北京自動化控制設(shè)備研究所，北京100854

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

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

但是SINS/BDS導(dǎo)航系統(tǒng)在無人車的實際應(yīng)用中也存在著一些問題，例如衛(wèi)星信號干擾情況下系統(tǒng)的抗干擾性、故障檢測能力差，噪聲突變導(dǎo)致導(dǎo)航精度下降、魯棒性變?nèi)醯取１疚囊源藶槌霭l(fā)點，提出一種基于SINS/BDS在城市峽谷等復(fù)雜干擾環(huán)境下可應(yīng)用于無人車的自適應(yīng)組合導(dǎo)航抗差算法，可有效提高系統(tǒng)的魯棒性、抗干擾能力及導(dǎo)航精度。

1 基于新息的自適應(yīng)卡爾曼濾波

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

1.1 卡爾曼濾波(KF)

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

(1)

根據(jù)SINS誤差傳播模型和慣性器件誤差傳播方程可得到系統(tǒng)的狀態(tài)方程為：

(2)

其中狀態(tài)轉(zhuǎn)移矩陣F(t)為：

(3)

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

(4)

FM(t)=06×6

(5)

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

(6)

(7)

對進行系統(tǒng)離散化后，狀態(tài)方程和量測方程為：

(8)

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

狀態(tài)一步預(yù)測：

(9)

狀態(tài)一步預(yù)測均方誤差陣：

(10)

濾波增益：

(11)

狀態(tài)后驗估值：

(12)

后驗均方誤差陣：

(13)

1.2 基于新息的自適應(yīng)濾波算法

(14)

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

(15)

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

(16)

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

(17)

濾波的統(tǒng)計新息的更新方程變?yōu)椋?/p>

(18)

(19)

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

(20)

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

2 抗差容錯算法設(shè)計

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

2.1 慣組數(shù)據(jù)異常檢驗

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

(21)

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

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

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為多項式系數(shù)。通過矩陣運算可求出各個擬合系數(shù)。將擬合的預(yù)測值乘以比例因子，與慣組實際輸出值進行比較，若超出則認為慣組輸出異常，進入異常處理，否則正常。

|f-ffit|

(24)

2.2 慣組數(shù)據(jù)異常處理策略

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

(25)

2.3 BDS數(shù)據(jù)異常檢驗

2.3.1 第一級檢驗

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

2.3.2 第二級檢驗

由于城市峽谷環(huán)境中BDS信號可能因干擾而存在較大誤差或因高樓遮擋出現(xiàn)斷續(xù)延遲等狀況，所以可以利用慣性導(dǎo)航短時精度較高的優(yōu)勢對BDS數(shù)據(jù)進行判斷。

當(dāng)BDS數(shù)據(jù)更新時，將BDS與SINS解算出的絕對位置與絕對速度進行比較。

(26)

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

(27)

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

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

2.4 BDS數(shù)據(jù)異常處理策略

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

1)BDS數(shù)據(jù)短時間異常時，根據(jù)對系統(tǒng)的建模，正常進行濾波的狀態(tài)預(yù)測和預(yù)測均方誤差的更新，利用狀態(tài)預(yù)測值對導(dǎo)航結(jié)果進行校正，具體如下式所示：

(28)

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

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

3 延遲校正補償

3.1 數(shù)據(jù)傳輸造成時延補償

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

文中采用的接收機刷新率為1Hz，時統(tǒng)為GPS秒，整秒時刻收到PPS秒脈沖。因為慣性導(dǎo)航短時精度較高，故將上述時間段內(nèi)的SINS速度/位置增量補償?shù)秸霑r刻的BDS速度/位置上，然后再與慣性導(dǎo)航解算結(jié)果進行數(shù)據(jù)融合。

(29)

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

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

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

4 試驗與分析

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

圖1 狀態(tài)轉(zhuǎn)移校正流程

圖2 半實物試驗軌跡圖

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

圖3 半實物試驗實物圖

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

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

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

圖4 位置誤差曲線

圖5 速度誤差曲線

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

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

圖6 位置誤差曲線

圖7 速度誤差曲線

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

5 結(jié)論

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