雙輪自平衡小車控制系統設計

蚌埠學院電子與電氣工程系 孟 帥 常德軍 陳章寶

雙輪自平衡小車控制系統設計

蚌埠學院電子與電氣工程系 孟 帥 常德軍 陳章寶

雙輪自平衡小車是雙輪機器人研究的基礎，具有多變量、非線性、強耦合等特性。本文以STC15W4K32S4為主控芯片，MPU6050采集車身的角度和角速度，通過互補濾波進行數據融合，得到優化的角度值，以TB6612FNG作為電機驅動芯片，霍爾元件進行車速測量，構建雙輪小車的姿態平衡控制和運動控制系統。通過姿態解算和PID控制，實現了小車的平穩運行。

雙輪自平衡小車；STC15W4K32S4；MPU6050；TB6612FNG；PID

引言

雙輪自平衡小車因其結構簡單、轉向靈活等特點，能在狹窄空間中行駛，以及在特殊環境下執行特殊任務，對變化的地形適應性強，引起新一輪研究熱潮和廣泛的應用開發。雙輪自平衡小車作為控制系統的經典模型，是驗證控制理論和力學理論的理想實驗平臺，為科學理論的實驗研究起到了硬件平臺的作用，有著巨大的研究價值[1]。

1.小車運動平衡分析

設小車模型車體重心到底盤重心長度為l，質量為m，當車輪產生水平向右的加速度α時，對車模受力分析如下圖1所示。

圖1 車模受力分析

根據小車平衡狀態受力和“角加速度與曲率半徑的乘積等于切向加速度”可得到車模運動過程中的動態數學模型[2]：

在初始狀態時，車輪加速度α為0，并進行工程近似：在θ較小時，，模型可以簡化為下式。

對上式微分方程等式兩邊同時進行拉普拉斯變換，得到靜止狀態下系統傳遞函數為：

由式可解系統有一個極點位于復頻域實軸的右半部分，所以在靜止時車身無法達到平衡，為此需引入比例微分控制，對車模傳遞函數進行校正，提高系統的穩定性。在增加比例微分負反饋后，相當于增加了系統的開環零點，系統兩極點均具有負實部，系統穩定[3]。

2.硬件電路設計

為實現雙輪自平衡小車的平衡控制和運動控制，系統硬件設計包括主控模塊、姿態檢測模塊（陀螺儀和加速度計）、測速模塊、直流電機驅動模塊、電源模塊和人機交互模塊，控制系統框圖如圖2所示。電壓轉換電路采用LM2940-5.0，將電池組的+12V電壓轉成+5V，用AMS1117-3.3穩壓電路將+5V轉成+3.3V，人機交互模塊使用LCD1602和四個獨立按鍵，實現系統調試時的參數設置和顯示。

圖2 控制系統方框圖

2.1 主控模塊設計

系統主控模塊采用STC15系列MCU，STC15W4K32S4是宏晶公司生產的增強型8051內核的單片機。工作電壓2.5～5.5V，有6路硬件PWM、7個定時器、4組串口、8路高速10位ADC，最大有61k程序Flash和4k字節SRAM，內部集成了時鐘和復位電路。通過引腳1、3與MPU6050進行I2C通信，采集姿態數據，引腳30～33、34、35連接電機驅動芯片TB6612，控制電機的轉向和轉速，通過引腳38、39捕獲霍爾傳感器輸入脈沖；通過引腳21、2、22、4可以對霍爾傳感器兩相A1(A2)、B1(B2)進行外部中斷觸發和計數器計數。

2.2 姿態檢測電路

姿態傳感器使用的是MPU6050，內部集成了三軸加速度計和三軸陀螺儀，所以又稱為六軸傳感器[4]，具有體積小、功耗低、分辨率高等優點。模塊內置三個16位模數轉換器，加速度計測量范圍±2、±4、±8、±16g，陀螺儀測量范圍±250、±500、±1000、±2000°/sec，可編程控制姿態傳感器的測量范圍，來提高讀取數據的最小精度，使用引腳23、24與主控間進行I2C通信。姿態檢測電路圖如圖3所示。

圖3 姿態檢測電路

2.3 電機驅動電路

電機驅動芯片選用的是東芝生產的TB6612，它的最大工作電壓高達15V，平均工作電流可達1.2A，瞬時最大電流可以達到3.2A，而且PWM控制頻率可高達100KHz[5]。AIN1、AIN2、BIN1、BIN2腳為輸入控制電平，控制電機正、反轉，PWM3、PWM4腳與MCU的PWM輸出口相連，控制電機加、減速，電機驅動電路如圖4所示。TB6612具有大電流H橋結構，雙通道電路輸出，可同時驅動兩個電機，AO1(BO1)、和AO2(BO2)之間可分別接電機。

