中型組足球機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的研制

1、中型組足球機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的研制黃永賢，楊宜民（廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院，廣東 廣州 510090）摘要：ROBOCUP中型組機(jī)器人足球賽是多個(gè)全自主機(jī)器人之間合作與對(duì)抗的一項(xiàng)競(jìng)賽。全自主足球機(jī)器人要完成踢球任務(wù)，需要一個(gè)高可靠性、高運(yùn)動(dòng)性能的運(yùn)動(dòng)控制系統(tǒng)。本文提出并研制成功基于雙DSP的中型組足球機(jī)器人運(yùn)動(dòng)控制系統(tǒng)，經(jīng)過(guò)反復(fù)的實(shí)驗(yàn)和機(jī)器人之間的比賽實(shí)戰(zhàn)，證明該控制系統(tǒng)具有高可靠性、硬件電路簡(jiǎn)潔、運(yùn)動(dòng)快速靈活，高可控性等特點(diǎn)，完全可以滿足中型組足球機(jī)器人的運(yùn)動(dòng)要求。關(guān)鍵詞：RoboCup中型組；足球機(jī)器人；雙DSP架構(gòu)；控制系統(tǒng) Motion control system for auton

2、omous soccer robot Huang Yong-xian, Yang Yi-min(College of Automation, Guangdong University of Technology, Guangzhou 510090, China)Abstract：It needs a rapid and reliable motion control system for the autonomous soccer robot to play football game. This paper focuses on the research and the design of

3、motion control system for autonomous soccer robot. After reiterative experiments and competition against other robots, this motion control system has been proved to be highly reliable, controllable and powerful which meets the motion demand of soccer robot completely, and can be used in other simila

4、r control systemsKey words：RoboCup middle size league; soccer robot; DSPs structure; control system1 引言“機(jī)器人足球賽”是以足球?yàn)檩d體的高科技研究和對(duì)抗，它廣泛涉及人工智能、計(jì)算機(jī)視覺(jué)、自動(dòng)控制、精密儀器、傳感和信息等一系列學(xué)科的創(chuàng)新研究和集成，反映出一個(gè)國(guó)家在信息和自動(dòng)化技術(shù)方面的實(shí)力1?？v觀中型組足球機(jī)器人的發(fā)展歷史，機(jī)器人的機(jī)構(gòu)種類越來(lái)越多，機(jī)構(gòu)實(shí)現(xiàn)方式越來(lái)越復(fù)雜；電路系統(tǒng)使用的芯片越來(lái)越高檔，電路實(shí)現(xiàn)越來(lái)越復(fù)雜；機(jī)器人的運(yùn)動(dòng)性能、控球能力、進(jìn)攻能力逐年提高，因此設(shè)計(jì)出性能優(yōu)越的機(jī)器人是

5、各球隊(duì)最重要的目標(biāo)。2 中型組足球機(jī)器人運(yùn)動(dòng)控制系統(tǒng)中型組足球機(jī)器人運(yùn)動(dòng)控制系統(tǒng)體系結(jié)構(gòu)主要有：機(jī)器人運(yùn)動(dòng)機(jī)構(gòu)和電路控制系統(tǒng)。2.1機(jī)器人運(yùn)動(dòng)機(jī)構(gòu) 設(shè)計(jì)運(yùn)動(dòng)快速靈活的機(jī)器人必須考慮兩個(gè)重要因素：機(jī)器人運(yùn)動(dòng)機(jī)構(gòu)的性能和運(yùn)動(dòng)機(jī)構(gòu)的可控性。運(yùn)動(dòng)機(jī)構(gòu)性能決定了機(jī)器人運(yùn)動(dòng)性能的潛能；運(yùn)動(dòng)機(jī)構(gòu)的可控性決定了運(yùn)動(dòng)機(jī)構(gòu)性能能否被有效的利用起來(lái)。由于采用全向輪運(yùn)動(dòng)機(jī)構(gòu)的機(jī)器人可以向任意方向做直線運(yùn)動(dòng)的同時(shí)可做自身旋轉(zhuǎn)運(yùn)動(dòng)調(diào)整機(jī)器的姿態(tài)，這樣，就使得全向移動(dòng)機(jī)器人在比賽中表現(xiàn)出運(yùn)動(dòng)快速靈活、易于控制、進(jìn)攻性強(qiáng)等優(yōu)點(diǎn)。隨著RoboCup比賽的發(fā)展，機(jī)器人的靈活性也越來(lái)越凸現(xiàn)其在比賽中的重要性。因此，全向移動(dòng)機(jī)器人已

