別紅玲,周 波

(1.上海城建職業(yè)學(xué)院人工智能應(yīng)用學(xué)院,上海 201415) (2.浙江交通職業(yè)技術(shù)學(xué)院海運(yùn)學(xué)院,浙江 杭州 311112)

隨著機(jī)器人技術(shù)的不斷發(fā)展和應(yīng)用,機(jī)器人在工業(yè)生產(chǎn)、服務(wù)領(lǐng)域等方面有著廣泛的應(yīng)用前景[1]。作為機(jī)器人的組成部分[2],機(jī)械臂在操作過程中經(jīng)常會(huì)面臨障礙物的干擾,這給其在復(fù)雜環(huán)境下的運(yùn)動(dòng)與避障帶來(lái)了挑戰(zhàn)[3-4]。

許多學(xué)者對(duì)機(jī)械臂避障路徑規(guī)劃進(jìn)行了研究,如楊英浩等[5]通過運(yùn)動(dòng)學(xué)分析了機(jī)械臂的運(yùn)動(dòng)且對(duì)快速擴(kuò)展隨機(jī)樹(randomized rapidly-exploring tree,RRT)算法進(jìn)行了改進(jìn),利用B樣條曲線擬合以及能量最小逆解法優(yōu)化路徑,實(shí)現(xiàn)了最佳的避障路徑規(guī)劃;朱戰(zhàn)霞等[6]對(duì)障礙物以及機(jī)械臂運(yùn)用包圍盒以及空間疊加思想進(jìn)行了簡(jiǎn)單描述,將機(jī)械臂視為多個(gè)直線段,障礙物視為球體,根據(jù)二者相對(duì)位置檢測(cè)是否碰撞,將機(jī)械臂的關(guān)節(jié)參數(shù)進(jìn)行參數(shù)化,通過粒子群算法得出最佳的機(jī)械臂避障路徑;熊志金等[7]利用動(dòng)力學(xué)模型對(duì)機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)添加了約束,應(yīng)用RBF神經(jīng)網(wǎng)絡(luò)減少逼近控制器的誤差,并減少機(jī)械臂冗余動(dòng)作,使機(jī)械臂獲得最佳的避障路徑。上述方法雖然能夠完成機(jī)器人機(jī)械臂的避障路徑規(guī)劃,但是也存在著一些不足,如對(duì)障礙物進(jìn)行簡(jiǎn)化建模雖然可以節(jié)省空間,但簡(jiǎn)化建模的障礙物體積過大,導(dǎo)致模型中存在大量機(jī)械臂能夠移動(dòng)的空間,進(jìn)而使模型不夠精確;規(guī)劃的路徑并非最優(yōu)路徑;路徑規(guī)劃失誤的比例較大等。

本文采用C-空間建模方法設(shè)計(jì)障礙物環(huán)境下機(jī)械臂的避障軌跡檢測(cè)方法。首先對(duì)機(jī)械臂進(jìn)行建模,并對(duì)障礙物進(jìn)行簡(jiǎn)化建模,將建立的模型映射至C-空間中,然后在C-空間內(nèi)采用改進(jìn)的A*算法進(jìn)行路徑規(guī)劃,得出最佳的避障軌跡。

1 機(jī)械臂避障軌跡

1.1 機(jī)械臂運(yùn)動(dòng)學(xué)建模

笛卡爾空間是一個(gè)三維空間,其中坐標(biāo)系的3個(gè)坐標(biāo)軸分別對(duì)應(yīng)x、y和z方向。機(jī)械臂在笛卡爾空間中的位置和姿態(tài)可以通過其關(guān)節(jié)角度和旋轉(zhuǎn)角度來(lái)描述。在D-H參數(shù)法中,使用這些參數(shù)來(lái)描述機(jī)械臂不同關(guān)節(jié)間的相對(duì)運(yùn)動(dòng)關(guān)系[8-9],并計(jì)算出不同關(guān)節(jié)間的運(yùn)動(dòng)矩陣[10]。以某機(jī)械臂為例[11],根據(jù)D-H參數(shù)法建立的運(yùn)動(dòng)學(xué)模型如圖1所示,其中(xi,yi,zi)為機(jī)械臂關(guān)節(jié)點(diǎn)Oi的三維坐標(biāo)。根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)模型得到機(jī)械臂的D-H參數(shù),見表1。

