基于MSP430 兩輪自平衡小車算法的研究

（彭水苗族土家族自治縣職業教育中心，重慶 409600）

1 前言

1.1 研究意義

應用意義：自平衡車巧妙地運用到了其自身的重力來保持其平衡，并把這種重力用整個控制系統轉換成動力，它本身的重力越大，行駛動能也會變得越大，比較環保。駕駛人員不必擔心如何來使小車自身平衡，平衡小車自身就有穩定控制系統，使其平衡。使用自平衡車解決了殘疾人不能騎自行車的問題。車身小巧靈活，可以作業在狹窄路段以及大轉角的工作場。平衡小車的優點有很多：環保、方便、快捷、靈活，具有很廣闊的應用前景。他將會成為未來的主流。

研究意義：自平衡小車車身不是完全穩定的，因此需要對電機進行控制以此來保持小車平衡的姿勢，由車身整體協調機構轉動電動車的平衡的控制和馬達、驅動輪子、傳感器、軟件，微處理器是多種功能的控制，控制信號由動態方向決定，執行和行為控制是一個具有綜合復雜特點的非線性系統，很難得到有效控制，控制算法比較靈活，具有挑戰性，因此理論研究價值很高。

1.2 本文的研究內容

有很多種方法可以使自平衡小車平衡，而本文主要研究兩種方法，其一是PID 控制算法，PID 調節器又稱為PID控制器[1]，主要是獲得自平衡小車的姿態信號作為電機的輸出信號，同時對自平衡小車電機的運動速度進行有效控制，使得小車保持平衡狀態。另一種算法是卡爾曼濾波算法，為了得到小車姿態準確穩定的信息，姿態檢測算法主要通過對互補濾波融合姿態傳感器(陀螺儀傳感器以及加速度傳感器)數據和卡爾曼濾波器和相結合計算相應平衡的參數[2]。

2 自平衡小車自平衡設計原理

2.1 自平衡小車直立控制

圖1 是單擺的受力分析，能夠分析出普通單擺由不平衡位置恢復到平衡位置的原理。

圖1 普通單擺受力

當單擺離開平衡位置之后，就會受到重力以及拉線的作用力，使物體向平衡的位置移動，最后回到平衡位置點。

2.2 平衡車的機械結構

圖2 平衡車機械結構

小車使用圖2 的結構制作研究，車體分為電池、主控制和電機驅動、姿態信號檢測兩層，電池層用于放置給驅動電機供電的12V6A 聚合物鋰電池，采用由TI 公司的MSP430F149 芯片組成的開發電路板構成，電機驅動層的電機驅動模塊采用L298N，姿態檢測是由MPU6050 芯片組成的GY-521 模塊[3]。

2.3 兩輪車傾倒的受力分析

兩輪車本身是極不穩定的，顯然如果沒有外力作用在車身，那么小車一定會發生傾倒的現象。其受力分析如圖3所示。

圖3 小車受力情況分析圖

在理想情況下，如果此時車身重力M 的方向與車輪支持力H 的方向之差相反時，系統合力為0，因此系統達到穩定狀態，θ 角度很小，可以忽略不計。在現實情況下是由很多的干擾，θ 的角度不是一直為0，只要θ 角有一定的值，即使很小的角度值，M 的方向與H 的方向亦產生了角度，合力就不為0，根據牛頓運動定律可知，由于質量的原因θ 角度越來越大，直至車身倒在地上[4]。

2.4 自平衡小車傾角的測量

六軸陀螺儀可以對當前的角度信號、加速度信號數據進行采集，采用積分的方式對角速度數據進行分析、處理，處理器在程序中通過運算可以得到相應的角度值[5]。因為陀螺儀自身采集數據比較靈敏，根據陀螺儀的時鐘每秒可以采集很多組數據，因此很容易受到周圍環境噪聲的干擾，所以對微型機械陀螺儀的操作要注意不能產生較大的噪聲，保證微型陀螺儀可以安全地工作。陀螺儀在一定的情況下會產生誤差，不能單獨采用陀螺儀來當作角度的測量的器件，所以，另外再利用加速度傳感器和三軸角度姿態傳感器共同完成姿態信號數據的檢測。

圖4 MPU-6050 的具體結構

對于車模傾角和傾角加速度的測量過程中難免出現一些問題，而這些問題增加了感測器的漂移以及設定給予所帶來的影響，所以就選擇了MPU-6050。

MPU-6050 是一款具有著高性能的三軸加速度+三軸陀螺儀的六軸傳感器模塊，基本引腳圖如圖4 所示。加速度模擬信號，如圖5 所示。

圖5 輸出3 軸方向

