基于簡(jiǎn)化三維線性倒立擺模型的模糊控制射門(mén)算法

,, ,,

(1.蘇州科技大學(xué) 天平學(xué)院，江蘇 蘇州 215009; 2.蘇州科技大學(xué) 江蘇省建筑智慧節(jié)能重點(diǎn)實(shí)驗(yàn)室，江蘇 蘇州 215009)

0 引言

機(jī)器人足球比賽是一項(xiàng)帶有人類(lèi)行為意識(shí)的競(jìng)技體育比賽。更為深層次的意義在于通過(guò)此類(lèi)比賽形式，以足球?yàn)檩d體，促進(jìn)包括人工智能、機(jī)器人學(xué)、傳感器、自動(dòng)控制和通訊等一系列領(lǐng)域的技術(shù)發(fā)展。Robocup全球委員會(huì)提出未來(lái)將建立一支機(jī)器人足球隊(duì)，并將打敗人類(lèi)足球世界杯冠軍隊(duì)作為奮斗目標(biāo)。國(guó)際機(jī)器人足球聯(lián)盟制定的規(guī)則表明，在規(guī)定比賽時(shí)間內(nèi)進(jìn)球最多的隊(duì)伍獲勝，因此雙足機(jī)器人射門(mén)成功率是比賽取勝的決定性因素。

目前，研究人員針對(duì)射門(mén)策略開(kāi)展了廣泛的研究。張彥鐸等人在傳統(tǒng)射門(mén)的基礎(chǔ)上提出了一種射門(mén)區(qū)間模型。在為機(jī)器人規(guī)劃最佳運(yùn)動(dòng)路徑的同時(shí)尋找出最優(yōu)射門(mén)點(diǎn)[1]。但該方法忽略了當(dāng)射門(mén)路徑上存在其他機(jī)器人時(shí)，對(duì)射門(mén)路徑造成的干擾。韓家新等人提出了一種改進(jìn)直沖射門(mén)方法，相比較傳統(tǒng)射門(mén)算法而言射門(mén)成功率有所提高[2]。但是該算法代碼的復(fù)雜度高、計(jì)算時(shí)間長(zhǎng)，無(wú)法保證機(jī)器人動(dòng)作的實(shí)時(shí)性。宋大雷等人提出的動(dòng)態(tài)橢圓射門(mén)方法從減少機(jī)器人射門(mén)運(yùn)動(dòng)距離入手，提高了射門(mén)成功率[3]。但是該方法對(duì)硬件有著苛刻的要求。梁家海、趙兵等人分別提出了利用機(jī)器學(xué)習(xí)和模糊算法這類(lèi)人工智能方法來(lái)優(yōu)化射門(mén)路徑。雖然這類(lèi)方法可以比較連貫的完成射門(mén)動(dòng)作與避障[4-5]。但是對(duì)機(jī)器人的硬件設(shè)備和計(jì)算能力要求較高，從而容易對(duì)射門(mén)實(shí)時(shí)性造成影響。

針對(duì)上述射門(mén)方法中存在的射門(mén)精度和實(shí)時(shí)性的問(wèn)題，本文主要借助于中國(guó)機(jī)器人大賽3D雙足機(jī)器人Robocup 11 vs 11仿真平臺(tái)[4]，將真實(shí)的比賽場(chǎng)景進(jìn)行高還原度的模擬。并通過(guò)分析本球隊(duì)射門(mén)機(jī)器人與本球隊(duì)成員、對(duì)方球隊(duì)成員的位置、行為之間的關(guān)系，提出了一種基于簡(jiǎn)化三維倒立擺模型的模糊控制算法來(lái)提高機(jī)器人射門(mén)精度和實(shí)時(shí)性。該方法利用模糊控制方法精確地修正機(jī)器人的射門(mén)姿態(tài)和角度，并在射門(mén)時(shí)提供較大的沖量，從而提高射門(mén)成功率。

1 簡(jiǎn)化三維線性倒立擺模型原理

1.1 簡(jiǎn)化三維線性倒立擺模型

在雙足機(jī)器人運(yùn)動(dòng)過(guò)程中，如果只考慮前后和上下方向的運(yùn)動(dòng)而忽略左右的運(yùn)動(dòng)，那么容易造成機(jī)器人失去平衡而摔倒。因此，需要將線性倒立擺拓展到三維環(huán)境，并對(duì)機(jī)器人進(jìn)行三維線性倒立擺建模。然而，直接分析研究雙足機(jī)器人多連桿結(jié)構(gòu)的過(guò)程往往較為繁瑣，因此本文等價(jià)地簡(jiǎn)化機(jī)器人為合理的數(shù)學(xué)模型，從而便于研究[6]。將機(jī)器人的身體視為一個(gè)質(zhì)點(diǎn)，腿視為無(wú)質(zhì)量的支撐桿，可以建立起一個(gè)三維線性倒立擺模型。它不需要已知機(jī)器人的參數(shù)，如質(zhì)量、質(zhì)心以及每個(gè)關(guān)節(jié)的慣性，而是通過(guò)模型導(dǎo)出，從而便于計(jì)算。

