基于改進高斯混合模型的機器人運動狀態估計

葛泉波 王賀彬 楊秦敏 張興國 劉華平

狀態估計理論被廣泛應用于各領域,例如太空監測[1]、無線通信、機器人運動跟蹤以及金融行業等[2].當系統為線性、噪聲統計特性服從高斯分布并完全已知時,卡爾曼濾波器是最優的解決方案[3-4].在大多數實際工程應用中,系統往往存在非線性、非高斯等復雜特征,此時若采用經典卡爾曼濾波方法來對實際系統進行狀態估計,將出現估計精度下降和收斂性變差等情形,甚至會出濾波發散現象,這嚴重制約了經典卡爾曼濾波理論在實際工程中的應用.因此,深入開展非線性非高斯系統下的卡爾曼濾波方法研究具有重要的意義.

近幾十年來,研究人員已提出諸多面向非線性系統的卡爾曼濾波方法,如擴展卡爾曼濾波、無跡卡爾曼濾波、容積卡爾曼濾波(Cubature Kalman filter,CKF)[5],以及一些相應的改進算法.其中,擴展卡爾曼濾波因結構簡單在早期大受歡迎,但該方法只能處理弱非線性問題,當系統非線性增強時其估計性能將急劇下降;與擴展卡爾曼濾波相比,無跡卡爾曼濾波引入了采樣近似技術,估計精度和穩定性有了很大提升;CKF 則是在無跡卡爾曼濾波基礎上提出的一種改進型卡爾曼濾波方法,基于三階球面徑向容積準則來進行采樣近似,是理論上當前最接近貝葉斯濾波的基礎性非線性濾波算法.現有研究表明,CKF 的數值穩定性及濾波精度優于傳統的擴展卡爾曼濾波和無跡卡爾曼濾波,尤其是在高維非線性系統狀態估計中具有更為明顯的算法優勢[6-7].

此外,針對非高斯系統的狀態估計問題研究也取得一些進展,目前已建立了基于高斯和、粒子濾波和極大相關熵等技術在內多種非高斯濾波方法[8].文獻[9]基于最大相關熵準則提出一種適用于非高斯噪聲的卡爾曼濾波算法,但是其高斯核大小對結果影響較大.為了克服這一缺陷,研究者們對極大相關熵準則中的核函數進行一系列的改進[10],如對高斯核大小進行自適應處理,但都難以從根本解決對核敏感的問題.高斯和技術是一種比較直觀和受歡迎的非高斯處理方法之一,其基本原理是采用多個有限高斯項概率密度去逼近非高斯概率密度,即利用多個高斯分布的和去近似表示一個非高斯分布,從而可以基于傳統的卡爾曼濾波框架實現濾波[11].因此,通過將高斯和技術和CKF 結合,可進一步構建相應的濾波算法來解決非線性非高斯系統的狀態估計問題[3,12].事實上,高斯和濾波器設計的關鍵問題主要包括高斯混合模型的參數估計和高斯項優化合并[13-14],其中高斯混合模型參數估計主要采用期望最大化(Expectation-maximum,EM)算法,該算法是由Dempster 等[15]于1977 年提出的一種迭代算法,其通過估計參數的極大似然值來分析不完全數據集,從而達到獲取最佳聚類結果的目的.然而,EM 算法存在對初始值比較敏感、收斂速度慢、容易陷入局部收斂以及需要知道混合成分個數等缺陷,在工程應用受到極大的限制[16-17],依然有待進一步深入改進.此外,在高斯和濾波過程中,高斯項的個數會呈指數型遞增,這使得對高斯項進行合理有效地合并就顯得非常必要.目前典型用于高斯項合并的距離度量包括馬氏(Mahalanobis)距離和Kullback-Leibler (KL)距離[18-19],其中馬氏距離度量傾向于采用剪枝方式來刪除權重較低的高斯項,而KL 距離度量更傾向于合并而不是剪枝方式[19].由于這兩種高斯混合項處理方式機理不同,所產生的高斯項約簡效果也有差異,一般很難從理論上驗證孰優孰劣.

