李碩明　陳越　吳雄英

摘 要 基于32位ARM7微型處理器LPC2119開(kāi)發(fā)移動(dòng)機(jī)器人伺服控制系統(tǒng)，設(shè)計(jì)了適應(yīng)于雙軸獨(dú)立驅(qū)動(dòng)的移動(dòng)小車伺服控制PI算法，采用PWM脈寬調(diào)速技術(shù)結(jié)合光電編碼反饋來(lái)構(gòu)成閉環(huán)電機(jī)伺服系統(tǒng).系統(tǒng)方案在自主設(shè)計(jì)的室內(nèi)移動(dòng)機(jī)器人小車XMU3上進(jìn)行了驗(yàn)證，實(shí)驗(yàn)結(jié)果能達(dá)到室內(nèi)服務(wù)機(jī)器人平臺(tái)應(yīng)用的控制精度要求.

關(guān)鍵詞 雙軸輪；伺服控制系統(tǒng)；ARM7；PI算法

中圖分類號(hào) TP393文獻(xiàn)標(biāo)識(shí)碼 A文章編號(hào) 10002537（2016）02005906

A ServoControl System for Biaxial Wheel Indoor Service Robots

LI Shuoming1， CHEN Yue 1，2， WU Xiongying3*

（1.School of Information Engineering， Zhongshan Polytechnic， Zhongshan 528404， China；

2.School of Information and Electronics， South China University of Technology， Guangzhou 510641， China；

3.Shenzhen Branch of Emerson Network Power Company Ltd.， Guangzhou 518057， China）

Abstract This paper designs a servocontrol system for mobile robots based on 32bit ARM7 microprocesser. The approach designs PI algorithm which suits biaxial wheel mobile robots servocontrol system， adopting PWM speed control and utilizing lightelectronic devices for feedbacking components to set up a closed motor servocontrol system. The system is tested on the selfdesigned XMU3 indoor intelligent mobile robots vehicle， which proves to be able to match the control accuracy requirements and meet the potential application platform needs of indoor service robot.

Key words biaxial wheel； servocontrol system； ARM7； PIalgorithm

一直以來(lái)， 移動(dòng)機(jī)器人以其廣泛的應(yīng)用性和高技術(shù)性而備受關(guān)注[1].近年來(lái)移動(dòng)機(jī)器人技術(shù)已經(jīng)成為計(jì)算機(jī)、自動(dòng)化以及人工智能等交叉領(lǐng)域的一個(gè)研究熱點(diǎn).輪式移動(dòng)機(jī)器人具備相對(duì)簡(jiǎn)單的機(jī)械結(jié)構(gòu)，靈活的運(yùn)動(dòng)，便捷的操作，較高的效率[2].不僅在工業(yè)自動(dòng)化、智能交通、遠(yuǎn)程醫(yī)療等行業(yè)中得到廣泛的應(yīng)用，在智能家居、城市安全、空間探測(cè)領(lǐng)域等有害與危險(xiǎn)場(chǎng)合得到很好的應(yīng)用[34].伺服系統(tǒng)位于移動(dòng)機(jī)器人控制系統(tǒng)的底層，直接負(fù)責(zé)驅(qū)動(dòng)電機(jī)、采集傳感器信息和人機(jī)交互[5]，是整個(gè)輪式移動(dòng)機(jī)器人研發(fā)的核心部分之一，在理論研究和工程實(shí)踐方面都有重要意義[67].

由服務(wù)機(jī)器人代替人力完成家務(wù)勞動(dòng)，將是一項(xiàng)具有良好應(yīng)用前景的高技術(shù)，特別是在21世紀(jì)中國(guó)即將步入老齡化社會(huì)，服務(wù)機(jī)器人市場(chǎng)前景廣闊[8].本課題研究的室內(nèi)服務(wù)自主式機(jī)器人的基礎(chǔ)應(yīng)用平臺(tái)已經(jīng)開(kāi)發(fā)完成，本文的內(nèi)容對(duì)應(yīng)其雙軸輪伺服控制系統(tǒng)的設(shè)計(jì).

