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

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

1 前言

1.1 研究意義

應(yīng)用意義：自平衡車巧妙地運(yùn)用到了其自身的重力來(lái)保持其平衡，并把這種重力用整個(gè)控制系統(tǒng)轉(zhuǎn)換成動(dòng)力，它本身的重力越大，行駛動(dòng)能也會(huì)變得越大，比較環(huán)保。駕駛?cè)藛T不必?fù)?dān)心如何來(lái)使小車自身平衡，平衡小車自身就有穩(wěn)定控制系統(tǒng)，使其平衡。使用自平衡車解決了殘疾人不能騎自行車的問題。車身小巧靈活，可以作業(yè)在狹窄路段以及大轉(zhuǎn)角的工作場(chǎng)。平衡小車的優(yōu)點(diǎn)有很多：環(huán)保、方便、快捷、靈活，具有很廣闊的應(yīng)用前景。他將會(huì)成為未來(lái)的主流。

研究意義：自平衡小車車身不是完全穩(wěn)定的，因此需要對(duì)電機(jī)進(jìn)行控制以此來(lái)保持小車平衡的姿勢(shì)，由車身整體協(xié)調(diào)機(jī)構(gòu)轉(zhuǎn)動(dòng)電動(dòng)車的平衡的控制和馬達(dá)、驅(qū)動(dòng)輪子、傳感器、軟件，微處理器是多種功能的控制，控制信號(hào)由動(dòng)態(tài)方向決定，執(zhí)行和行為控制是一個(gè)具有綜合復(fù)雜特點(diǎn)的非線性系統(tǒng)，很難得到有效控制，控制算法比較靈活，具有挑戰(zhàn)性，因此理論研究?jī)r(jià)值很高。

1.2 本文的研究?jī)?nèi)容

有很多種方法可以使自平衡小車平衡，而本文主要研究?jī)煞N方法，其一是PID 控制算法，PID 調(diào)節(jié)器又稱為PID控制器[1]，主要是獲得自平衡小車的姿態(tài)信號(hào)作為電機(jī)的輸出信號(hào)，同時(shí)對(duì)自平衡小車電機(jī)的運(yùn)動(dòng)速度進(jìn)行有效控制，使得小車保持平衡狀態(tài)。另一種算法是卡爾曼濾波算法，為了得到小車姿態(tài)準(zhǔn)確穩(wěn)定的信息，姿態(tài)檢測(cè)算法主要通過(guò)對(duì)互補(bǔ)濾波融合姿態(tài)傳感器(陀螺儀傳感器以及加速度傳感器)數(shù)據(jù)和卡爾曼濾波器和相結(jié)合計(jì)算相應(yīng)平衡的參數(shù)[2]。

2 自平衡小車自平衡設(shè)計(jì)原理

2.1 自平衡小車直立控制

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

圖1 普通單擺受力

當(dāng)單擺離開平衡位置之后，就會(huì)受到重力以及拉線的作用力，使物體向平衡的位置移動(dòng)，最后回到平衡位置點(diǎn)。

2.2 平衡車的機(jī)械結(jié)構(gòu)

圖2 平衡車機(jī)械結(jié)構(gòu)

小車使用圖2 的結(jié)構(gòu)制作研究，車體分為電池、主控制和電機(jī)驅(qū)動(dòng)、姿態(tài)信號(hào)檢測(cè)兩層，電池層用于放置給驅(qū)動(dòng)電機(jī)供電的12V6A 聚合物鋰電池，采用由TI 公司的MSP430F149 芯片組成的開發(fā)電路板構(gòu)成，電機(jī)驅(qū)動(dòng)層的電機(jī)驅(qū)動(dòng)模塊采用L298N，姿態(tài)檢測(cè)是由MPU6050 芯片組成的GY-521 模塊[3]。

2.3 兩輪車傾倒的受力分析

兩輪車本身是極不穩(wěn)定的，顯然如果沒有外力作用在車身，那么小車一定會(huì)發(fā)生傾倒的現(xiàn)象。其受力分析如圖3所示。

圖3 小車受力情況分析圖

在理想情況下，如果此時(shí)車身重力M 的方向與車輪支持力H 的方向之差相反時(shí)，系統(tǒng)合力為0，因此系統(tǒng)達(dá)到穩(wěn)定狀態(tài)，θ 角度很小，可以忽略不計(jì)。在現(xiàn)實(shí)情況下是由很多的干擾，θ 的角度不是一直為0，只要θ 角有一定的值，即使很小的角度值，M 的方向與H 的方向亦產(chǎn)生了角度，合力就不為0，根據(jù)牛頓運(yùn)動(dòng)定律可知，由于質(zhì)量的原因θ 角度越來(lái)越大，直至車身倒在地上[4]。

