基于模糊Kalman濾波的港口車輛組合定位方法研究

楊勇生,遲景成,姚海慶

(上海海事大學(xué) 物流科學(xué)與工程研究院,上海 201306)

0 引 言

港口環(huán)境下無人車輛的正常工作依賴于高精度的定位系統(tǒng),磁釘、磁帶導(dǎo)航是目前智能港口中應(yīng)用較為廣泛的定位方式,這些方法滿足了智能港口環(huán)境下無人車輛對于定位精度的要求,但存在建設(shè)成本高、車輛行駛路徑受限等問題．北斗衛(wèi)星導(dǎo)航系統(tǒng)(BDS)/慣性導(dǎo)航系統(tǒng)(INS)組合定位系統(tǒng)是一種較成熟的定位方案,其定位精度高、建設(shè)成本相對低廉,能有效彌補(bǔ)磁釘、磁帶導(dǎo)航中存在的車輛行駛路徑受限等問題,是一種理想的智能港口無人車輛定位解決方案．

BDS/INS組合定位系統(tǒng)多采用Kalman濾波作為主要的濾波方式．港口環(huán)境下金屬建筑物眾多,易對BDS接收機(jī)信號造成遮擋,造成短時間內(nèi)BDS定位精度的下降乃至失效,導(dǎo)致系統(tǒng)量測噪聲的增大進(jìn)而影響濾波效果,針對這一問題,通常采用自適應(yīng)濾波技術(shù)對模型參數(shù)、噪聲特性進(jìn)行修正以改善濾波精度,模糊Kalman濾波是當(dāng)前效果較為理想的方法之一[1].

利用模糊算法與Kalman濾波相結(jié)合的方式,將每一時刻的殘差實測方差與實際方差的比值作為模糊控制器的輸入,修正Kalman濾波中量測噪聲協(xié)方差陣,以提高系統(tǒng)的定位精度與魯棒性[2].

以加速度計采集值作為模糊控制器輸入,自適應(yīng)地估計Kalman濾波量測噪聲方差．二者所提出的方法濾波結(jié)果較好,但實現(xiàn)過程較為繁瑣．本文在將模糊控制器與Kalman濾波相結(jié)合的基礎(chǔ)上,利用BDS傳感器的工作狀態(tài)作為輸入,設(shè)計了一種易于實現(xiàn)的模糊邏輯控制器,利用該控制器對Kalman濾波中量測噪聲方差進(jìn)行實時調(diào)整,以提高在惡劣條件下系統(tǒng)的定位精度．通過港口環(huán)境下無人車輛的實驗數(shù)據(jù)對該算法進(jìn)行了驗證,實驗表明提出的算法相較于傳統(tǒng)算法定位精度更高,且具有更好的魯棒性．

1 模糊Kalman濾波

對于一般的連續(xù)系統(tǒng),其離散化的狀態(tài)方程與量測方程為[3]:

X(k)=Φk,k-1Xk-1+Γk,k-1Wk-1,

(1)

Zk=HkXk+Vk.

(2)

式中：Φk,k-1為k-1時刻到k時刻的狀態(tài)轉(zhuǎn)移矩陣；Hk為系統(tǒng)的量測矩陣;Γk,k-1、Wk,k-1、Vk分別為系統(tǒng)的噪聲驅(qū)動矩陣、過程噪聲、量測噪聲[4]．

對上述系統(tǒng)應(yīng)用Kalman濾波基本方程可得擴(kuò)展Kalman濾波(EKF)的遞推方程:

預(yù)測方程:

(3)

(4)

更新方程:

(5)

(6)

Pk=[I-KkHk]Pk,k-1.

(7)

式中將實際量測量與量測量的估計值的差值定義為預(yù)報殘差,通稱新息,即

rk=Zk-HkXk,k-1.

(8)

