基于STM32的雙輪平衡車

張志強

（武漢理工大學(xué) 湖北 武漢 430070）

雙輪平衡車突破了傳統(tǒng)意義上的車的概念，它的特點是：兩個輪子處于共軸的兩個平行平面上，差動式運動，零半徑轉(zhuǎn)向，依照倒立擺的原理達到動態(tài)平衡。最早由美國發(fā)明家狄恩·卡門 （Dean Kamen）與他的DEKA研發(fā)公司（DEKA Research and Development Corp.）發(fā)明設(shè)計，名為 Segway。 但是其售價也是令人咋舌：7萬人民幣。如何能使其功能更加豐富而又成本低廉，是本文著重解決的問題。

在微處理器的選擇上，基于ARM內(nèi)核的32位微控器STM32，CPU最高可工作在72 MHz，占據(jù)了高性價比和低功耗的優(yōu)勢，尤其是豐富的外設(shè)，快速的中斷，強勁的運算速度完全滿足設(shè)計平衡車的需求。

1 平衡車的力學(xué)模型

為了獲得平衡車的平衡方程[1-2]，需要分析其力學(xué)結(jié)構(gòu)，平衡車的主要構(gòu)成是車身和左右兩個車輪，影響平衡的參數(shù)有：重心、質(zhì)量、轉(zhuǎn)動慣量、半徑等。為保持動態(tài)平衡，車身重心應(yīng)置于車軸的垂直上方的一定范圍內(nèi)，可建立力學(xué)模型，如圖1所示。

假設(shè)平衡機器人為剛體，左右兩輪完全對稱，并且忽略車輪與地面之間的滑動與側(cè)向滑動，以左輪和車身為研究對象得到如下方程：

圖1 力學(xué)模型Fig.1 Mechanical model

左輪方程為：

其中，xRM：水平位移 （m）；MRL：左車輪質(zhì)量 （kg）；HL：車身施加于車輪的水平作用力 （N）；HTL：地面對車輪的水平作用力（N）；θRL：左輪相對于垂直分量的傾角（rad）；JRL：左輪相對于 Z軸轉(zhuǎn)動慣量（kgm2）；CL：左輪電機產(chǎn)生的扭矩（kg·m）；R：車輪半徑（m）。

車身的方程為：

其中，xp：車身水平位移分量 （m）；yp：車身垂直位移分量（m）； Mp：車身質(zhì)量（kg）；VL，VR：左右兩輪施加于車身的垂直作用力（N）；HL,HR：左右兩輪施加于車身的水平作用力（N）；L：質(zhì)心距離車軸的距離（m）；θp：車身對于 y 軸傾斜角（rad）；Jpθ：車身對于 z 軸的轉(zhuǎn)動慣量（kg·m2）；CL，CR：連接左右兩輪的電機產(chǎn)生的扭矩 （kg·m）.

當(dāng)平衡車達到平衡狀態(tài)時，sinθP≈θp，cosθp≈1； 根據(jù)式（1）（2）（3）（4）（5），得到微分方程：

2 控制系統(tǒng)設(shè)計

圖2 系統(tǒng)組成示意圖Fig.2 Schematic diagram of system components

當(dāng)平衡車進入平衡狀態(tài)時，安裝于車身的陀螺儀和加速計實時采集數(shù)據(jù)，并傳送至主控制器。主控制器將數(shù)據(jù)進行Kalman濾波[3]和平衡算法處理，得出姿態(tài)調(diào)整所需要的車輪加速度值，換算為電機的控制量，通過串口3發(fā)送到舵機驅(qū)動器，控制兩路由舵機改裝的伺服電機作姿態(tài)調(diào)整。安裝于車輪的編碼器得到實際速度和運行距離，反饋回主控制器，經(jīng)由PID算法進行誤差調(diào)整后再次將控制量發(fā)送到舵機控制器，形成一個閉環(huán)反饋。整個系統(tǒng)不斷進行調(diào)整便可以維持平衡車的平衡。

平衡車行走和轉(zhuǎn)彎是由遙控器的方向消息給出，當(dāng)遙控接收器收到消息后，引起主控制器中斷，通過平衡算法處理以后給出合適的控制量，控制左右兩個輪子行走和轉(zhuǎn)彎，同時LCD要顯示方向鍵的按下狀態(tài)。

當(dāng)檢測到平衡車失去平衡的時候，主控制器進入特別處理模式，將啟動舵機M1或M2，在機械臂的作用下重新歸位到平衡狀態(tài)，在動作完成后，機械臂回到初始位置。

3 硬件設(shè)計

