基于NI myRIO的兩輪自平衡小車設(shè)計(jì)

王學(xué)卿，莊曉東

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

0 引言

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

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

1 系統(tǒng)硬件設(shè)計(jì)

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

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

1.1 核心控制器myRIO

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

1.2 電源管理模塊

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

1.3 mpu6050六軸傳感器

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

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

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

1.5 TB6612電機(jī)驅(qū)動(dòng)電路

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

2 軟件和算法設(shè)計(jì)

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

圖2 .1

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

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

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

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

圖2 .1

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

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

頻域表達(dá)式為

圖2 .2 互補(bǔ)濾波器的原理框圖

2.2 平衡控制算法

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

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

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

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

3 系統(tǒng)調(diào)試與運(yùn)行測(cè)試

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

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

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

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

3.2 PID參數(shù)整定

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

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

4 結(jié)語(yǔ)

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

[1] 楊世勇，徐莉蘋，王培進(jìn). 單級(jí)倒立擺的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] 薛凡，孫京誥，嚴(yán)懷成. 兩輪平衡車的建模與控制研究[J]. 化工自動(dòng)化及儀表， 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] 傅忠云，陳秋陽(yáng)，劉文波. 基于直接轉(zhuǎn)矩控制的兩輪自平衡車系統(tǒng)設(shè)計(jì)[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的兩輪自平衡機(jī)器人姿態(tài)最優(yōu)估計(jì)研究[J]．機(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)的實(shí)現(xiàn)[J]. 計(jì)算機(jī)工程與應(yīng)用， 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]. 重慶大學(xué)學(xué)報(bào)（自然科學(xué)版）， 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算法實(shí)現(xiàn)[J]. 福建電腦，2016，32（12）：107-109.

[8] 謝斌，張超，毛恩榮，等 . 基于 myRIO 的電動(dòng)拖拉機(jī)驅(qū)動(dòng)控制器設(shè)計(jì)與室內(nèi)試驗(yàn) [J]. 農(nóng) 業(yè) 工 程 學(xué) 報(bào)， 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] 陳煒?lè)?，席萬(wàn)強(qiáng)，周峰，等. 基于物聯(lián)網(wǎng)技術(shù)的山體滑坡監(jiān)測(cè)及預(yù)警系統(tǒng)設(shè)計(jì)[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ī)控制設(shè)計(jì)中的應(yīng)用[J]. 電子設(shè)計(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] 詹長(zhǎng)書，邢博坤，樸光宇，等. 兩輪自平衡小車的控制策略設(shè)計(jì)[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)檢測(cè)與平衡控制 [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)檢測(cè)與平衡控制研究[J]. 工業(yè)儀表與自動(dòng)化裝置， 2014，（3）： 26-75.LU Feng. The research of vehicle attitude detection and balance control[J]. Industrial Instrumentation & Automation， 2014，（3）： 26-75.

[15] 佟遠(yuǎn)， 張莎 . 基于PID雙閉環(huán)的旋轉(zhuǎn)倒立擺控制系統(tǒng)[J]. 測(cè)控技術(shù)， 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.