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

張秀堅

摘 要 自平衡小車的平衡就像是一個鐘擺，它的狀態(tài)是不平衡的，因此它必須要有一個足夠使其穩(wěn)定平衡的控制力，來使其平衡。自平衡小車工作原理是將小車的加速度和當前的姿態(tài)信息進行整合，小車的空間姿態(tài)三軸數(shù)據(jù)和運動的空間加速度由MPU-6050來測量，小車車輪的轉(zhuǎn)速由光電編碼器所測得的脈沖信號來獲得。在運動學的基礎(chǔ)之上，由TI公司的MSP430芯片處理數(shù)據(jù)，運算得出保持平滑的姿態(tài)電機所需數(shù)據(jù)，再把這個數(shù)據(jù)來控制驅(qū)動馬達的PWM波形的輸出占空比， 以此來保持車體的平衡。小車的實際測試結(jié)果表明：小車在直立平衡時傾斜角度從小車的側(cè)面看為-6°。使用自平衡車不但體積小，而且結(jié)構(gòu)簡單，具體特點為工作空間狹窄，但動作靈活，在安全、軍事等領(lǐng)域應(yīng)用前景非常廣泛。

關(guān)鍵詞 陀螺儀 MSP430F149 光電編碼器 PWM 互補濾波 數(shù)據(jù)融合

中圖分類號：TN29 文獻標識碼：A 文章編號：1007-0745（2020）03-0009-07

1 前言

1.1 研究意義

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

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

1.2 本文的研究內(nèi)容

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

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

2.1 自平衡小車直立控制

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

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

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

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

2.3 兩輪車傾倒的受力分析

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

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

2.4 自平衡小車傾角的測量

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

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

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

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

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

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

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

F149單片機中運用到：

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

TA1-（PIN17）;

電機PWM驅(qū)動接口：

PWM（PIN12，13）：電機驅(qū)動PWM脈沖信號。

電機轉(zhuǎn)動方向控制：

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）。

3.2 電機驅(qū)動電路

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

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

3.3 MPU6050電路

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

3.4 供電電源電路

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

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

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

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

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

4.2 卡爾曼融合程序

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

子程序如下：

void Angle_Calcu（void）

{

//------加速度--------------------------

//換算關(guān)系：16384 LSB/g

//角度較小時，? ?x=sinx得到角度（弧度），? ? deg = rad*180/3.14

//因為? x>=sinx，? 故乘以 1.2 適當放大

Accel_x? = GetData（ACCEL_YOUT_H）;? ? ?//讀取Y軸加速度

Angle_ax? = （Accel_x - 1100） /16384;? ? ? //去除零點偏移，計算得到角度（弧度）

Angle_ax? = Angle_ax*1.2*180/3.14;? ? ?//弧度轉(zhuǎn)換為度，

//-------角速度-------------------------

Gyro_y? = GetData（GYRO_YOUT_H）;//

Gyro_y? = -（Gyro_y + 30）/16.4;? ? ? ?//計算角速度值，? ?負號為反方向

//-------卡爾曼濾波融合-----------------------

Kalman_Filter（Angle_ax，Gyro_y）;? ? //卡爾曼濾波計算傾角

}

（程序1? 卡爾曼濾波融合子程序）

4.2.1 IIC通信程序

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

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

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

4.2.2 卡爾曼濾波器

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

//******卡爾曼參數(shù)************

float Q_angle=0.002;

float Q_gyro=0.01;

float R_angle=0.003;

float dt=0.008;//dt為kalman濾波器采樣時間;

char? C_0 = 1;

float Q_bias， Angle_err;

float PCt_0， PCt_1， E;

float K_0， K_1， t_0， t_1;

float Pdot[4] ={0，0，0，0};

float PP[2][2] = { { 1， 0 }，{ 0， 1 } };

//*********************************************************

// 卡爾曼濾波

//*********************************************************

void Kalman_Filter（float Accel，float Gyro）