表1 機(jī)器人機(jī)械臂D-H參數(shù)

圖1 機(jī)械臂運(yùn)動(dòng)學(xué)模型

(1)

式中:c表示cos,s表示sin。

1.2 障礙物簡(jiǎn)化建模

機(jī)械臂夾持器在O4位置,該點(diǎn)的位置向量(px,py,pz)用公式表達(dá)為:

(2)

式中:a1、a2、a3為相對(duì)位置。

由式(2)可以看出,機(jī)械臂夾持器的位置O4由θ2和θ3確定,且機(jī)械臂夾角θ2與位置向量px、py和pz沒有關(guān)系?；诖?可以將以z軸為中心旋轉(zhuǎn)形成的立體圖形近似為障礙物。如圖2所示,使該障礙物在平面內(nèi)的投影位于扇形環(huán)內(nèi),且該扇形的圓心位于坐標(biāo)系原點(diǎn),圓心到扇形環(huán)內(nèi)邊緣最短距離為rmin,圓心到扇形環(huán)外邊緣最長(zhǎng)距離為rmax。障礙物在該坐標(biāo)系yOz、xOz平面內(nèi)的投影與z軸的最大距離為zmax,其與在xOy平面內(nèi)的投影距離為h。若存在某一障礙物,將該障礙物近似為一個(gè)三維扇環(huán),則圖2中截面ABCD的數(shù)學(xué)模型為:

圖2 障礙物建模示意圖

(3)

若將該障礙物近似為軸截面為圓的三維扇環(huán),則該障礙物的數(shù)學(xué)模型為:

(4)

(5)

式中:r*為該障礙物近似的截面圓的半徑。將兩個(gè)模型進(jìn)行組合,可以描述更加復(fù)雜的障礙物。

采用上述方法,雖然增加了障礙物建模量,但簡(jiǎn)化了問題,只需考慮θ2、θ3的變化便可以完成避障路徑規(guī)劃。

1.3 C-空間計(jì)算

機(jī)械臂的路徑規(guī)劃在考慮夾持器避障的同時(shí)也需要考慮其手臂的避障。將機(jī)械臂的關(guān)節(jié)軸視為坐標(biāo)系,進(jìn)行C-空間的建立[12],并將障礙物映射到C-空間中,C-空間中障礙補(bǔ)集便是機(jī)械臂的運(yùn)動(dòng)空間。由于只需考慮θ2、θ3的變化,因此問題便成了對(duì)2D-RR的機(jī)械臂避障路徑規(guī)劃[13],只需要計(jì)算θ2、θ3,躲避C-空間障礙物即可。桿(O2,O3)、桿(O3,O4)和θ2、θ3的方向如圖3所示。

圖3 R-R機(jī)械臂C-空間障礙計(jì)算

(6)

(7)

式中:θρ=arctan(x/z),r2為桿(O2,O3)的長(zhǎng)度。

通過式(6)、(7)便可以計(jì)算出機(jī)械臂的活動(dòng)空間,降低算法復(fù)雜程度。

1.4 基于改進(jìn)A*算法的機(jī)械臂避障軌跡

在計(jì)算出C-空間障礙物的位置以及機(jī)械臂可活動(dòng)的空間后,便可以進(jìn)行避障路徑規(guī)劃,實(shí)現(xiàn)機(jī)械臂避障軌跡檢測(cè)。

1.4.1改進(jìn)A*算法的避障路徑規(guī)劃

返回出發(fā)點(diǎn)或規(guī)避障礙物前行是行進(jìn)中遇到障礙時(shí)所給出的兩種決策。規(guī)劃一條兩點(diǎn)間的最優(yōu)路徑是A*算法的作用。但在未改進(jìn)的A*算法中,分別采用統(tǒng)一成本搜索和貪婪搜索算法并不是一種最佳方法[14],因此將統(tǒng)一成本搜索和貪婪搜索算法相結(jié)合,借助二者優(yōu)點(diǎn)實(shí)現(xiàn)路徑規(guī)劃。改進(jìn)后的A*算法不但減少了對(duì)存儲(chǔ)空間的要求,而且與未改進(jìn)前相比增加了路徑計(jì)算的深度,提高了計(jì)算速度。在本文中對(duì)A*算法進(jìn)行了改進(jìn),機(jī)械臂優(yōu)先對(duì)目標(biāo)點(diǎn)的位置進(jìn)行象限判斷,確定其所在象限后跳過對(duì)其他象限的搜索,直接向目標(biāo)前進(jìn),同時(shí)在到達(dá)目標(biāo)位置前持續(xù)迭代計(jì)算。由于排除了其他3個(gè)象限,因此能夠大幅度減少路徑規(guī)劃的時(shí)間,找到最佳路徑。

