基于新型慣性傳感器的兩輪自平衡車的設計

王恒+桑元俊

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

關鍵詞： 兩輪自平衡車； 慣性傳感器； 數據融合； 卡爾曼濾波； 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），其系統是一個高階次、不穩定、多變量、非線性、強耦合的系統[1]，核心問題就是如何保證各種工況下運動姿態的平衡控制及導航[2]。兩輪自平衡車作為檢驗各種控制理論的實驗平臺之一[3]，目前國內有不少院校都對兩輪自平衡車進行了研究[1?4]。國外也有很多對兩輪自平衡車的研究，如Slavov等基于線性二次型控制（Linear?Quadratic Regulator，LQR）對兩輪機器人進行了研究[5]，Kim等建立了兩輪自平衡移動機器人的動態模型[6]，Amano等采用魯棒極點配置方法對兩輪自平衡車進行了穩定性控制[7]，Silva等設計了一個PID控制器實現了對同軸兩輪移動機器人的控制等[8]。這說明作為進行各種平衡控制理論研究的第一步，對兩輪自平衡車實驗平臺的研制具有重要的意義。

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

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

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

1 系統設計

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

圖1 系統功能框圖

2 硬件電路設計

2.1 電源管理模塊

本系統主要使用的電平為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。為了防止姿態傳感器受到干擾，采用2片AMS1117_3.3分別得到模擬3.3 V和數字3.3 V來供電，模擬地和數字地采用0 [Ω]電阻連接。本電路部分比較簡單，所以未單獨列出。如有需求，電源模塊可增加微風扇或者散熱銅管。

2.2 MSP430F149微處理器模塊

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

2.3 電機驅動模塊

驅動電機為兩個無刷直流電機（電機A為左側電機，電機B為右側電機）。采用L298N芯片作為電機驅動電路的主芯片，車體的不同狀態運動通過電機的三種轉動方式（正轉、反轉、停止）實現，具體如表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位數字輸出，工作電流為0.25[mA]，功耗低至10 [μA]。考慮到空間布局有限，設計采用I2C 工作模式，地址為0011000B，并具有3D中斷功能。

2.5 陀螺儀傳感器模塊

陀螺儀傳感器模塊如圖5所示。系統采用的陀螺儀傳感器為L3G4200D，具有16位比特率值三軸數字輸出，三種可選全尺度模式（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無線通信模塊

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

具有高效GFSK調制。該模塊接口電路如

圖6所示。

2.7 TF卡存儲模塊

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

2.8 BSL下載模塊

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

3 系統軟件設計

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

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

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

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

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

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

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

5 討 論

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

6 結 論

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

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

參考文獻

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

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

[3] 秦勇，閆繼宏，王曉宇，等.兩輪自平衡機器人運動控制研究[J].哈爾濱工業大學學報，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] 楊凌霄，李曉陽.基于卡爾曼濾波的兩輪自平衡車姿態檢測方法[J].計算機仿真，2014，31（6）：406?409.

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

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

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

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

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

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

[18] 楊繼志，郭敬.基于MMA7260兩輪自平衡小車控制系統設計[J].機電產品開發與創新，2011，24（3）：144?145.

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

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

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

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