王素青，黃文龍

(南京航空航天大學(xué) 金城學(xué)院，南京 211156)

基于Android的雙輪機(jī)器人控制系統(tǒng)研究與設(shè)計(jì)*

王素青，黃文龍

(南京航空航天大學(xué) 金城學(xué)院，南京 211156)

雙輪機(jī)器人系統(tǒng)具有與傳統(tǒng)倒立擺相似的多變量、非線性、強(qiáng)耦合和參數(shù)不穩(wěn)定等特點(diǎn)。針對(duì)系統(tǒng)的復(fù)雜性，文章提出了基于Android平臺(tái)和藍(lán)牙通信模塊實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的無(wú)線控制方式，采用基于DMP和Kalman濾波相結(jié)合的姿態(tài)數(shù)據(jù)處理算法及雙閉環(huán)PID控制算法相結(jié)合的控制方法，實(shí)現(xiàn)雙輪機(jī)器人的自平衡控制。文中創(chuàng)新地在系統(tǒng)上增加兩個(gè)機(jī)械臂，借助機(jī)械臂能夠使系統(tǒng)快速恢復(fù)平衡以及當(dāng)系統(tǒng)休眠時(shí)，借助機(jī)械臂保持平衡實(shí)現(xiàn)節(jié)能的目的。測(cè)試結(jié)果表明，該系統(tǒng)能夠通過(guò)藍(lán)牙準(zhǔn)確地發(fā)送、接收數(shù)據(jù)，控制精度高、系統(tǒng)穩(wěn)定性好、時(shí)間響應(yīng)快。

雙輪機(jī)器人；Android；藍(lán)牙；DMP和Kalman濾波；PID控制

0 引言

隨著傳感器技術(shù)、通信技術(shù)、自動(dòng)控制技術(shù)和人工智能技術(shù)的快速發(fā)展，機(jī)器人在工業(yè)、農(nóng)業(yè)、醫(yī)學(xué)、軍事等領(lǐng)域的應(yīng)用越來(lái)越廣泛。雙輪機(jī)器人因其運(yùn)動(dòng)靈活、結(jié)構(gòu)簡(jiǎn)單、能夠適應(yīng)不同的工作環(huán)境等特點(diǎn)，在軍事領(lǐng)域及其他領(lǐng)域有著廣泛的應(yīng)用前景。雙輪機(jī)器人具有與傳統(tǒng)倒立擺相似的多變量、非線性、強(qiáng)耦合和參數(shù)不穩(wěn)定等特點(diǎn)，是研究多種控制方法的理想平臺(tái)[1]。針對(duì)雙輪自平衡機(jī)器人系統(tǒng)的復(fù)雜性，國(guó)內(nèi)外很多學(xué)者對(duì)其控制方法進(jìn)行了一定的研究，主要有PID控制法、狀態(tài)反饋控制器(LQR)控制法[2]、模糊控制法、神經(jīng)網(wǎng)絡(luò)控制法[3]等等。目前，較成熟的機(jī)器人控制方式主要有[4]：語(yǔ)音控制方式、紅外線遙控方式、基于WiFi無(wú)線網(wǎng)絡(luò)控制方式、基于GSM/GPRS網(wǎng)絡(luò)的遠(yuǎn)程控制方式等，這些控制方式基本滿足了現(xiàn)有機(jī)器人的應(yīng)用需求。但是這幾種控制方式存在不便于攜帶、移動(dòng)性較差、且控制環(huán)境需要網(wǎng)絡(luò)等問(wèn)題。

針對(duì)以上問(wèn)題，并且基于成熟的藍(lán)牙技術(shù)和Android在移動(dòng)互聯(lián)網(wǎng)市場(chǎng)份額的不斷增長(zhǎng)，本文提出了利用手機(jī)的藍(lán)牙通信功能實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人無(wú)線控制的解決方案，系統(tǒng)的控制方法則采用的是基于DMP和Kalman濾波相結(jié)合的姿態(tài)數(shù)據(jù)處理算法及雙閉環(huán)PID姿態(tài)控制算法，實(shí)現(xiàn)雙輪機(jī)器人的自平衡控制。另外，在系統(tǒng)硬件結(jié)構(gòu)上，創(chuàng)新地增加兩個(gè)機(jī)械臂，其作用有兩個(gè)方面：①在系統(tǒng)失去平衡時(shí)，能夠借助機(jī)械臂使系統(tǒng)快速恢復(fù)平衡；②在不需要工作時(shí)，可遙控系統(tǒng)使其處于休眠狀態(tài)，僅借助機(jī)械臂保持系統(tǒng)平衡，達(dá)到節(jié)能的目的。

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