通過如下方法確定機(jī)械臂所在象限。機(jī)械臂的初始坐標(biāo)為(Mx,My,Mz),設(shè)定目標(biāo)點(diǎn)坐標(biāo)為(Tx,Ty,Tz),通過對(duì)二者坐標(biāo)作差Dx=Tx-Mx,Dy=Ty-My,Dz=Tz-Mz,可以確定目標(biāo)點(diǎn)位于機(jī)械臂的某象限內(nèi)。機(jī)械臂需要確定目標(biāo)點(diǎn)與起始點(diǎn)在三維空間內(nèi)的相對(duì)位置,根據(jù)Dx,Dy,Dz的大小,機(jī)械臂可以判斷目標(biāo)點(diǎn)相對(duì)于起始點(diǎn)的位置。當(dāng)Dx=0,Dy=0,Dz>0時(shí),機(jī)械臂僅需要沿z軸正方向運(yùn)動(dòng),反之,Dx=0,Dy=0,Dz<0時(shí)則向反方向運(yùn)動(dòng)。以此類推,當(dāng)Dx=0,Dy≠0,Dz=0時(shí),機(jī)械臂沿y軸方向移動(dòng);當(dāng)Dx≠0,Dy=0,Dz=0時(shí),機(jī)械臂沿x軸方向移動(dòng)。最終,當(dāng)Dx=0,Dy=0,Dz=0時(shí),機(jī)械臂便到達(dá)了目標(biāo)位置。

機(jī)械臂在第l次由一個(gè)位置運(yùn)動(dòng)到下一個(gè)位置時(shí)需要計(jì)算成本函數(shù)f(l):

f(l)=g(l)+h(l)+p(l)

(8)

式中:g(l)為機(jī)械臂由當(dāng)前位置移動(dòng)到下一位置時(shí)消耗的成本,h(l)為啟發(fā)式成本,p(l)為機(jī)械臂由當(dāng)前位置移動(dòng)至下一位置轉(zhuǎn)彎成本。

啟發(fā)成本為從當(dāng)前位置移動(dòng)到停止位置時(shí)距離目標(biāo)點(diǎn)位置的歐幾里得距離,即啟發(fā)函數(shù),此即為A*算法的目標(biāo)函數(shù),計(jì)算公式如下:

(9)

式中:(x1,y1)為當(dāng)前狀態(tài)節(jié)點(diǎn)的位置,(x2,y2)為目標(biāo)狀態(tài)節(jié)點(diǎn)的位置。

在取得全部路徑點(diǎn)后采用貝塞爾曲線對(duì)路徑點(diǎn)進(jìn)行平滑優(yōu)化,貝塞爾n階曲線公式如下:

(10)

(11)

B(t)=(1-t)2P0+2t(1-t)P1+t2P2,t∈[0,1]

(12)

通過式(12)的二階貝塞爾曲線對(duì)A*算法完成路徑規(guī)劃后得到的多個(gè)點(diǎn)坐標(biāo)進(jìn)行連接,便可得到一條平滑的路徑。

1.4.2改進(jìn)A*算法的機(jī)械臂避障軌跡檢測(cè)實(shí)現(xiàn)步驟

1)建立搜索圖G,以初始位置節(jié)點(diǎn)S為基礎(chǔ),在OPEN表中添加初始位置節(jié)點(diǎn)S,令OPEN=f(S),CLOSED=(),其中CLOSED為空,f(S)=0+h(S),其中h(S)為初始化節(jié)點(diǎn)S到當(dāng)前節(jié)點(diǎn)的代價(jià)值。