2.4 自平衡小車傾角的測(cè)量

六軸陀螺儀可以對(duì)當(dāng)前的角度信號(hào)、加速度信號(hào)數(shù)據(jù)進(jìn)行采集，采用積分的方式對(duì)角速度數(shù)據(jù)進(jìn)行分析、處理，處理器在程序中通過(guò)運(yùn)算可以得到相應(yīng)的角度值[5]。因?yàn)橥勇輧x自身采集數(shù)據(jù)比較靈敏，根據(jù)陀螺儀的時(shí)鐘每秒可以采集很多組數(shù)據(jù)，因此很容易受到周圍環(huán)境噪聲的干擾，所以對(duì)微型機(jī)械陀螺儀的操作要注意不能產(chǎn)生較大的噪聲，保證微型陀螺儀可以安全地工作。陀螺儀在一定的情況下會(huì)產(chǎn)生誤差，不能單獨(dú)采用陀螺儀來(lái)當(dāng)作角度的測(cè)量的器件，所以，另外再利用加速度傳感器和三軸角度姿態(tài)傳感器共同完成姿態(tài)信號(hào)數(shù)據(jù)的檢測(cè)。

圖4 MPU-6050 的具體結(jié)構(gòu)

對(duì)于車模傾角和傾角加速度的測(cè)量過(guò)程中難免出現(xiàn)一些問題，而這些問題增加了感測(cè)器的漂移以及設(shè)定給予所帶來(lái)的影響，所以就選擇了MPU-6050。

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

圖5 輸出3 軸方向

測(cè)量陀螺儀與加速度計(jì)，能夠有效的將陀螺儀信號(hào)與加速度計(jì)的測(cè)量值相融合以此來(lái)得到較為準(zhǔn)確的傾角值。通過(guò)卡爾曼濾波的相應(yīng)的計(jì)算，就能使電機(jī)轉(zhuǎn)速得到控制，從而控制自平衡小車的平衡狀態(tài)。

3 自平衡小車硬件電路的設(shè)計(jì)

3.1 MSP430F149 單片機(jī)最小系統(tǒng)

本系統(tǒng)的主控制芯片采用的是16 位總線的FLASH 的MSP430 單片機(jī),它的性價(jià)比高，集成度也高于一般類型的單片機(jī),在市場(chǎng)得到中廣泛使用。具有可靠性能好、運(yùn)算速度快、工作穩(wěn)定的優(yōu)點(diǎn)。因此，可以用來(lái)做一些較為復(fù)雜的控制系統(tǒng)。自平衡小車單片機(jī)的最小系統(tǒng)電路如圖6 所示[6]。

F149 單片機(jī)中運(yùn)用到：

電機(jī)轉(zhuǎn)速脈沖接口：

TA1-（PIN17）；

電機(jī)PWM 驅(qū)動(dòng)接口：

PWM（PIN12,13）：電機(jī)驅(qū)動(dòng)PWM 脈沖信號(hào)。

電機(jī)轉(zhuǎn)動(dòng)方向控制：

IN1（P60);

IN2（P61);

IN3（P62）;

IN4（P63);

程序下載接口：

P1.1（PIN13）；

TCK（PIN56）；

REST（PIN57）；

P2.2（PIN22）。

串口監(jiān)控接口：

UTXT0（PIN32）；

URXT0（PIN33）。

MPU6050 模塊接口

SDA（PIN51）；

SCL（PIN50）。

圖6 MSP430F149 電路圖

圖7 電機(jī)驅(qū)動(dòng)電路

3.2 電機(jī)驅(qū)動(dòng)電路

上圖7 是由自平衡小車電機(jī)的驅(qū)動(dòng)電路。因?yàn)長(zhǎng)298N的輸入信號(hào)電壓和供電電壓是+5V，MSP430F149 單片機(jī)可以直接為L(zhǎng)298N 提供輸入信號(hào)。然而，+5V 的電壓不適合本車模，使該車模不能有一個(gè)很好的調(diào)速范圍。自平衡小車的電路供電電壓，不能是單一的+5V，還需要有足夠大的啟動(dòng)電流的電源，才能讓小車有動(dòng)力保持車身的平衡。聚合物鋰電池是比較合適的動(dòng)力電源[7]。

