基于STM32的兩輪自平衡車設(shè)計(jì)*

胡 斌，葉偉慧

（湛江科技學(xué)院，廣東湛江524094）

0 引言

隨著科學(xué)技術(shù)的迅速發(fā)展，越來越多的平衡小車走進(jìn)了人們的日常生活中，由于兩輪自平衡小車具有多變量、強(qiáng)耦合、非線性等特點(diǎn)，已成為國內(nèi)外學(xué)者研究各種控制算法的熱門研究對象。同時也為移動機(jī)器人的研究開創(chuàng)了新的方向，兩輪自平衡小車系統(tǒng)與傳統(tǒng)的移動機(jī)器人相比，是一個兩輪共軸、獨(dú)立驅(qū)動且通過運(yùn)動保持平衡的機(jī)器人設(shè)備，同時可以直立靜止運(yùn)動，具有高平衡、跟蹤、適應(yīng)性強(qiáng)的優(yōu)點(diǎn)。

兩輪自平衡小車具有傳統(tǒng)1階倒立擺系統(tǒng)特性，其核心問題就是在保持車模平衡的同時能夠完成前進(jìn)、后退、旋轉(zhuǎn)、尋跡、跟隨、避障等功能。楊凌霄[1]、馬思遠(yuǎn)等[2]利用MPU6050和加速度計(jì)測量了平衡車的角度信號和速度信號，通過卡爾曼濾波器實(shí)現(xiàn)了測量數(shù)據(jù)的數(shù)據(jù)融合，解決了只使用加速度計(jì)或陀螺儀時存在的測量誤差和零點(diǎn)漂移問題，提高了車模角度和速度測量的精度，而且提高了抑制干擾的能力。朱慧[3]、張吉昌[4]、周振德等[5]提出一種簡易互補(bǔ)濾波算法對陀螺儀和加速度計(jì)進(jìn)行數(shù)據(jù)融合，設(shè)計(jì)一種基于慣性傳感器的姿態(tài)檢測系統(tǒng)，有效地提高了單軸雙輪自平衡車姿態(tài)檢測性能。楊興明[6]、蔡志端[7]、薛晗[8]提出一種提出一種自適應(yīng)滑模控制方法，改善了兩輪自平衡車控制系統(tǒng)的品質(zhì)，并通過仿真驗(yàn)證了該控制方法的有效性。黃健[9]、何軍虎[10]、陳鵬展[11]、王素青等[12]通過理論分析建立了動力學(xué)方程，得到了各部分控制框圖和傳遞函數(shù)。設(shè)計(jì)中用MPU6050測量車模的旋轉(zhuǎn)角度、角速度、加速度，用編碼器測量電機(jī)轉(zhuǎn)速，構(gòu)建了反饋系統(tǒng)。軟件設(shè)計(jì)中用卡爾曼濾波器[13-15]消除干擾，用PD算法很好地控制了平衡、速度、轉(zhuǎn)向。

1 系統(tǒng)硬件設(shè)計(jì)

兩輪自平衡小車的主控制器采用性能更高、功耗更低的F405系列的單片機(jī)，其硬件電路結(jié)構(gòu)如圖1所示，主要包括電源和電源測量模塊、驅(qū)動模塊、超聲波模塊、線性CCD尋跡模塊、MPU6050模塊等幾部分構(gòu)成，能夠?qū)崿F(xiàn)直立、轉(zhuǎn)向、上位機(jī)調(diào)參、參數(shù)掉電保護(hù)、超聲波避障、超聲波跟隨、姿態(tài)識別、線性CCD尋跡等多種功能。

圖1 硬件電路結(jié)構(gòu)

1.1 電源模塊

兩輪自平衡車采用3節(jié)可充電電池供電，如圖2所示，電源經(jīng)LM2596T芯片轉(zhuǎn)為5 V電壓，再經(jīng)AMS1117芯片轉(zhuǎn)為3.3 V電壓，5 V和3.3 V作為電源為平衡車系統(tǒng)各個模塊供電。電量測量模塊使用高精度電阻分壓的方式對電池電壓進(jìn)行測量，近似表示電池的電量，在電池電量比較低的情況下，提醒充電。

