胡 斌，葉偉慧

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

0 引言

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

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

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

兩輪自平衡小車的主控制器采用性能更高、功耗更低的F405系列的單片機，其硬件電路結(jié)構(gòu)如圖1所示，主要包括電源和電源測量模塊、驅(qū)動模塊、超聲波模塊、線性CCD尋跡模塊、MPU6050模塊等幾部分構(gòu)成，能夠?qū)崿F(xiàn)直立、轉(zhuǎn)向、上位機調(diào)參、參數(shù)掉電保護、超聲波避障、超聲波跟隨、姿態(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)各個模塊供電。電量測量模塊使用高精度電阻分壓的方式對電池電壓進行測量，近似表示電池的電量，在電池電量比較低的情況下，提醒充電。

圖2 電源模塊電路

1.2 主控制器模塊

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

圖3 主控制器模塊電路

1.3 驅(qū)動模塊

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

圖4 TB6612FNG驅(qū)動電路

1.4 MPU6050模塊

MPU6050模塊整合了三軸加速度計和三軸陀螺儀，將測得平衡車的傾角和速度信號轉(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è)計

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

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

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

2.2 平衡車傾角和速度測量

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

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

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

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

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

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

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

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

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

由式（4）可以準確地確定出車模傾角。

2.3 平衡車速度控制

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

式中：a目標為目標加速度；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)系的方式（例如查表）進行轉(zhuǎn)換，也可以建立相應(yīng)的函數(shù)進行轉(zhuǎn)換，此處不作限定。

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

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

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

由此，得到了讓小車保持直立且速度為給定值的控制算法，由一個負反饋的直立PD控制器和一個正反饋的速度PI控制器組成?？刂圃韴D進行了演變，如圖7所示。

圖7 速度控制原理

2.4 濾波算法

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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