1 XMU3移動(dòng)小車方案

室內(nèi)自主移動(dòng)機(jī)器人小車XMU3采用“1+2”三輪式底盤(pán)設(shè)計(jì)，其底盤(pán)形狀如圖1所示.前輪為可以自由活動(dòng)的萬(wàn)向輪，兩個(gè)后輪為小車提供驅(qū)動(dòng)力由兩臺(tái)直流電機(jī)分別控制.直流電機(jī)與其對(duì)應(yīng)的驅(qū)動(dòng)輪采用速度或位置反饋閉環(huán)控制， 可以實(shí)現(xiàn)對(duì)小車速度和方向的控制[9].通過(guò)編程設(shè)置兩個(gè)伺服電機(jī)的電流供給可以控制驅(qū)動(dòng)輪之間的相對(duì)轉(zhuǎn)速，進(jìn)而實(shí)現(xiàn)小車的運(yùn)動(dòng)控制.當(dāng)兩個(gè)電機(jī)以同轉(zhuǎn)速同方向運(yùn)動(dòng)時(shí)能實(shí)現(xiàn)小車的直線運(yùn)動(dòng)，當(dāng)方向相反轉(zhuǎn)動(dòng)時(shí)能使車體繞兩輪軸線中點(diǎn)自轉(zhuǎn).底盤(pán)設(shè)計(jì)上參照Scott Jantz的研究結(jié)果[10]：圓形平面能避免棱角產(chǎn)生的碰撞，對(duì)于移動(dòng)機(jī)器人平臺(tái)來(lái)說(shuō)基座設(shè)計(jì)成圓形便于鎖定目標(biāo).

1.1 小車移動(dòng)能力模型

機(jī)器人小車在運(yùn)行時(shí)要同時(shí)克服摩擦力和重力，其受力分析如圖2所示.計(jì)算這兩個(gè)力的合力，可以得到式（1）.

Fapp=+Fw， （1）

式中，F(xiàn)app為機(jī)器人移動(dòng)需要的動(dòng)力，表示啟動(dòng)時(shí)車輪所處的滑動(dòng)摩擦力， 為機(jī)器人移動(dòng)受到的來(lái)自重力的阻力.機(jī)器人在水平面上移動(dòng)時(shí)Fw為0.當(dāng)車輪開(kāi)始轉(zhuǎn)動(dòng)，此時(shí)的滾動(dòng)摩擦力比啟動(dòng)前的摩擦阻力要小，因此在絕大多數(shù)時(shí)間電機(jī)都是處于過(guò)功率的狀態(tài).驅(qū)動(dòng)系統(tǒng)一定程度上受到驅(qū)動(dòng)輪的直徑大小的影響，即當(dāng)驅(qū)動(dòng)輪的直徑增大時(shí)，輸出速度對(duì)應(yīng)增大，驅(qū)動(dòng)力減小，反之亦然.XMU3小車主要擬用于室內(nèi)地面移動(dòng)，所以，輸出的轉(zhuǎn)矩除以驅(qū)動(dòng)輪的半徑，便可以得到驅(qū)動(dòng)輪的驅(qū)動(dòng)力.

1.2 直流電機(jī)選型分析

擬定小車總質(zhì)量為m=8 kg，啟動(dòng)時(shí)為靜摩擦，摩擦系數(shù)取μ=0.4；行進(jìn)中為滾動(dòng)摩擦，滾動(dòng)摩阻系數(shù)取μ=0.01， 已知驅(qū)動(dòng)輪外徑D=0.074 m，則r=0.037 m；并假設(shè)額定速度v=0.4 m/s.