圖1 簡(jiǎn)化三維線性倒立擺模型

圖1表示簡(jiǎn)化的三維線性倒立擺模型，假定質(zhì)心高度固定為zc，重力加速度為g，根據(jù)三維線性倒立擺模型，則ZMP軌跡(px(t)、py(t))與質(zhì)心軌跡(x(t)、y(t))運(yùn)動(dòng)的方程為：

(1)

(2)

由公式(1)、(2)可以看出，給定ZMP軌跡(px(t)、py(t))和質(zhì)心軌跡(x(t)、y(t))就可以求得質(zhì)心軌跡。再通過(guò)指定游動(dòng)腿軌跡，即能夠讓雙足機(jī)器人進(jìn)行步態(tài)行走。

1.2 雙腿支撐相射門(mén)姿態(tài)調(diào)整

在實(shí)際比賽時(shí)，射門(mén)成功率不單單取決于射門(mén)算法，雙足機(jī)器人射門(mén)前調(diào)整姿態(tài)所花費(fèi)的時(shí)間也對(duì)成功率有一定影響。機(jī)器人到達(dá)目標(biāo)射門(mén)點(diǎn)時(shí)需要調(diào)整其姿態(tài)后才能進(jìn)行射門(mén)，即將方向和速度調(diào)整到合理的數(shù)值。

在如圖2所示的雙腿支撐相中，可以得到機(jī)器人的質(zhì)心位置和加速度關(guān)系方程：

圖2 雙足機(jī)器人雙腿支撐相

(3)

(4)

(5)

給出質(zhì)點(diǎn)的初始方位、速度以及最終方位、速度，即已知：

則可得：

(6)

(7)

根據(jù)公式(4)～(7)可知，要想求得機(jī)器人在雙腿支撐相的身體質(zhì)點(diǎn)軌跡，只需要知道機(jī)器人的初始方位和速度、最終方位和速度。因此跟據(jù)上述方法可以實(shí)現(xiàn)雙足機(jī)器人一步停止，縮短了射門(mén)前的準(zhǔn)備時(shí)間。

1.3 雙足機(jī)器人單腿支撐相射門(mén)

當(dāng)雙足機(jī)器人開(kāi)始進(jìn)行踢球動(dòng)作時(shí)，就需要獲得最佳的射門(mén)動(dòng)作。在雙足機(jī)器人抬起游動(dòng)腿之前，都需要實(shí)現(xiàn)將質(zhì)心位置移動(dòng)到該單腿支撐項(xiàng)的位置。本文將公式(1)和公式(2)進(jìn)行變換，從而得到公式(8)和公式(9)：

px=c1*e4+c2*e3+org

(8)

vx=c1*e4/Tc-c2*e3/Tc

(9)

其中：e1=exp(-Ts/Tc)，e2=exp(Ts/Tc)，

e3=exp(-t/Tc)，e4=exp(t/Tc)，

c1=((x0-org)*e1-(c1-org))/(e1-e2)，

c2=((x0-org)*e2-(c-org))/(e2-e1)。

由于雙足機(jī)器人要實(shí)現(xiàn)單腿支撐，就需要對(duì)質(zhì)心的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，同時(shí)提前規(guī)劃其側(cè)平面軌跡。從而使質(zhì)心能夠垂直投影于ZMP范圍內(nèi)。實(shí)際比賽時(shí)，機(jī)器人只有通過(guò)迅速調(diào)整球與腳之間的距離，在躲避對(duì)方攔截和避開(kāi)干擾球員的同時(shí)，實(shí)現(xiàn)起腳射門(mén)動(dòng)作。因此，處于單腿支撐相時(shí)，本文將質(zhì)心沿y軸移動(dòng)的距離設(shè)置為一個(gè)變量。并通過(guò)動(dòng)態(tài)地對(duì)機(jī)器人射門(mén)腳與球之間的距離進(jìn)行調(diào)整，完成快速起腳射門(mén)的動(dòng)作。

2 動(dòng)腿軌跡規(guī)劃

本文規(guī)定射門(mén)的起始位置為ps(xs,ys,zs)，初始速度vs，初始角度為(αs,βs)。球的位置為po(xo,yo,zo)，目標(biāo)速度vo，目標(biāo)角度為(αo,βo)，α表示y軸方向上的角度，β表示x軸方向上的角度。關(guān)系式如下：