圖2 電源模塊電路

1.2 主控制器模塊

主控制器STM32F405RGT6內(nèi)核具有1個浮點(diǎn)單元（FPU）單精度，實(shí)現(xiàn)一套完整的DSP指令和1個內(nèi)存保護(hù)單元（MPU），提高了應(yīng)用程序的安全性。提供3個12位ADC、12個通用16位定時器，包括2個用于電機(jī)控制的PWM定時器、2個通用32位定時器，主要完成對MPU6050傳感器的數(shù)據(jù)采集和處理，通過寫入算法實(shí)現(xiàn)對平衡車的控制。

圖3 主控制器模塊電路

1.3 驅(qū)動模塊

本系統(tǒng)的驅(qū)動模塊采用直流電機(jī)驅(qū)動器件TB6612FNG構(gòu)成的驅(qū)動電路，驅(qū)動電路如圖4所示，有兩路PWM驅(qū)動信號同時驅(qū)動平衡車的2個電機(jī)，PWMA信號驅(qū)動平衡車左輪的轉(zhuǎn)速和方向，PWMB驅(qū)動平衡車右輪的轉(zhuǎn)速和方向，通過對平衡車雙輪的有效控制來實(shí)現(xiàn)相應(yīng)的功能。

圖4 TB6612FNG驅(qū)動電路

1.4 MPU6050模塊

MPU6050模塊整合了三軸加速度計(jì)和三軸陀螺儀，將測得平衡車的傾角和速度信號轉(zhuǎn)化為可輸出的數(shù)字量。如圖5所示，MPU6050模塊需電源提供3.3 V直流電源供電，通過一對差分信號SDA和SCL建立與主控制器的通信，同時還存在一對差分信號XDA和XCL、時鐘信號CLKOUT。MPU6050模塊首先測得車模的傾角和速度信號并通過內(nèi)置16位AD轉(zhuǎn)換器轉(zhuǎn)化為數(shù)字量，然后經(jīng)過400 kHz的I2C接口把數(shù)字信號傳輸給主控制器。

圖5 MPU6050模塊電路

2 系統(tǒng)軟件設(shè)計(jì)

2.1 系統(tǒng)總體流程

本系統(tǒng)的主函數(shù)初始化程序主要包括超聲波初始化、串口初始化、電機(jī)IO口初始化、PWM輸出初始化、MPU6050初始化、PID參數(shù)初始化及系統(tǒng)定時器初始化。初始化程序執(zhí)行完以后通過MPU6050獲取平衡車的角度信息，通過中斷程序完成車模的直立控制、速度控制以及方向控制。如圖6所示。

圖6 系統(tǒng)主程序流程

2.2 平衡車傾角和速度測量

為了保證獲取的車模傾角和傾角速度的簡便性和可靠性，本文申請實(shí)施例中的平衡車采用了集成有三軸加速度計(jì)和三軸陀螺儀的MPU6050作為平衡車的姿態(tài)檢測模塊，以便準(zhǔn)確獲取平衡車的車模傾角和傾角速度，以及由MPU6050一個模塊即可獲取三軸加速度計(jì)和三軸陀螺儀等檢測的參數(shù)，也有利于精簡平衡車的結(jié)構(gòu)。

測量車模傾角和傾角速度，可以通過安裝在車模上的加速度傳感器和陀螺儀實(shí)現(xiàn)，而MPU6050集成了三軸加速度計(jì)和三軸陀螺儀，因此，獲取MPU6050的參數(shù)后進(jìn)行相應(yīng)的處理，即可實(shí)現(xiàn)對平衡車的車模傾角和傾角速度的檢測。