本文主控制器采用意法半導(dǎo)體生產(chǎn)的STM32F103ZE[4]作為主控制器，它使用來自于ARM公司具有突破性的Cortex-M3內(nèi)核，該內(nèi)核集高性能、低功耗、實時應(yīng)用、競爭性價格于一體。STM32F103ZE的主要性能參數(shù)有：主頻最高72 MHz，90 DMips，1.25 DMips/MHz。 512 k 字節(jié)閃存程序存儲器，64 k 字節(jié) SRAM。 外設(shè)有： 定時器、ADC、SPI、I2C、USART和USB等。

三軸加速計選用飛思卡爾的MMA7260QT[5]，MMA7260QT的量程有 1.5 g、2 g、4 g、6 g 4個， 本文選擇 1.5 g的量程范圍，精度為800 mV/g，內(nèi)部集成了低通濾波器。將MMA7260QT的x軸y軸接入STM32的AD采樣，其中一個軸作冗余設(shè)計，以增強可靠性，12位的AD采樣精度可達0.8 mV，轉(zhuǎn)換時間為 1 μs。

陀螺儀采用亞德諾半導(dǎo)體的ADXRS300[6]，其量程為±300°/s，線性誤差小于 0.1%，使用 FPGA采樣積分后，以串口方式輸出角度信息。經(jīng)信號處理后的誤差小于0.5°，溫度漂移小于 0.6°。

舵機控制器是用于控制平衡車左右車輪伺服電機，它可以同時控制16路舵機，通過串口接收命令，并輸出PWM波形。舵機選擇了比亞迪公司生產(chǎn)的MG995，其速度為0.17 s/60°(4.8 V)，扭矩為 13 kg·cm，死區(qū)設(shè)定 4 μs。 用于車輪的舵機需要去除銷釘，并用兩個2.5kΩ電阻取代滑動變阻器。

遙控器部分是采用PT2262與PT2272,分別用于調(diào)制解調(diào)，PT2272解調(diào)后的信號引腳分別與STM32的Remote1～Remote4（PC8～PC11）連接，前后左右4個按鈕對應(yīng)其中Remote1～Remote4，當(dāng)遙控器有按鍵按下時STM32產(chǎn)生中斷并作相應(yīng)的處理。

編碼器由兩部分組成，即：碼盤和信號采樣電路。碼盤與車輪共軸，車輪轉(zhuǎn)動帶動碼盤轉(zhuǎn)動，采用兩個直射式紅外光電傳感器作采樣電路的設(shè)計。本文選用南旭科技的光電傳感器 ST150，ST150的響應(yīng)時間為 5 μs、光縫寬度 0.4 mm、遮光電流1 μA、通光電流0.25 mA，滿足設(shè)計要求。同時使用兩個ST150是為了得到相位差調(diào)整為90°兩路波形，這樣就可以檢測出車輪的旋轉(zhuǎn)方向。經(jīng)過施密特觸發(fā)器74HC14整形后輸入到STM32的Encode1～Encode4(PC4～PC7)。編碼器電路如圖3所示。

圖3 編碼器Fig.3 Encoder

LCD選用奕力科技的ILI9320，使用SPI總線與STM32主控制器連接。將以上幾個部分構(gòu)建為一個系統(tǒng)，主控制器STM32的電路圖如圖4所示。

USART2和USTAR3分別與陀螺儀和舵機控制器通訊，USART2與USART3經(jīng)MAX3232芯片將TTL電平轉(zhuǎn)換為RS232電平，RS232的工作范圍是-15～+15 V，即使存在電壓衰減傳輸信號也可以被有效識別。

4 軟件設(shè)計

本文采用實時操作系統(tǒng)uCOS-II[7]作為STM32的片上操作系統(tǒng)，根據(jù)不同的功能分為3個不同的任務(wù)。角度和角加速度等重要數(shù)據(jù)通過中斷實現(xiàn)，保證能夠快速得到處理。軟件的基本結(jié)構(gòu)如圖5所示。

圖5 軟件總體框圖Fig.5 Block diagram of software

圖4 主控制器設(shè)計Fig.4 Design of master controller

在STM32復(fù)位以后，首先執(zhí)行初始化操作。依次為PLL、SPI、串口、A/D 采樣、I/O、DMA、定時器、操作系統(tǒng)，在完成以上操作以后，啟動操作系統(tǒng)并創(chuàng)建任務(wù)，開始運行應(yīng)用程序。

uCOS-II任務(wù)設(shè)計為3個，根據(jù)優(yōu)先級由高到低得順序依次是：

1）任務(wù)一 處理陀螺儀傳輸來的數(shù)據(jù)，并和加速計采樣的數(shù)據(jù)通過Kalman算法濾波，濾波完成后發(fā)送信息量通知任務(wù)二處理。如果檢測到狀態(tài)為失去平衡，則通知任務(wù)二進行恢復(fù)平衡狀態(tài)的操作。