(10)

同理，根據(jù)po點(diǎn)得到p3關(guān)系式：

(11)

設(shè)射門(mén)周期為T(mén)，t∈[0,T]，得到以ps、po

為起始點(diǎn)的3次貝塞爾曲線方程為:

(12)

其中：0≤t≤T，vs≥0，vo≥0。

這樣就可以根據(jù)雙足機(jī)器人的初始位置和球的坐標(biāo)位置為起點(diǎn)和終點(diǎn)做一條平滑的曲線，作為雙足機(jī)器人的游動(dòng)腿軌跡。

3 模糊控制射門(mén)算法

3.1 模糊控制射門(mén)算法原理

傳統(tǒng)曲線射門(mén)的思路是讓雙足機(jī)器人沿著一段圓弧的軌跡運(yùn)動(dòng)到足球所在的位置，然后繼續(xù)帶球運(yùn)動(dòng)到目標(biāo)射門(mén)點(diǎn)之后將球踢出[7]。此類(lèi)算法雖然優(yōu)于傳統(tǒng)直線射門(mén)算法，但是當(dāng)機(jī)器人離足球與目標(biāo)射門(mén)點(diǎn)所成直線的距離較近時(shí)，圓弧很大，因此射門(mén)效果不夠理想。基于上述原因，本文提出了一種模糊控制射門(mén)算法。利用模糊規(guī)則讓機(jī)器人用較短的時(shí)間走直線取球，接著走一段圓弧軌跡，把方向調(diào)整準(zhǔn)確后踢球射門(mén)。具體實(shí)現(xiàn)步驟：

步驟2：從機(jī)器人R到圓O的切線也有兩條，選擇當(dāng)機(jī)器人到達(dá)B點(diǎn)的方向與由B指向G相同的那條為切線。如圖5中選擇點(diǎn)切點(diǎn)P所在的切線，此時(shí)切線方向角θ=β-α。其中，β為RP的方向角，α=arcsin(r/d)，d為線段OR的長(zhǎng)度。由此可以求出切線RP的方向角θ；

步驟3：讓機(jī)器人沿直線RP運(yùn)動(dòng)到P點(diǎn)，然后沿曲線軌跡射門(mén)。

3.2 射門(mén)方向修正

以上只解決了雙足機(jī)器人運(yùn)動(dòng)時(shí)的路徑規(guī)劃與射門(mén)方向問(wèn)題，沒(méi)有將機(jī)器人初始方向?qū)ι溟T(mén)成功率的影響納入討論。因?yàn)闄C(jī)器人的初始方向是任意的，并不會(huì)默認(rèn)與切線方向θ相同，所以會(huì)產(chǎn)生誤差。因此本文引入模糊控制方法來(lái)動(dòng)態(tài)調(diào)整機(jī)器人的初始方向。

本文提出的模糊規(guī)則表述如下：

當(dāng)左右腿中任意一個(gè)的速度達(dá)到設(shè)定值之后不再增加，同時(shí)另一條腿的速度也增加到設(shè)定值不再改變。如果機(jī)器人當(dāng)前的方向角與本文期望的切線方向角的誤差到達(dá)一定的值，則調(diào)整機(jī)器人腿的速度，使它們產(chǎn)生速度差以調(diào)整角度。其控制函數(shù)VM和Vm表達(dá)式如下：

VM=a1d+a2δ+Vc

(13)

Vm=a1d-a2δ+Vc

(14)

VM表示游動(dòng)腿的速度，Vm表示支撐腿速度，Vc為常數(shù)，a1，a2為常數(shù)，數(shù)值根據(jù)實(shí)際情況修改。

機(jī)器人走直線時(shí)伴隨著周期性的方向調(diào)整，當(dāng)方向角與切線方向角一致時(shí)，讓機(jī)器人走直線運(yùn)動(dòng)到P點(diǎn)后，最后讓機(jī)器人沿曲線軌跡完成射門(mén)操作。在仿真與實(shí)際測(cè)試的時(shí)候，可以只通過(guò)計(jì)算機(jī)器人和圓心的距離來(lái)判定其是否運(yùn)動(dòng)到P點(diǎn)。

4 實(shí)驗(yàn)與分析