6、逐漸成為各個(gè)球隊(duì)設(shè)計(jì)發(fā)展的趨勢(shì)。全向移動(dòng)機(jī)器人的全向輪、運(yùn)動(dòng)機(jī)構(gòu)如圖1-1所示: 圖1-1 全向輪與運(yùn)動(dòng)機(jī)構(gòu)示意圖2.2 中型組足球機(jī)器人電路系統(tǒng)的設(shè)計(jì)基于全向輪的運(yùn)動(dòng)機(jī)構(gòu)給電路系統(tǒng)設(shè)計(jì)增加了難度：為了充分發(fā)揮全向輪結(jié)構(gòu)的優(yōu)勢(shì)，必須選用轉(zhuǎn)速快、輸出扭矩大的電機(jī)組合，因此電機(jī)驅(qū)動(dòng)電路的負(fù)擔(dān)加重了；主控板的硬件資源和軟件處理能力也必須提高以滿足全向移動(dòng)機(jī)器人控制的需要。機(jī)器人電路系統(tǒng)的設(shè)計(jì)也同樣以高性能和功能完善為主要目標(biāo)，提高運(yùn)動(dòng)系統(tǒng)的可控性和穩(wěn)定性。整個(gè)電路系統(tǒng)的主要組成部分和各部分之間的關(guān)系如圖1-2所示，分為以下幾部分：·主控核心。主控芯片采用兩片TI公司的DSP(TMS320L

7、F2407A)。·通信系統(tǒng)。包括上層計(jì)算機(jī)與控制系統(tǒng)的通信與主從DSP之間的通信。·電機(jī)驅(qū)動(dòng)電路。·電機(jī)碼盤接口電路。·電平轉(zhuǎn)換電路。·外圍設(shè)備檢測(cè)電路·電池電壓檢測(cè)電路·其他輔助電路與功能擴(kuò)展接口電路射門機(jī)構(gòu)控制電 路電機(jī)組合3電機(jī)組合1電機(jī)驅(qū)動(dòng)電路3碼盤信號(hào)轉(zhuǎn)換及電平轉(zhuǎn)換電機(jī)驅(qū)動(dòng)電路1碼盤信號(hào)轉(zhuǎn)換及電平轉(zhuǎn)換上層計(jì)算機(jī)電子指南針模塊，電池電壓檢測(cè)模塊RS232電平轉(zhuǎn)換RS232電平轉(zhuǎn)換PWM QEP1 從DSP SCIPWM QEP2PWM QEP1SCI 主DSPPWM QEP2 碼盤信號(hào)轉(zhuǎn)換及電平轉(zhuǎn)換電機(jī)驅(qū)動(dòng)電路4碼盤

8、信號(hào)轉(zhuǎn)換及電平轉(zhuǎn)換電機(jī)驅(qū)動(dòng)電路2電機(jī)組合4電機(jī)組合2圖1-2電路系統(tǒng)示意圖TI公司的2X系列DSP是專門為工業(yè)控制和機(jī)器人控制設(shè)計(jì)的集成芯片，其主要優(yōu)點(diǎn)在于資源豐富，運(yùn)算速度快。單個(gè)TMS320LF2407A芯片具有雙事件管理器EVA和EVB，每個(gè)事件管理器只有一個(gè)電機(jī)碼盤接口（QEP）單元，因此單個(gè)DSP只能接兩路的電機(jī)碼盤信號(hào)，而4個(gè)全向輪的中型組機(jī)器人有四個(gè)電機(jī)，另外兩個(gè)電機(jī)的碼盤信號(hào)可以接專用的集成芯片或者可編程邏輯芯片PLD將碼盤信號(hào)處理后送回DSP，但是這種方法會(huì)導(dǎo)致電路復(fù)雜，電路可靠性降低，而且單個(gè)DSP要處理四個(gè)電機(jī)的信號(hào)和其他外圍事件，使得單個(gè)DSP芯片負(fù)擔(dān)很重，實(shí)時(shí)性很難保