雙輪機(jī)器人控制系統(tǒng)要實(shí)現(xiàn)的功能為：在沒(méi)有外界作用力的情況下，僅依靠自身控制系統(tǒng)，完成上位機(jī)發(fā)出的指令動(dòng)作(自啟、休眠、前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)和原地等待等)，在運(yùn)行過(guò)程中能保持姿態(tài)的平衡。根據(jù)系統(tǒng)功能要求，所設(shè)計(jì)的系統(tǒng)硬件電路結(jié)構(gòu)框圖如圖1所示，系統(tǒng)由上位機(jī)和下位機(jī)兩部分組成，上位機(jī)采用帶藍(lán)牙通訊功能的Android手機(jī)，下位機(jī)主要由微處理器模塊、姿態(tài)檢測(cè)模塊、機(jī)械臂模塊、直流電機(jī)驅(qū)動(dòng)模塊、編碼器、藍(lán)牙通信模塊及電源模塊等幾部分構(gòu)成。電源模塊負(fù)責(zé)系統(tǒng)各個(gè)模塊控制電路的電源。微處理器模塊是下位機(jī)的核心部件，用來(lái)接收姿態(tài)檢測(cè)模塊實(shí)時(shí)采集的姿態(tài)數(shù)據(jù)，根據(jù)上位機(jī)指令控制系統(tǒng)進(jìn)行相應(yīng)的動(dòng)作。姿態(tài)檢測(cè)模塊實(shí)時(shí)對(duì)系統(tǒng)的姿態(tài)信息進(jìn)行采集并將數(shù)據(jù)發(fā)送給微處理器。機(jī)械臂模塊根據(jù)微處理器輸出的PWM信號(hào)控制機(jī)械臂的相應(yīng)動(dòng)作。直流電機(jī)驅(qū)動(dòng)模塊根據(jù)微處理器輸出的PWM信號(hào)控制直流電機(jī)的速度和方向。編碼器負(fù)責(zé)實(shí)時(shí)對(duì)直流電機(jī)的速度和方向進(jìn)行測(cè)量，并將測(cè)量數(shù)據(jù)發(fā)送給微處理器，形成閉環(huán)控制。藍(lán)牙通信模塊負(fù)責(zé)上位機(jī)和下位機(jī)之間的數(shù)據(jù)進(jìn)行無(wú)線傳輸。

圖1 系統(tǒng)的硬件電路結(jié)構(gòu)框圖

(1) 微處理器模塊

系統(tǒng)中微處理器模塊選用的是意法半導(dǎo)體公司的STM32F103C8T6，它是一種基于Cortex-M3內(nèi)核的32位CPU，最高工作頻率為72MHz，片上集成32-512KB的Flash存儲(chǔ)器，6-64KB的SRAM存儲(chǔ)器。片上集成了SPI，I2C和USART等通信接口，可以方便地和姿態(tài)檢測(cè)傳感器和藍(lán)牙通訊模塊進(jìn)行數(shù)據(jù)交換。含有7個(gè)定時(shí)器，可輸出多路PWM信號(hào)能確保電機(jī)驅(qū)動(dòng)模塊獲得穩(wěn)定的PWM波形，避免軟件的頻繁中斷為系統(tǒng)帶來(lái)不穩(wěn)定因素。

(2)姿態(tài)檢測(cè)模塊

系統(tǒng)中姿態(tài)檢測(cè)模塊選用的是美國(guó)InvenSense公司的MPU-6050，整合了3軸陀螺儀和3軸加速度計(jì)，能夠準(zhǔn)確的對(duì)機(jī)器人的姿態(tài)進(jìn)行實(shí)時(shí)檢測(cè)[5-6]。系統(tǒng)中MPU-6050作為從機(jī)使用，通過(guò)SDA和SCL端口和主機(jī)STM32進(jìn)行通信，其檢測(cè)的數(shù)據(jù)經(jīng)過(guò)內(nèi)置的16位AD轉(zhuǎn)換器進(jìn)行轉(zhuǎn)換，然后將16位數(shù)字量通過(guò)I2C總線接口發(fā)送給主機(jī)。