針對上述問題,本文以自由度為3 (包括x、y與朝向)的地面移動機器人的運動狀態估計系統為對象,提出一類改進的非線性非高斯CKF 的設計.該工作主要創新包括建立一種改進的魯棒EM 算法,并提出了基于信息融合技術的高斯項合并方法.在魯棒EM 算法改進方面,本文將引入加權信息量概念來進一步完善EM 算法中的目標函數懲罰項,使得在優化過程中能考慮包括隱含信息量在內的更全面的參數信息.通過這種方式,得到的權重更新參數將更具有代表性,從而減少EM 算法的迭代次數并提高其收斂速度.在高斯項合并方法方面,綜合考慮上述兩類高斯項合并方法原理和性能上的差異性和互補性,基于信息融合技術提出一種新的高斯合并項融合方法:先單獨對馬氏距離和KL 距離度量進行高斯混合項合并,以改善高斯項合并距離度量的合理性;再以兩個距離度量獨立運行輸出的高斯合并項為基礎,提出對兩類高斯合并項進行加權融合的高斯混合項融合方法.基于傳統高斯和容積卡爾曼濾波的設計框架,本文提出新型非線性非高斯濾波方法,并通過理論分析和機器人轉彎運動狀態估計仿真場景來驗證新算法的有效性.

1 問題描述

1.1 系統描述

考慮如下一類非線性非高斯估計系統:

式中,xk∈Rn為系統的狀態變量,zk∈Rm為系統的量測向量,f(·)和h(·)分別為已知的狀態轉移函數和量測函數.wk-1∈Rn為非高斯過程噪聲,vk∈Rm為非高斯量測噪聲.實際上,非高斯噪聲如脈沖噪聲、閃爍噪聲等廣泛存在于工程應用中,會嚴重破壞以二階統計理論為基礎的高斯濾波算法的設計過程[20],因此為了提高濾波器工程應用能力,必須深入研究非高斯濾波方法的設計.

1.2 高斯和容積卡爾曼濾波

根據高斯和定理,任意分布的概率密度函數都能夠用N個高斯項的累加進行近似表示,即利用多個不同權重的高斯項求和去描述非高斯噪聲[11].過程和量測噪聲可用高斯混合模型表示為:

高斯和容積卡爾曼濾波(Gaussian-sum cubature Kalman filter,GSCKF)的基本框架如圖1所示.主要步驟包括:近似高斯分布集合估計、并行容積卡爾曼濾波、高斯項合并和信息融合等.其中,高斯分布集合估計是確定幾個高斯分布的和來近似表示原非高斯分布,并估計出各個高斯分布的特征參數,從而可以建立高斯混合模型;并行容積卡爾曼濾波是以多高斯分布估計為基礎,分別執行單高斯分布下的標準CKF 來獲得相應的狀態估計;隨著估計時間增加,高斯項個數將呈幾何級增長,因此進行高斯項即多個高斯分布的濾波估計結果合并約簡就顯得非常必要;當完成高斯項合并約簡后,將剩余的高斯分布濾波估計進行融合獲得最終的高斯和濾波估計.

圖1 高斯和容積卡爾曼濾波算法流程Fig.1 GSCKF algorithm process

現有的高斯和容積卡爾曼濾波方法研究重點集中在算法設計上,主要包括如下2 個核心問題:1)對于非高斯噪聲特性的參數估計研究.雖然通過高斯和原理可將非高斯噪聲表示為多個高斯分布的混合模型,但應用傳統的EM 算法及改進方法對高斯混合模型的參數進行估計時,往往需要提前確定混合成分個數,并且初始參數值對算法性能的影響較大,因此提高EM 算法的魯棒性對于改善高斯和濾波的性能非常重要;2)對高斯混合項的合并模型降階方法的研究.高斯和濾波過程將產生大量的高斯項,這是非高斯模型高斯化的必然結果.但過多的高斯項必然會對整個濾波過程和結果性能產生不利影響,尤其是高斯項合并結果將直接影響非高斯狀態估計的性能.現有研究大都采用單一距離的合并機制,如基于馬氏距離的高斯項合并,該方法主要傾向于合并均值相近的高斯項.事實上,高斯化后產生的眾多高斯項類型和特征各有不同,單一的合并基準或策略往往難以盡可能有效地衡量出所有需要合并或剪枝的高斯項,同時又需要考慮最大高斯分量數的約束,使得尋找一個合適的動態閾值來降低算法復雜度的同時又能保證算法精度是一個挑戰性的問題.因此,如何設計更加高效的高斯項合并策略成為改善高斯和濾波性能的一個重要方向.