理想狀態(tài)下新息均值為零．在這一狀態(tài)下,常規(guī)Kalman濾波的量測噪聲方差一直采用初始狀態(tài)先驗值,Kalman濾波器的結(jié)果較為理想．但是在現(xiàn)實條件下量測噪聲具有顯著的隨機(jī)性,量測噪聲保持不變將導(dǎo)致濾波結(jié)果精度的下降乃至濾波結(jié)果的發(fā)散[5]．為了得到精確的濾波值,需要對系統(tǒng)的量測噪聲方差進(jìn)行實時的調(diào)整．

將式(5)改寫為

tkRk]-1

(9)

式中,tk是通過模糊推理系統(tǒng)(FIS)得到的在k時刻對量測誤差方差進(jìn)行修正的數(shù)值[6]．相較于傳統(tǒng)Kalman濾波,模糊Kalman濾波通過不斷地對量測方差進(jìn)行調(diào)整,以得到更為精確的濾波結(jié)果．

在模糊Kalman濾波當(dāng)中,FIS決定了濾波結(jié)果的質(zhì)量．現(xiàn)階段多以新息的實測方差與理論方差的比值作為FIS的輸入來進(jìn)行FIS的設(shè)計．下面將介紹一種基于傳感器工作狀態(tài)的模糊Kalman濾波．

2 基于傳感器工作狀態(tài)的模糊Kalman濾波

在BDS系統(tǒng)中,實時動態(tài)(RTK)載波相位差分技術(shù)的解算狀態(tài)是影響定位精度的重要因素;在相同解算狀態(tài)下,系統(tǒng)搜星數(shù)對定位的精度有著一定的影響;在車速固定的情況下,BDS位置與估算位置距離的大小也是衡量BDS信號好壞的一個標(biāo)準(zhǔn)．根據(jù)以上特點(diǎn),采用多輸入單輸出的設(shè)計模式,將上述三個指標(biāo)進(jìn)行模糊化作為FIS的輸入,輸出值為t,設(shè)計了模糊邏輯控制器．用N表示BDS的搜星數(shù),將其劃分為四個模糊集,分別是Z、PS、PM、PB;根據(jù)RTK的四種解算狀態(tài)將其劃分為1、2、3、4四個模糊集,分別對應(yīng)單點(diǎn)解、差分解、固定解與浮點(diǎn)解;根據(jù)GPS位置與估算位置差值d的大小,將其劃分為PS、PM、PB三個模糊集．選取合適的隸屬度函數(shù)對以上各個模糊集進(jìn)行描述,得到的隸屬度函數(shù)如圖1、圖2、圖3所示:

圖1 搜星數(shù)N隸屬度函數(shù)

圖2 RTK解算狀態(tài)隸屬度函數(shù)

圖3 IMU估計誤差d隸屬度函數(shù)

圖4 量測誤差調(diào)整參數(shù)t隸屬度函數(shù)

模糊邏輯控制器的輸出分為Z、PM、PB三個模糊集,其隸屬度函數(shù)如圖4所示．

根據(jù)經(jīng)驗,當(dāng)BDS的解算狀態(tài)為固定解的時候,定位精度最高,差分解、浮點(diǎn)解次之,單點(diǎn)解定位精度最差;在解算狀態(tài)相同的條件下,BDS定位精度隨搜星數(shù)的增加而提高;車輛勻速行駛的情況下,BDS估算位置與加速度計估算位置相差越大,BDS數(shù)據(jù)出現(xiàn)故障的幾率越大．根據(jù)以上經(jīng)驗設(shè)計了模糊規(guī)則．同時考慮到模糊規(guī)則的數(shù)量對算法的運(yùn)行速度的影響,通過忽略小概率事件的方式對模糊規(guī)則進(jìn)行了簡化(例如當(dāng)搜星數(shù)小于等于4時解算狀態(tài)為固定解的概率幾乎為零,故在設(shè)計模糊規(guī)則的時候?qū)⑦@一組合忽略)．經(jīng)過試驗與調(diào)整,得到模糊邏輯控制器的輸出如圖5所示:

圖5 模糊邏輯控制器輸出(RTK-d)

