基于智能平面搜索的四軸機(jī)械臂避障規(guī)劃

汪洋，賀小鳳，郭晨

（1. 深圳信息職業(yè)技術(shù)學(xué)院 交通與環(huán)境學(xué)院，廣東 深圳 518172；2. 大連海事大學(xué)信息科學(xué)技術(shù)學(xué)院，遼寧 大連 116026）

四軸機(jī)械臂是自動化加工生產(chǎn)中不可或缺的因素，正被廣泛應(yīng)用于分配、裝載、包裝、安放以及碼垛等作業(yè)流程，涵蓋了電子、汽車、塑料、食品等眾多領(lǐng)域[1]。

隨著加工工藝的復(fù)雜性與精確性不斷提升，四軸機(jī)械臂在流水線上經(jīng)常需要與其他工業(yè)設(shè)備協(xié)同作業(yè)，難免會出現(xiàn)與障礙物發(fā)生碰撞的危險，存在較大的安全隱患。因此，合理規(guī)劃四軸機(jī)械臂末端的運(yùn)動路徑，規(guī)避工作范圍內(nèi)的障礙物，對于安全高效地完成作業(yè)任務(wù)便顯得尤為重要。

路徑規(guī)劃是指給定環(huán)境的障礙物條件，以及四軸機(jī)械臂末端運(yùn)動的起始點和目標(biāo)點位置，要求選擇一條從起始點到目標(biāo)點的路徑，使四軸機(jī)械臂末端能安全地規(guī)避所有的障礙物。這種自主避障并完成作業(yè)任務(wù)是四軸機(jī)械臂研究中的一個重要內(nèi)容[2]。而路徑規(guī)劃的實質(zhì)則是預(yù)先給定四軸機(jī)械臂末端作業(yè)軌跡上的關(guān)節(jié)坐標(biāo)值序列，使其沿預(yù)定路徑運(yùn)動。

如圖1所示，四軸機(jī)械臂從其底座至作業(yè)末端分別有四個自由度，依次為關(guān)節(jié)1至關(guān)節(jié)4。其中，關(guān)節(jié)1、2、4為旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)3為滑動關(guān)節(jié)。由于關(guān)節(jié)3的縱向運(yùn)動與關(guān)節(jié)4的末端帶夾具運(yùn)動均具有較強(qiáng)的靈活性，可以根據(jù)實際情況用簡單的上下運(yùn)動指令和旋轉(zhuǎn)運(yùn)動指令實時控制以規(guī)避障礙物。而關(guān)節(jié)1與關(guān)節(jié)2在水平面內(nèi)的關(guān)聯(lián)運(yùn)動在很大程度上決定了四軸機(jī)械臂的主要位姿，兩者的耦合是影響障礙物避碰的主導(dǎo)因素[3]。

圖1 四軸機(jī)械臂Fig.1 Four-ax manipulator

所以，合理規(guī)劃關(guān)節(jié)1與關(guān)節(jié)2在水平面內(nèi)的關(guān)節(jié)坐標(biāo)值序列，便成為四軸機(jī)械臂成功避障的關(guān)鍵和前提。本文即是將人工智能理論與平面幾何學(xué)相結(jié)合，基于智能平面搜索提出了一種避障規(guī)劃算法，用于解決四軸機(jī)械臂在X-Y平面內(nèi)的二維避障問題，并加以仿真驗證。

1 避障規(guī)劃算法的總體思路

從俯視圖投影的角度來看，工業(yè)現(xiàn)場的實際障礙物雖具有多種平面形狀，但任何形狀的障礙物投影均可被其最小外接圓包圍。為簡化對問題的描述又不失一般性，文中假定實際障礙物為圓柱體結(jié)構(gòu)，映射到X-Y水平面上即為一圓形區(qū)域[4]。

本算法的總體思路為：先將關(guān)節(jié)1與關(guān)節(jié)2構(gòu)成的關(guān)節(jié)空間q1-q2柵格化為有限個小方塊，并對每個小方塊編號，設(shè)定坐標(biāo)值；然后根據(jù)平面幾何學(xué)知識將直角坐標(biāo)系X-Y下的圓形障礙區(qū)域轉(zhuǎn)化為關(guān)節(jié)空間中不可達(dá)的小方塊群，求出它們的編號，并給可達(dá)矩陣賦值；再采用人工智能理論中具有啟發(fā)式搜索的A*算法在關(guān)節(jié)空間中求出避障路徑點的編號序列；最后將該編號序列轉(zhuǎn)換為對應(yīng)的關(guān)節(jié)坐標(biāo)值序列，關(guān)節(jié)1與關(guān)節(jié)2依次取對應(yīng)的關(guān)節(jié)角度值運(yùn)動即可實現(xiàn)對四軸機(jī)械臂的二維避障控制。