直流伺服電機(jī)能實(shí)現(xiàn)大范圍無(wú)級(jí)調(diào)速，同時(shí)能方便的實(shí)現(xiàn)直流調(diào)速系統(tǒng)地精確控制策略，滿足室內(nèi)應(yīng)用場(chǎng)景對(duì)調(diào)速性能的要求 [2，11].綜合考慮到成本因素，本課題以直流電機(jī)為對(duì)象設(shè)計(jì)伺服系統(tǒng).據(jù)受力分析計(jì)算，直流電機(jī)TG3812620060K 額定轉(zhuǎn)速≥91.3（r/min），可以滿足設(shè)計(jì)要求.

2 電機(jī)伺服控制系統(tǒng)設(shè)計(jì)及實(shí)現(xiàn)

輪式移動(dòng)機(jī)器人伺服控制系統(tǒng)是集電子電路、嵌入式技術(shù)、探測(cè)控制技術(shù)于一體的綜合技術(shù)，其核心是對(duì)伺服電機(jī)轉(zhuǎn)動(dòng)的控制，通過(guò)程序控制為電機(jī)提供特定大小的電流來(lái)獲得電機(jī)特定的轉(zhuǎn)速，進(jìn)而對(duì)輪式移動(dòng)小車行進(jìn)速度和方位的控制.因此高精度的控制移動(dòng)機(jī)器人，必須采取精準(zhǔn)的控制策略和高性能的微處理器[12].

通常伺服系統(tǒng)中可以在驅(qū)動(dòng)器前選擇性地設(shè)置幾處調(diào)節(jié)電流和反饋環(huán)路，從外向內(nèi)依次為位置調(diào)節(jié)、速度調(diào)節(jié)、電流調(diào)節(jié)以及位置反饋、速度反饋和電流反饋，其構(gòu)成如圖3所示.位置反饋信號(hào)可以從電機(jī)軸或負(fù)載上獲取出.本課題為了提高系統(tǒng)的響應(yīng)性能，剔除了電流環(huán)和速度環(huán)，只采用位置反饋的單閉環(huán)結(jié)構(gòu)，形成一種基于位置反饋的隨動(dòng)伺服系統(tǒng).

2.1 伺服控制系統(tǒng)設(shè)計(jì)

一個(gè)伺服系統(tǒng)通常由伺服電機(jī)、執(zhí)行器和微處理控制器構(gòu)成.本課題設(shè)計(jì)的數(shù)字化電機(jī)伺服系統(tǒng)以中央微處理器為控制核心部件，以PWM功放電路為驅(qū)動(dòng)模塊，設(shè)計(jì)選用直流伺服電機(jī)作為系統(tǒng)控制對(duì)象，選用位置反饋進(jìn)行調(diào)速，合理選擇主控芯片、測(cè)速方式并對(duì)電路進(jìn)行設(shè)計(jì)，以光電編碼元件作為反饋來(lái)構(gòu)成閉環(huán)電機(jī)伺服系統(tǒng)，系統(tǒng)結(jié)構(gòu)圖如圖4所示.

2.2 系統(tǒng)工作原理

系統(tǒng)給定一個(gè)模擬位置量作為伺服系統(tǒng)的輸入量，經(jīng)過(guò)包括位置調(diào)節(jié)子電路、速度調(diào)節(jié)子電路、電流調(diào)節(jié)子電路等一系列的硬件電路后輸出給功率放大器，驅(qū)動(dòng)電機(jī)運(yùn)動(dòng).由ARM7微型處理器（LPC2119）的PWM模塊產(chǎn)生單邊沿控制信號(hào)PWM5&6，與雙通道直流電機(jī)驅(qū)動(dòng)器L298N的第6引腳、第11引腳相連，WM5、P4、P5引腳控制左輪電機(jī)，PWM6、P12、P13引腳控制右輪電機(jī)，通過(guò)設(shè)置寄存器PWMMR5&6的值可以調(diào)整PWM信號(hào)的占空比，從而改變電機(jī)的轉(zhuǎn)速，即改變機(jī)器人的移動(dòng)速度.

2.3 PWM調(diào)速信號(hào)的產(chǎn)生