(3) 電機(jī)驅(qū)動(dòng)模塊

系統(tǒng)中直流電機(jī)驅(qū)動(dòng)模塊選用的是TOSHIBA公司的一款具有大電流MOSFET-H橋結(jié)構(gòu)，雙通道電路輸出的TB6612FNG驅(qū)動(dòng)模塊，它和傳統(tǒng)的L298N相比，不需外加散熱片，外圍電路簡(jiǎn)單，只需外接電源濾波電容就可以直接驅(qū)動(dòng)電機(jī)，體積減小很多。

(4) 機(jī)械臂模塊

系統(tǒng)中機(jī)械臂模塊選用的是Tower Pro公司生產(chǎn)的MG996R大扭矩舵機(jī)驅(qū)動(dòng)器，采用金屬齒輪及軸承，驅(qū)動(dòng)信號(hào)為PWM，根據(jù)PWM信號(hào)的占空比不同轉(zhuǎn)到對(duì)應(yīng)不同位置，其具有扭矩大、反應(yīng)快、精度高、性價(jià)比高等特點(diǎn)。

(5) 藍(lán)牙通信模塊

系統(tǒng)中無(wú)線數(shù)據(jù)通信模塊選用的是HC-06藍(lán)牙模塊，該模塊內(nèi)置藍(lán)牙V2.0協(xié)議標(biāo)準(zhǔn)的CSR藍(lán)牙通信芯片，可以與手機(jī)上的藍(lán)牙進(jìn)行匹配。HC-06模塊和Android手機(jī)通過(guò)串口形式進(jìn)行數(shù)據(jù)無(wú)線傳輸。

2 系統(tǒng)的下位機(jī)軟件設(shè)計(jì)

2.1 系統(tǒng)的下位機(jī)總體流程設(shè)計(jì)

系統(tǒng)的下位機(jī)主程序流程圖如圖2所示，主要由初始化程序、下位機(jī)與上位機(jī)的通訊程序、傳感器數(shù)據(jù)采集程序、Kalman濾波程序、直流電機(jī)控制程序和舵機(jī)控制程序等幾部分組成。

系統(tǒng)啟動(dòng)后，首先進(jìn)行初始化，然后執(zhí)行下位機(jī)與上位機(jī)的通訊程序，判斷是否已連接上位機(jī)，若連接上，則根據(jù)上位機(jī)的控制指令執(zhí)行相應(yīng)的動(dòng)作。每隔5ms采集一次MPU-6050傳感器的角度和角速度數(shù)據(jù)，然后調(diào)用Kalman濾波程序計(jì)算出最優(yōu)估計(jì)值，根據(jù)直流電機(jī)控制程序中雙閉環(huán)PID控制算法計(jì)算出PWM信號(hào)，根據(jù)PWM的輸出來(lái)控制直流電機(jī)的速度和方向。舵機(jī)控制程序根據(jù)上位機(jī)的控制指令來(lái)判斷機(jī)器人系統(tǒng)的兩個(gè)機(jī)械臂是收起還是放下。

圖2 系統(tǒng)的下位機(jī)主程序流程圖

2.2 數(shù)據(jù)濾波處理

由于自平衡機(jī)器人系統(tǒng)需要在動(dòng)靜態(tài)情況下均能準(zhǔn)確的測(cè)量出機(jī)器人的姿態(tài)數(shù)據(jù)，因此需要對(duì)陀螺儀和加速度計(jì)的采集數(shù)據(jù)進(jìn)行濾波處理而得到最優(yōu)值，本系統(tǒng)采用DMP濾波和Kalman濾波相結(jié)合的方案實(shí)現(xiàn)對(duì)機(jī)器人不穩(wěn)定的姿態(tài)數(shù)據(jù)進(jìn)行濾波處理[7]。

(1)DMP濾波