2 柵格化關(guān)節(jié)空間q1-q2

根據(jù)工業(yè)現(xiàn)場的實際經(jīng)驗，假定四軸機(jī)械臂關(guān)節(jié)1和關(guān)節(jié)2的運(yùn)動范圍均為[-120°,120°]，取細(xì)分精度為6°（可根據(jù)實際情況靈活選擇細(xì)分精度），可將關(guān)節(jié)坐標(biāo)軸q1、q2均劃分為[120-(-120)]/ 6 = 40條相等的小線段，分別在每條小線段的端點作q1軸與q2軸的平行線，所有平行線相交，可將關(guān)節(jié)空間平面劃分為40*40 = 1600個面積相等的小方塊，從而實現(xiàn)了關(guān)節(jié)空間q1-q2的柵格化，如圖4所示。

然后，對每個小方塊進(jìn)行編號并設(shè)定關(guān)節(jié)坐標(biāo)值，按照從左至右、從下至上依次增大的原則，編號取[1,1600]之間的整數(shù)，在圖4中，左下角小方塊編號為1，右上角編號為1600，1號小方塊正上方的小方塊編號為41，依此類推，可得出所有小方塊的唯一相異編號值。

最后，對每個小方塊賦坐標(biāo)值，為使坐標(biāo)值分布均勻，取每個小方塊的中心坐標(biāo)(q1,q2)為其坐標(biāo)值。則1號小方塊的坐標(biāo)值為(-117°,-117°)，1600號小方塊的坐標(biāo)值為(117°,117°)，41號小方塊的坐標(biāo)值為(-117°,-111°)，依此類推，可得出所有小方塊的唯一相異坐標(biāo)值。

3 確定障礙物所在區(qū)域

在圖2中的直角坐標(biāo)系下，O為坐標(biāo)原點，OA2與A2B2分別為四軸機(jī)械臂的大臂與小臂。

圖2 四軸機(jī)械臂的工作范圍與障礙物位置Fig.2 Working areas and obstacle positions of four-ax manipulator

由右手螺旋法則易知，實線OA2B2表示四軸機(jī)械臂處于關(guān)節(jié)坐標(biāo)為(-117 ,-117 )的位姿，對應(yīng)關(guān)節(jié)空間中的1號小方塊；虛線OMN表示四軸機(jī)械臂與X軸正向重合時的位姿；虛線OA1B1則表示四軸機(jī)械臂處于關(guān)節(jié)坐標(biāo)為(117 ,117 )的位姿，對應(yīng)關(guān)節(jié)空間中的1600號小方塊。圖中藍(lán)色與黑色虛線包圍的空間即為四軸機(jī)械臂的活動區(qū)域，在該區(qū)域內(nèi)假設(shè)存在兩個圓形障礙物圓K1與圓K2，兩圓半徑均已知。由于本算法要求在關(guān)節(jié)坐標(biāo)系下規(guī)劃避障路徑，所以要先將直角坐標(biāo)系下的障礙區(qū)域轉(zhuǎn)換為關(guān)節(jié)坐標(biāo)系下對應(yīng)的編號群。

由平面幾何學(xué)知識可知，當(dāng)大臂處于OG1與OJ1之間時，小臂可能與圓K1發(fā)生碰撞。僅考慮右手關(guān)節(jié)坐標(biāo)系時，這一區(qū)間內(nèi)的任一q1值均有兩個q2值：q20與q21與之對應(yīng)。其中q20為碰撞開始時的關(guān)節(jié)2角度，q21為碰撞結(jié)束時的關(guān)節(jié)2角度。兩者相差為類似于角Q1G1F1的兩倍的某個角度值。采用幾何學(xué)方法，設(shè)位姿OG1F1為小臂末端與圓K1發(fā)生碰撞的一個極限狀態(tài)，而OG1Q1為小臂延長線與圓K1相切時的位姿，切點為Q1。由圖中所示的幾何關(guān)系，可求出角Q1G1F1的角度值，同時在q20的基礎(chǔ)上加上兩倍的Q1G1F1的角度值，即為q21。

左手關(guān)節(jié)坐標(biāo)系下的情況完全相似，不再驁述。據(jù)此可求出在q1在可能碰撞區(qū)間內(nèi)的每個q2的最大最小值，從而確定障礙物K1在關(guān)節(jié)空間中的邊界值。

同法也可求出圓形障礙物K2在關(guān)節(jié)空間中的邊界值。