1.3 研究動機

由于準確確定高斯混合模型中混合成分的具體類簇和數量是非常困難的,從而導致傳統EM 算法的應用和性能受到很大的限制.因此,針對第1.1 節的問題1),本文借鑒文獻[21]引進信息量概念的思路,通過對傳統EM 算法目標函數添加懲罰項,優化EM 算法中權重參數的更新規則,得到一種中引進信息量概念來改進EM 算法輸出性能的思想,通過進一步增加傳統EM 算法目標函數懲罰項(即加權信息量)的方式來實現EM 算法中權重參數更新規則的深度優化,從而獲得一種改進的魯棒EM 算法.該改進算法將混合成分個數的初值作為采樣點數,從而能有效改善對初始參數值敏感的問題,隨后應用各個類簇競爭性的關系和迭代過程中刪除權重低的項來獲得混合成分的具體參數,從而EM 算法的魯棒性能夠得到有效改善.

針對問題2),考慮到基于單一距離指標的高斯項合并方式存在適用性和魯棒性等受限問題,加之高斯化得到的高斯混合項具有各自的特性以及每種距離指標擁有天然不同的明確傾向性,使得采用信息融合技術來實現多種距離指標下高斯項合并方法的融合成為可能,因此提出基于多距離指標高斯項的二次融合模式來設計一種改進的高斯混合項合并方法.理論上使用更多的距離指標更夠更加符合和逼近高斯混合項的復雜特征,但為了研究闡述的簡便性和直觀性,本文只討論基于馬氏距離和基于KL 距離的高斯混合項融合合并方法,即將基于上述兩種距離方法的合并結果進行凸組合加權融合[22]來獲得最終的非高斯狀態估計.需要提及的是,一般意義上更多距離指標的融合參與過程可類推獲得,但鑒于計算復雜性和實時性的要求,過多的距離指標使用也會產生一些新的問題.

2 改進魯棒EM 算法

EM 算法是一種針對不完全數據進行參數估計值求解的迭代算法,其在觀測數據的基礎上引進不可觀測的潛在數據,從而將復雜的不完全數據問題轉化為完全數據問題[23],并通過迭代算法完成不完全數據的參數的估計.

2.1 EM 算法

假定存在一組觀測數據X′=(x1,x2,···,xn),其中xi為p維向量,由高斯混合模型生成

式中,αk、μk、Σk分別表示高斯混合模型中第k個高斯項的權重、均值和協方差,c表示混合模型分量的個數即最終類簇.

經典EM 算法的流程如圖2 所示,其中zki為隱含參量,θ0=,混合模型分量數目c,在EM 算法中需要提前給出.

圖2 EM 算法迭代流程Fig.2 EM algorithm process

E-step:給定初始參數θ0,計算隱含參量zki[21]:

M-step:計算新一輪迭代模型參數:

1)混合高斯項的權重為:

2)高斯混合模型分量的均值為:

3)高斯混合模型分量的協方差為:

對于上述傳統EM 算法,一般需要提前給定混合成分的具體個數,且對于初始參數比較敏感,容易陷入局部最優,這些都限制了EM 算法應用.

2.2 一種改進魯棒EM 算法

Yang 等[21]提出一種魯棒EM 算法,通過對傳統EM 算法目標函數添加懲罰項,以優化EM 算法當中高斯混合項的權重更新方式,進而改變混合模型的具體參數更新規則.但添加的懲罰項僅考慮優化權重參數αk,因此只包含混合高斯項的權重信息.本文引入信息量的概念來進行懲罰項的添加,除考慮權重參數外,進一步考慮了數據中的隱含信息量.其中,隱含參量zki代表xi屬于第k個高斯分量的概率,其包含了混合高斯項的權重、均值和協方差.通過在EM 算法的基礎上引進加權信息量的思想,樣本總體加權隱含信息量可表示為,其中代表第i個樣本的含信息量,αk代表每個高斯項的權重.若要使總體的加權信息量最小,則意味著最大化.將其作為懲罰項加入到目標函數中可以得到:

其中

每次迭代時,若αk＜1/n或αk＜0,則認為該高斯項對混合成分的貢獻忽略不計,并移除此高斯項[21],然后重新調整高斯混合項數目c,具體為:

流程如圖3 所示,本文改進魯棒EM 算法具體步驟如下:

圖3 改進魯棒EM 算法迭代流程Fig.3 Improved robust EM algorithm process

本節提出改進魯棒EM 算法,對初始的參數值不敏感,迭代過程中各個高斯項之間是競爭關系,每次迭代刪除權重較小的高斯項,通過不斷迭代可得到混合成分具體參數估計.

3 改進高斯和容積卡爾曼濾波

3.1 高斯和容積卡爾曼濾波

本文將給出非高斯噪聲環境下的高斯和濾波算法(Improved GSCKF,IGSCKF),算法流程如圖4所示.在非高斯噪聲環境下,用改進魯棒EM 算法進行高斯混合模型的求解,通過高斯和原理,使用多個并行的CKF 進行狀態預測與更新,求解得到目標的狀態估計值.然而,隨著時間推移,高斯項的個數會越來越多從而限制算法的使用.為了有效地對高斯項進行合并,本文將利用現有兩類方法來進行高斯項的合并,并將合并后的高斯混合項進行凸組合融合.結合圖4 具體闡述改進后的高斯和容積卡爾曼濾波算法的濾波過程如下.

圖4 改進高斯和容積卡爾曼濾波Fig.4 Improved Gaussian-sum cubature Kalman filter

1)時間更新:

假設已知k－1 時刻xk-1|k-1統計特性為:

根據貝葉斯濾波遞推公式可得k時刻xk的一步預測概率密度估計p(xk|zk-1)為[12]:

式中,各中間變量為:

式中,βr為容積點權重,nx為狀態向量維數,χr為球面徑向規則確定的容積點.

2)量測更新:當接收到k時刻量測值時,可以得到后驗概率密度p(xk|z1:k)為[12]:

式中,各中間變量為:

量測更新后每一項的權重為:

xk|k和Pk|k可表示為:

和

3.2 基于凸組合技術的高斯混合項融合方法

高斯和濾波算法中初始高斯項有I項,經過1次濾波操作后變為I×J×L項,隨著時間的推移,k次濾波結束后將增加到I×Jk×Lk項.高斯項數的增加會導致計算量不斷增加,從而限制該算法的使用.因此,需要對式(26)和式(27)的高斯混合項進行合并,以減少高斯混合項的個數,降低計算的復雜度.就目前而言,已有的高斯混合項合并方法大多基于馬氏距離或者KL 距離,由于機理不同,其產生的效果也不一樣,難以從理論比較兩種算法的優劣性.本文將對這兩種方法合并后的高斯項進行加權融合估計.

3.2.1 基于馬氏距離的高斯項合并

Salmond[18]提出一種高斯項合并方法,首先定義一個馬氏距離為:

設定最大高斯項數為G,一旦高斯項超過該數值則進行合并.此時,首先刪除權重較低的高斯項,然后根據馬氏距離依次將距離最小的高斯項進行合并,高斯項合并的計算可表示為:

3.2.2 基于KL 距離的高斯項合并

B(·)準則[19]是一種基于KL 距離的高斯項合并方法,該算法的核心是計算兩個高斯項的KL 距離,若KL 距離越小則兩個高斯項越相近,此時更趨向于去合并他們.B(·)準則的計算方法定義如下:

依據B(·)準則,依次將最小B(m,n)的兩個高斯項進行合并,原則如下:

3.2.3 基于凸組合的高斯合并項融合

第3.2.1 節和第3.2.2 節分別介紹了2 種基于不同準則的高斯項合并方法,但是其均存在高斯項合并失真的問題.因此,為了保證高斯項合并的保真度,本文將對上述兩種方法得到的高斯合并項進行凸組合融合[22].融合過程可以表示為:

