卡爾曼濾波器在嵌入式控制系統(tǒng)中的應(yīng)用

1、嵌入式控制系統(tǒng)要用卡爾曼濾波器來觀測進(jìn)程中的變量，以便實(shí)現(xiàn)進(jìn)程控制。本文將介紹卡爾曼濾波器設(shè)計(jì)和實(shí)現(xiàn)的基本原理，然后以汽車導(dǎo)航問題為實(shí)例，說明汽車位置控制過程中，如何利用卡爾曼濾波器這個(gè)有效工具對(duì)汽車的當(dāng)前位置進(jìn)行可靠的估計(jì)。 卡爾曼濾波器最初是專為飛行器導(dǎo)航而研發(fā)的，目前已成功應(yīng)用在許多領(lǐng)域中?？柭鼮V波器主要用來預(yù)估那些只能被系統(tǒng)本身間接或不精確觀測的系統(tǒng)狀態(tài)。 許多工程系統(tǒng)和嵌入式系統(tǒng)都需要濾波。例如收到受噪音干擾的無線通訊信號(hào)時(shí)，良好的濾波算法在保留有用信息的同時(shí)，還可以從電磁信號(hào)中消除噪音。又如在電源電壓中，未受干擾的電源是那些為了消除不期望波動(dòng)而進(jìn)行線電壓過濾的電源設(shè)備，如果不消除

2、這些波動(dòng)，將會(huì)縮短電子設(shè)備(如計(jì)算機(jī)和打印機(jī))的壽命。 卡爾曼濾波器適用于觀測進(jìn)程中的變量，從數(shù)學(xué)角度看，卡爾曼濾波器可估計(jì)線性系統(tǒng)的狀態(tài)?？柭鼮V波器不僅能在實(shí)踐中發(fā)揮很好的作用，而且在理論上也頗引人注目，因?yàn)樵诟鞣N濾波器中，卡爾曼濾波器的估計(jì)誤差最小。嵌入式控制系統(tǒng)經(jīng)常需要使用卡爾曼濾波器，因?yàn)橐刂埔粋€(gè)進(jìn)程，首先必須精確地估計(jì)進(jìn)程中的變量。 本文將介紹卡爾曼濾波器設(shè)計(jì)和實(shí)現(xiàn)的基本原理。文章首先引入卡爾曼濾波器算法并用該算法解決汽車導(dǎo)航問題。為了控制汽車的位置，首先必須對(duì)汽車的當(dāng)前位置進(jìn)行可靠的估計(jì)?？柭鼮V波器正是具備這項(xiàng)功能的有效工具。線性系統(tǒng) 為了用卡爾曼濾波器消除信號(hào)中的噪音，被測

3、量的進(jìn)程必須能用線性系統(tǒng)描述。許多物理進(jìn)程，如路上行駛的車輛、圍繞地球軌道運(yùn)轉(zhuǎn)的衛(wèi)星、由繞組電流驅(qū)動(dòng)的電機(jī)軸或正弦射頻載波信號(hào)，均可用線性系統(tǒng)來近似。線性系統(tǒng)是指能用如下兩個(gè)方程描述的簡單進(jìn)程： 狀態(tài)方程： 輸出方程： 在上述方程中，A、B和C均為矩陣，k是時(shí)間系數(shù)，x稱為系統(tǒng)狀態(tài)，u是系統(tǒng)的已知輸入，y是所測量的輸出。w和z表示噪音，其中變量w稱為進(jìn)程噪音，z稱為測量噪音，它們都是向量，因此包含多個(gè)元素。x中包含系統(tǒng)當(dāng)前狀態(tài)的所有信息，但它不能被直接測量。因此要測量向量y，y是受到噪音z干擾的系統(tǒng)狀態(tài)x的函數(shù)。我們可以利用y獲取x的估計(jì)，但不一定必須藉由y的測量值來獲得x的估計(jì)，因?yàn)閥受到了

4、噪音的干擾。 例如在沿直線運(yùn)動(dòng)的汽車模型中，其狀態(tài)由汽車的位置p和速度v構(gòu)成。輸入u是控制加速度，而輸出y則是測量位置。假定每隔T秒時(shí)間都能改變加速度并測量汽車的位置，根據(jù)物理學(xué)基本定律，速度v將由下述方程控制： 也就是說，下一時(shí)刻(T秒之后)的速度將等于當(dāng)前速度加上控制加速度與T的乘積。但前面的方程并未給出vk+1時(shí)刻的精確值，因?yàn)閷?shí)際上該速度將受到陣風(fēng)及其它意外噪音的干擾。這些速度噪音是隨時(shí)間變化的隨機(jī)變量，因此下述方程能更好地反映v的實(shí)際情況： 其中(方程17)表示速度噪音。類似地，可以推導(dǎo)出位置p的方程： 其中(方程18)表示位置噪音。下式提出了由位置和速度構(gòu)成的狀態(tài)向量x： 最后，由

5、于測量輸出是汽車的位置，可以得到線性系統(tǒng)的狀態(tài)方程如下： zk表示由儀器誤差等因素帶來的測量噪音。如果希望藉由反饋系統(tǒng)控制汽車，則需要得到位置p和速度v的精確估計(jì)，換句話說，即需要對(duì)狀態(tài)x進(jìn)行估計(jì)?？柭鼮V波器正是狀態(tài)估計(jì)的有效工具?？柭鼮V波器的原理和算法 假定線性系統(tǒng)的模型如前所述，我們希望利用測量得到的y來估計(jì)系統(tǒng)x的狀態(tài)。由于系統(tǒng)的行為遵循其狀態(tài)方程，而且可以獲得系統(tǒng)的位置測量，那么如何確定狀態(tài)x的最佳估計(jì)呢？我們需要能對(duì)實(shí)際狀態(tài)進(jìn)行精確估計(jì)的預(yù)估器，盡管并不能直接測量該狀態(tài)。那么預(yù)估器又應(yīng)當(dāng)滿足哪些準(zhǔn)則呢？顯然必須滿足以下兩個(gè)條件。 首先，狀態(tài)估計(jì)的平均值應(yīng)等于實(shí)際狀態(tài)的平均值。換言

6、之，我們并不希望估計(jì)出現(xiàn)這樣或那樣的偏差。在數(shù)學(xué)上，人們總希望估計(jì)的期望值應(yīng)等于狀態(tài)的期望值。 其次，我們希望狀態(tài)估計(jì)與實(shí)際狀態(tài)之間的偏差盡可能小。也就是說，不僅希望狀態(tài)估計(jì)的平均值等于實(shí)際狀態(tài)的平均值，而且希望預(yù)估器的狀態(tài)估計(jì)與實(shí)際狀態(tài)之間的偏差盡可能達(dá)到最小。在數(shù)學(xué)上，人們總希望預(yù)估器的誤差方差盡可能最小。 卡爾曼濾波器正是能滿足以上兩條準(zhǔn)則的預(yù)估器，但卡爾曼濾波器解決方案也必須滿足特定的噪音條件。在系統(tǒng)模型中，w表示進(jìn)程噪音，而z表示測量噪音。因此，必須假定w和z的平均值均為0且w與z不相關(guān)。這樣，在任何時(shí)刻k、wk和xk均為不相關(guān)的隨機(jī)變量，而噪音協(xié)方差矩陣Sw和Sz分別為： 進(jìn)程噪音

7、協(xié)方差： 測量噪音協(xié)方差： 其中wT和zT分別表示隨機(jī)噪音向量w和z的轉(zhuǎn)置，E()表示期望值。 現(xiàn)在就可以開始研究卡爾曼濾波器方程了。首先，可以選擇不同的等價(jià)方程來描述卡爾曼濾波器，其中一種表述如下： 卡爾曼濾波器方程由3個(gè)方程組成，每個(gè)方程都包含矩陣運(yùn)算。在上述方程中，a-1表示矩陣求逆，而aT上表示矩陣轉(zhuǎn)置。K矩陣稱為卡爾曼增益，而P矩陣表示估計(jì)誤差的協(xié)方差。 上述狀態(tài)估計(jì)方程相當(dāng)直觀。方程中用來推導(dǎo)k + 1時(shí)刻狀態(tài)估計(jì)的第一項(xiàng)正好等于A與k時(shí)刻狀態(tài)估計(jì)的乘積加上B與k時(shí)刻已知輸入的乘積。如果不再進(jìn)行測量，那么該值就是系統(tǒng)的狀態(tài)估計(jì)。換言之，狀態(tài)估計(jì)可以像系統(tǒng)模型中的狀態(tài)向量一樣傳遞至后

8、續(xù)時(shí)刻。方程中第二項(xiàng)稱為校正項(xiàng)，表示在測量條件下，用于對(duì)傳遞的狀態(tài)估計(jì)進(jìn)行修正的校正量。 K方程的校驗(yàn)表明：測量噪音較大，Sz也將較大，因此K值應(yīng)當(dāng)較小，而且在計(jì)算下一時(shí)刻的測量值y時(shí)，不應(yīng)過分信賴該值；另一方面，測量噪音較小，Sz也將較小，這樣K值應(yīng)當(dāng)較大，而且在計(jì)算下一時(shí)刻的測量值時(shí)，可以充分信賴該值。 汽車導(dǎo)航應(yīng)用實(shí)例 現(xiàn)在考慮先前提出的汽車導(dǎo)航問題。汽車沿直線行駛，位置測量的誤差為10英尺(一個(gè)標(biāo)準(zhǔn)偏差)；命令加速度是常數(shù)，其值為1英尺/秒2；加速度噪音為0.2英尺/秒2 (一個(gè)標(biāo)準(zhǔn)偏差)；位置測量的周期為0.1秒(T=0.1)。那么如何得到運(yùn)動(dòng)汽車位置的最佳估計(jì)？由于存在較大的測量噪

9、音，可以肯定計(jì)算結(jié)果比測量值更好。因?yàn)門=0.1，表征系統(tǒng)的線性模型可用本文提出的系統(tǒng)建模方法推導(dǎo)得到： 因?yàn)闇y量噪音的標(biāo)準(zhǔn)偏差為10英尺，Sz矩陣可簡單地取值為100。 現(xiàn)在推導(dǎo)Sw矩陣。因?yàn)槲恢门c加速度的0.005倍成正比并且加速度噪音為0.2英尺/秒2，因此位置噪音的偏差為(0.005)2×(0.2) 2= 10-6。類似地，因?yàn)樗俣扰c加速度的0.1倍成正比，因此速度噪音的偏差為(0.1)2×(0.2)2=4×10-4。最后，位置噪音和速度噪音的協(xié)方差等于位置噪音的標(biāo)準(zhǔn)方差與速度噪音的標(biāo)準(zhǔn)方差的乘積，計(jì)算結(jié)果為(0.005×0.2)×(0

10、.1×0.2)=2×10-5。將以上計(jì)算結(jié)果相結(jié)合，即可得到矩陣Sw： 接著，為位置和速度設(shè)置最佳的初始值，并將(方程16)初始化為最初估計(jì)的誤差。然后每隔一個(gè)時(shí)間步長執(zhí)行一次卡爾曼濾波器算法。 我們使用Matlab模擬卡爾曼濾波器，模擬結(jié)果參見以下各圖。圖1顯示了車輛的實(shí)際位置和估計(jì)位置，兩條平滑曲線表示實(shí)際位置和估計(jì)位置，由于相隔太近而難以區(qū)分。較為粗糙的曲線表示測量位置。 圖2顯示了實(shí)際位置和測量位置之間的誤差以及實(shí)際位置與卡爾曼濾波器估計(jì)位置之間的誤差。測量誤差的標(biāo)準(zhǔn)偏差約為10英尺，偶爾也會(huì)出現(xiàn)30英尺(3)的峰值。估計(jì)位置的誤差則一直保持約為2英尺。 圖3顯示了

11、卡爾曼濾波器的優(yōu)勢。因?yàn)槠嚨乃俣仁菭顟B(tài)x的一部份，因此我們得到位置估計(jì)的同時(shí)也得到了速度估計(jì)。圖4顯示了實(shí)際速度和卡爾曼濾波器估計(jì)速度之間的誤差。 得到上述結(jié)果的Matlab程序如表1所示。Matlab是一種很容易理解的語言，很像偽碼(pseudocode)，但具有內(nèi)置矩陣運(yùn)算功能。使用Matlab執(zhí)行程序時(shí)，每次得到的結(jié)果會(huì)有所不同，這是因?yàn)槟M中存在隨機(jī)噪音，但總體上與這?給出的各圖都很相近。實(shí)際問題及其擴(kuò)展 卡爾曼濾波器的基本原理很簡單，但濾波器方程對(duì)矩陣代數(shù)的依賴性很強(qiáng)。表2顯示了用C語言描述的卡爾曼濾波器修正方程。表2中的矩陣代數(shù)參考可見。該網(wǎng)站列出的程序非常全面，如果問題足夠小，

12、程序還能相應(yīng)簡化。例如，矩陣的轉(zhuǎn)置： 等于： 因此，如果需要對(duì)矩陣進(jìn)行轉(zhuǎn)置，就可使用上述方程。矩陣運(yùn)算的補(bǔ)充C代碼和卡爾曼濾波可參見。具有超過3個(gè)狀態(tài)的系統(tǒng)將顯著增加程序的代碼長度和計(jì)算量。與矩陣轉(zhuǎn)置相關(guān)的計(jì)算量正比于n3(這?n表示矩陣的維數(shù))。這意味著，如果卡爾曼濾波器的狀態(tài)數(shù)目增加1倍，計(jì)算量將增至原來的8倍。對(duì)于一個(gè)維數(shù)適中的卡爾曼濾波器，幾乎所有的處理工作都花在矩陣運(yùn)算上。但穩(wěn)態(tài)卡爾曼濾波器在提供良好預(yù)估性能的同時(shí)，還能極大地削減運(yùn)算量，因此也不必過于擔(dān)心。在穩(wěn)態(tài)卡爾曼濾波器中，矩陣Kk和Pk均為常量，因此在代碼中可定義為常值，而卡爾曼濾波器方程中唯一需要實(shí)時(shí)實(shí)現(xiàn)的是(方程15)方程

13、，由簡單的乘積和加法運(yùn)算組成(如果使用的是DSP，則由乘積和累加運(yùn)算組成)。 我們已經(jīng)討論了線性系統(tǒng)的狀態(tài)估計(jì)，但如何估計(jì)非線性系統(tǒng)的狀態(tài)呢？事實(shí)上，在工程中幾乎所有的進(jìn)程都是非線性的。一些非線性系統(tǒng)非常接近線性系統(tǒng)，而另一些則差別甚大。這一點(diǎn)早在卡爾曼濾波器發(fā)展初期階段就引起了重視，因而推出了“擴(kuò)展卡爾曼濾波器”。擴(kuò)展卡爾曼濾波器只是簡單地將線性卡爾曼濾波原理推廣到非線性系統(tǒng)。 到目前為止，我們已經(jīng)逐步討論了藉由測量對(duì)狀態(tài)進(jìn)行估計(jì)的步驟。獲得整個(gè)測量時(shí)序記錄后，如果希望將狀態(tài)作為時(shí)間的函數(shù)進(jìn)行預(yù)估，又應(yīng)當(dāng)如何處理呢？例如在上述示例中，如何在獲得必要信息的基礎(chǔ)上，重構(gòu)汽車的行駛軌跡呢？我們似乎

14、完全可以比采用卡爾曼濾波器做得更好，因?yàn)樵趉時(shí)刻預(yù)估系統(tǒng)狀態(tài)時(shí)，我們不僅能測量k時(shí)刻之前(包括k時(shí)刻)的狀態(tài)，還能測量k時(shí)刻之后的狀態(tài)。為此必須修正卡爾曼濾波器，這就是卡爾曼平滑濾波器的由來。 卡爾曼濾波器不僅很實(shí)用，其工作原理也很有吸引力。卡爾曼濾波器可減小估計(jì)誤差的偏差，但如果我們更關(guān)心最壞情況下的估計(jì)誤差，或者希望減小“最壞”的估計(jì)誤差，而不是“平均”誤差時(shí)，問題又該如何解決呢？H濾波器可以解決這個(gè)問題。H濾波器(讀作“H無限大”濾波器，有時(shí)寫作H )是上世紀(jì)八十年代開發(fā)出的卡爾曼濾波替代方案。H濾波器應(yīng)用不如卡爾曼濾波器廣泛，因此也不像卡爾曼濾波器那樣廣為人知，但在某些情況下更為有效。

15、 卡爾曼濾波理論假定進(jìn)程噪音w與測量噪音z互不相關(guān)。如果在系統(tǒng)中進(jìn)程噪音和測量噪音相關(guān)，那么應(yīng)當(dāng)如何處理呢？這就是相關(guān)噪音問題，同樣可以藉由修正卡爾曼濾波器來解決這個(gè)問題。此外，卡爾曼濾波器還需要明確的噪音協(xié)方差Sw和Sz。如果Sw和Sz未知，又該如何處理呢？我們?nèi)绾文軌颢@得狀態(tài)的最佳估計(jì)？這需要再次求助于H濾波器。 卡爾曼濾波涉及的領(lǐng)域很廣泛，不是本文就能盡述的。自1960年問世以來，業(yè)界已經(jīng)發(fā)表了數(shù)千篇有關(guān)卡爾曼濾波的論文，出版了數(shù)十本教材。 發(fā)展歷史及其展望 卡爾曼濾波器最初由Rudolph Kalman開發(fā)設(shè)計(jì)，其研究成果發(fā)表在一家著名的期刊上，因?yàn)樗绕渌说难芯砍晒鼮橥ㄓ靡哺暾?/p>

16、，因而被命名為卡爾曼濾波器，有時(shí)也被稱為Kalman-Bucy濾波器，因?yàn)镽ichard Bucy早期曾與Kalman共同研究該濾波器。 卡爾曼濾波算法的起源可以追溯到1795年由年僅18歲的Karl Gauss提出的最小二乘理論?？柭鼮V波算法與許多新技術(shù)一樣，也是致力于解決特定問題，例如阿波羅空間計(jì)劃中的航天飛機(jī)導(dǎo)航問題。從此，卡爾曼濾波器逐漸應(yīng)用到許多領(lǐng)域，包括各種導(dǎo)航(航空太空、陸地和海洋)、核電站設(shè)備、人口統(tǒng)計(jì)建模、制造業(yè)、地層放射性探測以及模糊邏輯和神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)。 Dan Simon是Cleveland大學(xué)電子工程和計(jì)算機(jī)工程系的教授和工業(yè)顧問。他的教學(xué)和研究方向包括濾波、控制理論

17、、嵌入式系統(tǒng)、模糊邏輯和神經(jīng)網(wǎng)絡(luò)。目前，他正試圖利用卡爾曼濾波器實(shí)現(xiàn)基于DSP的電機(jī)控制器，可以藉由電子郵件：與他聯(lián)系。 參考文獻(xiàn) 1. Gelb, A. Applied Optimal Estimation. Cambridge, MA: MIT Press, 1974。這是一本“過時(shí)但仍然叫座”的參考書，它由麻省理工大學(xué)出版社出版，內(nèi)容淺顯易懂，不僅從介紹最基本的原理入手，而且還非常注重實(shí)際應(yīng)用。 2. Anderson, B. and J. Moore. Optimal Filtering. Englewood Cliffs, NJ: Prentice-Hall, 1979。這是一本純數(shù)學(xué)參考書，非常深?yuàn)W，但有助于理解卡爾曼濾波算法及相關(guān)問題的基本原理。 3. Grewal, M. and A. Andrews. Kalman Filtering Theory and Practice. Englewood Cliffs, NJ: Prentice-Hall, 1993。本書介于以上兩本參考書之間，在理論和實(shí)踐之間架起了一座橋梁。本書的一大特色是磁盤中包含卡爾曼濾波算法的原始碼，不足之處在于原始碼是用Fortran語言編寫的。 4. Sorenson, H