2）任務(wù)二 在平衡狀態(tài)下，如果收到信號量(Balace_sem)，PID算法將編碼器得到的加速度值與平衡算法計算的加速度值進行調(diào)整，并將調(diào)整后的控制量輸出到舵機控制器。如果收到信號量(Unbalace_sem)，則啟動機械臂使得平衡車恢復(fù)平衡狀態(tài)。

3）任務(wù)三 等待遙控器的信號量，如果有信號量便通知任務(wù)一進行方向調(diào)整，同時通過SPI發(fā)送到LCD顯示。

傳感器的數(shù)據(jù)都以中斷的方式處理，根據(jù)優(yōu)先級由高到低得順序依次是：

1）用于接收陀螺儀數(shù)據(jù)的串口2；

2）連接舵機控制器的串口3；

3）左右兩個編碼器的I/O；

4）遙控器4個方向按鈕的I/O。

加速計的AD采樣使用查詢的方式。在得到陀螺儀的數(shù)據(jù)后，立即進行AD采樣，使得加速計和陀螺儀得到的數(shù)據(jù)盡量保持在較短的時間內(nèi)。系統(tǒng)軟件流程如圖6所示。

圖6 系統(tǒng)軟件流程圖Fig.6 Flow chart of system software

5 實驗結(jié)論

使用ARM開發(fā)工具RealView MDK[8]對平衡車進行調(diào)試，該開發(fā)工具自帶的Logic Analyzer窗口可實時跟蹤變量的變化。因此，在平衡車進入平衡狀態(tài)時，用Logic Analyzer窗口觀測到車身的傾角變化，軟件中變量Gyro_degree代表陀螺儀得到的車身傾角θp，Logic Analyzer窗口得到的數(shù)據(jù)如圖7所示。

圖7 Logic Analyzer數(shù)據(jù)Fig.7 Data of Logic Analyzer

根據(jù)Gyro_degree的數(shù)據(jù)，θp的數(shù)據(jù)大部分在范圍-5°≤θp≤5°內(nèi)振蕩，說明系統(tǒng)運行平穩(wěn)可靠。

在室內(nèi)無坡度的地面上，使用遙控器做S型曲線運動，傾角的范圍為：-5°≤θp≤5°，并且LCD可正確顯示出方向信息。當(dāng) θp＞25°，或者 θp＜-25°時，平衡車失去平衡。此時舵機 M1與 M2啟動，將平衡車調(diào)整至-5°≤θp≤5°的范圍內(nèi)，重啟動平衡控制。

此樣車按照目前市場價格，總價約1 500人民幣，相對于Segway成本大大降低。

6 結(jié)束語

本文以STM32為處理器，從硬件搭建了一個自平衡系統(tǒng)，以實時系統(tǒng)uCOS-II為片上系統(tǒng)，進行軟件設(shè)計，并對平衡系統(tǒng)進行功能擴展。實驗證明，本系統(tǒng)功能豐富，成本低廉，其中自動恢復(fù)平衡的功能，可用于機器人排爆，野外探測等領(lǐng)域。

[1]Crasser F， D’Arrigo A,Colombi S， et al.JOE：A mobile，inverted pendulum [J].IEEE Transactions on Industrial Elec-tronics，2002， 49（1）：107-114.

[2]陳偉，延文杰，周超英，等.兩輪自平衡機器人控制系統(tǒng)設(shè)計[J].傳感器與微控制器，2008，27（4）：117-120.CHEN Wei，YAN Wen-jie，ZHOU Chao-ying.Control system design of two wheels self-balance robot[J].Transducer and Microsystem Technologies，2008,27(4)：117-120.

[3]袁澤睿.兩輪自平衡機器人控制算法的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2006：22-31.

[4]STMicroelectronics Integrated Products.STM32 Reference manual[EB/OL].(2011-01-01)[2011-04-01].http：//www.stmicroelectronics.com.cn/internet/com/TECHNICAL_RESOURC ES/TECHNICAL_LITERATURE/REFERENCE_MANUAL/CD00171190.pdf.

[5]Freescale Integrated Products.MMA7260QT[EB/OL].(2008-05-03)[2011-04-01].http：//cache.freescale.com/files/sensors/doc/data_sheet/MMA7260QT.pdf?fsrch=1＆sr=1.

[6]Analog Devices Integrated Products.ADXRS300[EB/OL].(2004-01-01)[2011-04-01].http：//www.datasheetarchive.com/pdf/getfile.php?dir=Datasheets-4＆file=DSA-67061.pdf＆scan=.

[7]Labrosse J J.嵌入式實時操作系統(tǒng)μCOS-Ⅱ[M].邵貝貝，等譯.北京：北京航天航空大學(xué)出版社,2003.

[8]李寧.ARM開發(fā)工具RealView MDK使用入門[M].北京：北京航空航天大學(xué)出版社，2008.