基于STM32的雙輪平衡車

張志強(qiáng)

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

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

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

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

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

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

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

左輪方程為：

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

車身的方程為：

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

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

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

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

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

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

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

3 硬件設(shè)計(jì)

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

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

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

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

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

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

圖3 編碼器Fig.3 Encoder

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

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

4 軟件設(shè)計(jì)

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

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

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

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

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

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

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

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

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

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

2）連接舵機(jī)控制器的串口3；

3）左右兩個(gè)編碼器的I/O；

4）遙控器4個(gè)方向按鈕的I/O。

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

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

5 實(shí)驗(yàn)結(jié)論

使用ARM開(kāi)發(fā)工具RealView MDK[8]對(duì)平衡車進(jìn)行調(diào)試，該開(kāi)發(fā)工具自帶的Logic Analyzer窗口可實(shí)時(shí)跟蹤變量的變化。因此，在平衡車進(jìn)入平衡狀態(tài)時(shí)，用Logic Analyzer窗口觀測(cè)到車身的傾角變化，軟件中變量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)振蕩，說(shuō)明系統(tǒng)運(yùn)行平穩(wěn)可靠。

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

此樣車按照目前市場(chǎng)價(jià)格，總價(jià)約1 500人民幣，相對(duì)于Segway成本大大降低。

6 結(jié)束語(yǔ)

本文以STM32為處理器，從硬件搭建了一個(gè)自平衡系統(tǒng)，以實(shí)時(shí)系統(tǒng)uCOS-II為片上系統(tǒng)，進(jìn)行軟件設(shè)計(jì)，并對(duì)平衡系統(tǒng)進(jìn)行功能擴(kuò)展。實(shí)驗(yàn)證明，本系統(tǒng)功能豐富，成本低廉，其中自動(dòng)恢復(fù)平衡的功能，可用于機(jī)器人排爆，野外探測(cè)等領(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]陳偉，延文杰，周超英，等.兩輪自平衡機(jī)器人控制系統(tǒng)設(shè)計(jì)[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]袁澤睿.兩輪自平衡機(jī)器人控制算法的研究[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.嵌入式實(shí)時(shí)操作系統(tǒng)μCOS-Ⅱ[M].邵貝貝，等譯.北京：北京航天航空大學(xué)出版社,2003.

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