王恒+桑元俊

摘 要： 兩輪自平衡車是檢驗各種控制算法的理想平臺之一。設計了兩輪自平衡車的實驗平臺，車體姿態(tài)數(shù)據(jù)采集采用新型加速度傳感器和陀螺儀傳感器組合，型號分別為ST 公司的LIS331DLH和L3G4200D。姿態(tài)檢測數(shù)據(jù)融合算法采用卡爾曼濾波算法，系統(tǒng)控制算法采用傾角環(huán)，角速度環(huán)與電流環(huán)的PID控制方法。最后，制作了兩輪自平衡車的原型，并給予了初步運動控制驗證。該實驗平臺能夠初步實現(xiàn)自平衡功能，后續(xù)的研究將著重對各種控制算法在該平臺的驗證研究。

關鍵詞： 兩輪自平衡車； 慣性傳感器； 數(shù)據(jù)融合； 卡爾曼濾波； MSP430F149

中圖分類號： TN876.3?34； TP24 文獻標識碼： A 文章編號： 1004?373X（2017）18?0049?06

Design of a two?wheeled self?balancing vehicle based on novel inertial sensors

WANG Heng1， SANG Yuanjun2

（1. Jincheng College， Nanjing University of Aeronautics and Astronautics， Nanjing 211156， China；

2. School of Mechanical Engineering， Shanghai Jiao Tong University， Shanghai 200240， China）

Abstract： The two?wheeled self?balancing vehicle is one of the ideal experimental platforms to test various control algorithms. An experimental platform of the two?wheeled self?balancing vehicle was designed. A novel combination of the acceleration sensor and the gyroscope sensor is used for the vehicle attitude data acquisition. Their models are LIS331DLH and L3G4200D made by ST Company. Kalman filter algorithm is adopted in vehicle attitude detection data fusion. The PID control method of the angle loop， the angular loop and the current loop is adopted as the system control algorithm. A prototype of two?wheeled self?balancing vehicle was manufactured， and its motion control was preliminarily verified. The experimental platform can preliminarily realize the self?balancing function， and future studies will focus on the validation of various control algorithms on the self?balancing vehicle experimental platform.

Keywords： two?wheeled self?balancing vehicle； inertial sensor； data fusion； Kalman filter； MSP430F149

0 引 言

兩輪自平衡車（two?wheeled self?balanced vehicle）或兩輪自平衡機器人（two?wheeled self?balanced robot），其系統(tǒng)是一個高階次、不穩(wěn)定、多變量、非線性、強耦合的系統(tǒng)[1]，核心問題就是如何保證各種工況下運動姿態(tài)的平衡控制及導航[2]。兩輪自平衡車作為檢驗各種控制理論的實驗平臺之一[3]，目前國內(nèi)有不少院校都對兩輪自平衡車進行了研究[1?4]。國外也有很多對兩輪自平衡車的研究，如Slavov等基于線性二次型控制（Linear?Quadratic Regulator，LQR）對兩輪機器人進行了研究[5]，Kim等建立了兩輪自平衡移動機器人的動態(tài)模型[6]，Amano等采用魯棒極點配置方法對兩輪自平衡車進行了穩(wěn)定性控制[7]，Silva等設計了一個PID控制器實現(xiàn)了對同軸兩輪移動機器人的控制等[8]。這說明作為進行各種平衡控制理論研究的第一步，對兩輪自平衡車實驗平臺的研制具有重要的意義。

兩輪自平衡車實驗系統(tǒng)主要包括機械結構平臺、硬件平臺及相應的軟件平臺。其中機械結構平臺相對來說大同小異，側重點在于車體質(zhì)心是否為定質(zhì)心，本文主要對定質(zhì)心兩輪自平衡車進行研究。硬件平臺的研究主要為微控制器和姿態(tài)測量傳感器的方案選型。軟件平臺設計則主要為各種控制算法的檢驗。國內(nèi)對兩輪自平衡車硬件平臺的設計主要采用的微控制器為以下幾家，如飛思卡爾的MC9S12XS128，宏晶科技的STC12C5A60S2，ST公司的STM32系列，TI公司的MSP430系列，TI的DSP MC56F8013等，而套件式的方式主要為LEGO Mindstorms NXT處理器套件[9?10]。自平衡車的車身姿態(tài)測量傳感器則主要為飛思卡爾的加速度計MMA7260配套陀螺儀ENC?03的方案[11?18]。單芯片的姿態(tài)傳感器的解決方案主要為把加速度計與陀螺儀集成在一起，比如美國Invensense公司的MPU6050[19?20]，MPU9250[21]。endprint