2)進(jìn)行循環(huán)判斷OPEN表是否為空,若該表為空,則表明無(wú)法尋找到目標(biāo),結(jié)束路徑規(guī)劃;若表不為空,進(jìn)行路徑規(guī)劃直至到達(dá)目標(biāo)位置。

3)選擇f值最小的一個(gè)節(jié)點(diǎn)進(jìn)行標(biāo)記,若該節(jié)點(diǎn)未在OPEN表中返回第一步,且該節(jié)點(diǎn)存在,則把OPEN表中該值刪除,存入CLOSED表。

4)若最佳節(jié)點(diǎn)為目標(biāo)點(diǎn),表明路徑規(guī)劃成功,算法結(jié)束,檢測(cè)初始節(jié)點(diǎn)到目標(biāo)點(diǎn)間的全部節(jié)點(diǎn),形成最佳路徑。

5)假如最佳節(jié)點(diǎn)m并非目標(biāo)點(diǎn),將最佳節(jié)點(diǎn)m進(jìn)行擴(kuò)展。擴(kuò)展后的點(diǎn)形成點(diǎn)集M=(m),在該節(jié)點(diǎn)集中的節(jié)點(diǎn)不包含初始節(jié)點(diǎn)的子節(jié)點(diǎn)。該節(jié)點(diǎn)集中的節(jié)點(diǎn)為最佳節(jié)點(diǎn)m的子節(jié)點(diǎn)。

6)對(duì)M進(jìn)行判斷,若節(jié)點(diǎn)m不在OPEN表和CLOSED表中,將其添加到OPEN表中。

7)若在OPEN表中存在重復(fù)的節(jié)點(diǎn)w和k,而且兩節(jié)點(diǎn)存在g(w)

8)查看CLOSED表是否重復(fù)。

9)將OPEN表中節(jié)點(diǎn)根據(jù)f值由小到大排序。

10)返回第2)步進(jìn)行循環(huán),直至獲得最佳的機(jī)械臂避障軌跡規(guī)劃結(jié)果,將最終輸出的避障路徑作為對(duì)避障軌跡的檢測(cè)結(jié)果。

改進(jìn)A*算法流程如圖4所示。以公式(8)的成本函數(shù)f(l)作為目標(biāo)函數(shù),以1.4節(jié)運(yùn)算過程的參數(shù)為變量,評(píng)估當(dāng)前狀態(tài)到目標(biāo)狀態(tài)的代價(jià)。

圖4 改進(jìn)A*算法流程

2 仿真實(shí)驗(yàn)

2.1 實(shí)驗(yàn)準(zhǔn)備

為驗(yàn)證本文方法的應(yīng)用效果,以某6自由度采摘型機(jī)械臂為實(shí)驗(yàn)對(duì)象,其臂展長(zhǎng)為13 700 mm、質(zhì)量為130 kg、重復(fù)精度為0.08 mm、最大負(fù)載為6 kg。在實(shí)驗(yàn)室內(nèi)模擬該機(jī)械臂應(yīng)用本文方法后的避障軌跡檢測(cè)效果。

設(shè)置一個(gè)模擬的果園環(huán)境,包括植物、樹木、地形等元素,為了便于實(shí)驗(yàn),將其用矩形、圓形、扇形、梯形和不規(guī)則形狀替代,以測(cè)試機(jī)械臂的避障能力。采用S2L激光雷達(dá)傳感器感知和檢測(cè)機(jī)械臂周圍的障礙物。傳感器的具體參數(shù)為:測(cè)量半徑0.05～18.00 m,采樣頻率32 kHz,掃描頻率10 Hz,角度分辨率0.12°。設(shè)置地圖網(wǎng)格分辨率為0.01,啟發(fā)式函數(shù)權(quán)重參數(shù)為0.25,障礙物網(wǎng)格的代價(jià)為0.3,網(wǎng)格連接性為四鄰域,運(yùn)動(dòng)代價(jià)為0.24。

2.2 機(jī)械臂C-空間映射驗(yàn)證

為驗(yàn)證本文方法使障礙物映射到C-空間的結(jié)果,對(duì)某障礙物利用本文方法進(jìn)行計(jì)算,得出障礙物映射到C-空間的結(jié)果如圖5所示。由圖5可以看出,實(shí)際障礙物映射到C-空間后在(0,0)位置呈現(xiàn)旋轉(zhuǎn)對(duì)稱狀態(tài),這是由于R-R機(jī)械臂沿y軸運(yùn)動(dòng)時(shí)根據(jù)障礙物的位置進(jìn)行了一定角度的旋轉(zhuǎn)所造成的。y=1 000表示在C-空間中機(jī)械臂的y坐標(biāo)為1 000。