9、證。為了解決這個(gè)問(wèn)題，本文采用雙DSP架構(gòu)，其中一個(gè)DSP芯片為主處理器，另外一個(gè)DSP芯片為從處理器，主從DSP共同處理底層硬件信息和控制執(zhí)行機(jī)構(gòu)：主處理器負(fù)責(zé)與上層計(jì)算機(jī)通信，接收上層計(jì)算機(jī)決策庫(kù)輸出的四個(gè)電機(jī)速度值和執(zhí)行機(jī)構(gòu)（射門機(jī)構(gòu)等）一些控制命令，返回底層的信息（電機(jī)碼盤信號(hào)，指南針，電池電壓）給上層計(jì)算機(jī)，實(shí)時(shí)監(jiān)控機(jī)器人底層硬件狀態(tài)；主從DSP通過(guò)SPI通信協(xié)議進(jìn)行通信，主DSP將從上層計(jì)算機(jī)接收到3號(hào)電機(jī)和4號(hào)電機(jī)的速度值發(fā)送給從DSP，從DSP將3、4號(hào)電機(jī)的編碼盤，電池電壓，指南針等信息傳送給主DSP。由于控制系統(tǒng)采用雙DSP的架構(gòu)，控制系統(tǒng)的硬件精簡(jiǎn)，提高電路的可靠性和穩(wěn)定

10、性；雙DSP協(xié)同處理信息，處理速度快，實(shí)時(shí)性強(qiáng)，可控性強(qiáng)，能滿足中型組足球機(jī)器人的運(yùn)動(dòng)要求。3控制系統(tǒng)軟件設(shè)計(jì)控制系統(tǒng)有兩個(gè)主要任務(wù)：接收命令和執(zhí)行命令。如圖1-3 所示。此外，控制系統(tǒng)還必須完成異常狀態(tài)的預(yù)防與處理，使機(jī)器人出現(xiàn)異常時(shí)，迅速恢復(fù)到正常的狀態(tài)。信號(hào)檢測(cè)上層計(jì)算機(jī)決策 庫(kù)底層控制系統(tǒng)驅(qū)動(dòng)電路各執(zhí)行機(jī)構(gòu)圖1-3 運(yùn)動(dòng)控制系統(tǒng)框圖3.1指令的接收與處理為了能讓機(jī)器人及時(shí)、準(zhǔn)確的接收到上層決策發(fā)下來(lái)的指令信息，通信的快速、可靠性及穩(wěn)定性非常的重要。上層計(jì)算機(jī)通過(guò)USB轉(zhuǎn)串口232的方式與底層控制系統(tǒng)通信。指令幀數(shù)據(jù)的格式見(jiàn)表1。 表1 指令幀格式0 1 2 34 5幀頭射門力度1號(hào)電機(jī)

11、給定轉(zhuǎn)速值2號(hào)電機(jī)給定轉(zhuǎn)速值3號(hào)電機(jī)給定轉(zhuǎn)速值4號(hào)電機(jī)給定轉(zhuǎn)速值0xFF02500254025402540254指令幀由6字節(jié)數(shù)據(jù)組成，數(shù)據(jù)幀第一字節(jié)為握手信號(hào)。數(shù)據(jù)幀第二字節(jié)為射門力度值，該值由上層計(jì)算機(jī)的射門決策庫(kù)給定。底層控制系統(tǒng)依據(jù)給定的射門力度值和根據(jù)實(shí)驗(yàn)擬合射門曲線，將射門力度值轉(zhuǎn)化為射門機(jī)構(gòu)電磁鐵的工作時(shí)間（電容的放電時(shí)間）。數(shù)據(jù)幀第三字節(jié)到第六字節(jié)分別為四個(gè)電機(jī)的給定速度值。每當(dāng)DSP接收一完整的數(shù)據(jù)幀，通信接收程序?qū)⒃撝噶顜４嫫饋?lái)，并且將通信接收標(biāo)志ReceValid置1，由此通知主循環(huán)新指令接收完畢。程序流程圖如圖1-4所示:系統(tǒng)初始化循環(huán)檢測(cè)檢測(cè)ReceValid標(biāo)志定

