王學卿，莊曉東

(青島大學 電子信息學院，青島 266590)

0 引言

近年來，兩輪自平衡電動車以其行走靈活、便利、節(jié)能等特點得到了很大的發(fā)展，但其本身是一種非線性、多變量和強耦合的類倒立擺系統(tǒng)[1-3]，其控制難度大、算法復雜。兩輪車保持直立平衡的前提是要通過傳感器獲得車體穩(wěn)定精確的姿態(tài)，但由于陀螺儀和加速度計等慣性傳感器的固有特性，測量的數(shù)值會隨時間產(chǎn)生不同程度的偏差和漂移[4]，本文通過使用互補濾波算法融合兩者的數(shù)據(jù)解決這個問題。其次簡單有效、健壯的控制算法是兩輪車保持平衡的關鍵，本設計首先使用角度PD控制器對車身傾角進負反饋控制，實驗結(jié)果顯示單純使用該控制算法不能讓兩輪車維持很好的平衡，因此又引入速度PI控制器，通過讓車體保持速度為零，從而增強平衡性能。

LabVIEW圖形化的編程環(huán)境和模塊化的軟件設計流程，非常符合工程思維[5-6]，配合NI的硬件平臺myRIO大大縮短了本設計的開發(fā)和調(diào)試周期。

1 系統(tǒng)硬件設計