為了驗(yàn)證本文提出射門(mén)算法的性能，先將NAO機(jī)器人為原型建立三維線性倒立擺模型，然后再將算法寫(xiě)入RoboCup 11vs11仿真平臺(tái)。在RoboCup 3D仿真平臺(tái)中完全模擬真實(shí)比賽的場(chǎng)景，對(duì)本文提出的模糊控制射門(mén)算法進(jìn)行抬腿、射門(mén)、持球、持球射門(mén)等項(xiàng)目的測(cè)試，如圖3所示。同時(shí)要求機(jī)器人在持球行走和射門(mén)的過(guò)程中自我調(diào)整方向，并實(shí)現(xiàn)快速地起腳射門(mén)。仿真實(shí)驗(yàn)表明，本文的算法射門(mén)成功率高于其他射門(mén)算法。

圖3 雙足機(jī)器人仿真射門(mén)過(guò)程

傳統(tǒng)射門(mén)方法通常采用直線方程生成ZMP軌跡[8]。本文為了在多角度和任意姿態(tài)下實(shí)現(xiàn)最優(yōu)射門(mén)動(dòng)作，分別將直線方程、貝塞爾曲線生成的游動(dòng)腿ZMP軌跡用于本文提出的基于模糊控制算法的雙足機(jī)器人射門(mén)動(dòng)作軌跡，并分別優(yōu)化獲得如圖4所示的適應(yīng)度函數(shù)值。

從圖4中不難發(fā)現(xiàn)直線方程的ZMP軌跡經(jīng)過(guò)迭代48次后收斂，而塞爾曲線的ZMP軌跡的適應(yīng)度函數(shù)經(jīng)過(guò)迭次78次后才趨于穩(wěn)定。由此可見(jiàn)ZMP軌跡為直線方程時(shí)收斂過(guò)早，并且其適應(yīng)度函數(shù)值最終也大于ZMP軌跡為塞爾曲線的情況。最后將兩種方法優(yōu)化后的參數(shù)分別應(yīng)用于仿真射門(mén)平臺(tái)進(jìn)行射門(mén)測(cè)試對(duì)比。測(cè)試結(jié)果也驗(yàn)證了本文游動(dòng)腿采用ZMP軌跡是塞爾曲線時(shí)，射門(mén)準(zhǔn)確性更高、用時(shí)也更短。

圖4 直線方程、貝塞爾曲線生成ZMP軌跡優(yōu)化效果對(duì)比

圖5為機(jī)器人射門(mén)全過(guò)程，采用Robocup實(shí)際比賽所用的NAO機(jī)器人，將程序輸入開(kāi)發(fā)接口進(jìn)行真實(shí)環(huán)境測(cè)試。測(cè)試過(guò)程分為初始化的起步階段，比賽時(shí)的正常行走，按照本文算法射門(mén)階段以及最后的進(jìn)球過(guò)程。

圖5 NAO機(jī)器人全過(guò)程射門(mén)

如圖6所示，在2000次射門(mén)實(shí)驗(yàn)中，隨機(jī)算法失球512次，被攔截236次，被撲出435次；基本算法失球235次，被攔截145次；切圓弧軌跡算法失球156次，被攔截81次，被撲出167次；本文算法在45°射門(mén)時(shí)失球133次，被攔截67次，被撲出136次。從圖中可以看出本文算法大幅優(yōu)于另外幾種算法，而且采用本算法比賽的足球隊(duì)所發(fā)生的失球、被攔截、被撲出的數(shù)量最少。

圖6 4種算法射門(mén)實(shí)驗(yàn)比較結(jié)果

此外，本文還從射門(mén)平均距離、平均調(diào)整時(shí)間、平均射門(mén)時(shí)間3個(gè)方面對(duì)比了4種算法，如表1所示。統(tǒng)計(jì)結(jié)果表明，本文提出的射門(mén)算法在平均調(diào)整時(shí)間和平均射門(mén)時(shí)間這兩項(xiàng)性能上都優(yōu)于其他3種算法。并且在射門(mén)距離上也接近切圓弧軌跡射門(mén)法，滿(mǎn)足了比賽中射門(mén)距離越遠(yuǎn)越好的需求。

表1 RoboCup3D平臺(tái)中的4種射門(mén)算法性能對(duì)比

5 結(jié)束語(yǔ)

本文提出的基于三維線性倒立擺的改進(jìn)圓弧射門(mén)算法最大的亮點(diǎn)在于改善了機(jī)器人按照既定軌跡運(yùn)動(dòng)前初始方向調(diào)整的問(wèn)題。在實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)路徑的平滑過(guò)渡的同時(shí)提高了運(yùn)動(dòng)的沖量，可以大大提高射門(mén)成功率，體現(xiàn)了本算法的可行性和優(yōu)越性。此外，蘇州科技大學(xué)STU機(jī)器人足球隊(duì)采用本文算法后，在2017年江蘇省大學(xué)生機(jī)器人比賽足球仿真組取得了亞軍一等獎(jiǎng)的好成績(jī)。