12、時(shí)（2ms）中斷處理子程序通信接收中斷子程序ReceVaild1？ N Y命令分類設(shè)置為SPI通信主機(jī)，將3、4號(hào)電機(jī)給定速度值發(fā)送給從DSP射門機(jī)構(gòu)挑球電機(jī)調(diào)速根據(jù)實(shí)驗(yàn)擬合射門曲線，將射門力度值轉(zhuǎn)化為電容的放電時(shí)間讀取電機(jī)編碼盤信息，數(shù)字PID閉環(huán)調(diào)速發(fā)送完畢？ N Y主DSP設(shè)置為SPI通信從機(jī)，等待從DSP發(fā)送電池電壓、指南針模塊等數(shù)據(jù)圖1-4 主DSP程序流程圖 3.2異常預(yù)防與處理 控制系統(tǒng)異常主要有兩方面：電路系統(tǒng)異常和執(zhí)行機(jī)構(gòu)異常。由于機(jī)器人驅(qū)動(dòng)電機(jī)、電磁鐵工作時(shí)會(huì)產(chǎn)生強(qiáng)的電磁干擾，而且電機(jī)驅(qū)動(dòng)電路、射門機(jī)構(gòu)升壓電路工作在開(kāi)關(guān)狀態(tài)，給機(jī)器人供電電源造成很大污染。DSP主控板工作在

13、這樣惡劣的環(huán)境中，可能會(huì)導(dǎo)致DSP程序的跑飛，出現(xiàn)異常。為了是機(jī)器人控制系統(tǒng)穩(wěn)定可靠工作，必須要有異常預(yù)防與處理的機(jī)制。異常預(yù)防與處理方法： ·隔離：DSP的輸入輸出端口經(jīng)過(guò)光耦隔離與電機(jī)驅(qū)動(dòng)電路、射門機(jī)構(gòu)電路連接；強(qiáng)電磁干擾的機(jī)構(gòu)(如電磁鐵)采用磁屏蔽的方法與電路系統(tǒng)隔離。 ·電源處理：機(jī)器人采用一組24伏電池供電。每路的電機(jī)驅(qū)動(dòng)電路和射門機(jī)構(gòu)升壓電路都有獨(dú)立的保護(hù)電路和濾波電路，將各個(gè)電路引起的不良影響減到最小，DSP主控板經(jīng)過(guò)精密，可靠的濾波電路和穩(wěn)壓電路供電。 ·DSP主控板有外圍硬件微處理器監(jiān)控電路，一旦出現(xiàn)異常后，200毫秒內(nèi)使DSP重新復(fù)位，保證控制

14、系統(tǒng)在短時(shí)間內(nèi)恢復(fù)正常運(yùn)行。 ·機(jī)器人電機(jī)長(zhǎng)時(shí)間堵轉(zhuǎn)會(huì)損壞電機(jī)，因此，底層控制程序?qū)Χ罗D(zhuǎn)電機(jī)進(jìn)行監(jiān)測(cè)，一旦發(fā)生電機(jī)堵轉(zhuǎn)，程序就進(jìn)入異常處理模式。電機(jī)堵轉(zhuǎn)檢測(cè)方法：1）電機(jī)輸出電壓高（18伏），但是電機(jī)的轉(zhuǎn)速低并且持續(xù)一段時(shí)間（250毫秒），則該電機(jī)處于堵轉(zhuǎn)狀態(tài)；2）通過(guò)實(shí)時(shí)計(jì)算電機(jī)電流來(lái)檢測(cè)電機(jī)的工作狀態(tài)判斷是否正常。本文采用第一種檢測(cè)方法。電機(jī)堵轉(zhuǎn)處理方法：電機(jī)處于堵轉(zhuǎn)時(shí)，程序進(jìn)入異常處理模式，使電機(jī)工作在鎖住的狀態(tài)，整個(gè)機(jī)器人表現(xiàn)為原地不動(dòng)。程序進(jìn)入異常處理模式后開(kāi)始計(jì)時(shí)，經(jīng)過(guò)2秒后底層控制程序切換到正常狀態(tài)，檢測(cè)電機(jī)工作是否正常，如此反復(fù)，直到機(jī)器人輪子停止堵轉(zhuǎn)。4實(shí)驗(yàn)結(jié)果及分