從上述文獻綜述來看，兩輪自平衡車的硬件平臺設計主要為相同或相似的設計方案， 即飛思卡爾的MMA7260配套ENC?03的方案，或者集成加速度計和陀螺儀的單芯片的MPU6050或MPU9250方案。其中的軟件平臺，前人大部分均未提出，或者僅是簡單地采集信號，并未建立完整的具有自主知識產(chǎn)權的采集軟件。所以，對兩輪自平衡車實驗系統(tǒng)的研究，很有必要提出新型的硬件與軟件平臺方案。

針對兩輪自平衡車的姿態(tài)檢測分析來看，為了準確測量車體的傾角，加速度計和陀螺儀是必不可少的，甚至需要外加傾角計等其他傳感器[22]。加速度計測量計算得到的角度在運動狀態(tài)易受到物體運動影響而變的不準確，所以，單獨使用加速度計是不合適的。單獨使用陀螺儀，則測到的角速度容易受到漂移誤差影響，通過時間積分的累積后得到的傾角角度的誤差更大，所以，單獨使用陀螺儀也是不合適的。因此，需要兩者組合使用，并需要控制算法的優(yōu)化來滿足系統(tǒng)的控制性能。在綜合分析前人文獻的基礎上，本文提出一種新型慣性傳感器組合的方案，來實現(xiàn)兩輪自平衡車姿態(tài)測量與控制，并同時設計了具有自主知識產(chǎn)權的軟件采集平臺。該新型慣性傳感器組合的設計方案，即車體姿態(tài)數(shù)據(jù)采集采用加速度傳感器和陀螺儀傳感器，型號分別為ST 公司的LIS331DLH和L3G4200D，姿態(tài)檢測數(shù)據(jù)融合算法采用卡爾曼濾波算法。采用的微處理器為TI 公司16位MSP430F149微處理器。系統(tǒng)控制算法采用傾角環(huán)，角速度環(huán)與電流環(huán)的PID控制方法。無線傳輸模塊采用Nordic公司的nRF24L01，傳輸頻率為2.4 GHz。同時，發(fā)送接收模塊均設計了外接大容量存儲Micro SD卡（TF卡）接口，方便把采集到的大量數(shù)據(jù)進行存儲。該新型軟件采集平臺方案為基于Microsoft Visual C# 2010設計。本文同時對該新型設計方案進行了實驗驗證。

1 系統(tǒng)設計

本設計采用模塊化設計思想，系統(tǒng)總體功能框圖如圖1所示，分為八個模塊，分別為：電源管理模塊、微處理器模塊、電機驅動模塊、加速度計數(shù)據(jù)采集模塊與陀螺儀數(shù)據(jù)采集模塊、無線通信模塊、車載存儲模塊、程序下載模塊。電源管理模塊負責整個系統(tǒng)的供電；微處理器模塊為系統(tǒng)的控制與數(shù)據(jù)處理核心，姿態(tài)數(shù)據(jù)采集模塊為采集車體的姿態(tài)（包括加速度計的加速度值和陀螺儀的角速度值），然后通過微處理器控制算法后，再驅動電機控制模塊運動以保持車體平衡；而無線通信模塊則為通過上位機控制系統(tǒng)的運行，同時，系統(tǒng)運行的數(shù)據(jù)也可以上傳到電腦上；車體的加速度計和陀螺儀采集的數(shù)據(jù)，也可存儲在車載TF卡上；下載模塊則設計在微處理器板上，用作程序代碼下載。

圖1 系統(tǒng)功能框圖

2 硬件電路設計

2.1 電源管理模塊

本系統(tǒng)主要使用的電平為12 V，5 V，3.3 V共三種電平，除了電機驅動模塊用12 V和5 V供電外，微處理器模塊等其他模塊均為3.3 V供電。所以，本設計總電源使用12 V的鋰電池供電，采用LM2576將鋰電池降壓為5 V，再利用AMS1117_3.3把得到的5 V電平降為3.3 V。為了防止姿態(tài)傳感器受到干擾，采用2片AMS1117_3.3分別得到模擬3.3 V和數(shù)字3.3 V來供電，模擬地和數(shù)字地采用0 [Ω]電阻連接。本電路部分比較簡單，所以未單獨列出。如有需求，電源模塊可增加微風扇或者散熱銅管。

2.2 MSP430F149微處理器模塊

