摘要：著重分析了兩輪自平衡小車的設(shè)計原理與控制算法，采用卡爾曼濾波算法融合陀螺儀與加速度計信號，得到系統(tǒng)姿態(tài)傾角與角速度最優(yōu)估計值，通過雙閉環(huán)數(shù)字PID 算法實現(xiàn)系統(tǒng)的自平衡控制。設(shè)計了以MPU-6050傳感器為姿態(tài)感知的兩輪自平衡小車系統(tǒng)，選用8位單片機HT66FU50A為控制核心處理器，完成對傳感器信號的采集處理、車身控制以及人機交互的設(shè)計，實現(xiàn)小車自主控制平衡狀態(tài)、運行速度以及轉(zhuǎn)向角度大小等功能。本文引用地址：http://www.eepw.com.cn/article/201602/287501.htm

引言

兩輪自平衡小車一般都是以倒立擺的結(jié)構(gòu)模型為基礎(chǔ)，是移動機器人研究中的一個重要領(lǐng)域，是一個非線性、強耦合、多變量和不穩(wěn)定的動態(tài)系統(tǒng)，可以很好地驗證控制理論及控制方法的優(yōu)秀平臺，具有很高的研究價值[1]。

平衡車論文普遍側(cè)重理論研究[2]，首先建立直立車體的運動學和動力學數(shù)學模型(Euler-Lagran方法)，設(shè)計反饋控制器來保證車體的平衡(極點配置和LQR兩種算法)，在此基礎(chǔ)上進行基于LabVIEW的計算及仿真，或者基于Matlab搭建控制系統(tǒng)模型，給定參數(shù)，顯示機器人的運動仿真圖和系統(tǒng)響應曲線，獲得兩種算法的最優(yōu)反饋矩陣

，系數(shù)K對應圖1四個電位器實時修正。

本文的側(cè)重點是理論與實踐結(jié)合，硬件電路設(shè)計、軟件設(shè)計與調(diào)試。其中，傳感器數(shù)據(jù)融合與電機控制PID算法是實現(xiàn)小車平衡的核心內(nèi)容，硬件設(shè)計結(jié)合關(guān)鍵源代碼分析是本文的亮點。

平衡車設(shè)計過程中一般是整定PID，難點是如何運用仿真模擬車體平衡的參數(shù)，換算不同系數(shù)，將控制器設(shè)計實施到實際的設(shè)計當中。

1 基本工作原理

利用HT66FU50微控制器，使用PID 閉環(huán)控制算法和卡爾曼濾波算法完成數(shù)據(jù)融合處理，得到平滑而穩(wěn)定車體控制值，通過電機驅(qū)動模塊來驅(qū)動電機產(chǎn)生前進或后退的加速度來控制車體保持平衡，同時系統(tǒng)還要根據(jù)速度的反饋量來完成對車體速度和方向的控制。通過藍牙模塊與外部設(shè)備連接來控制小車的運動狀態(tài)。自平衡小車的基本原理框圖如圖1所示。

2 加速度傳感器及其源代碼分析

加速度傳感器的輸出=地球重力(1g)×傾斜角的正弦(sin)(如圖2)。當傾斜角接近90°時，相對角度變化加速度傳感器的輸出變化就會越來越小。將加速度傳感器的輸出換算成傾斜角度時需使用反正弦函數(shù)arcsin。

根據(jù)單軸加速度求角度(含弧度換算)：

Accel=arcsin(Ax)*180/π (1)

Ax加速度傳感器的輸出值，Accel傾斜角X軸。單軸求角度范圍為+90°~-90°，超出部分出錯，因此，程序要限幅，1≤Ax≤-1。

如圖3(y=sinθ)，若傾斜角接近±30°的話，傾斜角與傳感器輸出的關(guān)系就會接近正比例關(guān)系，就沒必要取正弦函數(shù)進行計算了。

Accel≈k*Ax*180/π (2)