其中

進而,可以得到每一項的后驗概率密度:

每一項的權重為:

對權重歸一化后得到:

最終融合得到xk|k和Pk|k如下:

定理1.融合后的子濾波器精度高于基于不同距離高斯項合并方法中的子濾波器的估計精度[24]:

證明.由式 (35)可得:

同理可得:

從而可知式(41)成立. □

3.2.4 簡要分析

針對非高斯系統的狀態估計問題,通過高斯項混合成分特性參數估計和高斯項合并方式兩個角度的改進來開展新型高斯和容積卡爾曼濾波器設計,主要工作包括:1)算法的權重更新方式得到進一步優化,進而可獲得改進后的混合模型參數更新規則,最終實現非線性高斯和濾波估計性能的提高.此外,該方法不需要提前設定混合成分數目,能夠自適應估計混合成分的具體參數;2)針對單一距離指標的傾向性和基于單一距離指標的高斯項合并方法的局限性,提出基于多模式融合策略的高斯項合并思想,并以馬氏距離與KL 距離為代表給出了兩種高斯項合并方式下的多模式融合計算過程.需要注意的是,不同距離指標下高斯項融合合并實現包括2 種方式,本文采用第2)種方式:1)兩者距離指標計算直接融合形成新距離指標后再進行高斯項合并操作;2)先采用各自距離指標進行高斯項合并后再進行融合操作.同時,從信息論角度采用更多的距離指標能夠實現對高斯混合模型特征更好的表征,相應的融合方法也可以從兩個距離指標融合模式中類推獲得.然而,距離指標類別數增加必然會產生一些新的問題,如類別增加對高斯混合模型特征表達能力改善的量化表示、在狀態估計正方向改善目標下的類別增加優化選擇以及多距離指標融合下高斯項合并過程的收斂性與穩定性分析等,這些問題的進一步研究有助于推動更多距離指標參與下高性能高斯和容積卡爾曼濾波算法的設計.

4 仿真結果與分析

本文以機器人運動狀態估計為仿真場景[5],考慮機器人在進行半圓周運動.定義其運動狀態矢量xk=[xk,yk,ψk]T,其中xk和yk為機器人在x,y軸方向的位置,ψk為其方位角,機器人的運動方程表示為:

觀測模型可以表示為:

機器人的運動模型如下:

式中,控制量u為:

式中,Δt為采樣周期,分別為第l個地標的橫坐標與縱坐標.xk和zk分別為系統狀態向量和量測向量,wk-1和vk分別表示非高斯過程噪聲和非高斯量測噪聲.系統初始狀態,過程噪聲,量測噪聲,均由高斯混合模型表示如下:

式中,各權重參數設定為:αx=0.5,αw=0.3,αv=0.4,其余參數設置為:

4.1 仿真實驗1

為探究改進魯棒EM 算法對非高斯噪聲參數的估計效果,利用第3.2 節中改進魯棒EM 算法對非高斯系統噪聲和量測噪聲各采樣n=1 000 個點,然后進行高斯混合模型參數的求解.以非高斯過程噪聲p(wk)為例,設置改進魯棒EM 初始參數為:

式中,Wi為噪聲采樣數據,為采樣數據均值.

以過程噪聲為例,本文的改進魯棒EM 算法求解高斯混合模型參數的迭代過程如圖5 所示,修改為其中空心圓為噪聲的采樣點,實心點為用改進魯棒 EM 算法估計得到高斯項的均值(用MuE 表示),實線橢圓為其協方差(用SigmaE 表示),三角形與虛線橢圓代表真實的高斯混合模型的均值(用MuR表示)與方差(用SigmaR 表示).圖6 為文獻[21]的魯棒 EM 算法仿真,其中三角形為用魯棒 EM算法估計得到高斯項的均值(用MuE 表示),實線橢圓為其協方差(用SigmaE 表示).