采用TI公司的16位MCU MSP430F149， 該微處理器為16 b RISC架構， 具有60 KB+256 B FLASH Memory，2 KB RAM，125 ns指令周期時間。該芯片在工作狀態(tài)功耗為0.28 mA，Standby Mode功耗為1.6 μA。其最小系統(tǒng)電路圖如圖2所示。

2.3 電機驅動模塊

驅動電機為兩個無刷直流電機（電機A為左側電機，電機B為右側電機）。采用L298N芯片作為電機驅動電路的主芯片，車體的不同狀態(tài)運動通過電機的三種轉動方式（正轉、反轉、停止）實現(xiàn)，具體如表1所示。電機驅動電路原理圖如圖3所示。該電機驅動電路模塊單獨制作在一片PCB上，信號、電平等與微控制器模塊的PCB可通過插線連接，目的是盡可能減小電機對主板的干擾。

2.4 加速度傳感器模塊

加速度計模塊如圖4所示。

加速度傳感器采用ST公司的LIS331DLH，該線性加速度傳感器芯片可選模式為±2 g/±4 g/±8 g，輸出速率從0.5 Hz～1 kHz都可選。該芯片封裝為LGA 16，尺寸為3 mm×3 mm×1 mm。該芯片以6D方位檢測，16位數(shù)字輸出，工作電流為0.25[mA]，功耗低至10 [μA]?？紤]到空間布局有限，設計采用I2C 工作模式，地址為0011000B，并具有3D中斷功能。

2.5 陀螺儀傳感器模塊

陀螺儀傳感器模塊如圖5所示。系統(tǒng)采用的陀螺儀傳感器為L3G4200D，具有16位比特率值三軸數(shù)字輸出，三種可選全尺度模式（250/500/2 000 dps），本設計采用2 000 dps。該芯片為LGA 16封裝，尺寸為4 mm×4 mm×1 mm。設計兼容了I2C模式和SPI模式，具體應用采用I2C模式，當SDO為低電平時，地址為1101000B，當SDO為高電平時，地址為1101001B。

2.6 nRF24L01無線通信模塊

無線模塊可以實現(xiàn)自平衡車和上位PC機的數(shù)據(jù)收發(fā)和控制功能，接口兼容433 MHz和2.4 GHz的通信頻率。尤其是當2.4 GHz通信時，天線可以做成PCB板載天線，不需要外接短柱狀天線，節(jié)省空間。通信模塊的芯片為Nordic公司的nRF24L01芯片，在2.4 GHz工作時，最高工作速率2 Mb/s，

具有高效GFSK調(diào)制。該模塊接口電路如

圖6所示。

2.7 TF卡存儲模塊

圖7所示為TF卡存儲模塊接口電路。系統(tǒng)設計了外接大容量存儲Micro SD卡（TF卡）接口，方便地把采集到的大量數(shù)據(jù)進行存儲，具有自彈出功能，帶插拔偵測功能，設計最大儲存擴展為32 GB。

2.8 BSL下載模塊

圖8為控制系統(tǒng)的程序下載模塊。系統(tǒng)采用BSL下載模式，同時，接口電路可以兼容JTAG下載模式。采用BSL下載時，需要用MSP430F149的P2.2，P1.1端口實現(xiàn)Tx，Rx 信號連接。BSL所需信號線為RST，TCK，P1.1，P2.2。

3 系統(tǒng)軟件設計

系統(tǒng)的控制流程圖如圖9所示，主要包括：MSP430F149的初始化，采集加速度計的三軸方向加速度，同時采集陀螺儀的角速度，兩路數(shù)據(jù)信號送入MSP430F149后經(jīng)過卡爾曼濾波融合設計，得到傾角值，通過PID控制后，PWM控制信號輸出控制左、右電機。系統(tǒng)軟件開發(fā)平臺為Keil μVision4。

自平衡車的控制算法框圖如圖10所示，系統(tǒng)包括傾角反饋控制環(huán)、角速度反饋控制環(huán)與電流反饋控制環(huán)。加速度計和陀螺儀的數(shù)據(jù)濾波融合算法為卡爾曼濾波算法，傾角控制器、角速度控制器、電流控制器均采用PID控制算法。

