??? 摘 要： Robocup中型組的足球機(jī)器人" title="足球機(jī)器人">足球機(jī)器人控制系統(tǒng)" title="控制系統(tǒng)">控制系統(tǒng)設(shè)計,。描述了以S3C44BOX為主控芯片,，SM5004為驅(qū)動芯片的底層運(yùn)動控制" title="運(yùn)動控制">運(yùn)動控制模塊的硬件結(jié)構(gòu),，通過分析四輪全向機(jī)器人的運(yùn)動學(xué)模型，使用模糊自適應(yīng)PID控制算法完成對機(jī)器人的閉環(huán)控制,。實(shí)踐證明,，以這種控制器作為ROBOCUP中型組機(jī)器人的底層運(yùn)動控制系統(tǒng)" title="運(yùn)動控制系統(tǒng)">運(yùn)動控制系統(tǒng)，其速度,、位置及控制精度等指標(biāo)均達(dá)到了設(shè)計要求,。
??? 關(guān)鍵詞： Robocup中型組? 運(yùn)動控制? ARM? 四輪全向機(jī)器人? PID
?
1 足球機(jī)器人簡述
??? 機(jī)器人足球是人工智能領(lǐng)域與機(jī)器人領(lǐng)域的基礎(chǔ)研究課題，是一個極富挑戰(zhàn)性的高技術(shù)密集型項(xiàng)目,。機(jī)器人足球比賽的設(shè)想首先是由加拿大不列顛哥倫比亞大學(xué)的教授Alan Mackworth在1992年的論文《On Seeing Robots》中提出的,。如今，國際機(jī)器人足球聯(lián)合會（FIRA）和機(jī)器人世界杯賽聯(lián)合會（Robocup）已組織了各種形式的機(jī)器人足球競賽,，有力地促進(jìn)了這一活動的發(fā)展,。在競賽足球機(jī)器人中主要有以下關(guān)鍵技術(shù)：圖像采集與處理、路徑規(guī)劃,、決策,、運(yùn)動控制、多傳感器信息融合,、多智能體協(xié)作等,。
??? 機(jī)器人足球由四部分組成：機(jī)器人小車系統(tǒng)、視覺系統(tǒng)、通信系統(tǒng)和計算機(jī)決策系統(tǒng),。機(jī)器人小車作為比賽的具體執(zhí)行者,，其性能優(yōu)劣直接影響到策略水平的發(fā)揮。足球機(jī)器人底層控制系統(tǒng)的研究與開發(fā)是新型足球機(jī)器人小車設(shè)計任務(wù)中的重要組成部分,。小車的性能主要由車體性能與車載嵌入式控制系統(tǒng)——足球機(jī)器人底層控制系統(tǒng)決定,。當(dāng)小車車體具有良好的運(yùn)動性能后，小車的性能就由車載嵌入式系統(tǒng)決定,。因此構(gòu)建一個快速,、安全、可靠的實(shí)時嵌入式系統(tǒng)是整個系統(tǒng)的關(guān)鍵,。
2 東風(fēng)—Ⅱ型機(jī)器人
??? 東風(fēng)—Ⅱ型機(jī)器人是由第二炮兵工程學(xué)院獨(dú)立設(shè)計完成的Robocup中型組足球機(jī)器人,，其視覺子系統(tǒng)采用了基于全景視覺的圖像采集與處理，利用圖像信息,、慣性儀表與超聲波傳感器的信息融合為機(jī)器人自定位,，上位機(jī)利用機(jī)器人的自定位以及球場上本方、對方和球的位置信息做出路徑規(guī)劃,，并對下位機(jī)發(fā)出指令,。下位機(jī)根據(jù)接收到的指令把機(jī)器人的速度分解到四個輪子上，并由運(yùn)動控制芯片完成反饋控制,，其結(jié)構(gòu)如圖1所示,。
?
?
3 機(jī)器人的運(yùn)動控制系統(tǒng)
??? 本機(jī)器人的硬件系統(tǒng)由PC機(jī)和32位嵌入式微處理器、4軸運(yùn)動控制芯片,、驅(qū)動芯片,、存儲芯片以及接口芯片等組成。其中PC機(jī)作為上位機(jī),，主要完成圖像處理,、控制決策等任務(wù)；而機(jī)器人小車的運(yùn)動控制主要由嵌入式系統(tǒng)和運(yùn)動控制卡完成,。系統(tǒng)框圖如圖2,。
?
?
3.1 SM5004的主要功能
??? SM5004為深圳斯邁迪公司研制的DSP運(yùn)動控制專用芯片，內(nèi)嵌強(qiáng)大的軟件插補(bǔ)和監(jiān)控模塊,，能同時控制四個脈沖型伺服電機(jī)驅(qū)動器或步進(jìn)電機(jī)驅(qū)動器,，SM5004還有伺服電機(jī)反饋輸入（編碼器信號、在位信號和報警信號）,、加減速驅(qū)動,、軟硬件限位、自動原點(diǎn)搜索,、同步動作,、輸入信號濾波等功能。
??? 在本系統(tǒng)中，SM5004完成的主要功能如下：
??? (1)接收CPU的命令和數(shù)據(jù),，并進(jìn)行相應(yīng)的驅(qū)動操作,。SM5004根據(jù)接收到的命令，輸出相應(yīng)的脈沖串,，控制四個伺服電機(jī)并分別進(jìn)行定速控制,。
??? (2)反饋控制。SM5004每個軸都有2個32位位置計數(shù)器,。一個是邏輯位置計數(shù)器,，用于計算實(shí)際輸出的脈沖數(shù)，以此記錄驅(qū)動所處的邏輯位置,；另一個是實(shí)際位置計數(shù)器,，用于計算從外部編碼器來的脈沖，以此記錄驅(qū)動實(shí)際位置,。每軸還有兩個32位比較寄存器,，用于比較邏輯位置計數(shù)器和實(shí)際位置計數(shù)器，并根據(jù)差值關(guān)系實(shí)現(xiàn)軟件限位,，從而達(dá)到閉環(huán)控制,。
3.2 ARM微控器S3C44BOX
??? S3C44BOX是由SAMSUNG公司推出的16/32位RISC處理器，是一款高性價比和高性能的微控制器,。S3C44BOX使用ARM7TDMI內(nèi)核,，采用0.25μm CMOS工藝制造，它提供了豐富的片上外設(shè),，主要包括：8KB Cache和內(nèi)部SRAM，帶自動握手的2通道UART,，4通道DMA,，帶PWM功能的5通道定時器和一個內(nèi)部定時器，I/O端口,，通過片內(nèi)DLL倍頻器可實(shí)現(xiàn)最大66MHz的工作頻率,。S3C44BOX與SM5004的連接如圖3所示。
?
3.3 PWM控制信號輸出模塊
??? SM5004有四路PWM輸出,，可同時控制四個伺服電機(jī)進(jìn)行運(yùn)動,，但從芯片輸出的PWM信號不足以直接驅(qū)動電機(jī)轉(zhuǎn)動，需要使用驅(qū)動器將其轉(zhuǎn)換為可用的驅(qū)動信號,。驅(qū)動電路中H橋電路選取了允許大電流大電壓的功率場效應(yīng)管IRF4905和IRF3205,，并使功率場效應(yīng)管完全工作于飽和區(qū)，有效防止了因電路發(fā)熱而浪費(fèi)能源,，設(shè)計中也注意消除了共態(tài)導(dǎo)通問題,；輸入端采用光電隔離消除電路對控制端的影響。實(shí)驗(yàn)證明，該驅(qū)動電路設(shè)計完全滿足要求,，可應(yīng)用于中型機(jī)器人比賽,。驅(qū)動電路如圖4所示。
?
?
圖4 驅(qū)動電路
?
3.4 測速反饋單元
??? 為了提高反饋信號的精度,，對光電碼盤輸出的光電脈沖進(jìn)行四倍頻,，將四倍頻后的脈沖信號計數(shù)，在轉(zhuǎn)速較高的時刻可以滿足精度要求,，即使在轉(zhuǎn)速較低,、脈沖很少時，四倍頻后的測速脈沖依然可以獲得較高的分辨率,。該電路除了四倍頻信號外還具有電機(jī)鑒相功能,。當(dāng)周期電機(jī)轉(zhuǎn)速方向改變時，兩個脈沖周期Encoder A與Encoder B的相位發(fā)生變化,，在第二個觸發(fā)器的輸入與輸出端將出現(xiàn)不同的電平,，此時同或門會輸出低電平，使CLEAR置零,，16位計數(shù)器復(fù)位,，開始新的轉(zhuǎn)動方向轉(zhuǎn)速計數(shù)。邏輯原理圖如圖5所示,。
?
圖5 測速反饋單元邏輯原理
4 四輪全向機(jī)器人的運(yùn)動控制
??? 東風(fēng)—Ⅱ型機(jī)器人采用了四輪全向移動的運(yùn)動方式,，具有全向運(yùn)動能力的系統(tǒng)使機(jī)器人可以向任意方向做直線運(yùn)動，而之前不需要做旋轉(zhuǎn)運(yùn)動,，并且這種輪系可滿足一邊做直線運(yùn)動一邊旋轉(zhuǎn)的要求,，達(dá)到終狀態(tài)所需要的任意姿態(tài)角。全向輪系的應(yīng)用將使足球機(jī)器人具有運(yùn)動快速靈活,、控球穩(wěn)定,、進(jìn)攻性強(qiáng)及易于控制等優(yōu)點(diǎn)，使機(jī)器人在賽場上更具競爭力,。
4.1 全向輪
??? 本機(jī)器人采用的全向輪在大輪的周圍均勻分布著小輪子,，大輪由電機(jī)驅(qū)動，小輪可自由轉(zhuǎn)動,。這種全方位輪可有效避免普通輪子不能側(cè)滑所帶來的非完整性約束,，使機(jī)器人具有平面運(yùn)動的全部三個自由度，機(jī)動性增強(qiáng),?；谝陨戏治觯x擇使用這種全向輪,。
4.2 運(yùn)動學(xué)分析
??? 在建立機(jī)器人的運(yùn)動模型前,，先做以下假設(shè)：
??? (1)小車是在一個理想的平面上運(yùn)動,，地面的不規(guī)則可以忽略。
??? (2)小車是一個剛體,，形變可以忽略,。
??? (3)輪子和地面之間滿足純滾動的條件，沒有相對滑動,。
??? 全方位移動機(jī)器人由四個全向輪作為驅(qū)動輪,，且為間隔90°均勻分布，其簡化運(yùn)動學(xué)模型如圖6所示,。其中xw-yw為絕對坐標(biāo)系,；xm-ym為固連在機(jī)器人車體上的相對坐標(biāo)系，其坐標(biāo)原點(diǎn)與機(jī)器人中心重合,；θ為xw與xm的夾角,；δ為輪子與ym的夾角；L為機(jī)器人中心到輪子中心的距離,；vi為第i個輪子的沿驅(qū)動方向的速度,。
?
?
??? 可求出運(yùn)動學(xué)方程如下：
??? 
??? 因?yàn)檩喿訛閷ΨQ分布，常數(shù)δ為45°,，故得到全向移動機(jī)器人的運(yùn)動模型：
??? v=PS
??? 其中
為輪子的速度,，
為機(jī)器人整體期望速度。
??? 
P為轉(zhuǎn)換矩陣,。
??? 這樣,，就可以將機(jī)器人整體期望速度，解算為分別到四個輪子的速度,，把數(shù)據(jù)傳送到控制器中,，就可完成對機(jī)器人的控制。
4.3 控制算法
??? 傳統(tǒng)直流電機(jī)調(diào)速系統(tǒng)一般采用PID算法,，PID控制器設(shè)計完成后,，參數(shù)就固定不變，無法根據(jù)實(shí)際負(fù)載的變化而調(diào)整,。為了能夠提高系統(tǒng)的動態(tài)性能及軌跡精度，本文提出了一種模糊自適應(yīng)控制算法,，從而使速度響應(yīng)具有超調(diào)小,、響應(yīng)速度快、適應(yīng)性強(qiáng)及受對象參數(shù)變化影響小等性能,。系統(tǒng)取得了較好的動靜態(tài)性能,。
??? 模糊自適應(yīng)PID控制是模糊控制" title="模糊控制">模糊控制與傳統(tǒng)PID控制的結(jié)合, 它的設(shè)計思想是運(yùn)用模糊數(shù)學(xué)的基本理論和方法，把規(guī)則的條件,、操作用模糊集表示,，并把這些模糊控制規(guī)則以及有關(guān)信息(如評價指標(biāo),、初始PID參數(shù)等)作為知識存入知識庫中，然后根據(jù)控制系統(tǒng)的實(shí)際響應(yīng)情況,，運(yùn)用模糊推理,，即可自動實(shí)現(xiàn)對PID參數(shù)的最佳調(diào)整。這就是模糊自適應(yīng)PID控制,。
??? 自適應(yīng)模糊PID控制器以誤差e和誤差變化ec作為輸入,，找出PID三個參數(shù)與e和ec之間的模糊關(guān)系，在運(yùn)行中通過不斷檢測e和ec,，根據(jù)模糊控制規(guī)則對三個參數(shù)進(jìn)行在線修改,，以滿足不同e和ec時對控制參數(shù)的不同要求。其結(jié)構(gòu)如圖7所示,。
?
?
??? 模糊控制設(shè)計的核心是總結(jié)工程設(shè)計人員的技術(shù)知識和實(shí)際操作經(jīng)驗(yàn),，建立合適的模糊規(guī)則表，得到針對kp,、ki,、kd三個參數(shù)分別整定的模糊控制表。根據(jù)kp,、ki,、kd三個參數(shù)分別的作用，可制定模糊控制規(guī)則,。以kp為例,，列規(guī)則如表1，ki,、kd可類似推出,。
?
??? 其中，將e,、ec,、kp、ki,、kd分別定義了七個模糊子集,，{NB，NM,，NS,，O，PS,，PM,，PB}，子集中元素分別代表負(fù)大,、負(fù)中,、負(fù)小,、零、正小,、正中,、正大。設(shè)e,、ec和kp,、ki、kd均服從正態(tài)分布,，因此可得出各模糊子集的隸屬度,。根據(jù)各模糊子集的隸屬度賦值表和各參數(shù)模糊控制模型，應(yīng)用模糊合成推理設(shè)計PID參數(shù)的模糊矩陣表,，查出修正參數(shù)代入下式計算：
??? 
??? 在線運(yùn)行過程中,，控制系統(tǒng)通過對模糊邏輯規(guī)則的結(jié)果處理、查表和運(yùn)算,，完成對PID參數(shù)的在線自校正,。其工作流程圖如圖8所示。
?
??? 機(jī)器人運(yùn)動控制系統(tǒng)是整個Robocup機(jī)器人系統(tǒng)的執(zhí)行機(jī)構(gòu),，在場上的表現(xiàn)直接影響了整個足球機(jī)器人系統(tǒng),。機(jī)器人小車的性能優(yōu)劣對整個系統(tǒng)起著舉足輕重的作用。本文基于合理的理論假設(shè),，分析了機(jī)器人的運(yùn)動模型,，介紹了基于ARM+FPGA控制器的足球機(jī)器人運(yùn)動控制的實(shí)現(xiàn)。實(shí)驗(yàn)證明,，以這種控制器作為Robocup中型組機(jī)器人的底層運(yùn)動控制系統(tǒng),，速度、位置及控制精度等指標(biāo)均達(dá)到了設(shè)計要求,。與以DSP為主控芯片的控制系統(tǒng)相比,，減低了設(shè)計成本。這種機(jī)器人運(yùn)動控制的實(shí)現(xiàn)方法對全方位移動機(jī)器人有一定的普適性,。
參考文獻(xiàn)
[1] 李巖,，榮盤祥.基于S3C44BOX嵌入式μCLinux系統(tǒng)原理及應(yīng)用.北京：清華大學(xué)出版社，2005.
[2] 張翮,，熊蓉,，褚健，等.一種全方位移動機(jī)器人的運(yùn)動分析與控制實(shí)現(xiàn).浙江大學(xué)學(xué)報(工學(xué)版),，2004,，38(12).
[3] 劉金琨.先進(jìn)PID控制MATLAB仿真.北京：電子工業(yè)出版社,，2003.