PWM（PulseWidth Modulation脈寬調(diào)制）技術(shù)是直流電機(jī)調(diào)速中最為有效的方法，具有功耗低、效率高、穩(wěn)定可靠等優(yōu)點(diǎn).本課題采用單極性控制，在反復(fù)對(duì)比10～20 kHz頻率段內(nèi)系統(tǒng)運(yùn)行時(shí)的噪聲后，取11 kHz為脈沖切換頻率，系統(tǒng)電路如圖5.

2.4 基于光電編碼器的反饋閉環(huán)控制

本課題利用自制的光電編碼器實(shí)現(xiàn)反饋閉環(huán)控制，采用數(shù)字控制算法完成電機(jī)的閉環(huán)控制.在車輪的一側(cè)表面上粘貼一個(gè)自制的碼盤(pán)，該碼盤(pán)將直徑為70 mm的圓平分為64份（碼盤(pán)的直徑稍小于車輪的直徑），并成黑白相間條紋.然后將可辨別黑白色的反射式光電開(kāi)關(guān)安裝在碼盤(pán)的正對(duì)位置.為了減少可見(jiàn)光對(duì)傳感器的影響，我們將傳感器置于電機(jī)正下方.當(dāng)碼盤(pán)上的黑白條紋交替對(duì)準(zhǔn)光電傳感器時(shí)，將產(chǎn)生開(kāi)關(guān)的關(guān)斷，得到一個(gè)交替變化的開(kāi)關(guān)量.然后經(jīng)過(guò)如圖6所示的一個(gè)轉(zhuǎn)換電路，將開(kāi)關(guān)量變?yōu)橄鄳?yīng)的脈沖量輸入到微處理器的CAP1.2和CAP1.3引腳.

本課題所采用的光電傳感器為直流三線式，工作電壓在10～30 VDC范圍內(nèi)，檢測(cè)距離可調(diào).根據(jù)傳感器的內(nèi)部結(jié)構(gòu)和引出線定義，選擇工作電壓12 VDC，與直流電機(jī)電壓一致可以避免增加新的電源模塊.將光電傳感器簡(jiǎn)化為光電開(kāi)關(guān)S1、S2，考慮到隔離干擾信號(hào)，采用TLP5122光電轉(zhuǎn)換芯片進(jìn)行信號(hào)轉(zhuǎn)換.其管腳定義及內(nèi)部結(jié)構(gòu)如圖5所示，有2個(gè)光電轉(zhuǎn)換通道，左輪開(kāi)關(guān)信號(hào)連接1、2引腳，右輪開(kāi)關(guān)信號(hào)連接3、4引腳，當(dāng)S1/S2閉合時(shí)1、2之間/3、4之間的發(fā)光二極管導(dǎo)通，使得7、8間/6、5間的光電三極管導(dǎo)通，這樣圖中的右邊形成回路，根據(jù)ARM7微處理器的DC操作條件，其輸入高電平范圍2.0～3.3 V，通過(guò)一個(gè)300 Ω和一個(gè)220 Ω的串連電阻分壓，在兩個(gè)電阻中間取出需要的電平信號(hào)，此時(shí)為高電平，且為2.73 V滿足電平要求.同理，當(dāng)S1/S2斷開(kāi)時(shí)，1、2之間/3、4之間的發(fā)光二極管不導(dǎo)通，使得7、8間/6、5間的光電三極管不導(dǎo)通，這時(shí)在兩個(gè)電阻中間取出的電平信號(hào)為低電平.將這兩個(gè)電平信號(hào)通過(guò)P0.17、P0.18管腳輸入ARM7微處理器，通過(guò)設(shè)置寄存器PINSEL0將P0.17、P0.18管腳定義為CAP1.2、CAP1.3，從而可以利用定時(shí)器的捕獲功能.再設(shè)置捕獲控制寄存器T1CCR為CAP1.2、CAP1.3上升沿、下降沿脈沖捕獲，產(chǎn)生事件中斷，并通過(guò)數(shù)字控制算法實(shí)現(xiàn)對(duì)系統(tǒng)的閉環(huán)控制.其中產(chǎn)生的事件中斷用以完成對(duì)應(yīng)任務(wù).