基于Microsoft Visual C# 2010設計了具有自主知識產(chǎn)權的上位PC機數(shù)據(jù)采集界面，如圖11所示。采集到濾波后的數(shù)據(jù)可以存儲為Excel格式或者txt格式，然后可以繪出動態(tài)數(shù)據(jù)圖?？紤]到傳輸速率的原因，上位機沒有接收原始的加速度計或陀螺儀的輸出數(shù)據(jù)，而是將這些未經(jīng)過濾波處理的數(shù)據(jù)存儲在車身TF卡上，后續(xù)提取出來進行濾波器優(yōu)化設計。

4 自平衡車原型與實驗結果

本設計車體驅動部分采用兩個無刷直流電機控制，選擇飛思卡爾競賽使用的C型車模進行改裝得到車體機械結構，底盤使用一塊面包板來代替，板子和支架部分固定在一起。車體的姿態(tài)測量模塊（加速度計和陀螺儀模塊）、電機控制模塊L298、鋰電池模塊、降壓模塊、MSP430F149微處理器模塊、nRF24L01無線收發(fā)模塊等都安裝在面包板上面。完整車體如圖12所示。

兩輪自平衡車的啟動、勻速前行與停止控制結果如圖13所示。

圖13（a）為自平衡車的速度變化圖，圖13（b）為車體的傾角變化圖。從圖中可以看到，自平衡車的運行狀態(tài)可分為三部分，第一部分為啟動加速，第二部分為勻速前行，第三部分為停止減速過程。在實驗過程中，實驗設計運行地面平整，摩擦系數(shù)恒定，摩擦系數(shù)約為0.45。在啟動狀態(tài)，車體為直立狀態(tài)，此時傾角為0°，先緩慢前行（5～10 s區(qū)間）。緩慢啟動后，車體往前傾斜，傾角加大，然后驅動電機加速運行以恢復車體傾角為0°（10～15 s區(qū)間）。第二部分為勻速前行狀態(tài)（15～22 s區(qū)間），車體可以保持恒定車速，此時傾角已經(jīng)恢復且保持為0°。第三部分為停止減速過程（22～25 s區(qū)間），此時車體傾角變大，但是為負值，即車體往后傾斜（與前進方向相反），此時速度快速減低為0，車體恢復為0°且停止運行。該兩輪自平衡車系統(tǒng)從受到擾動（車身傾角發(fā)生改變）到恢復平衡狀態(tài)（車身傾角為0°）的時間約為5 s，同時車體達到平衡時沒有較大的波動，一定程度上實現(xiàn)了該系統(tǒng)的自平衡功能。

5 討 論

本文研究目的就是希望提出一種新型的傳感器組合方案，并設計自有的數(shù)據(jù)采集處理軟件，來實現(xiàn)兩輪自平衡車的實驗平臺設計。采用新型的慣性傳感器組合，即車體姿態(tài)數(shù)據(jù)采集采用ST公司的LIS331DLH加速度傳感器和L3G4200D陀螺儀傳感器，該傳感器組合與前人傳感器平臺價格相當，且采集精度、穩(wěn)定性等優(yōu)于前人傳感器平臺。同時，本文設計具有自主知識產(chǎn)權的姿態(tài)檢測平臺與上位機采集軟件平臺，從系統(tǒng)功能上來說并不是性能最優(yōu)，同樣存在一定的不足。比如微處理器MSP430F149性能較低，在單位時間內(nèi)處理少量數(shù)據(jù)的時候，運行速度還是能接受的。但是，該微處理器并不適合多個傳感器或復雜控制算法的應用場合，后續(xù)的研究可以直接更換為更高性能的微處理器，比如高性能的DSP處理器或者Cortex?M系列內(nèi)核的處理器等，本設計預留了平臺更新所需的接口。本文提出的方案中陀螺儀和加速度計傳感器均為一個，后續(xù)的研究將采用多個陀螺儀、加速度計、傾角計等多傳感器融合設計，這樣，系統(tǒng)的姿態(tài)數(shù)據(jù)采集才可能更加精確?？傊瑑奢喿云胶廛嚨目刂品桨冈O計不是惟一不變的，且性能上可能存在差異性。雖然這些控制平臺的差異性未見前人作對比研究，但是，本文認為，采用高性能的微處理器，以及抗干擾性能優(yōu)秀的姿態(tài)采集傳感器，可以使得系統(tǒng)的控制算法大大簡化，若采用更精確的控制算法和濾波算法，則整個兩輪自平衡車的控制性能將達到更優(yōu)。本文所提出的兩輪自平衡車實驗平臺的設計方案，希望能作為兩輪自平衡車控制平臺的一個新的選擇。

6 結 論