為了讓小車保持一定的平衡，驅(qū)動(dòng)電機(jī)的PWM 波形使用了定時(shí)器 A 自動(dòng)產(chǎn)生兩路PWM 波形輸出?？梢岳觅Y源捕獲的功能來(lái)產(chǎn)生所需要的波形，以便于很好地分析小車平衡的條件。

3.3 MPU6050 電路

MPU6050 芯片將系統(tǒng)中的加速傳感器和陀螺儀傳感器通過(guò)整合，可以得到一些有用的數(shù)字信號(hào)，當(dāng)前自平衡小車的狀態(tài)位置信息由MSP430 控制器通過(guò)一定的方式讀出，如IIC 的通信方式的特點(diǎn)就比較符合[8]。所以，該芯片適合本系統(tǒng)的模塊電路，設(shè)計(jì)起來(lái)相對(duì)比較簡(jiǎn)單。該模塊的電路連接如圖8 所示。

圖8 MPU6050 模塊電路

圖9 系統(tǒng)主要程序框圖

3.4 供電電源電路

因?yàn)樽云胶庑≤嚨目刂葡到y(tǒng)電路是需要+5V 的電源，而電機(jī)需要大電流電源+12V 電源供電才能使電機(jī)有足夠的爆發(fā)力轉(zhuǎn)動(dòng)達(dá)到平衡，因此，需要采用兩路電源。本系統(tǒng)的控制器電源由3 節(jié)5 號(hào)電池提供，電機(jī)驅(qū)動(dòng)電壓是采用的12V/6A 電流6800MA 聚合物鋰電池提供，這樣才能達(dá)到小車啟動(dòng)時(shí)需要大電流的要求。

4 自平衡小車軟件設(shè)計(jì)

4.1 系統(tǒng)程序結(jié)構(gòu)分析

如圖9 所示，如果要使自平衡小車保持平衡，需要多個(gè)子程序，同時(shí)也能夠更好的優(yōu)化程序，子程序可以是PID調(diào)節(jié)程序、IIC 通信程序、卡爾曼濾波程序、PWM 波形產(chǎn)生程序和車輪轉(zhuǎn)速檢測(cè)程序等[9]。

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

4.2 卡爾曼融合程序

卡爾曼融合的子程序的流程圖如下圖11 所示，圖中讀取Y 軸加速也就是F149 通過(guò)I2C 來(lái)控制MPU6050 并傳輸MPU6050 所檢測(cè)的加速度、角速度兩個(gè)數(shù)據(jù)。卡爾曼濾波器則是將這兩個(gè)數(shù)據(jù)進(jìn)行濾波、融合得到小車的傾角。

圖10 主程序流程圖

圖11 卡爾曼算法融合數(shù)據(jù)子程序流程圖

子程序如下：

4.2.1 IIC 通信程序

IIC 串行通信總線。IIC 總線通過(guò)串行數(shù)據(jù)線SDA 和串行時(shí)鐘線SCL 兩根線將多個(gè)具有IIC 總線接口的器件接到總線上，可以發(fā)送和接收數(shù)據(jù)。ICC 總線分為三種類型的信號(hào)傳輸，分別是開始信號(hào)、結(jié)束信號(hào)和應(yīng)答信號(hào)[10]。

本系統(tǒng)的IIC 通信是單向通訊，該系統(tǒng)的MSP430 為主控制器件，同時(shí)也是單向接收端，而MPU6050 為發(fā)送端的形式進(jìn)行通訊。

圖12 IIC 通信程序流程圖

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

4.2.2 卡爾曼濾波器

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

圖13 卡爾曼算法推導(dǎo)

程序2 是卡爾曼濾波的子程序，它是依據(jù)圖13 的推導(dǎo)原理來(lái)編寫的。它的算法是屬于一種遞推算法?？柭鼮V波采用一定的變量可以反映出系統(tǒng)的狀態(tài)特征。它的模型有兩種，一種是狀態(tài)空間模型，該模型反映的是系統(tǒng)運(yùn)動(dòng)的規(guī)律；而另一種模型，觀測(cè)模型則是反映系統(tǒng)觀測(cè)值與狀態(tài)變量之間的關(guān)系[13]。這樣程序就可以自動(dòng)預(yù)判下一時(shí)刻的狀態(tài)，就可以更快的讓系統(tǒng)穩(wěn)定。

當(dāng)加速度和角速度信號(hào)經(jīng)過(guò)卡爾曼濾波器，就可以得到一個(gè)相對(duì)精確、穩(wěn)定的角度信號(hào)。

4.3 計(jì)數(shù)程序設(shè)計(jì)