DMP濾波是采用MPU-6050自帶的數(shù)字運(yùn)動(dòng)處理器(DMP：Digital Motion Processing)，它可接收并處理來(lái)自陀螺儀和加速度計(jì)的采集數(shù)據(jù)，處理結(jié)果可直接從DMP寄存器讀出，且比直接從陀螺儀和加速度計(jì)讀出的數(shù)據(jù)更加穩(wěn)定。

(2)Kalman濾波

Kalman濾波算法是以最小均方誤差為最佳估計(jì)準(zhǔn)則的遞歸數(shù)據(jù)處理算法，利用前一時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的測(cè)量值來(lái)更新對(duì)狀態(tài)變量的估計(jì)，最終求出當(dāng)前時(shí)刻的最優(yōu)估計(jì)值。

根據(jù)Kalman濾波原理，要估計(jì)k時(shí)刻的傾角，需要根據(jù)k-1時(shí)刻的傾角值來(lái)預(yù)測(cè)k時(shí)刻傾角，并得到k時(shí)刻的高斯噪聲偏差。系統(tǒng)中，陀螺儀測(cè)量的角速度作為預(yù)測(cè)值的控制量，加速度計(jì)測(cè)量的角度作為測(cè)量值，根據(jù)兩者的協(xié)方差不斷進(jìn)行遞歸算法直到估計(jì)出最優(yōu)傾角。Kalman濾波方程如式(1)～式(5)所示[8]。

X(k|k-1)=AX(k-1|k-1)+BU(k)

(1)

P(k|k-1)=AP(k-1|k-1)AT+Q

(2)

Kg(k)=P(k|k-1) HT/(HP(k|k-1)HT+R)

(3)

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))

(4)

P(k|k)=(I- Kg(k)H)P(k|k-1)

(5)

式(4)為狀態(tài)估計(jì)方程，Z(k)=Accel為加速度計(jì)的角度測(cè)量值。

2.3 姿態(tài)控制算法

經(jīng)過(guò)硬件濾波和軟件濾波后的機(jī)器人姿態(tài)數(shù)據(jù)，其數(shù)據(jù)質(zhì)量有明顯改善，有利于對(duì)機(jī)器人進(jìn)行姿態(tài)控制。本系統(tǒng)采用雙閉環(huán)PID控制算法對(duì)機(jī)器人系統(tǒng)進(jìn)行自平衡控制，其結(jié)構(gòu)框圖如圖3所示，速度閉環(huán)控制作為系統(tǒng)的內(nèi)環(huán)，姿態(tài)閉環(huán)控制作為系統(tǒng)的外環(huán)。

姿態(tài)閉環(huán)控制[11]采用了PD控制器，利用數(shù)據(jù)濾波處理后的傾角和角速度的最優(yōu)值和給定姿態(tài)傾角進(jìn)行比較，得到誤差信號(hào)，然后經(jīng)過(guò)PID控制器處理后得到輸出值，其輸出值作為內(nèi)環(huán)的輸入量。PID的輸出控制電機(jī)PWM信號(hào)的占空比，表達(dá)式為：

PWM=Kp×θ+Kd×θ·

(6)

速度閉環(huán)控制采用了P控制，利用增量編碼器檢測(cè)到的實(shí)時(shí)速度和給定速度進(jìn)行比較得到誤差信號(hào)，然后經(jīng)過(guò)PID控制器處理后得到輸出值，其輸出作為電機(jī)PWM信號(hào)占空比的增量值，表達(dá)式為：

ΔPWM=Ksp×speed

(7)

姿態(tài)閉環(huán)控制和速度閉環(huán)控制是相互影響的[12]，將其融合后的PWM信號(hào)為：

PWMzong=Kp×θ+Kd×θ·+Kps×speed

(8)

圖3 系統(tǒng)雙閉環(huán)PID控制算法框圖

經(jīng)過(guò)PID控制算法后得到PWM信號(hào)，由微處理器直接發(fā)送給直流電機(jī)驅(qū)動(dòng)模塊，從而控制直流電機(jī)的速度和方向。電機(jī)驅(qū)動(dòng)模塊輸出兩路PWM信號(hào)，使用時(shí)通過(guò)給其中一路占空比為零，另外一路不為零的方法來(lái)控制電機(jī)的正反轉(zhuǎn)[13]。

