基于卡爾曼濾波和PID控制的兩輪自平衡車

張圣軍

【摘 要】針對兩輪自平衡車的穩(wěn)定和運動過程中的控制問題，我們在信號處理的過程中引入卡爾曼濾波對信號進行處理并且采用傳統(tǒng)的PID控制，將控制過程分為三個部分，即站立、直線運動和轉向。由于車體運動分為這三個部分，并且這三個部分必須幾乎同時控制，所以采用分時控制每一部分的方法，該方法被成功應用于“飛思卡爾”智能車大賽，并且取得良好效果。

【關鍵詞】倒立擺系統(tǒng)；自平衡車；卡爾曼濾波；PID控制

引言

倒立擺系統(tǒng)是控制系統(tǒng)的一個重要的分支和典型的應用。實際上它可以理解成在計算機的控制下，通過對系統(tǒng)各種狀態(tài)參數(shù)的實時分析，使系統(tǒng)在水平方向或垂直方向上的位移和角度（角速度）的偏移量控制在允許的范圍以內，從而使系統(tǒng)保持平衡。自平衡車就是以倒立擺系統(tǒng)為工作原理的成品，兩輪自平衡智能小車直立行走是要求仿照兩輪自平衡電動車的行進模式，讓車模以兩個后輪驅動進行直立行走。近年來，兩輪自平衡電動車以其行走靈活、便利、節(jié)能等特點得到了很大的發(fā)展。國內外有很多這方面的研究，也有相應的產(chǎn)品。相對于傳統(tǒng)的四輪行走的車模競賽模式，車模直立行走在硬件設計、控制軟件開發(fā)以及現(xiàn)場調試等方面提出了更高的要求。

實物圖如下：

一、系統(tǒng)構成

整個模型車分為兩個部分組成，即硬件電路和軟件兩部分。硬件電路主要由加速度計、陀螺儀、微控制器、編碼器、線性CCD、電機驅動電路組成。由微處理器對陀螺儀、濾波電路和加速度計構成的傳感器組進行高速A/D采樣后，通過卡爾曼濾波器對傳感器數(shù)據(jù)進行補償和信息融合，得到準確的姿態(tài)角度信號，此角度信號再通過PID控制器運算，輸出給電子調速器轉換成PWM 信號，進而對電機進行控制。系統(tǒng)結構框圖如下圖所示：

二、卡爾曼濾波

加速度計用于測量物體的線性加速度，加速度計的輸出值與傾角呈非線性關系，隨著傾角的增加而表現(xiàn)為正弦函數(shù)變化。因此對加速度計的輸出進行反正弦函數(shù)處理，才能得到其傾角值。測量數(shù)據(jù)噪聲與帶寬的平方根成正比，即噪聲會隨帶寬的增加而增加。

式中： 為傳感器帶寬（單位為Hz）。因此在設計卡爾曼濾波器時，首先要確定被測加速度的頻率范圍，然后再設計濾波器的參數(shù)，盡量使濾波器的帶寬略高于被測頻率，這樣不僅有助于濾除高頻干擾，而且也有利于降低系統(tǒng)噪聲干擾。但是如果要得到精確的傾角值，帶寬就需要設置得比較小，而這時加速度計動態(tài)響應慢，不適合跟蹤動態(tài)角度運動，如果期望快速的響應，又會引入較大的噪聲。再加上其測量范圍的限制，使得單獨應用加速度計檢測小車傾角并不合適，需要與其他傳感器共同使用。

陀螺儀的作用是用來測量角速度信號，通過對角速度積分，便能得到角度值。陀螺儀本身極易受噪聲干擾，微機械陀螺不能承受較大的震動，同時由于溫度變化、不穩(wěn)定力矩等因素，陀螺儀會產(chǎn)生漂移誤差，并會隨著時間的推移而累加變大，通過積分會使得誤差變得很大。因此，也不能單獨使用陀螺儀作為本系統(tǒng)的傾角傳感器。

（一）卡爾曼濾波過程

建立系統(tǒng)的狀態(tài)方程與測量方程。

其中，為車體傾斜的真是角度，為陀螺儀的常值溫漂，為包含固定偏差的陀螺儀輸出角速度，為陀螺儀測量噪聲，為加速度計經(jīng)處理后得到的角度值，為加速度計的測量噪聲。

令系統(tǒng)的采樣周期為，得到離散系統(tǒng)的狀態(tài)方程：

在進行卡爾曼遞歸運算過程中，需要知道系統(tǒng)過程噪聲協(xié)方差陣 以及測量誤差的協(xié)方差矩陣，對卡爾曼濾波器進行校正。

與矩陣的形式如下：

式中， 和分別是加速度計和陀螺儀測量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對其傳感器數(shù)據(jù)的信任程度，值越小，表明信任程度越高.在該系統(tǒng)中陀螺儀的值更為接近準確值，因此取的值小于的值。

卡爾曼遞歸運算的相關算式：

為時刻的最優(yōu)估計值，為對應的協(xié)方差。為根據(jù)時刻的最優(yōu)估計得到的 時刻的預測值。為卡爾曼增益。為時刻測量的角度值。這一次得到的最優(yōu)估計值，作為下一次預測值得參考，這樣周而復始反復循環(huán)計算這些算式，可以使收斂到最小，這樣就是最優(yōu)值。

（二）濾波結果

在進行卡爾曼濾波過程中，需要根據(jù)實際情況調試與，可以使濾波后的波形平滑，毛刺少，并且與實際波形的相位差小。

在小車控制中

卡爾曼濾波前后的波形如下（綠色為濾波前，紅色為濾波后）：

三、PID控制

在卡爾曼濾波后和信號處理好后，小車的重點就在控制上面。小車的運動可以分為三個部分，即為站立、直線運動和轉向。由于車體運動分為這三個部分，并且這三個部分必須幾乎同時控制，所以采用分時控制每一部分的方法，類似CPU時間分片。

程序的控制和信號處理都是在中端里面執(zhí)行，通過設置全局變量來確定執(zhí)行什么操作。程序設置1毫秒中斷，程序進入中斷后，全局變量加1，判斷全局變量的值，進入不同的執(zhí)行部分，執(zhí)行相應的操作。

中斷服務程序的框架：

其中，除了上述基本框架外，在每次進入中斷后，首先執(zhí)行卡爾曼濾波程序，并且執(zhí)行單片機AD采樣。根據(jù)我們實際獲得的經(jīng)驗，卡爾曼濾波是比較耗時間的，并且AD采樣同樣也是很消耗時間，如果在中斷執(zhí)行的總時間超過1毫秒，這將會影響卡爾曼濾波的效果，因此在這個方面，必須考慮AD所開的通道數(shù)目，當通道數(shù)目過多，將會導致一次中斷服務程序執(zhí)行的時間超過1毫秒。這時可以選擇增加中斷定時時間，進而也就需要改變卡爾曼的 。

在控制中，我們采用傳統(tǒng)PID控制。我們對直立控制采用PD控制，這主要原因是在直立控制中的主要要求就是快速性，因而舍棄積分單元。對于速度控制采用PID控制，積分單元消除誤差。對于方向控制采用PD控制。

在直立控制過程中，P越大，車體抗干擾能力越強，但過大會導致車體振蕩，這時可以通過加大微分參數(shù)消除振蕩。當P達到一定程度時，車體會抖動并不是振蕩，這時調節(jié)D并不能消除抖動。車體抖動的主要原因來源于卡爾曼濾波后的信號精度，當精度比較低時，P過大，會直接放大信號的幅度，進而表現(xiàn)為車體的抖動。

在做車的速度控制過程中，速度控制的周期會影響車體的運行的穩(wěn)定性。在我們實際測試中當速度控制周期定為100毫秒時，低速運行車體比較穩(wěn)定。但當車速度給定為1.3m/s時，車體在轉彎后，恢復平穩(wěn)比較困難。設置速度控制周期為200毫秒后，效果改善明顯。

在方向控制中，我們使用分段PID控制。這主要原因是車體在直道運行時，稍微偏離中心線對車體運行沒有影響，也就是在知道上方向控制不需要太靈敏，太靈敏不利于車在直道的穩(wěn)定和進入彎道的穩(wěn)定。相反，在彎道就需要方向控制的靈敏，靈敏度越高越好，但前提是保證車體的直立基本穩(wěn)定。

在實際中，車體陀螺儀傳感器的安裝水平程度對車體運行是否穩(wěn)定影響很大，安裝越水平越好，越利于車的提速。安裝不水平會出現(xiàn)車載轉彎的過程中加速和減速的現(xiàn)象。

參考文獻：

[1] 百度百科.自平衡電動車[DB/OL]. http：//baike.baidu.com/view/2362793.htm.2013-8-26/2013-8-26.

[2] 馮志勇，曾翰，張力，趙藝欣，黃偉.基于陀螺儀及加速度計信號融合的姿態(tài)角度測量.[J]西南師范大學學報（自然科學版），2011（8）：137-141.

[3] 何友，王國宏，彭應寧編著.多傳感器信息融合及應用[M].2版.北京：電子工業(yè)出版社，2007：12-23.