{

Angle+=（Gyro - Q_bias） * dt; //先驗估計

Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協(xié)方差的微分

Pdot[1]=- PP[1][1];

Pdot[2]=- PP[1][1];

Pdot[3]=Q_gyro;

PP[0][0] += Pdot[0] * dt;? ?// Pk-先驗估計誤差協(xié)方差微分的積分

PP[0][1] += Pdot[1] * dt;? ?// =先驗估計誤差協(xié)方差

PP[1][0] += Pdot[2] * dt;

PP[1][1] += Pdot[3] * dt;

Angle_err = Accel - Angle;//zk-先驗估計

PCt_0 = C_0 * PP[0][0];

PCt_1 = C_0 * PP[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * PP[0][1];

PP[0][0] -= K_0 * t_0; //后驗估計誤差協(xié)方差

PP[0][1] -= K_0 * t_1;

PP[1][0] -= K_1 * t_0;

PP[1][1] -= K_1 * t_1;

Angle+= K_0 * Angle_err; //? 后驗估計

Q_bias+= K_1 * Angle_err; //? 后驗估計

Gyro_y? = Gyro - Q_bias; //? 輸出值? （后驗估計）的微分=角速度

}

（程序2? 卡爾曼濾波子程序）

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

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

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

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

void jishuqi（）

{

P5DIR |= 0x05; //設(shè)輸出端口

CCTL0 = CCIE; //CCR0開中斷允許

CCR0 = 521-1; //向CCR0捕獲/比較寄存器裝入初值，用于比較

TACTL = TASSEL_2 + MC_1; //選 ACLK為定時器 A時鐘源， 增計數(shù)模

}

// Timer A2中斷服務(wù)程序

#pragma vector=TIMERA0_VECTOR

__interrupt void Timer_A （void）

{

ave_diff1=TACCR1;

}

（程序3? 小車車輪轉(zhuǎn)速檢測子程序）

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

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

為了保證系統(tǒng)的安全，需要對控制器的輸出值進行適當?shù)南拗疲欢鴮τ跀?shù)控來說，對輸出值進行適當?shù)南拗?，即是要對u（k）進行限制，設(shè)置一個限制值u（m），根據(jù)需要設(shè)定u（k）與u（m）的關(guān)系，對于這種情況還需要對u（k）的積分設(shè)置相應(yīng)的限制值，以此來保護系統(tǒng)響應(yīng)不受外來信息的干擾。具體的控制程序流程圖如下：

float PWM，? PWM_L，? PWM_R;

float Kp = 41， Ki = 11.3， Kd = 3.6;

float err，? ?speed_need = 0，turn_need = 0; //speed_need = 300，turn_need = 20

float position_dot，? position_dot_filter? ;

void PID （float setPoint）

{

jishuqi（）;

position_dot = ave_diff1*0.5;//編碼器值讀出

position_dot_filter*=? ?0.85;

position_dot_filter +=? ?position_dot*0.15;

if（Angle<-35 || Angle>35）? //角度過大，關(guān)閉電機

{

P6OUT = 0X00;

return;

}

err = Angle - setPoint;//得到實際偏差角度

PWM = - err*Kp

- Gyro_y*Ki

+ position_dot_filter*Kd ;

motorspeedout（PWM）;? ?//電機輸出

}

（程序4? PID控制程序）

5 總結(jié)

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

參考文獻：

[1] 張軒，徐苗，鄭德聰，李志偉.基于變論域模糊PID控制的BLDCM仿真分析[J].微特電機，2020，48（03）：54-57.

[2] 黃文杰，向鳳紅.一種基于卡爾曼濾波的板球系統(tǒng)RBF-PID控制研究[J].電子測量技術(shù)，2020，43（02）：91-96.

[3] 徐猛.基于STM32自平衡小車的設(shè)計與制作[J].計算機產(chǎn)品與流通，2019（09）：110.

[4] 李詳鵬，陳春，周子文，施娜，孫雅琪，楊陽.基于ME MS傳感器的兩輪平衡小車設(shè)計[J].科技創(chuàng)新與應(yīng)用，2019 （22）：27-29.

[5] 馮智勇，曾瀚，張力.基于陀螺儀及加速度計信號融合的姿態(tài)角度測量[J].西南大學，2011（04）：137-141.

[6] 桑勝舉，沈丁.單片機原理及應(yīng)用[M].北京：中國鐵道出版社，2010.

[7] 鄧怡佳，李昆侖.基于STC單片機的自平衡小車設(shè)計[J].電子測試，2019（11）：33-34，29.

[8] 李瑞通. 基于陀螺儀技術(shù)的管線三維可視化研究[D].太原理工大學，2019.

[9] 何軍虎，鞠劍平.自平衡車控制方法研究及仿真分析[J].機械設(shè)計與制造，2019（07）：144-146.

[10] 郝立果，鄒玉茹.無線充電智能小車的設(shè)計[J].電子制作，2019（23）：34-36.

[11] 林浩.MSP430單片機Timer_A的應(yīng)用[J].信息記錄材料，2019，20（12）：84-85.

[12] 黃強，唐杰，林立，劉興，張英洪.三相PWM整流器的雙閉環(huán)前饋解耦控制研究[J].邵陽學院學報（自然科學版），2020，17（01）：40-47.

[13] 孫永忠，姜毅龍.載波移相PWM調(diào)制下的MMC電壓平衡控制策略[J].電氣傳動，2020，50（02）：41-49.

[14] 孟春見，李開龍，張夢得.捷聯(lián)慣性基組合導(dǎo)航濾波算法比較研究[J].電光與控制，2020，27（02）：18-21.

[15] 文常保，戚一娉，宿建斌，巨永鋒.基于新型卡爾曼濾波算法的稱重系統(tǒng)實現(xiàn)研究[J].自動化與儀表，2020，35（02）： 52-56.

[16] 李慧敏，于興林.基于穩(wěn)定圖算法的發(fā)電機轉(zhuǎn)動慣量參數(shù)辨識[J].電氣自動化，2019（05）：8-11.

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