深圳市眾為興數(shù)控技術(shù)有限公司事業(yè)二部 汪洋
摘要：針對(duì)SCARA機(jī)器人在水平面內(nèi)躲避障礙物的軌跡規(guī)劃問題，基于幾何學(xué)與人工智能學(xué)理論，提出一種智能控制算法，用于生成SCARA機(jī)器人避障路徑上的關(guān)節(jié)坐標(biāo)值序列。仿真結(jié)果表明，該控制算法能快速有效地實(shí)現(xiàn)SCARA機(jī)器人在X-Y平面內(nèi)的二維避障，具有較強(qiáng)的工程實(shí)用性。
關(guān)鍵詞：SCARA機(jī)器人； 二維避障； 智能控制； A*算法
1． 引 言
當(dāng)前，SCARA機(jī)器人廣泛應(yīng)用于電子、汽車、塑料、食品等工業(yè)領(lǐng)域，其主要職能是完成搬運(yùn)和裝配工作。隨著加工工藝的復(fù)雜性與精確性不斷提升，SCARA機(jī)器人在流水線上經(jīng)常需要與其他工業(yè)設(shè)備協(xié)同作業(yè)，難免會(huì)出現(xiàn)與障礙物發(fā)生碰撞的危險(xiǎn)。因此，對(duì)工作范圍內(nèi)存在障礙物的SCARA機(jī)器人的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃便顯得尤為重要。
軌跡規(guī)劃(Path Planning)是指給定環(huán)境的障礙物條件，以及起始點(diǎn)和目標(biāo)點(diǎn)位置，要求選擇一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑，使SCARA機(jī)器人能安全、無碰撞地通過所有的障礙物。這種自主地躲避障礙物并完成作業(yè)任務(wù)是SCARA機(jī)器人研究中的一個(gè)重要內(nèi)容。軌跡規(guī)劃的實(shí)質(zhì)是預(yù)先給定SCARA機(jī)器人作業(yè)路徑上的關(guān)節(jié)坐標(biāo)值序列，使其沿預(yù)定路徑運(yùn)動(dòng)。對(duì)于四軸機(jī)器人而言，關(guān)節(jié)3的縱向運(yùn)動(dòng)與關(guān)節(jié)4的帶夾具運(yùn)動(dòng)均具有較強(qiáng)的靈活性，可以根據(jù)實(shí)際情況用簡(jiǎn)單的運(yùn)動(dòng)指令實(shí)時(shí)控制以躲避障礙物；而關(guān)節(jié)1與關(guān)節(jié)2在水平面內(nèi)的關(guān)聯(lián)運(yùn)動(dòng)在很大程度上決定了SCARA機(jī)器人的主要位姿，是障礙物避碰的主導(dǎo)因素。所以，合理規(guī)劃關(guān)節(jié)1與關(guān)節(jié)2在水平面內(nèi)的關(guān)節(jié)坐標(biāo)值序列是實(shí)現(xiàn)避障的前提和關(guān)鍵。本文即是針對(duì)SCARA機(jī)器人的二維避障問題，基于幾何學(xué)與人工智能學(xué)理論，提出了一種智能控制算法并加以仿真驗(yàn)證。
2． 二維避障智能控制算法設(shè)計(jì)
工業(yè)現(xiàn)場(chǎng)的實(shí)際障礙物雖具有多種平面形狀，但任何形狀的障礙物均可用其外接圓柱體包圍。為簡(jiǎn)化對(duì)問題的描述又不失一般性，文中假定實(shí)際障礙物為圓柱體結(jié)構(gòu)，映射到X-Y水平面上即為一圓形區(qū)域。本算法的總體思路為：先將關(guān)節(jié)1與關(guān)節(jié)2構(gòu)成的關(guān)節(jié)空間q1-q2柵格化為有限個(gè)小方塊，并對(duì)每個(gè)小方塊編號(hào)，設(shè)定坐標(biāo)值；然后將直角坐標(biāo)系X-Y下的圓形障礙區(qū)域轉(zhuǎn)化為關(guān)節(jié)空間中不可達(dá)的小方塊群，求出它們的編號(hào)，并給可達(dá)矩陣賦值；再采用人工智能理論中具有啟發(fā)式搜索的A*算法在關(guān)節(jié)空間中求出避障路徑點(diǎn)的編號(hào)序列；最后將該編號(hào)序列轉(zhuǎn)換為對(duì)應(yīng)的關(guān)節(jié)坐標(biāo)值序列，關(guān)節(jié)1與關(guān)節(jié)2依次取對(duì)應(yīng)的關(guān)節(jié)角度值運(yùn)動(dòng)即可實(shí)現(xiàn)二維避障控制.
2.1 關(guān)節(jié)空間q1-q2柵格化
假定SCARA機(jī)器人關(guān)節(jié)1和關(guān)節(jié)2的運(yùn)動(dòng)范圍均為[-120 ,120 ]，取細(xì)分精度為6 (可根據(jù)實(shí)際情況靈活選擇細(xì)分精度)，可將關(guān)節(jié)坐標(biāo)軸q1、q2均劃分為[120-(-120)] / 6 = 40條相等的小線段，分別在每條小線段的端點(diǎn)作q1軸與q2軸的平行線，所有平行線相交，可將關(guān)節(jié)空間平面劃分為40*40 = 1600個(gè)面積相等的小方塊，從而實(shí)現(xiàn)了關(guān)節(jié)空間q1-q2的柵格化，如圖3所示。
然后，對(duì)每個(gè)小方塊進(jìn)行編號(hào)并設(shè)定關(guān)節(jié)坐標(biāo)值，按照從左至右、從下至上依次增大的原則，編號(hào)取[1,1600]之間的整數(shù)，在圖3中，左下角小方塊編號(hào)為1，右上角編號(hào)為1600，1號(hào)小方塊正上方的小方塊編號(hào)為41，依此類推，可得出所有小方塊的唯一相異編號(hào)值。
最后對(duì)每個(gè)小方塊賦坐標(biāo)值，為使坐標(biāo)值分布均勻，取每個(gè)小方塊的中心坐標(biāo)(q1,q2)為其坐標(biāo)值。則1號(hào)小方塊的坐標(biāo)值為(-117 ,-117 )，1600號(hào)小方塊的坐標(biāo)值為(117 ,117 )，41號(hào)小方塊的坐標(biāo)值為(-117 ,-111 )，依此類推，可得出所有小方塊的唯一相異坐標(biāo)值。
2.2 確定障礙區(qū)域
在圖1中的直角坐標(biāo)系下，O為坐標(biāo)原點(diǎn)，OA2與A2B2分別為SCARA機(jī)器人的大臂與小臂，由右手螺旋法則易知，實(shí)線OA2B2表示本體處于關(guān)節(jié)坐標(biāo)為(-117 ,-117 )的位姿，對(duì)應(yīng)關(guān)節(jié)空間中的1號(hào)小方塊；虛線OMN表示本體與X軸正向重合時(shí)的位姿；虛線OA1B1則表示本體處于關(guān)節(jié)坐標(biāo)為(117 ,117 )的位姿，對(duì)應(yīng)關(guān)節(jié)空間中的1600號(hào)小方塊。圖中藍(lán)色與黑色虛線包圍的空間即為本體的活動(dòng)區(qū)域，在該區(qū)域內(nèi)假設(shè)存在兩個(gè)圓形障礙物圓K1與圓K2，兩圓半徑均已知。由于本算法要求在關(guān)節(jié)坐標(biāo)系下規(guī)劃避障路徑，所以要先將直角坐標(biāo)系下的障礙區(qū)域轉(zhuǎn)換為關(guān)節(jié)坐標(biāo)系下對(duì)應(yīng)的編號(hào)群。
當(dāng)大臂處于OG1與OJ1之間時(shí)，小臂可能與圓K1發(fā)生碰撞。僅考慮右手關(guān)節(jié)坐標(biāo)系時(shí)，這一區(qū)間內(nèi)的任一q1值均有兩個(gè)q2值：q20與q21與之對(duì)應(yīng)。其中q20為碰撞開始時(shí)的關(guān)節(jié)2角度，q21為碰撞結(jié)束時(shí)的關(guān)節(jié)2角度。兩者相差為類似于角Q1G1F1的兩倍的某個(gè)角度值。采用幾何學(xué)方法，設(shè)位姿OG1F1為小臂末端與圓K1發(fā)生碰撞的一個(gè)極限狀態(tài)，而OG1Q1為小臂延長(zhǎng)線與圓K1相切時(shí)的位姿，切點(diǎn)為Q1。由圖中所示的幾何關(guān)系，可求出角Q1G1F1的角度值，同時(shí)在q20的基礎(chǔ)上加上兩倍的Q1G1F1的角度值，即為q21。
左手關(guān)節(jié)坐標(biāo)系下的情況完全相似，不再驁述。據(jù)此可求出在q1在可能碰撞區(qū)間內(nèi)的每個(gè)q2的最大最小值，從而確定障礙物K1在關(guān)節(jié)空間中的邊界值。
同法也可求出圓形障礙物K2在關(guān)節(jié)空間中的邊界值。
根據(jù)連通性原理，直角坐標(biāo)中連通閉合的圓形區(qū)域在關(guān)節(jié)空間中必然也是連通閉合的區(qū)域。只要將求出的邊界值連接起來，閉合的范圍就是關(guān)節(jié)坐標(biāo)系下的障礙區(qū)域，而這些區(qū)域覆蓋到的小方塊群即為圓形障礙區(qū)域映射到關(guān)節(jié)空間內(nèi)的障礙區(qū)域。
在算法中需設(shè)定一個(gè)可達(dá)矩陣，其維數(shù)為1600*1600，各元素取0或1，0表示兩個(gè)小方塊之間不可達(dá)，1表示可達(dá)。將障礙區(qū)域編號(hào)群中的每個(gè)編號(hào)值在可達(dá)矩陣中對(duì)應(yīng)的行和列均取為0。同時(shí)為了保證每個(gè)小方塊只能與其緊鄰的八個(gè)小方塊相通(關(guān)節(jié)空間四角的小方塊只與其緊鄰的三個(gè)小方塊相通，四邊的小方塊只與其緊鄰的五個(gè)小方塊相通)，而不出現(xiàn)跳步，還應(yīng)在可達(dá)矩陣的相應(yīng)位置賦0值。
2.3 搜索避障路徑編號(hào)序列
在關(guān)節(jié)空間中搜索避障路徑是本算法的根本所在，而搜索方式又具有決定性影響，本算法中采用A*算法實(shí)現(xiàn)搜索。A*算法是人工智能中的一種典型的啟發(fā)式搜索算法，在實(shí)時(shí)系統(tǒng)、智能控制等方面應(yīng)用較廣，其智能性主要體現(xiàn)在僅需已知起始點(diǎn)S、目標(biāo)點(diǎn)G的編號(hào)與障礙區(qū)域的編號(hào)群，無需外部條件與人為判斷即可實(shí)現(xiàn)最優(yōu)避障路徑的自動(dòng)搜索。
本算法中先建立兩個(gè)表OpenList與ClosedList，分別存放搜索過程中已生成而未擴(kuò)展的小方塊編號(hào)與擴(kuò)展后的小方塊編號(hào)；再定義估價(jià)函數(shù) f(n) = g(n) + h(n) 用于決定搜索方向和擴(kuò)展對(duì)象，其中g(shù)(n)表示起始點(diǎn)S與當(dāng)前小方塊N之間的關(guān)節(jié)坐標(biāo)距離，h(n)表示當(dāng)前小方塊N與目標(biāo)點(diǎn)G之間的距離；在每一步搜索時(shí)取對(duì)應(yīng)f(n)值最小的小方塊進(jìn)行擴(kuò)展，并將該路徑的編號(hào)依次存放在表ClosedList中；當(dāng)搜索到的當(dāng)前小方塊N的編號(hào)等于目標(biāo)點(diǎn)G的編號(hào)時(shí)結(jié)束搜索；最后將ClosedList中的編號(hào)回溯即為所求的避障路徑編號(hào)序列。
2.4 確定避障路徑關(guān)節(jié)坐標(biāo)值序列
由關(guān)節(jié)空間柵格化過程可知，小方塊編號(hào)值與關(guān)節(jié)坐標(biāo)值是唯一對(duì)應(yīng)的，求出避障路徑編號(hào)序列后，只需將編號(hào)依次與關(guān)節(jié)坐標(biāo)對(duì)應(yīng)，即可求出最終的避障路徑關(guān)節(jié)坐標(biāo)值序列。當(dāng)SCARA機(jī)器人的關(guān)節(jié)1和關(guān)節(jié)2依次取該序列中的每一個(gè)坐標(biāo)值運(yùn)動(dòng)時(shí)，反映在直角坐標(biāo)系下即為從起始位姿到目標(biāo)位姿的二維智能避障全過程。
3． 仿真實(shí)例
以深圳市眾為興數(shù)控技術(shù)有限公司自主研發(fā)的SCARA機(jī)器人MR-401-600為例，進(jìn)行仿真驗(yàn)證。MR-401-600的大臂、小臂長(zhǎng)度分別為350mm、250mm，關(guān)節(jié)1和關(guān)節(jié)2的取值范圍及空間劃分參照2.1小節(jié)中的數(shù)據(jù)，設(shè)關(guān)節(jié)空間中的起始點(diǎn)S為1號(hào)小方塊，目標(biāo)點(diǎn)G為1600號(hào)小方塊。圓形障礙物K1、K2在直角坐標(biāo)系下的圓心坐標(biāo)分別為(350,400)、(200,-420)，半徑分別為80mm、60mm。將可能發(fā)生碰撞的關(guān)節(jié)1取值區(qū)間劃分為180個(gè)離散點(diǎn)，以充分逼近關(guān)節(jié)空間內(nèi)的障礙區(qū)域。采用MATLAB語言進(jìn)行仿真試驗(yàn)，本二維避障智能控制算法進(jìn)行路徑規(guī)劃的搜索時(shí)間為1.5449秒，仿真結(jié)果如圖2至圖4所示。其中，圖2為左右手系下的障礙區(qū)域；圖3為關(guān)節(jié)坐標(biāo)空間中二維避障路徑的規(guī)劃結(jié)果，左下角的綠色五角星為起始點(diǎn)，右上角的紅色五角星為目標(biāo)點(diǎn)，紅色點(diǎn)陣為障礙區(qū)域，綠色點(diǎn)陣連線為避障路徑，表示MR-401-600從大小臂均為負(fù)最大角度的位姿開始運(yùn)動(dòng)并依次躲避兩個(gè)圓形障礙物，到達(dá)大小臂均為正最大角度的位姿；圖4為MR-401-600在直角坐標(biāo)系下實(shí)現(xiàn)二維避障的動(dòng)態(tài)演示過程，圖中黑色為大臂每時(shí)刻狀態(tài)，藍(lán)色為小臂每時(shí)刻狀態(tài)，綠色虛線為小臂末端的實(shí)際運(yùn)動(dòng)軌跡。仿真結(jié)果表明，SCARA機(jī)器人MR-401-600能成功躲避障礙物并完成預(yù)定的路徑任務(wù)，實(shí)現(xiàn)從起始點(diǎn)到目標(biāo)點(diǎn)的無碰撞運(yùn)動(dòng)。
4. 結(jié) 論
本文提出的二維避障智能控制算法能有效規(guī)劃出優(yōu)化路徑，成功解決SCARA機(jī)器人在平面內(nèi)的障礙物避碰問題，具有一定的理論研究?jī)r(jià)值與較強(qiáng)的實(shí)際應(yīng)用潛力。需要指出的是，由于本算法基于圖形中小方塊的搜索，關(guān)節(jié)空間的細(xì)分精度對(duì)算法的效率影響較大。細(xì)分精度過高會(huì)降低搜索速度，過少則會(huì)降低搜索精度。因此，用戶應(yīng)根據(jù)工藝要求與實(shí)際情況適中取值，以使算法發(fā)揮其最大效用。
參考文獻(xiàn)
[1] 黃獻(xiàn)龍，梁斌等. 機(jī)器人避碰規(guī)劃綜述[J]. 控制工程. 2000(6)， pp. 28-35.
[2] 丁富強(qiáng)， 費(fèi)燕瓊等. 雙臂SCARATES機(jī)器人實(shí)時(shí)無碰撞運(yùn)動(dòng)規(guī)劃[J]. 上海交通大學(xué)學(xué)報(bào). 37(11)， pp. 1690-1693， 2003.
[3] 錢東海， 馬毅瀟等. 雙臂SCARA型機(jī)器人C空間快速建立算法[J]. 機(jī)械科學(xué)與技術(shù). 18(1)， pp. 65-68， 1999.
