基于MEMS慣性傳感器的兩輪自平衡小車設(shè)計(jì)

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

引言

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

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

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

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

1 基本工作原理

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

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

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

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

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

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

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

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

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

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

　　Accel=arctan(A_x/A_y)*180/π (3)

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

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

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

　　Accel_x= GetData(ACCEL_XOUT_H);

　　//讀取X軸加速度

　　Angle_ax = (Accel_x - 800) /16384;

　　//去除零點(diǎn)偏移800，計(jì)算得到加速度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時(shí)，換算關(guān)系16.4 LSB/(deg/s)

　　Gyro_y = GetData(Gyro_Yout_H);

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

　　Gyro_y = -(Gyro_y + 80)/16.4;

　　//去零點(diǎn)偏移，計(jì)算角速度值，負(fù)號(hào)為方向處理

　　Kalman_Filter(Angle_ax,Gyro_y);

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

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

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

　　卡爾曼濾波標(biāo)識(shí)：ˉ代表先驗(yàn)，^代表估計(jì)。如表1所示，對(duì)于角度估算，先預(yù)測(cè)，可以近似認(rèn)為是上一時(shí)刻的角度值加上此刻陀螺儀測(cè)得的角速度值乘以時(shí)間，因?yàn)?img title="基于MEMS公式2.jpg" style="height:30px;width:90px" border="0" hspace="0" src="http://editerupload.eepw.com.cn/201603/5351456899054.jpg" width="90" height="30" />。通過重力加速度來矯正陀螺儀的角度漂移^[7]，即為多傳感器數(shù)據(jù)處理，通俗稱為“數(shù)據(jù)融合”。

　　卡爾曼方程1：先驗(yàn)估計(jì)。

(4)

先驗(yàn)估計(jì)值，前次運(yùn)算估計(jì)值，后驗(yàn)估計(jì)值(算后數(shù)值，也是再次換算的，為此刻陀螺儀測(cè)的角速度的值。A與B是系統(tǒng)參數(shù)。

　　卡爾曼方程4：由觀測(cè)變量z_k，更新估計(jì)。

(5)

　　為卡爾曼增益，對(duì)應(yīng)源代碼中的K_0 、K_1，z_k就是此刻測(cè)得角度Accel，是測(cè)得角度與先驗(yàn)估計(jì)的誤差比較，對(duì)應(yīng)源代碼中的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角度(由加速度計(jì)求得角度)

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

　　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：

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

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

4 電機(jī)控制PID算法

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

　　位置式PID算法如下：

　　其中k是采樣序號(hào)，k=0，1，2，……;

u_k是k第次采樣時(shí)刻的計(jì)算機(jī)輸出值;

e_k是k第次采樣時(shí)刻輸入的偏差值;

e_k-1是k-1第次采樣時(shí)刻輸入的偏差值;

k_i是積分系數(shù)，;

k_d是微分系數(shù)，。

　　Arduino平衡車的PID代碼如下：

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

　　電機(jī)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ù)代號(hào)，編者特意不寫moto_Ki，根據(jù)PID系數(shù)的公式，應(yīng)為累積偏差系數(shù)。諸多文獻(xiàn)前后系數(shù)倒置，如文獻(xiàn)[6];文獻(xiàn)[1]解釋為D系數(shù);文獻(xiàn)[5]，文字解釋透徹，但論文圖中PID系數(shù)與源代碼有不符。

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

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

　　PID調(diào)參步驟：

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

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

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

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

　　speed_dot=(speed_real_LH+speed_real_RH)*0.5;

　　// speed_dot為此時(shí)左右車輪速度，求平均

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

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

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

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

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