15、析 圖1-5記錄了機(jī)器人以1m/s的速度直線運(yùn)動(dòng)時(shí)各輪子的速度響應(yīng)曲線，其中正負(fù)號(hào)分別表示輪子轉(zhuǎn)向是順時(shí)針和逆時(shí)針。 左后輪 右前輪 左前輪 右后輪 圖1-5電機(jī)響應(yīng)曲線圖由圖可知，各輪子的響應(yīng)時(shí)間小于300ms，轉(zhuǎn)速平穩(wěn)，機(jī)器人響應(yīng)時(shí)間短，運(yùn)動(dòng)快速靈活。5 結(jié)語(yǔ)中型組足球機(jī)器人控制系統(tǒng)采用雙DSP架構(gòu)，克服了DSPCPLD、DSP+集成芯片方法造成電路復(fù)雜，可靠性低，實(shí)時(shí)性不強(qiáng)等缺點(diǎn)。經(jīng)過(guò)反復(fù)的實(shí)驗(yàn)和機(jī)器人之間的比賽實(shí)戰(zhàn)，證明該控制系統(tǒng)具有高可靠性、硬件電路簡(jiǎn)潔、運(yùn)動(dòng)快速靈活，高可控性等特點(diǎn)，完全可以滿足中型組足球機(jī)器人的運(yùn)動(dòng)要求。此外，該控制系統(tǒng)適當(dāng)改進(jìn)就可以應(yīng)該其他小型的嵌入式控制裝置中

16、。參考文獻(xiàn)：1李永新RoboCup小型組足球機(jī)器人結(jié)構(gòu)設(shè)計(jì)與分析中國(guó)科技大學(xué)2003年12月2 Tamas KalmarNagyRaffaello D Andrea，Pritam GangulyNearoptimal dynamic tr0jectory generation and control of an omnidirectional vehicleEBOLhttp:/wwwmaccornel1edu/Raff/MultiAgentSystems/nagRAS04.pdf.2005-09-203 Dkada HEndoN SKikuchi MOn fracture analysis u

17、sing an element overlay techniqueJAdvances in Fractrue and Failure Prevention2000，15(5)：2612634張培仁,張志堅(jiān),鄭旭東,張華賓編著基于16/32位DSP機(jī)器人控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)北京：清華大學(xué)出版社，20065陳剛,程蔭杭.基于TMS320F2812的移動(dòng)機(jī)器人運(yùn)動(dòng)控制系統(tǒng). 文北京交通大學(xué)電子信息工程學(xué)院. 2006-04-30.6陳浩杰.改進(jìn)型的足球機(jī)器人小車系統(tǒng)的研究. 科學(xué)技術(shù)與工程. 第6卷 第l4期 2006年7月1671-l815(2006)14-2057-04.7 黃慶成，洪炳镕，Javaid Khurshi.全自主足球機(jī)器人的運(yùn)動(dòng)控制系統(tǒng)研究.哈爾濱工業(yè)大學(xué)學(xué)報(bào). 第36卷 第7期 2004年7月.作者簡(jiǎn)介：黃永賢（1982），男，廣東工業(yè)大學(xué)碩士生，主要從事智能控制方面的研究。楊宜民（1945），男