根據(jù)連通性原理，直角坐標(biāo)中連通閉合的圓形區(qū)域在關(guān)節(jié)空間中必然也是連通閉合的區(qū)域[5]。只要將求出的邊界值連接起來，閉合的范圍就是關(guān)節(jié)坐標(biāo)系下的障礙區(qū)域，而這些區(qū)域覆蓋到的小方塊群即為圓形障礙區(qū)域映射到關(guān)節(jié)空間內(nèi)的障礙區(qū)域。

在算法中需設(shè)定一個可達(dá)矩陣，其維數(shù)為1600*1600，各元素取0或1，0表示兩個小方塊之間不可達(dá)，1表示可達(dá)。將障礙區(qū)域編號群中的每個編號值在可達(dá)矩陣中對應(yīng)的行和列均取為0。同時為了保證每個小方塊只能與其緊鄰的八個小方塊相通（關(guān)節(jié)空間四角的小方塊只與其緊鄰的三個小方塊相通，四邊的小方塊只與其緊鄰的五個小方塊相通），而不出現(xiàn)跳步，還應(yīng)在可達(dá)矩陣的相應(yīng)位置賦0值。

4 搜索避障路徑編號序列

在關(guān)節(jié)空間中搜索避障路徑是本算法的根本所在，而搜索方式又具有決定性影響，本算法中采用A*算法實現(xiàn)搜索[6]。

A*算法是人工智能理論中一種典型的啟發(fā)式搜索算法，在實時系統(tǒng)、智能控制等方面應(yīng)用較廣，其智能性主要體現(xiàn)在僅需已知起始點S、目標(biāo)點G的編號與障礙區(qū)域的編號群，無需外部條件與人為判斷即可實現(xiàn)最優(yōu)避障路徑的自動搜索。

本算法中先建立兩個表OpenList與ClosedList，分別存放搜索過程中已生成而未擴(kuò)展的小方塊編號與擴(kuò)展后的小方塊編號；再定義估價函數(shù) f(n) = g(n)+ h(n) 用于決定搜索方向和擴(kuò)展對象，其中g(shù)(n)表示起始點S與當(dāng)前小方塊N之間的關(guān)節(jié)坐標(biāo)距離，h(n)表示當(dāng)前小方塊N與目標(biāo)點G之間的距離；在每一步搜索時取對應(yīng)f(n)值最小的小方塊進(jìn)行擴(kuò)展，并將該路徑的編號依次存放在表ClosedList中；當(dāng)搜索到的當(dāng)前小方塊N的編號等于目標(biāo)點G的編號時結(jié)束搜索；最后將ClosedList中的編號回溯即為所求的避障路徑編號序列。

5 確定避障路徑關(guān)節(jié)坐標(biāo)值序列

由關(guān)節(jié)空間柵格化過程可知，小方塊編號值與關(guān)節(jié)坐標(biāo)值是唯一對應(yīng)的，求出避障路徑編號序列后，只需將編號依次與關(guān)節(jié)坐標(biāo)對應(yīng)，即可求出最終的避障路徑關(guān)節(jié)坐標(biāo)值序列。當(dāng)四軸機(jī)械臂的關(guān)節(jié)1和關(guān)節(jié)2依次取該序列中的每一個坐標(biāo)值運(yùn)動時，反映在直角坐標(biāo)系X-Y下即為從起始位姿到目標(biāo)位姿的二維智能避障全過程。

6 仿真驗證實例

本文以深圳市眾為興技術(shù)股份有限公司自主研發(fā)的四軸機(jī)械臂SCARA為例（如圖1所示），進(jìn)行仿真驗證。

SCARA的大臂、小臂長度分別為350mm、250mm，關(guān)節(jié)1和關(guān)節(jié)2的取值范圍及空間劃分參照2.1小節(jié)中的數(shù)據(jù)，設(shè)關(guān)節(jié)空間中的起始點S為1號小方塊，目標(biāo)點G為1600號小方塊。圓形障礙物K1、K2在直角坐標(biāo)系下的圓心坐標(biāo)分別為(350,400)、(200,-420)，半徑分別為80mm、60mm。將可能發(fā)生碰撞的關(guān)節(jié)1取值區(qū)間劃分為180個離散點，以充分逼近關(guān)節(jié)空間內(nèi)的障礙區(qū)域。

采用MATLAB語言進(jìn)行仿真試驗，本避障規(guī)劃算法進(jìn)行路徑規(guī)劃的搜索時間為1.5449秒，仿真結(jié)果如圖3至圖5所示。