所采用的模糊卡爾曼濾波器的系統(tǒng)結(jié)構(gòu)采用輸出矯正的方式,結(jié)構(gòu)圖如圖6所示:

圖6 模糊Kalman濾波器系統(tǒng)結(jié)構(gòu)圖

3 實驗與結(jié)果分析

3.1 仿真實驗與結(jié)果分析

根據(jù)前文所述方法,系統(tǒng)的狀態(tài)方程與量測方程表示為:

(10)

Z=HXINS+VBDS.

(11)

選取INS的東、北向位置誤差,東、北向速度誤差,歐拉角誤差,加速度計東、北、天向零偏,陀螺儀東、北、天向漂移作為狀態(tài)變量,即:

XINS=[δxδyδVxδVyδψδθx

(12)

系統(tǒng)矩陣與初始參數(shù)詳見文獻(xiàn)[3]．系統(tǒng)噪聲WINS設(shè)為均值為零,方差為預(yù)估值Q的零均值白噪聲．

選擇BDS與INS的位置、偏航角誤差作為量測量,則量測量為:

(13)

式中，xBDS、yBDS、ψBDS分別為BDS得到的車輛在導(dǎo)航坐標(biāo)系下的坐標(biāo)位置與偏航角．量測矩陣H=[I3×3O10×10]．量測噪聲方差陣VBDS是方差為預(yù)估值R的零均值白噪聲．

仿真中假設(shè)載體由原點(diǎn)向正東方向勻速行駛,初始速度為東向10 m/s,北向0 m/s,初始東向與北向的位置誤差為[2 2],初始速度誤差為[0.1 0.1 0.1],東北天向初始平臺誤差為[20" 20" 20"],加速度計常值漂移為1×10-4g,陀螺儀的隨機(jī)漂移為3 deg/h．采樣周期T為1 s．在0 s—600 s期間量測噪聲保持預(yù)估值,在600 s —1 200 s期間將量測噪聲變?yōu)轭A(yù)估值的5倍,同時保持其他參數(shù)不變,將對應(yīng)的RTK解算狀態(tài)變?yōu)?,誤差d變?yōu)?,以模擬BDS信號受建筑物遮擋的情況．仿真結(jié)果如圖7、圖8、圖9所示:

圖7 x,y方向誤差

圖8 x方向均方差

圖9 y方向均方差

由仿真可以看出,當(dāng)量測噪聲保持不變時,提出的模糊Kalman濾波能很好的對車輛的運(yùn)動信息進(jìn)行估計．當(dāng)量測噪聲產(chǎn)生變化時,模糊Kalman濾波可以通過對系統(tǒng)的量測噪聲方差進(jìn)行調(diào)整,使得量測噪聲的變化短時間內(nèi)不能對濾波結(jié)果產(chǎn)生顯著影響,系統(tǒng)仍保持良好的濾波精度,兩個方向的均方差仍保持收斂．

3.2 實驗設(shè)計與結(jié)果分析

3.2.1 實驗設(shè)計

在模擬港口環(huán)境下的無人車輛上進(jìn)行了數(shù)據(jù)采集．?dāng)?shù)據(jù)采集過程如下:無人車上分別搭載了北斗星通公司生產(chǎn)的M600雙天線定位定向全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)接收機(jī)與Inertial labs公司生產(chǎn)的型號為IMU-P(Industrial)的慣性測量單元(IMU),負(fù)責(zé)GPS信號與加速度信號的采集．GNSS接收機(jī)的水平方向上雙頻RTK的定位精度為±(10+0.5×10-6×D) mm(D為定位天線之間的距離,實驗中設(shè)為3 m),垂直方向上定位精度為±(10+0.5×10-6×D) mm;IMU中陀螺儀的隨機(jī)漂移為3 deg/h,加速度計的常值漂移誤差為1×10-4．車輛沿預(yù)定路線行駛,依次經(jīng)過空曠地帶、完全遮擋地帶與半遮擋地帶,車輛行駛路線如下圖所示,依次經(jīng)過A、B、C、D四個點(diǎn),如圖10所示:

圖10 車輛路線示意圖

在實驗中以BDS信號基站作為坐標(biāo)原點(diǎn),正東方向為X軸,正北方向為Y軸建立導(dǎo)航坐標(biāo)系,在此基礎(chǔ)上進(jìn)行了多次的數(shù)據(jù)采集工作,以此為基礎(chǔ)對提出的算法進(jìn)行驗證．

3.2.2 實驗結(jié)果與分析

實驗中,由BDS得到的車輛定位數(shù)據(jù)如圖11所示.

圖11 BDS定位結(jié)果

BDS在空曠地帶定位結(jié)果較為理想．當(dāng)車輛駛?cè)胝趽醯貛?、半遮擋地?如岸橋下方、集裝箱空隙之間)時,由于金屬建筑物對信號的屏蔽,BDS的定位精度產(chǎn)生了明顯的下降,且車輛運(yùn)動軌跡不連續(xù),無法滿足港口內(nèi)無人車輛的定位需求,需要依賴BDS/INS組合導(dǎo)航系統(tǒng)得到更為理想的定位數(shù)據(jù)．在組合導(dǎo)航系統(tǒng)中,根據(jù)建立的模型和采集的數(shù)據(jù),分別采用傳統(tǒng)Kalman濾波與基于傳感器工作狀態(tài)的模糊Kalman濾波兩種濾波算法進(jìn)行數(shù)據(jù)的處理,得到兩種算法的濾波結(jié)果如圖12所示.

圖12 濾波結(jié)果對比

根據(jù)文獻(xiàn)[7]采用的方法,以平均定位誤差作為衡量定位效果優(yōu)劣的指標(biāo),分別將兩種方法所得到的定位結(jié)果與車輛真實行駛軌跡進(jìn)行比較,結(jié)果如表1所示．

表1 定位誤差對比

在整個濾波過程中,提出的模糊Kalman濾波的濾波結(jié)果相較于傳統(tǒng)Kalman濾波的結(jié)果更為平滑．空曠地帶下,二者的濾波結(jié)果相似,平均定位誤差相互接近．在遮擋與半遮擋的區(qū)域內(nèi),BDS信號較弱,傳統(tǒng)Kalman濾波過度依賴BDS提供的量測信息,當(dāng)車輛處于遮擋區(qū)域時,濾波結(jié)果差,平均定位誤差大幅增加,增幅達(dá)102%;提出的濾波算法根據(jù)模糊邏輯控制器的輸出對量測噪聲進(jìn)行了及時的調(diào)整,降低了在遮擋情況下BDS失效對系統(tǒng)定位精度的影響,由于模糊邏輯控制器的輸出為非線性的,故在試驗車輛進(jìn)出遮擋區(qū)域之時濾波結(jié)果相對于車輛的真實軌跡有一定的偏差,但相對于傳統(tǒng)Kalman濾波平均誤差增幅較小,增幅僅為42%,同時濾波結(jié)果更接近于車輛的真實運(yùn)動軌跡．

4 結(jié) 論

本文針對港口環(huán)境下無人車輛的定位需求,提出了一種基于傳感器工作狀態(tài)的模糊Kalman濾波算法．提出了一種基于傳感器狀態(tài)的模糊邏輯控制器,通過對傳感器狀態(tài)的分析對量測噪聲方差進(jìn)行實時調(diào)整,提高了無人車輛的定位精度．建立了系統(tǒng)的時變模型并進(jìn)行了仿真,在此基礎(chǔ)上在類港口環(huán)境下的無人車輛上進(jìn)行了實驗驗證．實驗表明,提出的算法能準(zhǔn)確地反映車輛的運(yùn)動狀態(tài),描述車輛的真實運(yùn)動,并且相較于傳統(tǒng)的Kalman濾波有著更好的魯棒性,在BDS信號受到干擾的情況下仍保持較高的定位精度,同時該系統(tǒng)易于實現(xiàn),能有效解決港口環(huán)境下無人車輛的定位問題．