自適應(yīng)無色卡爾曼濾波算法的設(shè)計(jì)及其在機(jī)器人狀態(tài)參數(shù)估計(jì)中的應(yīng)用

徐 偉，蔡忠貿(mào)，汪塬皓，何亞妮，羅錦瑞，郭正陽，曹俊杰，劉曉東，屈斌文

（江漢大學(xué) 工程訓(xùn)練中心，湖北 武漢 430056）

0 引言

機(jī)器人系統(tǒng)要實(shí)現(xiàn)在復(fù)雜環(huán)境中安全、高效的運(yùn)行，需要對其本身的能力狀態(tài)（即機(jī)器人運(yùn)動狀態(tài)及能力模型參數(shù)等）有比較準(zhǔn)確的掌控。控制方可以離線推導(dǎo)機(jī)器人系統(tǒng)的動力學(xué)模型，利用離線辨識方法來確定動力模型中的參數(shù)，便于控制器設(shè)計(jì)。但這種方法存在以下問題：①機(jī)器人系統(tǒng)動力學(xué)模型中不可避免地存在一些難以實(shí)現(xiàn)精確測量的參數(shù)，如飛行機(jī)器人的空氣動力學(xué)參數(shù)，水面、水下機(jī)器人的水動力學(xué)參數(shù)等；②外界環(huán)境和機(jī)器人自身能力狀態(tài)可能會隨時間發(fā)生改變，即具有時變特性的模型參數(shù)，如由能量損耗引起的機(jī)器人總質(zhì)量的變化、由故障引起的模型參數(shù)甚至結(jié)構(gòu)的改變等；③移動機(jī)器人本身有很多狀態(tài)無法直接利用傳感器進(jìn)行測量，如執(zhí)行器部分失效引起的機(jī)器人故障參數(shù)變化。鑒于這些問題，完全靠離線的方式來確定所需要的動力學(xué)模型參數(shù)是不現(xiàn)實(shí)的。這時可以采取的補(bǔ)償措施就是通過在線估計(jì)方法實(shí)時地、高精度地得到相應(yīng)的狀態(tài)參數(shù)信息［1-3］。實(shí)時預(yù)測估計(jì)方法是機(jī)器人系統(tǒng)處于在線模式下，根據(jù)離線參考模型和傳感器信息數(shù)據(jù)，實(shí)時預(yù)測出移動機(jī)器人系統(tǒng)所處環(huán)境狀態(tài)參數(shù)的方法。環(huán)境狀態(tài)參數(shù)主要包括不可預(yù)測的風(fēng)險環(huán)境、目標(biāo)環(huán)境、自然環(huán)境和本身行為能力等［4-5］。

卡爾曼濾波（Kalman filter，KF）方法是在未知環(huán)境中進(jìn)行狀態(tài)參數(shù)估計(jì)的常用方法，但由于卡爾曼濾波方法僅適用于線性系統(tǒng)，限制了其在實(shí)際問題中的應(yīng)用范圍［6-8］。擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）方法是一種非線性濾波估計(jì)方法，通過Taylor 展開式將實(shí)際問題中的非線性系統(tǒng)近似線性化，然后對近似線性系統(tǒng)實(shí)施標(biāo)準(zhǔn)的卡爾曼濾波［9］。但是，擴(kuò)展卡爾曼濾波方法存在計(jì)算量大、濾波不穩(wěn)定的問題，雖然對濾波方法提出了各種分解、補(bǔ)償算法以改善計(jì)算的穩(wěn)定性和提高計(jì)算效率［10-11］，但卡爾曼濾波方法所固有的缺陷仍然無法解決。無色卡爾曼濾波（unscented Kalman filter，UKF）方法通過一定數(shù)量的采樣點(diǎn)，對狀態(tài)的概率密度函數(shù)做近似［12］，具有比擴(kuò)展卡爾曼濾波方法更好的估計(jì)精度和魯棒性，以及相當(dāng)?shù)挠?jì)算量，但由于無色卡爾曼濾波算法仍然是以系統(tǒng)準(zhǔn)確的先驗(yàn)信息為基礎(chǔ)，因此本文提出一種基于MIT 規(guī)則的自適應(yīng)無色卡爾曼濾波（MIT adaptive unscented kalman filter，MIT-AUKF）算法，該方法基于自適應(yīng)控制中利用梯度下降法調(diào)整或更新未知參數(shù)的方法，設(shè)計(jì)以信息方差的實(shí)際值與估計(jì)值的差作為無色卡爾曼濾波器的自適應(yīng)指標(biāo)參數(shù)，在線對濾波器中的噪聲方差參數(shù)進(jìn)行調(diào)節(jié)。

1 無色卡爾曼濾波算法原理

無色卡爾曼濾波算法的原理是基于“高斯分布比其他非線性函數(shù)更易于逼近”這一理論。無色變換是無色卡爾曼濾波的核心基礎(chǔ)，即通過相當(dāng)數(shù)目的采樣點(diǎn)來近似輸入變量的概率分布，將其輸入到非線性函數(shù)中，獲得相應(yīng)的非線性輸出，然后再根據(jù)新的輸出點(diǎn)來獲得輸出量概率分布的近似值［13］。

無色卡爾曼濾波仍然以線性卡爾曼濾波為基礎(chǔ)，只是在求狀態(tài)參數(shù)經(jīng)非線性方程傳播后的均值時，對非線性方程進(jìn)行了無色變換［14］。其具體算法如下：

1）初始化

式中，表示初始狀態(tài)向量的期望均值；P0表示初始狀態(tài)量的協(xié)方差期望值。

2）計(jì)算sigma 點(diǎn)，根據(jù)隨機(jī)向量x的均值和方差Px構(gòu)造一組位于均值附近且關(guān)于其對稱的離散sigma 點(diǎn)，記為χi（i=1，2，…，2n）：

式中，（*）i代表矩陣*的第i列；λ=n（α2-1），α為控制sigma 點(diǎn)分布的常數(shù)。

3）時間更新

式中，Qω表示已知系統(tǒng)的過程噪聲；和表示權(quán)系數(shù)；h(*)表示相應(yīng)維數(shù)的系統(tǒng)方程；uk表示相對應(yīng)的可控制輸入量。

4）測量更新

式中，Qυ表示測量噪聲方差矩陣的估計(jì)值，且

式中，β表示非負(fù)常數(shù)，其作用是使變換后的方差含有部分的高階信息。

2 基于MIT 規(guī)則的自適應(yīng)無色卡爾曼濾波算法的設(shè)計(jì)

與其他非線性估計(jì)方法相比，無色卡爾曼濾波算法在狀態(tài)參數(shù)估計(jì)的準(zhǔn)確性、算法的實(shí)時性等方面有較大優(yōu)勢，但是由于無色卡爾曼濾波本質(zhì)上還是一種卡爾曼類濾波算法，所以無色卡爾曼濾波良好的濾波性必須以系統(tǒng)準(zhǔn)確的先驗(yàn)信息為基礎(chǔ)［15］。但是系統(tǒng)本身器件的不穩(wěn)定性和外部環(huán)境的不確定性給實(shí)際環(huán)境中過程噪聲、測量噪聲統(tǒng)計(jì)特性的準(zhǔn)確描述帶來較大影響。如果先驗(yàn)噪聲統(tǒng)計(jì)特性與系統(tǒng)的實(shí)際噪聲統(tǒng)計(jì)特性不相符時，無色卡爾曼濾波性能可能會嚴(yán)重下降，甚至導(dǎo)致濾波呈現(xiàn)發(fā)散。

為了克服無色卡爾曼濾波上述缺點(diǎn)，設(shè)計(jì)具有噪聲統(tǒng)計(jì)特性自適應(yīng)能力的濾波器是行之有效的方法，即當(dāng)系統(tǒng)的實(shí)際噪聲統(tǒng)計(jì)特性未知或發(fā)生變化時，自適應(yīng)濾波器能夠自動地調(diào)節(jié)自身參數(shù)，以滿足實(shí)驗(yàn)的需求，減少先驗(yàn)噪聲信息對于濾波器性能的影響，有效地提高濾波器的穩(wěn)定性和估計(jì)的準(zhǔn)確性。

本文是基于由麻省理工學(xué)院提出的MIT 規(guī)則，即自適應(yīng)控制中利用梯度下降法調(diào)整或更新未知參數(shù)的方法［16］。該方法以信息方差的實(shí)際值與估計(jì)值的差作為自適應(yīng)無色卡爾曼濾波器的指標(biāo)參數(shù)，在線對無色卡爾曼濾波器中的噪聲方差參數(shù)進(jìn)行調(diào)節(jié)。

在（1）式～（4）式中有6 個需要設(shè)計(jì)的參數(shù)，即：無色變換設(shè)計(jì)參數(shù)α和β、過程噪聲矩陣Qω、測量噪聲矩陣Qυ、初值方差xˉ0、初始矩陣P0。其中，①初值方差和初始矩陣對濾波器估計(jì)方法的影響不大，隨著濾波過程的進(jìn)行可以忽略不計(jì)；②無色變換設(shè)計(jì)參數(shù)α和β僅會對非線性系統(tǒng)的高階近似項(xiàng)產(chǎn)生影響，不能改進(jìn)算法的性能；③作為系統(tǒng)的先驗(yàn)信息，過程噪聲矩陣和測量噪聲矩陣會影響算法估計(jì)的精度和魯棒性，如果兩者的分布特性大于實(shí)際噪聲，在統(tǒng)計(jì)結(jié)果中可能會使濾波發(fā)散；如果分布特性小于實(shí)際噪聲，就會造成真值的不確定范圍過小，產(chǎn)生偏估計(jì)。所以，過程噪聲矩陣和測量噪聲矩陣是在線進(jìn)行自適應(yīng)調(diào)節(jié)的主要參數(shù)。

從算法設(shè)計(jì)角度來看，自適應(yīng)濾波器的設(shè)計(jì)應(yīng)該能夠同時對過程噪聲和測量噪聲進(jìn)行調(diào)節(jié)，但是實(shí)際環(huán)境中很難確定是哪一種噪聲引起的誤差，如果同時對這兩者進(jìn)行更新，算法很難保證穩(wěn)定性。因此，本文進(jìn)行實(shí)驗(yàn)驗(yàn)證時，只討論過程噪聲發(fā)生突變時系統(tǒng)的自適應(yīng)濾波器的性能。

2.1 自適應(yīng)機(jī)制和指標(biāo)參數(shù)

由于過程噪聲和測量噪聲的影響，如果以最小化測量值與估計(jì)值的差為目標(biāo)值，這種算法本身的收斂性有待理論上的證明，而且還存在數(shù)值不穩(wěn)定的問題，導(dǎo)致濾波器性能的下降［17］。所以，本文算法以新息方差的估計(jì)值與真實(shí)值的差作為設(shè)計(jì)濾波器的目標(biāo)值。

新息方差的真實(shí)值可以用以批量為N的實(shí)際新息方差的均值Sk來近似計(jì)算：

式中，yk為時刻k的測量值；為相應(yīng)的無色卡爾曼濾波器估計(jì)值，兩者的差vk是新息值。

由無色卡爾曼濾波算法的測量更新（4）式，可得新息方差的無色卡爾曼濾波估計(jì)值

選取自適應(yīng)的指標(biāo)函數(shù)為

式中，ddiag（*）是以ΔSk為對角線元素的對角矩陣，tr（*）是矩陣的跡。與新息本身相比，ΔSk對系統(tǒng)統(tǒng)計(jì)特性的變化更加敏感。

基于MIT 參數(shù)更新過程，將相對于參數(shù)的負(fù)梯度方向作為指標(biāo)函數(shù)參數(shù)的更新方向，即

式中，為k時刻噪聲協(xié)方差矩陣第m行的對角線元素；ηk是自適應(yīng)調(diào)節(jié)率，用來控制收斂速度。

2.2 自適應(yīng)的無色卡爾曼濾波算法

按照MIT 規(guī)則，自適應(yīng)參數(shù)的迭代機(jī)制可表示為

式中，

由（8）式得

根據(jù)（5）式和（6）式可得

由（7）式計(jì)算濾波估計(jì)值的導(dǎo)數(shù)得

1）初始化

2）時間更新：將上文中標(biāo)準(zhǔn)的無色卡爾曼濾波算法（2）式和（3）式逐項(xiàng)對求導(dǎo)，可得

3）測量更新，對標(biāo)準(zhǔn)的無色卡爾曼濾波測量更新方程（4）逐項(xiàng)對求導(dǎo)，可得

2.3 算法的漸近穩(wěn)定性分析

如（9）式所示，ηk很小，（11）式中最后更新項(xiàng)的值也很小，那么變化比較慢，k時刻之前的估計(jì)值對梯度下降的影響很小，可以近似認(rèn)為是一個常量qm0，參數(shù)的迭代更新方程就會趨近于

式中

則（19）式變化為

這樣，qk就會漸近于下面的常微分方程解的曲線：

式中，g（q）如（20）式所示，可以得出當(dāng)時間k趨向于無窮大時，qk會以概率1 收斂于JE（q）的局部極小點(diǎn)；由于無色卡爾曼濾波是一個穩(wěn)定的穩(wěn)態(tài)濾波過程，（17）式和（18）式在無色卡爾曼濾波基礎(chǔ)上對進(jìn)行求導(dǎo)運(yùn)算，那么（17）式和（18）式所示的系統(tǒng)也應(yīng)該是穩(wěn)定的，進(jìn)而系統(tǒng)的估計(jì)狀態(tài)應(yīng)該是一致有界的，即本文設(shè)計(jì)的自適應(yīng)無色卡爾曼濾波是穩(wěn)定的。

2.4 實(shí)驗(yàn)驗(yàn)證

通過如圖1 的正交全方位輪式移動機(jī)器人來驗(yàn)證自適應(yīng)的無色卡爾曼濾波算法的應(yīng)用。

圖1 正交全方位輪式機(jī)器人Fig.1 Orthogonal omni-directional wheeled robots

移動機(jī)器人的動力學(xué)模型如下：

式中，所有參數(shù)的含義及初值見表1 所示。

表1 參數(shù)含義及初值Tab.1 The meaning and initial values of parameters

定義系統(tǒng)狀態(tài)向量和測量向量為

設(shè)采樣周期為Ts= 0.01 s，系統(tǒng)測量值受到協(xié)方差矩陣為Rv= diag（10-8，10-8，10-8）的白噪聲干擾。選擇無色卡爾曼濾波的參數(shù)如下：

同時，為了驗(yàn)證自適應(yīng)無色卡爾曼濾波算法的估計(jì)性能，假設(shè)系統(tǒng)的過程噪聲在t= 10 s 時發(fā)生突變，使系統(tǒng)在10 s 后先驗(yàn)過程噪聲與實(shí)際情況不相符，以此檢驗(yàn)算法的穩(wěn)定性。

算法的其他參數(shù)設(shè)置如下：

從常規(guī)無色卡爾曼濾波（UKF）算法和基于MIT 規(guī)則的自適應(yīng)無色卡爾曼濾波（MITAUKF）算法的運(yùn)動仿真結(jié)果圖2 和圖3 可以明顯看出：當(dāng)噪聲強(qiáng)度發(fā)生變化時，由于先驗(yàn)信息不能夠準(zhǔn)確反映這種變化，普通無色卡爾曼濾波算法的估計(jì)具有較大的誤差；而基于MIT 規(guī)則的自適應(yīng)無色卡爾曼濾波算法則能通過自適應(yīng)性能在線地估計(jì)過程噪聲方差，在噪聲變化的時刻仍然能保持良好的估計(jì)結(jié)果。同時，由圖3 可以看出，當(dāng)系統(tǒng)實(shí)際的過程噪聲發(fā)生突變時，雖然基于MIT 規(guī)則的自適應(yīng)無色卡爾曼濾波算法給出了較好的估計(jì)結(jié)果，但其狀態(tài)估計(jì)誤差與發(fā)生變化前后相比還是有所增大，因?yàn)橄到y(tǒng)本身的原始噪聲越大，估計(jì)算法的準(zhǔn)確程度就越小，所以該結(jié)果仍然符合客觀物理規(guī)律。

圖2 普通UKF 運(yùn)行結(jié)果Fig.2 Simulations of UKF

圖3 MIT-AUKF 運(yùn)行結(jié)果Fig.3 Simulations of MIT-AUKF

3 結(jié)語

卡爾曼濾波和無色卡爾曼濾波算法在狀態(tài)參數(shù)估計(jì)中應(yīng)用雖然十分廣泛，但其算法自身存在的局限可能會嚴(yán)重影響其性能。本文在基于MIT 規(guī)則的基礎(chǔ)上設(shè)計(jì)了一種帶有自適應(yīng)機(jī)制的無色卡爾曼濾波算法，以過程噪聲矩陣為主要調(diào)節(jié)參數(shù)。以信息方差的實(shí)際值與估計(jì)值的差作為指標(biāo)參數(shù)，利用梯度下降法更新未知參數(shù)實(shí)現(xiàn)自適應(yīng)控制。從理論分析和實(shí)驗(yàn)結(jié)果可以看出，具有自適應(yīng)機(jī)制的無色卡爾曼濾波對于噪聲突變能保持良好的估計(jì)結(jié)果。如該算法的自適應(yīng)機(jī)制（11）式所示，每個自適應(yīng)參數(shù)只對應(yīng)一個可調(diào)節(jié)參數(shù)，調(diào)節(jié)起來相對比較簡單，使程序迭代設(shè)計(jì)更加方便簡潔。該算法對于機(jī)器人系統(tǒng)中無法精確測量的環(huán)境狀態(tài)參數(shù)、具有時變性的機(jī)器人特性參數(shù)等的估計(jì)具有良好的魯棒性，在工程實(shí)踐環(huán)境中具有良好的應(yīng)用前景。