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