系統(tǒng)的硬件部部分主要包括核心控制器myRIO、電源管理模塊、mpu6050六軸傳感器、帶電磁(霍爾）編碼器的直流減速電機以及TB6612電機驅(qū)動模塊。下圖是系統(tǒng)硬件的整體框圖。

圖1 .1 系統(tǒng)硬件整體框圖

1.1 核心控制器myRIO

NI myRIO是NI（美國國家儀器有限公司）針對教學和學生創(chuàng)新應用而推出的嵌入式系統(tǒng)開發(fā)平臺，內(nèi)嵌Xilinx Zynq芯片，這款芯片集成了雙核ARM Cortex-A9處理器以及Xilinx FPGA[7-8]，LabVIEW程序可以自動編譯并在ARM實時處理器中執(zhí)行。控制器除了常見的模擬輸入、模擬輸出、數(shù)字I/O之外，還包括I2C總線、SPI總線、PWM、編碼器、UART等接口，是一款強大的嵌入式實時處理器。

1.2 電源管理模塊

本模塊的核心是DC-DC升壓電路，主要負責給各部分模塊提供穩(wěn)定供電。該模塊輸入電壓為11.1v ，由3S航模鋰電池提供；經(jīng)過DC-DC升壓后輸出電壓為12v，直接為myRIO控制器和TB6612電機驅(qū)動模塊供電。

1.3 mpu6050六軸傳感器

MPU-60X0 系列是全球首例 9 軸運動處理傳感器。它集成了 3 軸 MEMS陀螺儀,3 軸 MEMS 加速度計,以及一個可擴展的數(shù)字運動處理器 DMP(Digital Motion Processor)[9]。能夠同時輸出x,y,z三個軸上的加速度與角速度數(shù)字信號。由該模塊采集兩輪車的當前姿態(tài)數(shù)據(jù)包括z軸上的加速度與y軸的角速度，并通過I2C接口將數(shù)據(jù)傳送給myRIO。

1.4 帶電磁(霍爾）編碼器的直流減速電機

直流減速電機為雙輪平衡車提供動力，減速比為30：1，電機尾部自帶了 13 線的AB相增量式磁（霍爾）編碼器車輪轉(zhuǎn)一圈，電機可以輸出 390個脈沖，編碼器的額定工作電壓是 5V,集成了上拉電阻和比較整形功能，可以直接輸出方波，通過與myRIO的編碼器接口相連，便可以獲取平衡車的速度信息。

1.5 TB6612電機驅(qū)動電路

TB6612FNG是東芝半導體公司生產(chǎn)的一款直流電機驅(qū)動器件，具有大電流MOSFET-H橋結(jié)構(gòu)[10]，雙通道電路輸出，每通道輸出最高1A的連續(xù)驅(qū)動電流，4種電機控制模式：正轉(zhuǎn)/反轉(zhuǎn)/制動/停止。myRIO控制器將一定頻率、占空比的PWM信號輸入到該模塊，改模塊繼而控制電機轉(zhuǎn)速和方向。

2 軟件和算法設計

本系統(tǒng)的程序部分采用myRIO版本的圖形化編程開發(fā)環(huán)境LabVIEW 2017開發(fā)，它可以直觀而又高效地修改控制參數(shù)、實時顯示平衡車的速度和姿態(tài)數(shù)據(jù)。LabVIEW中已經(jīng)內(nèi)置了多種現(xiàn)成的函數(shù)，并且針對NI myRIO各種外圍I/O提供不同層次的驅(qū)動函數(shù)，包括模擬輸入、模擬輸出、數(shù)字I/O，I2C總線、SPI總線、PWM、編碼器、UART等接口驅(qū)動函數(shù)，這都大大縮短了整個系統(tǒng)的開發(fā)和調(diào)試周期。下圖是系統(tǒng)軟件的流程圖和上位機界面。

圖2 .1

程序采用LabVIEW狀態(tài)機架構(gòu)，增強了程序的靈活性，方便代碼維護。系統(tǒng)上電開機后程序首先執(zhí)行初始化，包括初始化myRIO上的編碼器、設定輸出PWM波的頻率，初始化mpu6050傳感器并設定其工作方式。之后進入狀態(tài)機循環(huán)，第一個狀態(tài)是“數(shù)據(jù)采集”，在此狀態(tài)下myRIO通過I2C通信協(xié)議讀取mpu6050寄存器內(nèi)z軸加速度值和y軸的角速度值，同時讀取編碼器模塊的計數(shù)值；之后程序進入“PID控制狀態(tài)”，首先myRIO通過互補濾波算法將加速度計數(shù)據(jù)與陀螺儀數(shù)據(jù)融合得到穩(wěn)定的車體傾角，同時對編碼器的計數(shù)值做滑動濾波作為平衡車當前的速度，緊接著利用對角度誤差和速度誤差進行PID控制得到輸出即PWM波的占空比，將其輸入給PWM模塊繼而控制電機的轉(zhuǎn)速；最后程序來到“判斷停止”狀態(tài)，讀取上位機“停止”控件的值，如果為真則將PWM占空比置零、清空編碼計數(shù)值，終止整個程序，如果為假程序又會進入“數(shù)據(jù)采集”狀態(tài)，完成新一輪的控制過程。本系統(tǒng)的控制周期為5ms，即實現(xiàn)兩次控制的時間間隔為5ms。下面將分別介紹幾個關鍵算法的實現(xiàn)。

2.1 基于互補濾波的姿態(tài)測量算法

本系統(tǒng)使用的mpu6050六軸傳感器可以同時輸出加速度值和角速度值，當單獨使用加速度計時，車體的傾角可由公式（1）計算：

其中代表單獨使用速度計得到的第次的車體與豎直方向的夾角，為加速度計在z軸上采集的第n次數(shù)據(jù)。由公式（1）得到的姿態(tài)角度誤差不會隨時間積累，低頻性能好；但實際車模運行過程中，由于車體本身的擺動所產(chǎn)生的加速度會產(chǎn)生很大的干擾信號，使測量的角度中攜帶較高頻率的噪聲，如圖2.1 a)所示。

圖2 .1

當單獨使用陀螺儀的時，myRIO循環(huán)采樣獲取陀螺儀的角速率信息，即每隔一段很短的時間采樣一次，采用累加的方法實現(xiàn)積分的功能來計算角度值：

從以上的分析來看，單獨使用加速度計或陀螺儀得到的姿態(tài)信息并不能準確的反應車體姿態(tài)，從而難以讓平衡車保持直立狀態(tài)。但二者在頻域上具有互補特性，可采用互補濾波器對這兩種傳感器數(shù)據(jù)融合，提高姿態(tài)角度測量的精度和動態(tài)響應的性能[12]，其主要思想是充分利用加速度計提供的低頻角度信號和陀螺儀給出的高頻角速度信號，對加速度計和陀螺儀估計出的角度信號和分別通過低通濾波器F1(s)和高通濾波器F2(s)進行濾波，濾除相應干擾信號[13]，最后融合得到較為精確和穩(wěn)定的車體傾角。其原理框圖如圖2.2所示：

頻域表達式為

圖2 .2 互補濾波器的原理框圖

2.2 平衡控制算法

ek表示對誤差的第k次采樣，T為系統(tǒng)的采樣周期，Ti、Td分別為PID控制器的積分、微分時間。由于二輪平衡小車的高度不穩(wěn)定性需要對干擾做出迅速響應，同時積分會存在累積誤差，故本設計只引入式（7）實現(xiàn)的PD控制：

車體直立平衡時車體傾角為0°，因此PD控制器的輸入偏差

由于本設計的機械結(jié)構(gòu)所限，myRIO控制器只能放在平衡小車頂部，造成車體重心較高，加之電機質(zhì)量問題使得單純對角度進行PD控制來實現(xiàn)車體直立平衡的效果不好，所以本系統(tǒng)又引入了速度控制環(huán)，通過PI控制器使速度為零進一步增強車體直立平衡的效果。對速度實現(xiàn)PI控制的過程如式：

將（10）、（11）的運算結(jié)果Δ uk1、Δ uk2疊加作為控制電機轉(zhuǎn)速的PWM波的占空比，并作用到電機驅(qū)動電路上繼而控制左右電機實現(xiàn)負反饋控制。

3 系統(tǒng)調(diào)試與運行測試

3.1 互補濾波器參數(shù)選擇

在式（4）中當確定了系統(tǒng)采樣周期T，參數(shù)τ就決定了式（5）中K1、K2的大小。當τ大于采樣周期T時，K1＞K2，加速度傳感器測定的角度值比重比陀螺儀的大；反之，陀螺儀的積分起主要的作用[14]。本系統(tǒng)中采樣周期T=5ms，經(jīng)過實際測試τ取0.245s時互補濾波的效果最好，此時式K1=0.98，K2=0.02。本文同時對比了卡爾曼濾波算法，結(jié)果曲線如圖3.1所示。

采用互補濾波算法相較于卡爾曼濾波算法效果相當，但顯然互補濾波算法更加簡單，能夠大大節(jié)省控制器的運算資源。

圖3 .1 互補濾波后的車體傾角曲線

3.2 PID參數(shù)整定

本設計中存在兩個PID控制環(huán)，但速度PI控制器的引入僅僅是輔助角度PD控制，增強系統(tǒng)的平衡性能。首先調(diào)節(jié)角度PD控制器的參數(shù)，在實際調(diào)試中遵循先比例后微分的過程。調(diào)節(jié)式（10）中的比例參數(shù)讓其一直增加，直到車體出現(xiàn)往復的低頻抖動，此時引入微分參數(shù)使其不斷增加[15]，直到車體能夠保持平衡但有頻率較高的抖動出現(xiàn)，至此便確定了P、D參數(shù)的最大值，最終將每個系數(shù)乘以0.6得到理想的參數(shù)值。

由于機械結(jié)構(gòu)的缺陷，經(jīng)過測試發(fā)現(xiàn)無論怎樣調(diào)節(jié)角度PD控制器的參數(shù)，系統(tǒng)始終不能達到一個理想的直立平衡效果，總是產(chǎn)生一定速度的前后搖擺。此時引入速度PI控制器輔助直立平衡的控制，同樣遵循先比例后積分的原則，找到合適的Kp2之后可把Ki2的值設置為Ki2=Kp2/200。實驗結(jié)果表明使用角度PD控制器配合速度PI控制器，本系統(tǒng)能夠很好的實現(xiàn)直立平衡，并且具有一定的抗干擾能力。

4 結(jié)語

文中首先設計了二輪平衡車的整個硬件系統(tǒng)，之后又講解了基于LabVIEW 2017開發(fā)的軟件系統(tǒng)及其工作流程。針對mpu6050的加速度和陀螺儀輸出的角度信號的特點，深入的研究了互補濾波算法，從原理以及實現(xiàn)過程上都做了較為詳細的講解。實驗數(shù)據(jù)直觀的展示了互補濾波算法的有效性，它能為PID控制器提供穩(wěn)定、實時的車體傾角信號。為了實現(xiàn)系統(tǒng)的直立平衡，引入了角度PD控制器和速度PI控制器，在選擇了合適的參數(shù)后，實驗結(jié)果表明該系統(tǒng)實現(xiàn)了穩(wěn)定的直立平衡。隨著兩輪平衡小車的普及，本文設計的簡單有效的互補濾波算法和PID控制算法會有更廣的應用價值。

[1] 楊世勇，徐莉蘋，王培進. 單級倒立擺的PID控制研究[J]. 控制工程，2007（B05）：23-24．YANG Shi-yong， XU Li-ping， WANG Pei-jin. Study on PID Control of a Single Inverted Pendulum System[J]. CONTROL ENGINEERING OF CHINA， 2007（B05）： 23-24．

[2] 薛凡，孫京誥，嚴懷成. 兩輪平衡車的建模與控制研究[J]. 化工自動化及儀表， 2012，39（11）：1450-1497.XUE Fan， SUN Jing-gao， YAN Huan-cheng. Study on Modeling and Control of Two-wheeled Auto-balancing Vehicle[J]. Control and Instruments in Chemical Industry， 2012，39（11）：1450-1497.

[3] 傅忠云，陳秋陽，劉文波. 基于直接轉(zhuǎn)矩控制的兩輪自平衡車系統(tǒng)設計[J]. 壓電與聲光， 2014， 36（6）： 967-971 FU Zhongyun， CHEN Qiuyang， LIU Wenbo. The Design of Two-wheel Self-balanced Vehicle System Based on Direct Torque Control[J].Piezoelectrics & Acoustooptics，2014，36（6）：967-971.

[4] 趙杰，王曉宇，秦勇，等．基于UKF的兩輪自平衡機器人姿態(tài)最優(yōu)估計研究[J]．機器人，2006，28（6）：605-609．ZHAO Jie， WANG Xiao-yu， QIN Yong， et al. UKF-based Optimal Attitude Estimation of Two-wheeled Self-balanced.Robots[J]. ROBOT，2006，28（6）：605-609．

[5] 王建群，南金瑞，孫逢春，等. 基于LabVIEW的數(shù)據(jù)采集系統(tǒng)的實現(xiàn)[J]. 計算機工程與應用， 2003， 39（21）： 122-125.Wang Jianqun， Nan Jinrm， Sun Fengchun， et al. Realization of the Data Acquisition System Based on LabVIEW[J]. COMPUTER ENGINEERING AND APPLICATIONS， 2003，39（21）：122-125.

[6] 楊忠仁， 饒程， 鄒建，等. 基于LabVIEW數(shù)據(jù)采集系統(tǒng)[J]. 重慶大學學報（自然科學版）， 2004， 27（2）： 32-35.YANG Zhong-ren， RAO Cheng， ZOU Jian， et al. Study on DAQ System by Using LabVIEW[J]. JOURNAL OF CHONGQING UNIVERSITY（NATURAL SCIENCE EDITION）， 2004，27（2）：32-35.

[7] 茅穎. 基于myRIO控制器的自平衡車的PID算法實現(xiàn)[J]. 福建電腦，2016，32（12）：107-109.

[8] 謝斌，張超，毛恩榮，等 . 基于 myRIO 的電動拖拉機驅(qū)動控制器設計與室內(nèi)試驗 [J]. 農(nóng) 業(yè) 工 程 學 報， 2015， 31（18）： 55-62.Xie Bin， Zhang Chao， Mao Enrong. et al. Motor controller design and indoor experiment for electric tractor based on myRIO[J]. Transactions of the Chinese Society of Agricultural Engineering，2015，31（18）：55-62.

[9] 陳煒峰，席萬強，周峰，等. 基于物聯(lián)網(wǎng)技術的山體滑坡監(jiān)測及預警系統(tǒng)設計[J]. 電子器件，2014，37（2）：279-282.CHEN Weifeng， XI Wanqiang， ZHOU Feng， et al. Landslide Monitoring and Warning System Based on IOT Technology[J]. Chinese Journal of Electron Devices，2014，37（2）：279-282.

[10] 王建平， 盧杉， 武歡歡. TB6612FNG在直流電機控制設計中的應用[J]. 電子設計工程，2010，18（6）：65-67.WANG Jian-ping， LU Shan， WU Huan-huan. Application of TB6612FNG in DC motor control system[J]. ELECTRONIC DESIGN ENGINEERING， 2010，18（6）：65-67.

[11] 傅忠云， 朱海霞， 孫金秋，等. 基于慣性傳感器MPU6050的濾波算法研究[J]. 壓電與聲光，2015，37（5）：821-829.FU Zhongyun， ZHU Haixia， SUN Jinqiu， et al. Study on Filtering Algorithm Based on Inertial Sensors MPU6050[J]. Piezoelectrics &Acoustooptics， 2015，37（5）：821-829.

[12] 詹長書，邢博坤，樸光宇，等. 兩輪自平衡小車的控制策略設計[J]．森林工程，2015，31（4）：73-82．Zhan Changshu， Xing Bokun， Piao Guangyu， et al. Control Strategy of Two-wheeled Self-balancing Car[J]. Forest Engineering， 2015， 31（4）：73-82．

[13] 陳鵬展， 朱年華， 李杰 . 兩輪自平衡車姿態(tài)檢測與平衡控制 [J]. 控制工程， 2017， 24（8）： 1574-1578.CHEN Peng-zhan， ZHU Nian-hua， LI Jie. Attitude Detection and Balance Control of Two-wheeled Self-balancing Vehicles[J]. Control Engineering of China， 2017，24（8）：1574-1578.

[14] 盧鋒. 兩輪小車姿態(tài)檢測與平衡控制研究[J]. 工業(yè)儀表與自動化裝置， 2014，（3）： 26-75.LU Feng. The research of vehicle attitude detection and balance control[J]. Industrial Instrumentation & Automation， 2014，（3）： 26-75.

[15] 佟遠， 張莎 . 基于PID雙閉環(huán)的旋轉(zhuǎn)倒立擺控制系統(tǒng)[J]. 測控技術， 2016， 35（8）： 85-88.TONG Yuan， ZHANG Sha. Rotary Inverted Pendulum Control System Based on PID Double Closed Loop[J]. Measurement & Control Technology， 2016，35（8）： 85-88.