測量陀螺儀與加速度計，能夠有效的將陀螺儀信號與加速度計的測量值相融合以此來得到較為準確的傾角值。通過卡爾曼濾波的相應的計算，就能使電機轉速得到控制，從而控制自平衡小車的平衡狀態。

3 自平衡小車硬件電路的設計

3.1 MSP430F149 單片機最小系統

本系統的主控制芯片采用的是16 位總線的FLASH 的MSP430 單片機,它的性價比高，集成度也高于一般類型的單片機,在市場得到中廣泛使用。具有可靠性能好、運算速度快、工作穩定的優點。因此，可以用來做一些較為復雜的控制系統。自平衡小車單片機的最小系統電路如圖6 所示[6]。

F149 單片機中運用到：

電機轉速脈沖接口：

TA1-（PIN17）；

電機PWM 驅動接口：

PWM（PIN12,13）：電機驅動PWM 脈沖信號。

電機轉動方向控制：

IN1（P60);

IN2（P61);

IN3（P62）;

IN4（P63);

程序下載接口：

P1.1（PIN13）；

TCK（PIN56）；

REST（PIN57）；

P2.2（PIN22）。

串口監控接口：

UTXT0（PIN32）；

URXT0（PIN33）。

MPU6050 模塊接口

SDA（PIN51）；

SCL（PIN50）。

圖6 MSP430F149 電路圖

圖7 電機驅動電路

3.2 電機驅動電路

上圖7 是由自平衡小車電機的驅動電路。因為L298N的輸入信號電壓和供電電壓是+5V，MSP430F149 單片機可以直接為L298N 提供輸入信號。然而，+5V 的電壓不適合本車模，使該車模不能有一個很好的調速范圍。自平衡小車的電路供電電壓，不能是單一的+5V，還需要有足夠大的啟動電流的電源，才能讓小車有動力保持車身的平衡。聚合物鋰電池是比較合適的動力電源[7]。

為了讓小車保持一定的平衡，驅動電機的PWM 波形使用了定時器 A 自動產生兩路PWM 波形輸出?？梢岳觅Y源捕獲的功能來產生所需要的波形，以便于很好地分析小車平衡的條件。

3.3 MPU6050 電路

MPU6050 芯片將系統中的加速傳感器和陀螺儀傳感器通過整合，可以得到一些有用的數字信號，當前自平衡小車的狀態位置信息由MSP430 控制器通過一定的方式讀出，如IIC 的通信方式的特點就比較符合[8]。所以，該芯片適合本系統的模塊電路，設計起來相對比較簡單。該模塊的電路連接如圖8 所示。

圖8 MPU6050 模塊電路

圖9 系統主要程序框圖

3.4 供電電源電路

因為自平衡小車的控制系統電路是需要+5V 的電源，而電機需要大電流電源+12V 電源供電才能使電機有足夠的爆發力轉動達到平衡，因此，需要采用兩路電源。本系統的控制器電源由3 節5 號電池提供，電機驅動電壓是采用的12V/6A 電流6800MA 聚合物鋰電池提供，這樣才能達到小車啟動時需要大電流的要求。

4 自平衡小車軟件設計

4.1 系統程序結構分析

如圖9 所示，如果要使自平衡小車保持平衡，需要多個子程序，同時也能夠更好的優化程序，子程序可以是PID調節程序、IIC 通信程序、卡爾曼濾波程序、PWM 波形產生程序和車輪轉速檢測程序等[9]。

主程序的運行流程圖相對比較簡單，如圖10 是所示，程序的核心是調用卡爾曼濾波子程序和PID 控制子程序，這兩個子程序包含的內容豐富，下面分別對這兩個主要的子程序進行詳細的理解?？柭鼮V波子程序的任務是將MPU6050 檢測到的信號濾波，但是由于外界干擾較大，直立穩定的小車不斷地向車輪前后方向傾斜。此時，MSP430F149 單片機通過通訊協議，將MPU6050 中小車擺動的角速度和水平方向上的加速度讀取出來，再對其加工處理?？柭鼮V波器通過一定的測量方法可以得到很準確的傾角值。PID 子程序主要是得到的傾角使控制器輸出相應的占空比波形來使電機的運轉得到有效控制，由卡爾曼濾波器輸出的角度來調整電機相應的運轉方向，以此使小車保持平衡。

4.2 卡爾曼融合程序

卡爾曼融合的子程序的流程圖如下圖11 所示，圖中讀取Y 軸加速也就是F149 通過I2C 來控制MPU6050 并傳輸MPU6050 所檢測的加速度、角速度兩個數據?？柭鼮V波器則是將這兩個數據進行濾波、融合得到小車的傾角。

圖10 主程序流程圖

圖11 卡爾曼算法融合數據子程序流程圖

子程序如下：

4.2.1 IIC 通信程序