3 基于PI算法在雙軸驅(qū)動(dòng)電機(jī)

在數(shù)字控制系統(tǒng)上實(shí)現(xiàn)PI控制規(guī)律的編程，其理論分析如下.取T0為采樣周期可以用矩形面積求和的方法近似積分作用，即

式（4）中，Kp為比例系數(shù)，Ki為積分系數(shù).u（k）即控制回路的輸出量，也是控制對(duì)象的輸入量，通常簡(jiǎn)寫(xiě)為CV.對(duì)于機(jī)器人電機(jī)的控制回路來(lái)說(shuō)，CV是決定PWM信號(hào)占空比的一個(gè)不斷變化的值.將得到的CV作為設(shè)定PWM信號(hào)占空比的參數(shù)，完成對(duì)電機(jī)的閉環(huán)控制過(guò)程.

3.1 PI控制算法實(shí)現(xiàn)

在同軸輪式移動(dòng)機(jī)器人運(yùn)動(dòng)控制中，要求同時(shí)對(duì)兩個(gè)后輪獨(dú)立驅(qū)動(dòng)的電機(jī)提供有效的伺服控制.其典型的控制包括如下兩種：當(dāng)左右兩輪電機(jī)同向同速運(yùn)動(dòng)時(shí)使得機(jī)器人能夠按照設(shè)計(jì)的路徑行走；設(shè)置兩輪之間特定的轉(zhuǎn)速差時(shí)可以實(shí)現(xiàn)小車的轉(zhuǎn)向控制.偽代碼如下：

Loop：PVLT=ReadLeftSpeed （ ） //讀左輪電機(jī)速度反饋值；

PVRT=ReadRightSpeed （ ） //讀右輪電機(jī)速度反饋值；

Isum=Isum + （PVLT -PVRT ） //積分項(xiàng)；

LeftErr=SP-PVLT//左輪電機(jī)偏差值；

RightErr=SP-PVRT //右輪電機(jī)偏差值；

CVLeft=Kp* LeftErr-Ki* Isum //左輪電機(jī)控制器輸出變量；

CVRight=Kp* RightErr+Ki* Isum//右輪電機(jī)控制器輸出變量；

SetRightPWM（CVRight） //根據(jù)CV值設(shè)置PWM信號(hào)占空比；

SetLeftPWM（CVLeft） //根據(jù)CV值設(shè)置PWM信號(hào)占空比；

Goto Loop

3.2 機(jī)器人小車運(yùn)動(dòng)狀態(tài)控制

根據(jù)L298N控制芯片引腳使能邏輯關(guān)系，通過(guò)設(shè)置控制芯片的方向控制信號(hào)引腳P0.4、P0.5和P0.12、P0.13實(shí)現(xiàn)對(duì)機(jī)器人小車運(yùn)動(dòng)狀態(tài)的控制，其控制規(guī)律如表1所示，幾種運(yùn)動(dòng)狀態(tài)的自由組合可以實(shí)現(xiàn)小車原地零半徑轉(zhuǎn)彎，增加轉(zhuǎn)向精度和反映速度.

3.3 左右輪電機(jī)最高轉(zhuǎn)速分析

根據(jù)CVLeft值分別設(shè)置左右輪電機(jī)PWM占空比，實(shí)現(xiàn)PWM調(diào)速閉環(huán)控制，其中80.91 r/min和7766 r/min為占空比100%即最高電壓下的左右輪電機(jī)轉(zhuǎn)速.根據(jù)實(shí)驗(yàn)數(shù)據(jù)獲得最高電壓下左右輪轉(zhuǎn)速，取其數(shù)學(xué)平均值作為最高電壓轉(zhuǎn)速參數(shù)，數(shù)據(jù)如表2所示，以便在精確控制時(shí)補(bǔ)償左右輪電機(jī)本身參數(shù)和安裝精度等的誤差.