圖5 障礙物C-空間映射結(jié)果(y=1 000)

改變y軸與障礙物的位置,障礙物在C-空間映射的結(jié)果如圖6所示。由圖6可以看出,隨著障礙物與坐標(biāo)原點(diǎn)的距離越來(lái)越小,在C-空間映射的障礙范圍則越來(lái)越大,機(jī)械臂能夠活動(dòng)的空間則越來(lái)越小,這與實(shí)際情況一致。

圖6 障礙物C-空間映射結(jié)果

2.3 軌跡檢測(cè)

在機(jī)械臂抓取范圍內(nèi)設(shè)置矩形、圓形兩種障礙物,并設(shè)定機(jī)械臂目標(biāo)點(diǎn),進(jìn)行機(jī)械臂避障測(cè)試。設(shè)定障礙物位置及機(jī)械臂初始位置和目標(biāo)位置,機(jī)械臂避障移動(dòng)中,關(guān)節(jié)角度變化如圖7所示。由圖7可以看出,機(jī)械臂從初始位置移動(dòng)到目標(biāo)位置過程中,檢測(cè)到θ2、θ3兩個(gè)角度經(jīng)過了多次微小的角度變化,可以看出,本文方法能夠有效實(shí)現(xiàn)機(jī)械臂避障軌跡檢測(cè)。

圖7 機(jī)械臂關(guān)節(jié)角度變化

2.4 避障效果

為驗(yàn)證本文方法實(shí)際應(yīng)用效果,多次更換障礙物并改變障礙物位置,統(tǒng)計(jì)機(jī)械臂是否能順利到達(dá)目標(biāo)位置,結(jié)果見表2。由表2可以看出,運(yùn)用本文方法進(jìn)行避障軌跡檢測(cè)能夠順利地完成機(jī)械臂的避障,雖然在第五次試驗(yàn)中,機(jī)械臂手臂與障礙物有接觸,但是并未對(duì)機(jī)械臂造成任何傷害,二者接觸后機(jī)械臂便立刻停止了運(yùn)動(dòng)。

表2 不同情況下機(jī)械臂避障效果

2.5 避障效率

為驗(yàn)證本文方法的避障效率,運(yùn)用文獻(xiàn)[5]～[7]方法分別進(jìn)行了100次3種障礙物的避障測(cè)試,與本文方法測(cè)試結(jié)果進(jìn)行對(duì)比,結(jié)果如圖8所示。由圖8可以看出,避障路徑規(guī)劃開始20次時(shí)文獻(xiàn)[7]方法路徑規(guī)劃的成功率高于文獻(xiàn)[5]和文獻(xiàn)[6]方法,但隨著實(shí)驗(yàn)次數(shù)的增加,文獻(xiàn)[7]方法避障路徑的規(guī)劃成功率反而逐漸降低,文獻(xiàn)[5]和文獻(xiàn)[6]方法的路徑規(guī)劃成功率逐漸上升,到實(shí)驗(yàn)結(jié)束時(shí),文獻(xiàn)[6]方法的路徑規(guī)劃成功率高于其余兩種方法。而本文方法在實(shí)驗(yàn)全過程中路徑規(guī)劃成功率均高于其他3種方法,雖然路徑規(guī)劃的成功率有一定的波動(dòng),但一直大于98%。

圖8 不同方法的避障結(jié)果

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

通過實(shí)驗(yàn)可以看出,采用C-空間進(jìn)行障礙物映射可以真實(shí)地反映障礙物的形狀,即距離越近障礙物越大。使用改進(jìn)后的A*算法能夠更加準(zhǔn)確地進(jìn)行路徑規(guī)劃,在存在多種類型障礙物無(wú)阻擋的情況下機(jī)械臂也能順利到達(dá)目標(biāo)位置。在確定起始位置和目標(biāo)位置后,能夠準(zhǔn)確獲得機(jī)械臂的移動(dòng)角度測(cè)量結(jié)果。