IIC 串行通信總線。IIC 總線通過串行數據線SDA 和串行時鐘線SCL 兩根線將多個具有IIC 總線接口的器件接到總線上，可以發送和接收數據。ICC 總線分為三種類型的信號傳輸，分別是開始信號、結束信號和應答信號[10]。

本系統的IIC 通信是單向通訊，該系統的MSP430 為主控制器件，同時也是單向接收端，而MPU6050 為發送端的形式進行通訊。

圖12 IIC 通信程序流程圖

因此，由單片機P5.0（SCL）、P5.1（SDA）模擬實現IIC 通信程序的流程圖如上圖12 所示[11]。

4.2.2 卡爾曼濾波器

斯坦利·施密特(Stanley Schmidt)開創卡爾曼濾波這一先河，首次實現了這一濾波方式，阿波羅飛船導航器為飛船提供了有效可靠的導航數據?？柭鼮V波器的實質就是利用遞推估算法總結之前數據的干擾，它同時利用小均方誤差的結合，對前一時刻的估算值和此時刻的測量值來估算下一時刻的值，以此類推，相互影響。這種方法具有效率高的特點，可以解決現實中的很多問題，有效減少誤差，使采集回的信號更加平穩、有效。在工業控制、通信系統、電力系統、環境污染控制、生活家電、航空航天、軍事設施設備星號處理等很多地方都得到了廣泛的應用[12]。在圖像處理方面，也可以利用卡爾曼濾波處理對模糊的圖像進行還原，是圖像顏色分界處變得更細膩、更緩線性的變色?？柭惴ㄈ缦聢D13 所示：

圖13 卡爾曼算法推導

程序2 是卡爾曼濾波的子程序，它是依據圖13 的推導原理來編寫的。它的算法是屬于一種遞推算法。卡爾曼濾波采用一定的變量可以反映出系統的狀態特征。它的模型有兩種，一種是狀態空間模型，該模型反映的是系統運動的規律；而另一種模型，觀測模型則是反映系統觀測值與狀態變量之間的關系[13]。這樣程序就可以自動預判下一時刻的狀態，就可以更快的讓系統穩定。

當加速度和角速度信號經過卡爾曼濾波器，就可以得到一個相對精確、穩定的角度信號。

4.3 計數程序設計

本系統是采用選擇的F149ACLK 為定時器 A 時鐘源,增計數模式來測得自平衡小車當前速度的。自平衡小車的速度需要計算，此過程需要借助于轉速中斷程序。對于自平衡小車速度的測量需要用到兩個中斷子程序。當其中一個中斷子程序獲得“允許測量”信號的時候，開始執行檢測中斷的任務，當檢測到旋轉編碼器脈沖時，進入另外一個中斷子程序，開始計數，為了保證計數的同時不被外界信號所干擾，需要控制系統暫停檢測中斷的任務，同時打開時間計數器[14]。當時間計數器達到計數值時，就會發出不再計數的信號，接到此信號的中斷子程序又開始檢測中斷，以此循環，直至認為控制關閉。具體程序如下所示：

4.4 PID 調節器程序設計

采用PID 調節器對系統響應進行調節，PID 調節又叫PID 控制，閉環自動控制可以增加調節的精度。在基于自動控制正負反饋的原理之上，測量系統的實際值與測量值相兩者之差，實現調節控制系統的相應誤差[15]。本設計將采用比例、積分，微分來控制小車電機轉動方向的速度。

為了保證系統的安全，需要對控制器的輸出值進行適當的限制，然而對于數控來說，對輸出值進行適當的限制，即是要對u(k)進行限制，設置一個限制值u(m)，根據需要設定u(k)與u(m)的關系，對于這種情況還需要對u(k)的積分設置相應的限制值，以此來保護系統響應不受外來信息的干擾。具體的控制程序流程圖如下:

圖14 PID 控制調節器程序的流程圖

5 總結

一般將車模分為平衡、行走和方向三個部分進行控制，三項任務相互耦合，互相作用最終都會達到平衡，本次研究主要是在小車平衡的算法，小車平衡主要是控制好小車輪子的正轉、反轉和速度來保持小車的平衡，在控制平衡過程中，由于不穩定的車身，需要穩定，所以要求控制信號必須平滑且高效，所以這里主要運用了PID 算法和卡爾曼濾波等算法能夠實現這一要求[16]，利用濾波后的平滑數據控制PWM 穩定、有效的控制自平衡小車自身車輪，使其保持相對平衡，這是一個實時動態平衡的過程。系統不斷的檢測整合三軸角速度、三軸加速度、上一時刻姿態數據、預估影響值、車輪的轉速等數據進行整理、融合不斷控制PWM 輸出信號形成閉環控制系統，不斷地使三軸陀螺儀檢測的數據達到預設的值，也就是相對的平衡狀態。