實(shí)驗(yàn)表明該XMU3平臺(tái)可以實(shí)現(xiàn)對(duì)小車的精確伺服控制，能對(duì)小車伺服控制和零半徑自轉(zhuǎn)，平面旋轉(zhuǎn)精度達(dá)到±5度，最高行進(jìn)速度達(dá)到77.66 r/min，約合每分鐘行進(jìn)240 m，并可實(shí)現(xiàn)和ARM節(jié)點(diǎn)和遠(yuǎn)程PC節(jié)點(diǎn)的實(shí)時(shí)CAN通信.

4 結(jié)論

本文設(shè)計(jì)了一項(xiàng)自主移動(dòng)機(jī)器人平臺(tái)的伺服控制方案，建立了一個(gè)差分驅(qū)動(dòng)雙軸輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型，并實(shí)現(xiàn)了零半徑自轉(zhuǎn)及平面內(nèi)的全向轉(zhuǎn)動(dòng).實(shí)驗(yàn)結(jié)果表明雙軸輪旋轉(zhuǎn)控制方案能保證電機(jī)轉(zhuǎn)動(dòng)的準(zhǔn)確性和穩(wěn)定性，具有較強(qiáng)的魯棒性，可作為室內(nèi)服務(wù)機(jī)器人平臺(tái)進(jìn)行深度開(kāi)發(fā).

參考文獻(xiàn)：

[1] 葉長(zhǎng)龍，馬書(shū)根，回 麗，等. 一種全方位移動(dòng)機(jī)器人[J].中國(guó)科學(xué)，2011，41（2）：181189.

[2] 楊 敏. 輪式移動(dòng)機(jī)器人控制算法研究及其伺服系統(tǒng)設(shè)計(jì)[D].南京： 南京航空航天大學(xué)，2014.

[3] 王珊珊. 輪式移動(dòng)機(jī)器人控制系統(tǒng)設(shè)計(jì)[D].南京：南京理工大學(xué)，2013.

[4] 劉 磊，向 平，王永驥，等. 非完整約束下的輪式移動(dòng)機(jī)器人軌跡跟蹤[J]. 清華大學(xué)學(xué)報(bào)：自然科學(xué)版，2007，S2（2）：18841889.

[5] 宋廣鋼，李文鋒，宋 威，等. 基于 ARM9 微處理器的移動(dòng)機(jī)器人控制系統(tǒng)設(shè)計(jì)[J].微計(jì)算機(jī)信息， 2010，26（2）：146149.

[6] KOEI M. Trajectory tracking control of mobile robots without using longitudinal velocity measurements [J].Art Life Robot， 2009，14（14）：352356.

[7] CHANG W C. Visual navigation and control of mobile robots based on environment mapping[C]//International Conference on Advanced Mechatronic Systems （ICAMechS ）， Kumamoto， Japan， 2014：469474.

[8] MIYAZAKI T. Formation control of mobile robots with obstacle avoidance[C]//The 14th International Conference on Control， Automation & System （ICCAS）， Seoul， Korea， 2014：121126.

[9] 宋廣鋼，盧博友，雷永鋒，等.基于 ARM 的智能機(jī)器人小車控制系統(tǒng)設(shè)計(jì)[J].微計(jì)算機(jī)信息，2008，24（2）：239241.

[10] JANTZ S. Final report EEL 5666[R]. Machine intelligence laboratory， University of florida，2000，2532.

[11] BREZAK M. Timeoptimal trajectory planning along predefined path for mobile robots with velocity and acceleration constraints[J]. IEEE/ASME International conference on advanced intelligent mechatronics， 2011， 30（1）：942947.

[12] ZAMAN S. ROSbased diagnostic board for detecting and repairing hardware faults in autonomous mobile robots[J]. International conference on robotics & emerging allied technologies in engineering， 2014，32（1）：95102.

（編輯 CXM）