【正文】 算法中可設(shè)置一個(gè)閥值，當(dāng)該等式計(jì)算出來(lái)的誤差超過(guò)了閥值就認(rèn)為不成立。方程()可改寫(xiě)為：（v1,v2,v3,v4 ）T =P(vx,vy ,)T ()其中(v1,v2，v3 , v4)T為機(jī)器人四組驅(qū)動(dòng)輪的線速度向量，把它簡(jiǎn)寫(xiě)為；(,)T為機(jī)器人整體速度矢量向量，把它簡(jiǎn)寫(xiě)為；P為34階矩陣，是足球機(jī)器人的速度分解轉(zhuǎn)換矩陣。在實(shí)際使用中，我們是檢測(cè)機(jī)器人是否有驅(qū)動(dòng)輪打滑的現(xiàn)象發(fā)生。經(jīng)過(guò)實(shí)驗(yàn)，我們知道我們的電機(jī)驅(qū)動(dòng)最大扭矩是大于驅(qū)動(dòng)輪與地面的最大靜摩擦力力矩的。極限情況下，一組驅(qū)動(dòng)輪甚至是完全懸空而不與地面發(fā)生接觸的。 機(jī)器人速度矢量分解四輪全向機(jī)器人速度矢量分解環(huán)節(jié)是個(gè)開(kāi)環(huán)部分，它所依照的運(yùn)動(dòng)學(xué)方程如下: ()該環(huán)按照公式()將速度軌跡生成的機(jī)器人整體速度、加速度矢量Vx、VY`﹑分解到各驅(qū)動(dòng)輪上，得到各組驅(qū)動(dòng)輪的速度控制量v1﹑v2﹑v3﹑。該部分為開(kāi)環(huán)，根據(jù)四輪全向足球機(jī)器人的運(yùn)動(dòng)學(xué)公式，將速度軌跡設(shè)定的機(jī)器人車(chē)體的速度和加速度分配到各個(gè)驅(qū)動(dòng)輪上。4；I是機(jī)器繞重心軸的轉(zhuǎn)動(dòng)慣量；則機(jī)器人在x方向和y方向的加速度分別為：Max= –f2 sin+f3sin +f4 sin ()May=f1cosf2cosf3cos+f4cos ()對(duì)于均勻的圓柱體，轉(zhuǎn)動(dòng)慣量為I=1/2MR2，對(duì)于均勻的圓環(huán)I=MR2。從上節(jié)分析可知，各輪子角度左右差別并不大，只有5度，為了簡(jiǎn)化分析我們假定各輪對(duì)稱分布則其簡(jiǎn)化模型如下圖48所示：F4F3F2F1YX 圖48 四輪機(jī)器人動(dòng)力學(xué)分析簡(jiǎn)化示意圖則機(jī)器人質(zhì)心的平動(dòng)加速度為： a= (F1+F2+F3+F4) ()M是機(jī)器人的質(zhì)量；因?yàn)樗械牧Χ甲饔迷跈C(jī)器人輪子所在的同一個(gè)圓的切向，所以計(jì)算旋轉(zhuǎn)加速度時(shí)用力向量的大小就可以了，則機(jī)器人的旋轉(zhuǎn)加速度為： () R是機(jī)器人輪子所在的圓半徑；代表力向量的大小，=l驅(qū)動(dòng)輪角度布置四輪全向機(jī)器人的四組驅(qū)動(dòng)輪分布在一個(gè)圓周上互相成一定角度，通過(guò)四組驅(qū)動(dòng)輪的驅(qū)動(dòng)矢量的合成實(shí)現(xiàn)全方位運(yùn)動(dòng)。從以上分析可以看出，摩擦系數(shù)對(duì)機(jī)器人的運(yùn)動(dòng)性能具有極其重要的影響以前采用雙排全向輪結(jié)構(gòu)，該型輪子可沿周向和軸向兩個(gè)方向運(yùn)動(dòng)。當(dāng)車(chē)輪純滾動(dòng)時(shí)，車(chē)輪旋轉(zhuǎn)角速度與小車(chē)平移速度V及車(chē)輪半徑r之間有如下關(guān)系：F1F3 F F2 ItIf圖46驅(qū)動(dòng)輪受力示意圖 ()根據(jù)平衡條件，有： （） 公式(4．27)中：F地面對(duì)車(chē)輪的摩擦力；m車(chē)體質(zhì)量；m輪一車(chē)輪質(zhì)量；k一車(chē)輪承受車(chē)體載荷分布系數(shù)；F1=mkv一驅(qū)動(dòng)軸作用于車(chē)輪的水平力；F2車(chē)輪所受支反力；F3=mkg一車(chē)輪承受車(chē)體載荷J一車(chē)輪的轉(zhuǎn)動(dòng)慣量；f產(chǎn)車(chē)輪滾動(dòng)阻力系數(shù)；Ff=F2f一車(chē)輪滾動(dòng)阻力；Tf=Ffr=F2fr一車(chē)輪滾動(dòng)阻力矩；T1=iηmηi一電動(dòng)機(jī)驅(qū)動(dòng)力矩；其中,ηm ,ηi分別為電動(dòng)機(jī)輸出轉(zhuǎn)矩、電動(dòng)機(jī)傳動(dòng)效率、減速器減速比、減速器傳動(dòng)效率；地面對(duì)車(chē)輪的摩擦力F的最大值為 Fmax=μN(yùn)=μmkg ()根據(jù)公式()，車(chē)體的加速度為： a=F/mf/m ()令墨業(yè)為作用于車(chē)輪上的驅(qū)動(dòng)力，整理得機(jī)器人驅(qū)動(dòng)方程式： （）即 綜合公式()()()，可知為了使機(jī)器人加速性能比較好，車(chē)輪及車(chē)體質(zhì)量要盡可能小些，選用高輸出轉(zhuǎn)矩電動(dòng)機(jī)。這些轉(zhuǎn)矩的一部分消耗在使其它全向輪產(chǎn)生軸向滑動(dòng)上。圖45為全向驅(qū)動(dòng)輪機(jī)構(gòu)比較圖，圖中分別為雙排全向輪和單排全向輪。所以它能夠讓機(jī)器人從給定的一點(diǎn)到另一點(diǎn)走一條直線路徑，而不用先轉(zhuǎn)向：它可以在期望路徑上進(jìn)行平移運(yùn)動(dòng)時(shí)能同時(shí)進(jìn)行旋轉(zhuǎn)運(yùn)動(dòng)，因而能以正確的姿態(tài)到達(dá)目標(biāo)點(diǎn)它；它還可以精確的實(shí)現(xiàn)各種復(fù)雜的路徑運(yùn)動(dòng)。由上節(jié)討論可知，速度軌跡時(shí)間執(zhí)行時(shí)間依靠邊界條件和控制量，也就是： ()則對(duì)于x和y方向，其時(shí)間計(jì)算得到的速度軌跡得到的時(shí)間應(yīng)該相等，也就是說(shuō)： ()由于x和Y方向還必須滿足：Ax2+Ay2=1 ()根據(jù)公式（），得到： ()因?yàn)樵趞AX|∈(0,1]區(qū)間是嚴(yán)格單調(diào)遞減，而在此區(qū)間是嚴(yán)格單調(diào)遞增的，所以是嚴(yán)格單調(diào)遞減的。下面我們來(lái)對(duì)x方向、方向來(lái)做同步處理。根據(jù)公式()，我們可知起始點(diǎn)與目標(biāo)點(diǎn)的距離、初時(shí)刻速度、末時(shí)刻速度這些邊界條件都是已經(jīng)給定了的，是“任務(wù)”設(shè)定的和客觀實(shí)在的數(shù)據(jù)。如果不同步，在實(shí)際的控制中就會(huì)出現(xiàn)一個(gè)方向上先達(dá)到目標(biāo)值，然后機(jī)器人保持此方向上的值不變，在其它方向上的繼續(xù)運(yùn)動(dòng)達(dá)到設(shè)定值的動(dòng)作。機(jī)器人速度軌跡跟隨子系統(tǒng)實(shí)時(shí)控制機(jī)器人速度，使機(jī)器人每個(gè)時(shí)刻的自身速度達(dá)到VTG生成的速度軌跡曲線的對(duì)應(yīng)時(shí)刻的值，這樣就可實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)控制。 () 因?yàn)闄C(jī)器人的位移和速度是連續(xù)的，所以在t=t1時(shí)方程()、()是相等，因此得到方程： () ()由上述方程()、()可以計(jì)算得到： 其中 （） 其中 （） 則由基于bang—bang的速度軌跡生成算法的點(diǎn)到點(diǎn)的最小時(shí)間為： （）在足球機(jī)器人的控制系統(tǒng)中，需要生成一條離散的速度軌跡。也就是控制作用 （）為常數(shù)，代表此方向上的最大控制作用(最大加速度)。．算法原理下面將使用bang—bang控制的原理來(lái)求解，得到速度軌跡生成算法。要得到時(shí)間最優(yōu)的速度軌跡生成算法就需要求解機(jī)器人滿足()、()、()條件的時(shí)間最優(yōu)控制解。方程()、()、()是方程()、()、()典型的邊界條件：, ( ), ( ), ()、分別為三個(gè)方向上的初始位置。因此，在速度軌跡生成算法中，對(duì)于足球機(jī)器人x方向、Y方向以及角方向上的控制量，分別在三個(gè)方向各自獨(dú)立進(jìn)行控制。分析足球機(jī)器人控制系統(tǒng)，得到其控制用的運(yùn)動(dòng)合成方程如下： ()ω一表示各驅(qū)動(dòng)輪的轉(zhuǎn)動(dòng)角速度ψ一表示各驅(qū)動(dòng)輪與x方向夾角ι一驅(qū)動(dòng)輪所在圓周半徑Vx Vy Vθ 一機(jī)器人自身坐標(biāo)系下的x、y方向速度和角速度由于系統(tǒng)過(guò)于復(fù)雜，很難得到實(shí)際可用的對(duì)可解耦的控制方程。 Vx 1 4 Vo 2 3 VyP1 P4 P2 P3圖42足球機(jī)器人速度合成圖如圖42，足球機(jī)器人共有四組驅(qū)動(dòng)輪，成一定角度分布在一個(gè)圓上。(4)機(jī)器人以每秒25次的頻率接收到傳送來(lái)的指令，并使用運(yùn)動(dòng)控制算法的速度軌跡跟隨算法控制機(jī)器人達(dá)到指令要求的速度值。(2)視覺(jué)信息每更新一次，決策控制部分就執(zhí)行一次工作流程，為各機(jī)器人分配“任務(wù)”。所謂速度軌跡即機(jī)器人在特定的時(shí)間點(diǎn)應(yīng)該達(dá)到的速度。4 四輪全向足球機(jī)器人運(yùn)動(dòng)控制算法研究足球機(jī)器人運(yùn)動(dòng)控制就是按照多智能體協(xié)調(diào)控制軟件為每個(gè)足球機(jī)器人設(shè)定的“任務(wù)”控制機(jī)器人完成該任務(wù)。如果儲(chǔ)能電容因?yàn)轵?qū)動(dòng)電磁鐵等原因造成電壓下降，就會(huì)觸發(fā)PWM發(fā)生器。此過(guò)程中，電感會(huì)因?yàn)殡娏飨陆刀a(chǎn)生感應(yīng)電壓，該電壓與電源電壓疊加給儲(chǔ)能電容沖電。利用開(kāi)關(guān)電源的原理，設(shè)計(jì)升壓電路如所示： 圖39 DCDC升壓電路模塊原理圖如圖39，大功率MOS管是一個(gè)電子開(kāi)關(guān)，PWM發(fā)電器發(fā)出的PWM波控制它的導(dǎo)通和關(guān)斷。每一路傳感器均由紅外發(fā)光管、紅外光電管、發(fā)光管驅(qū)動(dòng)電路、光電管信號(hào)前置放大電路等組成。傳感器的紅外發(fā)光管發(fā)出紅外光，光電管接收到紅外光。其中敵方機(jī)器人、球及邊框等障礙物的識(shí)別主要由懸掛在場(chǎng)上方的攝像頭采集的圖象經(jīng)過(guò)圖象識(shí)別技術(shù)來(lái)得到。該控制電路如圖3—5所示： 圖35“踢球”電磁鐵的驅(qū)動(dòng)電路為了防止高壓電路影響邏輯電路，我們使用了光耦來(lái)隔離高壓電路和低壓電路。檢測(cè)電阻兩端的電壓，即可得到電機(jī)電樞電流的大小。MCU也需要對(duì)電機(jī)實(shí)時(shí)調(diào)速，因此電機(jī)會(huì)頻繁的變速和反轉(zhuǎn)。根據(jù)其電機(jī)電流和轉(zhuǎn)速的反饋信號(hào)，MCU使用模糊PID控制算法計(jì)算出控制量，以PWM方式對(duì)四組電機(jī)進(jìn)行實(shí)時(shí)調(diào)速。MCU模塊的讀數(shù)電路對(duì)編碼器信號(hào)進(jìn)行計(jì)數(shù)。實(shí)際設(shè)計(jì)中，使用了兩個(gè)I／O作為握手信號(hào)，主DSP根據(jù)握手信號(hào)發(fā)送不同的數(shù)據(jù)。無(wú)線模塊接收的指令經(jīng)過(guò)主DSP的接收和校驗(yàn)后，通過(guò)SCI發(fā)送從DSP。下面來(lái)介紹在主、從DSP的軟件設(shè)計(jì)，說(shuō)明是如何實(shí)現(xiàn)各種功能的。必要的時(shí)候可支持控制程序的無(wú)線下載。其中兩組正交碼盤(pán)信號(hào)由從DSP自身“事件管理器”讀出，另兩組正交碼盤(pán)信號(hào)向DSP請(qǐng)求，主DSP讀出后通過(guò)SCI接口，發(fā)送給從DSP。另一片DSP2407為從MCU，其相應(yīng)工作為：(1)接收主DSP處理完畢的無(wú)線指令，解析并執(zhí)行指令，控制機(jī)器人運(yùn)動(dòng)、帶球、擊球。這樣主DSP就可以配置從DSP的工作狀態(tài)，管理從DSP重啟、從片內(nèi)或者片外執(zhí)行程序；(2)主DSP配合無(wú)線通訊模塊接收無(wú)線通訊指令。綜合上面的問(wèn)題，在四輪全向足球機(jī)器人的控制電路設(shè)計(jì)方案中，采用雙DSP2407的來(lái)構(gòu)建控制系統(tǒng)。另外，我們選用TMS320LF2407來(lái)作為控制用MCU有一個(gè)很大的因素是因?yàn)樗摹笆录芾砥鳌本哂姓淮a盤(pán)信號(hào)處理的能力，但是2407只有兩個(gè)“事件管理器”。由于驅(qū)動(dòng)輪控制的定時(shí)中斷需要很高的系統(tǒng)開(kāi)銷(xiāo)，用于機(jī)器人控制的MCU無(wú)法以中斷的方式配合無(wú)線通訊模塊來(lái)進(jìn)行高波特率的無(wú)線指令接收和處理。TMS320LF2407A的具體參數(shù)為：最高內(nèi)部頻率為40MHz，時(shí)鐘周期為25ns，具有40MIPS的處理能力；32K內(nèi)部FLASH為，可加密；兩個(gè)事件管理器(Event Managers)，每個(gè)EM包括：兩個(gè)16位時(shí)鐘；8個(gè)通道的脈寬調(diào)制(PWM)單元；三個(gè)事件捕獲單元(Cap)；10位A／D轉(zhuǎn)換器，16通道，375500ns的轉(zhuǎn)換時(shí)間；CAN2．0、異步串行口(SCI)、同步串行口(SPI)；40個(gè)管腳可用作通用10。由于這些處理所需要的運(yùn)算量相對(duì)比較龐大，而且需要使用大量的控制接口，所以一些傳統(tǒng)的簡(jiǎn)單運(yùn)算芯片(如8051等)和單片機(jī)無(wú)法滿足我們的需求，我們采用運(yùn)算能力相對(duì)強(qiáng)大的DSP芯片。足球機(jī)器人使用可充電鎳氫電池組供電，電源變換模塊將電壓升高到DC200v，儲(chǔ)存到大容量的電容中，作為擊球電磁鐵的電源。MCU模塊可配合無(wú)線通訊模塊接收上位機(jī)(PC)發(fā)送的無(wú)線通訊信號(hào)，根據(jù)接收到的指令和自身傳感器信息完成對(duì)機(jī)器人的實(shí)時(shí)控制。3 以雙DSP為核心的控制系統(tǒng)電路設(shè)計(jì) 控制電路系統(tǒng)結(jié)構(gòu) RoboCup小型組足球機(jī)器人在場(chǎng)上是實(shí)時(shí)、高速、高對(duì)抗的．為了滿足比賽要求，足球機(jī)器人控制電路系統(tǒng)必須具備以下功能：(1)MCU運(yùn)算速度高，能夠執(zhí)行運(yùn)算量大的控制算法，方便擴(kuò)展各種接口電路；(2)能配合無(wú)線通訊模塊實(shí)現(xiàn)波特率115200bps的無(wú)線信號(hào)接收；(3)能夠以脈寬調(diào)制的方式驅(qū)動(dòng)四路驅(qū)動(dòng)輪電機(jī)；(4)可以處理四路以上的正交碼盤(pán)信號(hào)、檢測(cè)各驅(qū)動(dòng)電機(jī)電樞電流信號(hào)盤(pán)來(lái)實(shí)時(shí)地對(duì)四組驅(qū)動(dòng)輪進(jìn)行精確的閉環(huán)速度控制；(5)對(duì)機(jī)器人供電電池組進(jìn)行DCDC升壓變換至200v，用以驅(qū)動(dòng)“踢球”電磁鐵高速擊出“球”；(6)使用多路紅外傳感器和加速度計(jì)來(lái)感知機(jī)器人狀態(tài)。（2）足球機(jī)器人的系統(tǒng)工作模式，根據(jù)機(jī)器人的決策部分在整個(gè)系統(tǒng)作用位置來(lái)決定?；谝陨系姆治?，采用電磁鐵擊球機(jī)構(gòu)。彈簧擊球瞬間的爆發(fā)力與彈簧的壓縮量和彈簧剛度成正比，彈簧的壓縮量越大則擊球的爆發(fā)力越大，也會(huì)增加給小球沖量的持續(xù)時(shí)間t；但增加彈簧壓縮量使擊球機(jī)構(gòu)尺寸增大，可靠性降低。針對(duì)以上問(wèn)題，帶球機(jī)構(gòu)主要設(shè)計(jì)方案：(1)設(shè)計(jì)了懸掛式的帶球機(jī)構(gòu)；(2)帶球桿的位置不是固定的，可以上下調(diào)整，在不同的場(chǎng)地比賽時(shí)地面摩擦力不同，調(diào)整帶球桿的位置相當(dāng)于調(diào)整球和帶球桿的摩擦力，以適應(yīng)場(chǎng)地的摩擦力，就能獲得最佳的帶球效果。根據(jù)這一要求，選擇使用帶球棍旋轉(zhuǎn)摩擦球使球跟隨機(jī)器人運(yùn)動(dòng)，從而來(lái)實(shí)現(xiàn)帶球功能。全向足球機(jī)器人可以很方便的實(shí)現(xiàn)任意方向運(yùn)動(dòng)。機(jī)器人總體結(jié)構(gòu)從上往下分為三部分：最上部分足上色標(biāo)板，用于機(jī)器人視覺(jué)系統(tǒng)的識(shí)別機(jī)器人位置、方向及編號(hào)：中間安裝的是控制電路