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

楊勇生,遲景成,姚海慶

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

0 引 言

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

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

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

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

1 模糊Kalman濾波

對于一般的連續系統,其離散化的狀態方程與量測方程為[3]:

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

(1)

Zk=HkXk+Vk.

(2)

式中：Φk,k-1為k-1時刻到k時刻的狀態轉移矩陣；Hk為系統的量測矩陣;Γk,k-1、Wk,k-1、Vk分別為系統的噪聲驅動矩陣、過程噪聲、量測噪聲[4]．

對上述系統應用Kalman濾波基本方程可得擴展Kalman濾波(EKF)的遞推方程:

預測方程:

(3)

(4)

更新方程:

(5)

(6)

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

(7)

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

rk=Zk-HkXk,k-1.

(8)

理想狀態下新息均值為零．在這一狀態下,常規Kalman濾波的量測噪聲方差一直采用初始狀態先驗值,Kalman濾波器的結果較為理想．但是在現實條件下量測噪聲具有顯著的隨機性,量測噪聲保持不變將導致濾波結果精度的下降乃至濾波結果的發散[5]．為了得到精確的濾波值,需要對系統的量測噪聲方差進行實時的調整．

將式(5)改寫為

tkRk]-1

(9)

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

在模糊Kalman濾波當中,FIS決定了濾波結果的質量．現階段多以新息的實測方差與理論方差的比值作為FIS的輸入來進行FIS的設計．下面將介紹一種基于傳感器工作狀態的模糊Kalman濾波．

2 基于傳感器工作狀態的模糊Kalman濾波

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

圖1 搜星數N隸屬度函數

圖2 RTK解算狀態隸屬度函數

圖3 IMU估計誤差d隸屬度函數

圖4 量測誤差調整參數t隸屬度函數

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

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

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

所采用的模糊卡爾曼濾波器的系統結構采用輸出矯正的方式,結構圖如圖6所示:

圖6 模糊Kalman濾波器系統結構圖

3 實驗與結果分析

3.1 仿真實驗與結果分析

根據前文所述方法,系統的狀態方程與量測方程表示為:

(10)

Z=HXINS+VBDS.

(11)

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

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

(12)

系統矩陣與初始參數詳見文獻[3]．系統噪聲WINS設為均值為零,方差為預估值Q的零均值白噪聲．

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

(13)

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

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

圖7 x,y方向誤差

圖8 x方向均方差

圖9 y方向均方差

由仿真可以看出,當量測噪聲保持不變時,提出的模糊Kalman濾波能很好的對車輛的運動信息進行估計．當量測噪聲產生變化時,模糊Kalman濾波可以通過對系統的量測噪聲方差進行調整,使得量測噪聲的變化短時間內不能對濾波結果產生顯著影響,系統仍保持良好的濾波精度,兩個方向的均方差仍保持收斂．

3.2 實驗設計與結果分析

3.2.1 實驗設計

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

圖10 車輛路線示意圖

在實驗中以BDS信號基站作為坐標原點,正東方向為X軸,正北方向為Y軸建立導航坐標系,在此基礎上進行了多次的數據采集工作,以此為基礎對提出的算法進行驗證．

3.2.2 實驗結果與分析

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

圖11 BDS定位結果

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

圖12 濾波結果對比

根據文獻[7]采用的方法,以平均定位誤差作為衡量定位效果優劣的指標,分別將兩種方法所得到的定位結果與車輛真實行駛軌跡進行比較,結果如表1所示．

表1 定位誤差對比

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

4 結 論

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