3 系統(tǒng)的上位機(jī)軟件設(shè)計(jì)

上位機(jī)軟件運(yùn)行平臺(tái)采用Android平臺(tái)，由運(yùn)行在Android手機(jī)上的APP應(yīng)用程序來(lái)實(shí)現(xiàn)機(jī)器人的無(wú)線控制，且Android手機(jī)能夠?qū)崟r(shí)接收、存儲(chǔ)和顯示系統(tǒng)的姿態(tài)信息。Android應(yīng)用程序采用Java語(yǔ)言編程軟件實(shí)現(xiàn)，其程序流程圖如圖4所示，主要由藍(lán)牙通信程序設(shè)計(jì)[14]、UI控制程序設(shè)計(jì)、UI顯示程序設(shè)計(jì)。

圖4 系統(tǒng)的上位機(jī)程序流程圖

3.1 藍(lán)牙通信程序設(shè)計(jì)

Android設(shè)備和藍(lán)牙模塊進(jìn)行通信，需要完成4個(gè)步驟[15]：①藍(lán)牙打開：使用getDefaultAdapter( )方法來(lái)判斷設(shè)備是否支持藍(lán)牙傳輸功能，確認(rèn)有藍(lán)牙后再調(diào)用isEnabled( )方法來(lái)檢查藍(lán)牙是否可用，打開藍(lán)牙。②藍(lán)牙搜索：調(diào)用startDiscovery( )方法搜索附近已配對(duì)或可用的設(shè)備，將搜索到的設(shè)備名稱和MAC地址存儲(chǔ)在列表中。③藍(lán)牙連接：藍(lán)牙配對(duì)后還必須通過(guò)藍(lán)牙連接，連接成功后才可以進(jìn)行數(shù)據(jù)傳輸，手機(jī)藍(lán)牙和藍(lán)牙模塊要彼此共享RFCOMM通道，調(diào)用Bluetoothdevice類中的createRfcommSocketToServiceRecord(MY-UUID)方法，獲取BluetoothSocket后才能建立連接。④數(shù)據(jù)通信：連接成功后，手機(jī)藍(lán)牙和藍(lán)牙模塊擁有同一個(gè)BluetoothSocket，通過(guò)getInputStream( )和getOutputStream( )方法獲取輸入輸出數(shù)據(jù)流，然后調(diào)用write( )、read( )方法寫入或讀出數(shù)據(jù)，實(shí)現(xiàn)藍(lán)牙數(shù)據(jù)的通信。藍(lán)牙通信程序設(shè)計(jì)即按照藍(lán)牙通信的4個(gè)步驟進(jìn)行相應(yīng)的程序設(shè)計(jì)。

3.2 UI控制程序設(shè)計(jì)

UI控制程序設(shè)計(jì)是指在上位機(jī)上編寫界面及程序用以實(shí)現(xiàn)對(duì)機(jī)器人的遠(yuǎn)程控制。UI控制界面上共布局9個(gè)Button按鈕，分別控制系統(tǒng)的運(yùn)行狀態(tài)，按鈕布局如圖5所示，界面中間的5個(gè)按鈕分別控制的動(dòng)作為：左轉(zhuǎn)、前進(jìn)、右轉(zhuǎn)、后退、停止(中間按鈕)，界面下方的4個(gè)按鈕分別控制的動(dòng)作為：休眠、附加功能(待擴(kuò)展用)、機(jī)械臂收、機(jī)械臂放。每個(gè)按鈕組件的屬性都需進(jìn)行設(shè)置，此外，每個(gè)按鈕組件都綁定一個(gè)監(jiān)聽(tīng)事件，當(dāng)單擊某一按鈕時(shí)，就會(huì)啟動(dòng)對(duì)應(yīng)的監(jiān)聽(tīng)事件onClick(View arg0)，從而執(zhí)行監(jiān)聽(tīng)事件里的程序，即執(zhí)行代碼sendMessage(“ ” +1)(每個(gè)按鈕對(duì)應(yīng)一個(gè)字符串：+1～+9，此次“+1”代表“前進(jìn)”按鈕)，調(diào)用sendMessage( )函數(shù)，執(zhí)行函數(shù)中代碼：byte[] send=message.getBytes( ); mChatService.write(send)，調(diào)用write( )函數(shù)，執(zhí)行函數(shù)中代碼：mmOutStream.write(buffer);mHandler.obtainMessage(BluetoothChat.MESSAGE_WRITE,-1,-1,buffer).sendToTarget()，將數(shù)據(jù)寫入到緩沖區(qū)并給mHandler發(fā)送消息。