當系數(shù)取0.92時，角度范圍可以擴大到-45°~+45°。

2條檢測軸需要相互正交，并且都與旋轉(zhuǎn)軸垂直，隨著一個軸的靈敏度下降，另一個軸的靈敏度會上升，通過使用雙軸加速度傳感器就能精確測量傾斜角。雙軸求角度范圍為+180° ~-180°，通過公式(3)計算得到。

Accel=arctan(Ax/Ay)*180/π (3)

Ax與Ay加速度傳感器的輸出值，Accel傾斜角。

關(guān)鍵源代碼如下：

//加速度范圍為±2g時，換算關(guān)系：16384 LSB/g，角度較小時，也可以采用近似x=sinx得到角度(弧度), deg = rad*180/3.14。

Accel_x= GetData(ACCEL_XOUT_H);

//讀取X軸加速度

Angle_ax = (Accel_x - 800) /16384;

//去除零點偏移800，計算得到加速度g

Angle_ax = asin(Angle_ax)*180/3.14;

//Angle_ax = Angle_ax*180/3.14;

//反正弦求角度，而后弧度轉(zhuǎn)換為度

//傾斜角與arctan(Ax/Ay)成正比(如圖4)

// acc = atan2(Ax,Ay)*180/π

//角速度范圍為2000deg/s時，換算關(guān)系16.4 LSB/(deg/s)

Gyro_y = GetData(Gyro_Yout_H);

//靜止時角速度Y軸輸出為-80左右，去除偏移

Gyro_y = -(Gyro_y + 80)/16.4;

//去零點偏移，計算角速度值，負號為方向處理

Kalman_Filter(Angle_ax,Gyro_y);

//執(zhí)行卡爾曼濾波，平滑角度與角速度。

3 卡爾曼濾波及其源代碼分析

卡爾曼濾波器可分為：時間更新方程和測量更新方程。時間更新方程也可視為預估方程，測量更新方程可視為校正方程。根據(jù)上一狀態(tài)的估計值和當前狀態(tài)的觀測值推出當前狀態(tài)的估計值。

卡爾曼濾波標識：ˉ代表先驗，^代表估計。如表1所示，對于角度估算，先預測，可以近似認為是上一時刻的角度值加上此刻陀螺儀測得的角速度值乘以時間，因為

。通過重力加速度來矯正陀螺儀的角度漂移[7]，即為多傳感器數(shù)據(jù)處理，通俗稱為“數(shù)據(jù)融合”。

卡爾曼方程1：先驗估計。

(4)

先驗估計值，

前次運算估計值，