式中：g為重力加速度；β為所述第一傾角；a為平衡車當(dāng)前的加速度；α為傾角速度。

根據(jù)Y軸加速度測量值、傾角速度、平衡車當(dāng)前的線加速度，確定出平衡車的第一傾角的具體方式：獲取MPU6050的安裝位置到平衡車的電機(jī)軸的距離h；而后根據(jù)距離h、傾角速度α，平衡車當(dāng)前的線加速度a1，得出：

結(jié)合式（2）則可以確定出平衡車當(dāng)前的加速度a，再根據(jù)確定出的平衡車當(dāng)前的加速度a、傾角速度α、Y軸加速度測量值A(chǔ)ccel_Y、重力加速度g，結(jié)合式（1），即可確定出平衡車的第一傾角β。h為MPU6050的安裝位置到平衡車的電機(jī)軸的距離，理論上應(yīng)當(dāng)把MPU6050安裝的離電機(jī)軸越近越好，以減小平衡車運(yùn)動角加速度對角度測量的影響，但是在工程實(shí)踐中，將MPU6050安裝的離電機(jī)軸太近，會使傳感器容易受到電機(jī)的影響導(dǎo)致穩(wěn)定性下降。而且就算是避免了運(yùn)動的角加速度，線加速度也是無法避免的，所以無法徹底消除運(yùn)動加速度對角度測量的影響。因此h可以為平衡車整體高度的30%～90%，而電機(jī)軸通常安裝在平衡車的底部區(qū)域。

MPU6050輸出的陀螺儀的原始數(shù)據(jù)是-32 768～32 768，轉(zhuǎn)換成單位為°/s的數(shù)據(jù)，可得：

式中：k和初始化時的量程有關(guān)，可以在MPU6050的手冊中查到，當(dāng)初始化為±2 000°/s時，k=16.4，加速度計(jì)測量的角度信號在受到外界干擾的情況下，會有很大的干擾，而通過算法融合得到的角度值則非常穩(wěn)定。

為了保證車模傾角的準(zhǔn)確性和可靠性，可以對第一傾角和第二傾角進(jìn)行濾波融合。示例性的，平衡車可以將第一傾角和第二傾角代入互補(bǔ)濾波關(guān)系式：

式中：angle為濾波融合后的角度，即車模傾角；anglem為第一傾角；（angle+gyrom?dt）為第二傾角；dt為采樣周期；N為濾波器系數(shù)，此處，濾波器系數(shù)N可以選取0.02。

由式（4）可以準(zhǔn)確地確定出車模傾角。

2.3 平衡車速度控制

直立控制在前面有介紹，使用的PD控制，因?yàn)榫幋a器可能存在的噪聲，為防止噪聲被放大并消除系統(tǒng)的靜差，此處的速度控制可以使用PI控制，則有：

式中：a目標(biāo)為目標(biāo)加速度；kp為比例控制參數(shù)；kd為微分控制參數(shù)；θ為車模傾角；θ?為傾角速度；為將調(diào)節(jié)加速度a1轉(zhuǎn)換為調(diào)節(jié)傾角。

而將調(diào)節(jié)加速度a1轉(zhuǎn)換為調(diào)節(jié)傾角f(a1)，可以通過預(yù)設(shè)對應(yīng)關(guān)系的方式（例如查表）進(jìn)行轉(zhuǎn)換，也可以建立相應(yīng)的函數(shù)進(jìn)行轉(zhuǎn)換，此處不作限定。

調(diào)節(jié)加速度a1作為速度控制器（PI控制）的輸出，有：

式中：a1為調(diào)節(jié)加速度；kp1為速度控制器的比例控制參數(shù)；ki1為速度控制器的積分控制參數(shù)；k為第k次采樣。

整合式（5）～（6），可以得到：

由此，得到了讓小車保持直立且速度為給定值的控制算法，由一個負(fù)反饋的直立PD控制器和一個正反饋的速度PI控制器組成。控制原理圖進(jìn)行了演變，如圖7所示。