3.3 UI顯示程序設(shè)計(jì)

UI顯示程序設(shè)計(jì)是指在上位機(jī)界面上實(shí)時(shí)顯示遠(yuǎn)程機(jī)器人的當(dāng)前傾角和實(shí)時(shí)速度。UI顯示界面上共布置2個(gè)TextView組件，組件的名稱屬性設(shè)置語(yǔ)句分別為：android:id=“@+id/qingjiao”，android:id=“@+id/sudu”，組件初始化語(yǔ)句分別為：qingjiao.setText(“當(dāng)前傾角: ”)，sudu.setText(“實(shí)時(shí)速度: ”)。Android設(shè)備和藍(lán)牙模塊都有一個(gè)ConnectedThread線程，一旦有數(shù)據(jù)進(jìn)來(lái)，就會(huì)調(diào)用mHandler. obtainMessage(BluetoothChat.MESSAGE_READ,bytes,-1,buffer).sendToTarget()，即給mHandler發(fā)送消息。當(dāng)前傾角和實(shí)時(shí)速度的信息更新是利用read( )方法從緩沖區(qū)讀取數(shù)據(jù)，其更新代碼為：qingjiao.setText(“當(dāng)前傾角: ”+readMessage.substring(1)); sudu.setText(“實(shí)時(shí)速度: ”+readMessage.substring(0))。

4 系統(tǒng)調(diào)試

4.1 系統(tǒng)的整體調(diào)試

調(diào)試之前，先檢查系統(tǒng)的接線是否正確，供電是否正常，硬件檢查沒(méi)有問(wèn)題后便可進(jìn)行下載程序，然后分模塊進(jìn)行調(diào)試，具體為：測(cè)試電機(jī)驅(qū)動(dòng)電路，測(cè)試機(jī)械臂模塊，測(cè)試姿態(tài)檢測(cè)模塊，測(cè)試藍(lán)牙通信模塊等，調(diào)試都通過(guò)后即可進(jìn)行整體調(diào)試。

在手機(jī)上安裝上位機(jī)APP應(yīng)用程序：BluetoothChat.apk，打開程序，進(jìn)行藍(lán)牙配對(duì)和連接，連接成功后操作手機(jī)控制界面上不同功能的按鈕，系統(tǒng)能夠完成和按鈕所對(duì)應(yīng)的動(dòng)作。系統(tǒng)在運(yùn)行時(shí)，手機(jī)顯示界面上能夠?qū)崟r(shí)顯示當(dāng)前傾角和實(shí)時(shí)速度具體數(shù)值并且數(shù)值能夠更新。

4.2 系統(tǒng)的PID參數(shù)整定

系統(tǒng)的PID控制參數(shù)需要通過(guò)多次實(shí)驗(yàn)進(jìn)行試湊法進(jìn)行整定，整定出能使系統(tǒng)平衡的最佳參數(shù)。

姿態(tài)閉環(huán)PID控制的調(diào)試過(guò)程：首先調(diào)整Kp值，讓系統(tǒng)能站立且抖動(dòng)相對(duì)較?。唤又３諯p不變，調(diào)整Kd值，進(jìn)一步減少抖動(dòng)；最后微調(diào)Kp和Kd，使系統(tǒng)處于最佳狀態(tài)，經(jīng)多次調(diào)試得到，當(dāng)Kp=4.45、Kd=0.75時(shí)，系統(tǒng)能夠在10s內(nèi)達(dá)到平衡狀態(tài)。

采用同樣方法，對(duì)速度閉環(huán)PID參數(shù)進(jìn)行多次調(diào)試，當(dāng)Ksp=6.5時(shí)，系統(tǒng)能快速穩(wěn)定的達(dá)到平衡狀態(tài)。