本系統(tǒng)是采用選擇的F149ACLK 為定時(shí)器 A 時(shí)鐘源,增計(jì)數(shù)模式來(lái)測(cè)得自平衡小車當(dāng)前速度的。自平衡小車的速度需要計(jì)算，此過(guò)程需要借助于轉(zhuǎn)速中斷程序。對(duì)于自平衡小車速度的測(cè)量需要用到兩個(gè)中斷子程序。當(dāng)其中一個(gè)中斷子程序獲得“允許測(cè)量”信號(hào)的時(shí)候，開始執(zhí)行檢測(cè)中斷的任務(wù)，當(dāng)檢測(cè)到旋轉(zhuǎn)編碼器脈沖時(shí)，進(jìn)入另外一個(gè)中斷子程序，開始計(jì)數(shù)，為了保證計(jì)數(shù)的同時(shí)不被外界信號(hào)所干擾，需要控制系統(tǒng)暫停檢測(cè)中斷的任務(wù)，同時(shí)打開時(shí)間計(jì)數(shù)器[14]。當(dāng)時(shí)間計(jì)數(shù)器達(dá)到計(jì)數(shù)值時(shí)，就會(huì)發(fā)出不再計(jì)數(shù)的信號(hào)，接到此信號(hào)的中斷子程序又開始檢測(cè)中斷，以此循環(huán)，直至認(rèn)為控制關(guān)閉。具體程序如下所示：

4.4 PID 調(diào)節(jié)器程序設(shè)計(jì)

采用PID 調(diào)節(jié)器對(duì)系統(tǒng)響應(yīng)進(jìn)行調(diào)節(jié)，PID 調(diào)節(jié)又叫PID 控制，閉環(huán)自動(dòng)控制可以增加調(diào)節(jié)的精度。在基于自動(dòng)控制正負(fù)反饋的原理之上，測(cè)量系統(tǒng)的實(shí)際值與測(cè)量值相兩者之差，實(shí)現(xiàn)調(diào)節(jié)控制系統(tǒng)的相應(yīng)誤差[15]。本設(shè)計(jì)將采用比例、積分，微分來(lái)控制小車電機(jī)轉(zhuǎn)動(dòng)方向的速度。

為了保證系統(tǒng)的安全，需要對(duì)控制器的輸出值進(jìn)行適當(dāng)?shù)南拗?，然而?duì)于數(shù)控來(lái)說(shuō)，對(duì)輸出值進(jìn)行適當(dāng)?shù)南拗?，即是要?duì)u(k)進(jìn)行限制，設(shè)置一個(gè)限制值u(m)，根據(jù)需要設(shè)定u(k)與u(m)的關(guān)系，對(duì)于這種情況還需要對(duì)u(k)的積分設(shè)置相應(yīng)的限制值，以此來(lái)保護(hù)系統(tǒng)響應(yīng)不受外來(lái)信息的干擾。具體的控制程序流程圖如下:

圖14 PID 控制調(diào)節(jié)器程序的流程圖

5 總結(jié)

一般將車模分為平衡、行走和方向三個(gè)部分進(jìn)行控制，三項(xiàng)任務(wù)相互耦合，互相作用最終都會(huì)達(dá)到平衡，本次研究主要是在小車平衡的算法，小車平衡主要是控制好小車輪子的正轉(zhuǎn)、反轉(zhuǎn)和速度來(lái)保持小車的平衡，在控制平衡過(guò)程中，由于不穩(wěn)定的車身，需要穩(wěn)定，所以要求控制信號(hào)必須平滑且高效，所以這里主要運(yùn)用了PID 算法和卡爾曼濾波等算法能夠?qū)崿F(xiàn)這一要求[16]，利用濾波后的平滑數(shù)據(jù)控制PWM 穩(wěn)定、有效的控制自平衡小車自身車輪，使其保持相對(duì)平衡，這是一個(gè)實(shí)時(shí)動(dòng)態(tài)平衡的過(guò)程。系統(tǒng)不斷的檢測(cè)整合三軸角速度、三軸加速度、上一時(shí)刻姿態(tài)數(shù)據(jù)、預(yù)估影響值、車輪的轉(zhuǎn)速等數(shù)據(jù)進(jìn)行整理、融合不斷控制PWM 輸出信號(hào)形成閉環(huán)控制系統(tǒng)，不斷地使三軸陀螺儀檢測(cè)的數(shù)據(jù)達(dá)到預(yù)設(shè)的值，也就是相對(duì)的平衡狀態(tài)。