其中，圖3為左右手系下的障礙區(qū)域；圖4為關(guān)節(jié)坐標(biāo)空間中二維避障路徑的規(guī)劃結(jié)果，左下角的綠色五角星為起始點，右上角的紅色五角星為目標(biāo)點，紅色點陣為障礙區(qū)域，綠色點陣連線為避障路徑，表示SCARA從大小臂均為負(fù)最大角度的位姿開始運(yùn)動并依次規(guī)避兩個圓形障礙物，到達(dá)大小臂均為正最大角度的位姿；圖5為SCARA在直角坐標(biāo)系下實現(xiàn)二維避障的動態(tài)演示過程，圖中黑色為大臂每時刻狀態(tài)，藍(lán)色為小臂每時刻狀態(tài)，綠色虛線為小臂末端的實際運(yùn)動軌跡。

仿真結(jié)果表明，四軸機(jī)械臂SCARA能成功規(guī)避障礙物并完成預(yù)定的路徑任務(wù)，實現(xiàn)從起始點到目標(biāo)點的無碰撞運(yùn)動。

圖3 雙手關(guān)節(jié)坐標(biāo)系下的障礙物區(qū)域Fig.3 Obstacle region in the double-hand joint coordinate frame

圖4 四軸機(jī)械臂在關(guān)節(jié)坐標(biāo)系下的二維避障路徑Fig.4 Two-dimension obstacle-avoidance path of four-ax manipulator in joint coordinate frame

圖5 四軸機(jī)械臂在直角坐標(biāo)系下的二維避障動態(tài)過程Fig.5 Obstacle-avoidance dynamic process of four-ax manipulator in square coordinate frame

7 結(jié) 論

本文提出的智能平面搜索避障算法能有效規(guī)劃出優(yōu)化路徑，成功解決四軸機(jī)械臂在平面內(nèi)的障礙物避碰問題，具有一定的理論研究價值與較強(qiáng)的實際應(yīng)用潛力。

需要特別指出的是，由于本算法基于圖形中小方塊的搜索，關(guān)節(jié)空間的細(xì)分精度對算法的效率影響較大。細(xì)分精度過高會降低搜索速度，過少則會降低搜索精度。因此，用戶應(yīng)根據(jù)工藝要求與實際情況適中取值，以使算法發(fā)揮其最大效用。

References)

[1]張捍東，董保華，岑豫皖，等. 柵格編碼新方法在機(jī)器人路徑規(guī)劃中的應(yīng)用[J]. 華中科技大學(xué)學(xué)報(自然科學(xué)版)，2007，35(1)：50-53.ZHANG Handong, DONG Baohua, CEN Yuwan, et al.Application of path encoding novel mechanism based on grids in path planning for mobile robot[J]. J. Huazhong Univ. of Sci. & Tech. (Nature Science Edition), 2007, 35(1):50-53. (in Chinese)

[2]M. Bennewitz, W. Burgard, S. Thrun. Finding and optimizing solvable priority schemes for decoupled path planning techniques for teams of mobile robots[J]. Robotics and Autonomous Systems, 2002, 41:89-99.

[3]王健強(qiáng)，程汀. SCARA機(jī)器人結(jié)構(gòu)設(shè)計及軌跡規(guī)劃算法[J]. 合肥工業(yè)大學(xué)學(xué)報(自然科學(xué)版)，2008，31(7)：1026-1028 +1041.WANG Jianqiang, CHENG Ting. Structural design and algorithms for trajectory planning of the SCARA robot[J]. J Hefei University of Technology (Nature Science Ed), 2008,31(7): 1026-1028+1041. (in Chinese)

[4]Suryawanshi A. B.，Jose M. B.，Dasgupta B.，et al.Domain mapping as an expeditionary strategy for fast pathPlanning[J]. Mechanism and Machine Theory，2003,(38)11-20．

[5]封岸松，戴炬. 冗余自由度機(jī)械手的避障控制[J]. 機(jī)器人，2002，24(3)：213-216.FENG Ansong, DAI Ju. Obstacle avoidance control abouredundant manipulator[J]. Robot, 2002, 24(3): 213-216.(in Chinese)

[6]田立中，付宜利，馬玉龍， 等. 裝配路徑規(guī)劃中基于動態(tài)坐標(biāo)的A*搜索算法[J]. 計算機(jī)集成制造系統(tǒng)，2002，8(4)：316-319.TIAN Lizhong, FU Yili, MA Yulong, et al. A* Search arithmetic based on dynamic coordinate in assembly path plan[J]. Computer Integrated Manufacturing Systems, 2002,8(4): 316-319. (in Chinese)