4.3 系統(tǒng)的動(dòng)態(tài)調(diào)試

Kalman濾波方程中的協(xié)方差矩陣Q中D(Angle)和D(Q_bias)的不同取值代表濾波器對(duì)傳感器的不同信任程度且對(duì)濾波效果造成不同影響。通過(guò)多次實(shí)驗(yàn)修正，當(dāng)D(Angle)值為0.001，D(Q_bias)值為0.003，角度測(cè)量噪聲值R_angle值為0.5時(shí)，濾波器能達(dá)到較好的濾波效果。

當(dāng)系統(tǒng)失去平衡時(shí)(平衡條件：連續(xù)50組傳感器輸出傾角均小于±100)，通過(guò)12次實(shí)驗(yàn)測(cè)試可得：在有機(jī)械臂情況下，系統(tǒng)能夠在10s內(nèi)重新恢復(fù)平衡；在無(wú)機(jī)械臂情況下，系統(tǒng)能夠在25s內(nèi)重新恢復(fù)平衡。表1中僅列出了6組實(shí)驗(yàn)測(cè)試數(shù)據(jù)。

表1 系統(tǒng)恢復(fù)平衡的時(shí)間比較表

系統(tǒng)平衡時(shí)，施加一個(gè)外力使系統(tǒng)傾角±45°≤θ≤±60°時(shí)，要求系統(tǒng)能迅速響應(yīng)這個(gè)外力作用，并能快速減小傾角變化，重新建立平衡狀態(tài)。通過(guò)多次調(diào)試及修正PID參數(shù)，最終系統(tǒng)受到這樣的外力作用時(shí)，能夠在25s內(nèi)通過(guò)自身調(diào)節(jié)達(dá)到自平衡狀態(tài)。系統(tǒng)運(yùn)行時(shí)，手機(jī)控制界面如圖5所示，此時(shí)系統(tǒng)處于“左轉(zhuǎn)”運(yùn)行狀態(tài)，當(dāng)前傾角為-5.737°，實(shí)時(shí)速度為1.867轉(zhuǎn)/s。機(jī)器人平衡時(shí)效果圖如圖6所示。

圖5 手機(jī)控制界面

圖6 機(jī)器人平衡時(shí)效果圖

5 結(jié)束語(yǔ)

本文研究和設(shè)計(jì)了一個(gè)基于Android的雙輪機(jī)器人控制系統(tǒng)，利用手機(jī)的藍(lán)牙功能實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的無(wú)線控制，系統(tǒng)采用基于DMP和Kalman濾波相結(jié)合的姿態(tài)數(shù)據(jù)處理算法及雙閉環(huán)PID姿態(tài)控制算法，實(shí)現(xiàn)系統(tǒng)的自平衡控制。系統(tǒng)能夠借助機(jī)械臂，使其快速恢復(fù)平衡或?qū)崿F(xiàn)節(jié)能的目的。實(shí)驗(yàn)結(jié)果表明，通過(guò)藍(lán)牙可以實(shí)時(shí)地、靈活地控制機(jī)器人的運(yùn)行狀態(tài)，并能實(shí)時(shí)顯示當(dāng)前姿態(tài)信息，系統(tǒng)也能快速穩(wěn)定地達(dá)到自平衡狀態(tài)。文中提出的無(wú)線控制方案及控制方法在小型移動(dòng)機(jī)器人系統(tǒng)的控制方面有一定的應(yīng)用前景。

[1] 季鵬飛，朱燕，程傳統(tǒng),等. 基于STM32的兩輪自平衡小車控制系統(tǒng)設(shè)計(jì)[J]. 電子科技，2014，27(11)：96-99.

[2] 武俊峰，孫雷. 兩輪自平衡機(jī)器人的控制方法研究[J]. 哈爾濱理工大學(xué)學(xué)報(bào)，2014，19(6)：22-26.

[3] 林文建，鐘杭，黎福海,等. 兩輪自平衡機(jī)器人控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[J]. 電子測(cè)量與儀器學(xué)報(bào)，2013，27(8)：750-759.

[4] 李瑞，李曉明. 基于Mobile-Android小型移動(dòng)機(jī)器人平臺(tái)控制系統(tǒng)[J]. 機(jī)電工程，2013，30(11)：1414-1419.