兩輪自平衡車實驗平臺，可作為較為理想的驗證各種控制理論的實驗平臺之一。本文對兩輪自平衡車提出了一種新型慣性傳感器的方案設計，并設計了系統(tǒng)的硬件平臺、軟件平臺。該系統(tǒng)能夠初步實現(xiàn)自平衡車的運動平衡控制。本文所提出的系統(tǒng)方案可作為兩輪自平衡車控制平臺的一個新的選擇。后期的研究將著重從理論控制算法的仿真，與參數(shù)融合優(yōu)化方面進行研究。

注：本文通訊作者為桑元俊。

參考文獻

[1] 阮曉鋼，任紅格.兩輪自平衡機器人動力學建模及其平衡控制[J].計算機應用研究，2009，26（1）：99?101.

[2] 阮曉鋼，趙建偉.基于Hopfield網(wǎng)絡的柔性兩輪自平衡機器人控制[J].機器人，2010，32（3）：405?413.

[3] 秦勇，閆繼宏，王曉宇，等.兩輪自平衡機器人運動控制研究[J].哈爾濱工業(yè)大學學報，2008，40（5）：721?726.endprint

[4] 程剛，屈勝利，劉學超.兩輪自平衡小車可控角度的推導研究[J].機床與液壓，2009，37（5）：115?116.

[5] SLAVOV T， KRALEV J， PETKOV P. Linear?quadratic control of a two?wheeled robot [J/OL]. [2014?06?19]. https：//www.researchgate.net/publication/289608821.

[6] KIM S， KWON S J. Dynamic modeling of a two?wheeled inverted pendulum balancing mobile robot [J]. International journal of control automation & systems， 2015， 13（4）： 926?933.

[7] AMANO Y. Stability control for two?wheeled mobile robot using robust pole?placement method [J]. Springer international publishng， 2014， 293： 259?268.

[8] FELIPE O E S. Design and implementation of a PID control system for a coaxial two?wheeled mobile robot [C]// Proceedings of IEEE International Conference on Industrial Electronics. [S.l.]： IEEE， 2013： 1?6.

[9] 李宗帥，王修巖，趙卉，等.基于光電傳感器的兩輪自平衡機器人控制研究[J].機床與液壓，2014，42（15）：32?35.

[10] 張金學，掌明.兩輪自平衡機器人的LQR實時平衡控制[J].自動化與儀表，2013，28（5）：5?9.

[11] 楊凌霄，李曉陽.基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測方法[J].計算機仿真，2014，31（6）：406?409.

[12] 柯國松，陳陽，許志文.兩輪自平衡小車的研究與實現(xiàn)[J].工業(yè)控制計算機，2014，27（11）：44?46.

[13] 劉二林，姜香菊.基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測[J].自動化與儀器儀表，2015（2）：52?54.

[14] 劉二林，姜香菊.基于PD算法的兩輪自平衡車直立控制[J].自動化與儀器儀表，2015（1）：203?206.

[15] 蔣緯洋，鄧遲，肖曉萍.兩輪自平衡小車系統(tǒng)制作研究[J].國外電子測量技術，2012，31（6）：76?79.

[16] 譚苗，吳益飛，陳揚，等.電磁導引的兩輪自平衡移動機器人設計[J].華中科技大學學報（自然科學版），2013，41（z1）：249?253.

[17] 鄭曉偉，陳萬米.基于DSP的兩輪自平衡小車系統(tǒng)的研究[J].工業(yè)控制計算機，2012，25（9）：33?35.

[18] 楊繼志，郭敬.基于MMA7260兩輪自平衡小車控制系統(tǒng)設計[J].機電產(chǎn)品開發(fā)與創(chuàng)新，2011，24（3）：144?145.

[19] 傅忠云，陳秋陽，劉文波.基于直接轉矩控制的兩輪自平衡車系統(tǒng)設計[J].壓電與聲光，2014，36（6）：967?971.

[20] 賴義漢，王凱.基于MPU6050的雙輪平衡車控制系統(tǒng)設計[J].河南工程學院學報（自然科學版），2014，26（1）：53?57.

[21] 劉春陽，徐軍領，程洪濤，等.MPU9250傳感器的姿態(tài)檢測與數(shù)據(jù)融合[J].河南科技大學學報（自然科學版），2015，36（4）：14?17.

[22] 郜園園，阮曉鋼，宋洪軍，等.兩輪自平衡機器人慣性傳感器濾波問題的研究[J].傳感技術學報，2010，23（5）：696?700.endprint