表1 為本文利用估計分布與原始分布的馬氏距離衡量改進魯棒EM 算法前后的性能對比,馬氏距離越小表明估計分布越接近于原始分布,其估計精度越高.從圖5、圖6 和表1 可以看出,改進的魯棒EM算法能有效地解決了EM 算法魯棒性差的缺點,相對于魯棒EM 算法其參數性能、收斂速度都表現優越.以馬氏距離作為衡量標準改進后的魯棒EM 算法性能相比改進前提升83.56%.

圖5 改進魯棒EM 算法迭代過程Fig.5 Improved robust EM algorithm iterative process

圖6 魯棒EM 算法迭代過程Fig.6 Robust EM algorithm iterative process

表1 改進前后魯棒EM 算法對比Table 1 Comparison of robust EM algorithms before and after improvement

同時,由迭代結果可以看出,改進魯棒EM 算法可以解決高斯混合模型的參數求解問題,獲得了較好的高斯混合模型參數,且收斂速度較快.同理,量測噪聲的求解與過程噪聲相同,二者用改進魯棒EM 算法迭代出來的結果為:

4.2 仿真實驗2

仿真實驗2 是為了比較基于馬氏距離的Salmond 方法[18]、基于KL 距離的B(·)準則方法和本文建立的凸組合融合估計方法(GSCKF 和IGSCKF)在跟蹤精度上的優劣關系.在仿真場景中,采用仿真一中改進魯棒EM 算法估計所得到的非高斯噪聲的高斯混合模型參數,作為高斯和容積卡爾曼濾波狀態估計的非高斯噪聲參數,分別對其進行蒙特卡洛仿真,以各算法的均方根誤差(Root mean square error,RMSE)作為目標跟蹤精度的指標.

對機器人進行運動狀態估計的仿真結果如圖7所示,其對應的機器人位置的均方根誤差計算結果如圖8 所示,方位角度誤差如圖9 所示.

圖7 3 種算法對于機器人狀態估計Fig.7 Three algorithms for robot state estimation

圖8 3 種算法的RMSEFig.8 RMSE of three algorithms

圖9 3 種算法的方位角誤差Fig.9 The azimuth error of three algorithms

由圖7～9 可以看出,基于凸組合融合的高斯混合項合并方法的濾波精度要高于常規的Salmond 方法,以及B(·)準則的高斯混合項合并方法.由表2 可以看出,相比于其余兩種方法,本文基于凸組合融合的濾波方法的計算復雜度并沒有明顯增加.仿真結果表明:1)改進的魯棒EM 算法可以很好地解決高斯混合模型的參數估計問題;2)采用凸組合來融合兩種高斯混合項的合并方法,有效提升了濾波算法的精度與可靠性.

表2 3 種算法RMSE 及運行時間Table 2 RMSE and running time of 3 algorithms

5 結束語

針對非高斯非線性環境下的機器人狀態估計問題,本文首先對于非高斯噪聲環境建立高斯混合模型,并利用改進的魯棒EM 算法進行高斯混合模型參數的求解,相比傳統EM 算法,改進魯棒EM 算法對于初始參數不敏感,不需要提前設定混合成分個數.其次,利用由改進魯棒EM 算法得到的高斯混合模型參數,進行高斯和容積卡爾曼濾波的設計.考慮到高斯和濾波過程中會出現高斯項冗余的情況,本文利用凸組合融合,得到了Salmond 方法與B(·)準則方法兩種方法合并后的高斯混合項,并給出相應的理論分析和對比仿真實驗.結果表明,經過融合后濾波算法的狀態估計精度相比于傳統的兩種方法有明顯的提升. 同時,通用場景下的飛行測試數據驗證分析也表明了新方法的有效性.未來進一步究工作包括有效高斯項數目動態優化、非高斯噪聲統計特性動態估計、更多合并距離方法的融合策略以及復雜場景的數據非高斯動態建模和基于可信度高斯和濾波器設計等.

附錄A

對αk求偏導,可得:

對αk的二階導數為:

由式(A3)可見,二階導數小于0 說明J為嚴格凹函數,根據嚴格凹函數的性質可知,嚴格凹函數的任何極大值也是最大值.將式 (A2)兩邊同時乘αk,可得:

將式(A6)代入式(A2),可得: