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

張圣軍

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

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

引言

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

實物圖如下：

一、系統構成

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

二、卡爾曼濾波

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

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

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

（一）卡爾曼濾波過程

建立系統的狀態方程與測量方程。

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

令系統的采樣周期為，得到離散系統的狀態方程：

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

與矩陣的形式如下：

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

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

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

（二）濾波結果

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

在小車控制中

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

三、PID控制

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

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

中斷服務程序的框架：

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

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

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

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

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

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

參考文獻：

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

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

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