后驗估計值(算后數(shù)值，也是再次換算的

，

為此刻陀螺儀測的角速度的值。A與B是系統(tǒng)參數(shù)。

卡爾曼方程4：由觀測變量zk，更新估計。

(5)

為卡爾曼增益，對應源代碼中的K_0 、K_1，zk就是此刻測得角度Accel，

是測得角度與先驗估計的誤差比較，對應源代碼中的Angle_err。

部分源代碼如下：

float Q_angle=0.001; //初始值

float Q_gyro=0.003;

float R_angle=0.5;

char code C_0 = 1;

void Kalman_Filter(float Accel,float Gyro)

{

//Accel角度(由加速度計求得角度)

//Gyro角速度(陀螺儀測得角速度)

Angle= Angle +(Gyro - Q_bias) * dt;

…

…

…

Angle_err = Accel - Angle;//誤差

Angle = Angle + K_0 * Angle_err;/

Q_bias = Q_bias + K_1 * Angle_err; /

Gyro_x = Gyro - Q_bias;

// Angle誤差與卡爾曼增益來修正后的角度

// Q_bias誤差與卡爾曼增益來修正偏移

// Gyro_x卡爾曼修正后的角速度

}

理解卡爾曼濾波方程1：

等號左邊Angle為此刻的角度預測值，等號右邊Angle為上一時刻的值，Gyro為陀螺儀測的角速度的值，dt是兩次濾波之間的時間間隔， Q_bias是上一時刻陀螺儀靜態(tài)偏移量。(Gyro-Q_bias)*dt就是當前狀態(tài)的觀測值。陀螺儀有個靜態(tài)漂移，而且還是變化的，計算時要除去。

陀螺儀與加速度經(jīng)過卡爾曼濾波前后波形的優(yōu)劣比較，采用上位機調(diào)試工具(串口虛擬示波器)Serial_Digital_Scope V2實時觀察，參考文獻[4]，本文略。

4 電機控制PID算法

數(shù)字式PID控制算法可以分為直接計算法(也稱位置式PID)和增量計算法。所謂增量計算法就是相對于標準算法的相鄰兩次運算之差，得到的結(jié)果為增量。

位置式PID算法如下：

其中k是采樣序號，k=0，1，2，……;

uk是k第次采樣時刻的計算機輸出值;

ek是k第次采樣時刻輸入的偏差值;

ek-1是k-1第次采樣時刻輸入的偏差值;

ki是積分系數(shù)，

;

kd是微分系數(shù)，

。

Arduino平衡車的PID代碼如下：

Output = kp * error+ki * errSum+kd * dErr

電機PWM控制數(shù)據(jù)，靜止過程的平衡，為角度與角速度采用PD算法;行駛過程的平衡，為速度與位移采用PI算法[4]。

PWM1 =Angle_Kp* Angle + Angle_Kd*Gyro_x;

Angle反映傾角，Gyro_x反映快慢。Angle_Kd是微分控制，具有超前性，他的控制是和偏差輸入的變化率有關(guān)，也就是角速度。

PWM2 =moto_Kp* speed + moto_Kpn* position;

speed為速度，position為n次速度的累積，理解為速度的積分(位移)，moto_Kpn為積分系數(shù)[6]，慎重調(diào)節(jié)。此處系數(shù)代號，編者特意不寫moto_Ki，根據(jù)PID系數(shù)的公式，應為累積偏差系數(shù)。諸多文獻前后系數(shù)倒置，如文獻[6];文獻[1]解釋為D系數(shù);文獻[5]，文字解釋透徹，但論文圖中PID系數(shù)與源代碼有不符。

最終控制參數(shù)PWM = PWM1+ PWM2，也就是雙閉環(huán)PID控制。

Angle_Kp、Angle_Kd、moto_Kp、moto_Kpn對應四個電位器(如圖1)，經(jīng)過單片機ADC轉(zhuǎn)換，可以實時PID系數(shù)的調(diào)節(jié)，可以由上位機或者液晶監(jiān)控數(shù)據(jù)。

PID調(diào)參步驟：

首先，程序內(nèi)置PWM2=0;

第二，設(shè)定D=0，逐漸增加P，當車體出現(xiàn)震蕩，加入D，當出現(xiàn)抖動，增加P，反復調(diào)節(jié)P、D即可實現(xiàn)直立;

第三，加入PWM2，先P由小變到大，觀察各次響應，直至得到反應快、超調(diào)小的響應，再調(diào)節(jié)積分控制作用I參數(shù)來消除控制穩(wěn)態(tài)誤差。

電機轉(zhuǎn)速和位移值計算，關(guān)鍵源代碼如下：

speed_dot=(speed_real_LH+speed_real_RH)*0.5;

// speed_dot為此時左右車輪速度，求平均

speed *=0.85; //車輪速度濾波，k-1時刻取值

speed += speed _dot*0.15;//車輪速度濾波+k時

position+=speed; //速度的累積

position+=Speed_Need; // Speed_Need給定速度

本文來源于中國科技核心期刊《電子產(chǎn)品世界》2016年第2期第51頁，歡迎您寫論文時引用，并注明出處。

總結(jié)

以上是生活随笔為你收集整理的matlab两轮自平衡小车,(2-3合刊) 基于MEMS惯性传感器的两轮自平衡小车设计的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。