圖4 電機驅動電路

2.4 霍爾測速電路

霍爾傳感器是基于霍爾效應的磁場傳感器，主要由磁塊和磁電檢測裝置組成[5]。將磁鐵粘在非磁性圓盤的邊沿，靠近圓盤邊沿處放置霍爾傳感器，當圓盤轉動時，霍爾傳感器就輸出脈沖，相應的計數變量就加1，在單位時間內測得的脈沖數越多代表速度越快。脈沖采集電路主要由電壓比較器LM393、霍爾元件3144和磁感應探頭組成，采用非接觸檢測方式，D6為感應指示燈，當感應到磁場時，輸出低電平，信號燈D6亮，3144感應磁場的強度并輸出數字信號，再由LM393轉化成為脈沖。

3.系統軟件設計

采用模塊化的設計思想，系統軟件設計包括：數據采集與傳輸，PID控制，PWM驅動及其他模塊程序設計。系統上電初始化后，首先檢測傾角是否在可控范圍內，條件為真時進行數據采集與傳輸，通過角度PD和速度PI控制PWM輸出，調節PWM占空比來調節車速，從而保持車模的動態平衡，通過改變給定的車體傾角θ實現小車的運行。

六軸姿態傳感器MPU6050內置了三軸加速度計和三軸陀螺儀，知道六個軸的數據寄存器的地址，就能從數據寄存器中讀取數據，MPU6050中自帶了I2C程序，利用由數據線SDA和時鐘線SCL構成的兩線式串行總線，主控單片機就能通過I2C通信讀取姿態傳感器的未經過濾波的角度，通過互補濾波進行數據融合，得到小車的傾角值。

PID（比例、積分、微分）控制以車身傾角為設定值[6]，以偏差e(t)為輸入，構建雙閉環PID調節控制器，通過第一級角度PD控制計算出小車保持動態平衡需要的車輪速度值，然后以直流電機上霍爾測速裝置構成第二級的速度PI控制。第一級角度反饋不斷去補償角度偏差直至最后接近零，第二級速度反饋調節加到電機上保持小車以特定速度運行。

PWM即脈沖寬度調制，它是指將輸出信號的最小周期固定，通過調整最小周期內工作周期的大小來控制輸出電壓的方法。本設計中，通過PWM調寬可以快速地控制輸入直流電機的電壓大小來達到調速的目的，設最小周期固定時，輸出電壓最大為，設占空比為，則平均輸出電壓為，當改變占空比α時，就可以得到不同大小的輸出電壓，從而達到調速的目的。

4.總結

本次設計以STC15W4K32S4為主控芯片，通過互補濾波和PID控制算法，設計出能夠自平衡的兩輪直立前進后退、左右轉向的控制系統。通過互補濾波算法，濾波后的角度，既能在靜態時緊緊趨近于加速度計的角度，也能在動態時隨陀螺儀角度的變化而變化，角度PD控制算法通過互補濾波輸出優化的角度并乘以比例增益，再相加未經濾波的角速度乘以微分增益的值，兩者共同輸出角度PD控制下的電機驅動PWM信號；速度PI控制算法將設定與實測速度值的偏差作為輸入，先積分后乘以積分增益，再結合輸入乘以比例增益得到的值，兩者共同輸出速度PI作用下的電機驅動PWM信號，角度與速度輸出的PWM值相加得到最終電機驅動PWM信號。實際運行結果表明，小車上電后的動態特性良好，能夠有效實現對小車整體平穩運行。本設計實現了小車在平整路面上的運動控制，如何實現小車在不平整及障礙路面執行特定的任務，需在以后的實踐中進一步進行研究。

[1]徐國保,尹怡欣等.智能移動機器人技術現狀及展望[J].機器人技術與應用,2010.3:29-32.

[2]上海交通大學物理教研室.大學物理學(第四版)[M].上海:上海交通大學出版社,2010:115-117.

[3]高金源,夏潔.計算機控制系統[M].北京:清華大學出版社,2010:159-164.

[4]賴義漢,王凱.基于MPU6050的雙輪平衡車控制系統設計[J].河南工程學院學報,2014,26(1):53-57.

[5]劉江.兩輪自平衡機器人系統設計及控制研究[D].北京:北京工業大學控制科學與工程系,2007.

[6]孟慶明.自動控制原理(非自動化類)(第二版)[M].北京:高等教育出版社,2008:179-183.

孟帥（1995－），男，本科，電子信息工程，研究方向：嵌入式系統開發，機器人運動控制。

大學生創新創業計劃項目（AH201411305036），2016蚌埠學院教學研究項目（2016JYXM28）。