[5] 盧秀和，李建波. 基于STM32的四旋翼姿態(tài)控制系統(tǒng)[J]. 計(jì)算機(jī)測(cè)量與控制，2014，22(3)：761-763.

[6] 楊莘，劉海濤. 基于STM32的兩輪自平衡遙控小車[J]. 數(shù)字技術(shù)與應(yīng)用，2014(5)：151-152.

[7] 萬(wàn)云霞，許倫豹，胡龍,等. 基于Arduino的四旋翼飛行器控制系統(tǒng)設(shè)計(jì)[J]. 吉林大學(xué)學(xué)報(bào)(信息科學(xué)版)，2015，33(4)：389-396.

[8] Liu Kun,Ming Bai,Yuhua Ni. Two-wheel self-balanced carbased on Kalman filtering and PID algorithm[C].Industrial Engineering and Engineering Management (IE&EM) 2011 IEEE 18thInternational Conference, 2011, (1) : 281-285.

[9] 張團(tuán)善，何穎. 卡爾曼濾波在兩輪自平衡代步車姿態(tài)檢測(cè)中的應(yīng)用[J]. 單片機(jī)與嵌入式系統(tǒng)應(yīng)用，2014(5)：33-35.

[10] 楊凌霄，李曉陽(yáng). 基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測(cè)方法[J]. 計(jì)算機(jī)仿真，2014，31(6)：406-409.

[11] 馬思遠(yuǎn)，魯庭勇，張麗君. 兩輪自平衡車運(yùn)動(dòng)姿態(tài)的測(cè)量和控制[J]. 測(cè)控技術(shù)，2015，34(4)：71-73.

[12] 梁光勝，杜夢(mèng)楠，周子豪,等. 基于互補(bǔ)濾波的兩輪自平衡車姿態(tài)控制[J]. 測(cè)控技術(shù)，2015，34(5)：72-74.

[13] 周牡丹，康愷，蔡普郎，等. 兩輪自平衡車控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J]. 自動(dòng)化技術(shù)與應(yīng)用，2014，33(10)：4-8.

[14] 龔成瑩. 一種基于藍(lán)牙的無(wú)線傳感網(wǎng)測(cè)控小車設(shè)計(jì)[J]. 無(wú)線電工程，2016，46(5)：72-74.

[15] 汪國(guó)磊. 基于Android的數(shù)據(jù)采集處理實(shí)驗(yàn)系統(tǒng)設(shè)計(jì)[J]. 數(shù)字通信，2014，41(5)：70-73.

(編輯 李秀敏)

The Research and Design of the Two-Wheel Robot Control System Based on Android

WANG Su-qing, HUANG Wen-long

(JinCheng College, Nanjing University of Aeronautics and Astronautics， Nanjing 211156, China)

The two-wheel robot system had the traditional pendulum similar multivariable, nonlinear, strong coupling and parameter instability and so on. For the complexity of the system, realization of wireless mobile robot control based on the Android platform and Bluetooth communication module is proposed in this paper. The self-balancing control of the two-wheel robot is achieved using a combination of control methods which are attitude data processing algorithm based on DMP and Kalman filter and double-loop PID control algorithm. The two arms are increased innovatively on the system, with which can quickly restore balance and maintain balance in order to achieve energy-saving purposes when the system is dormant. The test results show that the system can accurately send data and receive data via bluetooth. The system has high accuracy, good stability, and fast response time.

two-wheel robot; Android; bluetooth; DMP and Kalman filter; PID control

1001-2265(2016)12-0060-05

10.13462/j.cnki.mmtamt.2016.12.017

2016-08-08；

2016-09-18

南京航空航天大學(xué)金城學(xué)院大學(xué)生實(shí)踐創(chuàng)新訓(xùn)練計(jì)劃項(xiàng)目(2015048YP)

王素青(1976—)，女，江蘇鹽城人，南京航空航天大學(xué)金城學(xué)院講師，碩士，研究方向?yàn)榫W(wǎng)絡(luò)控制系統(tǒng)，(E-mail) wsq0214@nuaa.edu.cn。

TH166;TG659

A