圖7 速度控制原理

2.4 濾波算法

平衡車獲取姿態(tài)角的濾波算法一般為卡爾曼濾波和一階互補(bǔ)濾波，但MPU6050內(nèi)置的DMP可以直接輸出和姿態(tài)相關(guān)的四元數(shù)，所以獲取平衡車角度通常有3種方法：（1）Way_Angle=1：通過DMP獲取四元數(shù)，并算出角度；（2）Way_Angle=2：通過卡爾曼濾波對陀螺儀和重力加速度進(jìn)行數(shù)據(jù)融合；（3）Way_Angle=1：通過互補(bǔ)濾波對陀螺儀和重力加速度進(jìn)行數(shù)據(jù)融合。

3 系統(tǒng)調(diào)試

3.1 平衡車上電調(diào)試

首先插上平衡車電源紅色接頭，打開主開關(guān)，此時小車上面的指示燈和顯示屏都會亮，可通過點(diǎn)擊用戶按鍵使小車保持平衡。平衡車能夠自動識別被放置或被拿起，當(dāng)處于平衡位置附近時，當(dāng)小車檢測到輪胎被動旋轉(zhuǎn)時，自動啟動平衡系統(tǒng)并保持直立平衡狀態(tài)。

3.2 數(shù)據(jù)查看

OLED顯示屏可顯示平衡車的信息，共有6行信息，第一行顯示濾波器和小車的運(yùn)行模式，第二行顯示當(dāng)前溫度和超聲波測量值，第三行顯示左輪編碼器速度，第四行顯示右輪編碼器速度，第五行顯示電池電壓，第六行顯示小車的平衡傾角。

3.3 超聲波避障調(diào)試

長按用戶鍵2 s即可進(jìn)入超聲波避障模式，OLED顯示屏?xí)@示小車進(jìn)入避障模式，并實(shí)時顯示前方的距離。如果需要切換到跟隨功能，只需要在上位機(jī)自定義按鍵就會實(shí)現(xiàn)從避障模式到跟隨模式的切換。

但是因?yàn)槌暡ǖ奶匦裕暡y距時會有兩個弊端。（1）如果超聲波的反射面不是平面或者反射平面傾角大于15°的時候，由于折射原因會造成測距不準(zhǔn)；（2）當(dāng)超聲波模塊距離反射平面距離不足1 cm時超聲波模塊會出現(xiàn)盲區(qū)，造成測距不準(zhǔn)。要解決上述兩個問題，首先可以優(yōu)化算法，提高平衡車的穩(wěn)定性，盡量避免小車因外界干擾造成傾角過大，測試時盡量與超聲波模塊保持一定的距離，避免出現(xiàn)盲區(qū)，跟隨模式同避障模式遇到的一樣，處理方法相似。

3.4 調(diào)試結(jié)果

小車平衡時的效果如圖8所示，能夠?qū)崿F(xiàn)實(shí)現(xiàn)自平衡、避障和跟隨3個功能，可繼續(xù)開源拓展功能，比如線性CCD巡線功能、斜坡懸停功能等，這就需要繼續(xù)開發(fā)硬件和添加新的控制程序。

4 結(jié)束語

本文針對平衡車控制系統(tǒng)運(yùn)行不穩(wěn)定，操控的準(zhǔn)確度和靈敏度不佳等問題，設(shè)計(jì)一種以STM32F405RGT6微處理器為主控制器的全方位監(jiān)控平衡小車控制系統(tǒng)，配備小車電量監(jiān)控功能，電量低于20%時，上位機(jī)會通知提醒用戶充電；能夠?qū)崟r顯示小車兩輪的速度信息；圖形化實(shí)時顯示小車平衡傾角；實(shí)時顯示接收到的小車下位機(jī)指令。最后通過實(shí)驗(yàn)驗(yàn)證了兩輪自平衡車系統(tǒng)硬件、軟件控制方案的工程有效性和可行性。