趙華東， 雷超帆， 江 南

(鄭州大學 機械工程學院 河南 鄭州 450001)

0 引言

在工業(yè)4.0大背景下，工業(yè)機器人技術(shù)及產(chǎn)品已成為制造業(yè)中不可或缺的部分，同時也引發(fā)了國內(nèi)外專家對相關(guān)技術(shù)的研究熱潮。機械臂的路徑規(guī)劃問題是指在滿足工作條件的基礎(chǔ)上，尋找一條從初始點到目標點距離最短且能避開障礙的最優(yōu)路徑[1]。具有代表性的規(guī)劃方法主要有自由空間法和人工勢場法等[2]。文獻[3]借助C空間法的思想，構(gòu)造障礙物映射至C空間的轉(zhuǎn)換關(guān)系，實現(xiàn)機械臂在線實時避障，但這種空間映射方法的計算量較大，尤其是對于關(guān)節(jié)數(shù)較多的機械臂而言，適用性較差。文獻[4]通過構(gòu)造引力場和斥力場，有效避免了碰撞的發(fā)生，很好地解決了機械臂在處理全局規(guī)劃中的避障問題，但由于勢場法的固有缺陷，該方法容易陷入局部最小處，規(guī)劃效果欠佳。文獻[5]提出了基于速度的C空間法來實現(xiàn)動態(tài)環(huán)境中的路徑規(guī)劃，但該方法僅適用于移動機器人。文獻[6]利用A*算法與動力學約束相結(jié)合，達到避免碰撞實現(xiàn)路徑規(guī)劃的目的，但是這種方式需要進行大量的動力學計算，且公式推導極其復雜，不適用于多關(guān)節(jié)型機械臂。本文以ABB1410型號機械臂為研究對象，使用D-H法構(gòu)建機械臂模型，進而推導出其正、逆運動學變換公式，實現(xiàn)末端執(zhí)行器從空間坐標至關(guān)節(jié)角度的映射；通過合理的建模方式對機械臂和障礙物進行簡化，設(shè)計碰撞檢測方法；對蟻群算法進行了改進，以此實現(xiàn)機械臂在工作空間中的避障路徑規(guī)劃，并通過模擬機械臂的避障運動，對該方法的可行性進行了驗證。

1 機械臂運動學研究

1.1 D-H運動模型的建立

D-H法是由Denavit和Hartenberg于1955年設(shè)計的一種表示機械臂各連桿間運動關(guān)系的通用方法。其基本原理是在機械臂的關(guān)節(jié)處設(shè)定坐標系來表示相連剛體桿件的空間位姿，使相鄰兩連桿間的空間位置關(guān)系通過4*4階層的齊次坐標變換矩陣進行描述[7]。通過六連桿間依次變換，最終得到機械臂末端執(zhí)行器相對于底座的位姿。

以ABB1410型號機械臂為例，建立的D-H運動模型如圖1所示，連桿參數(shù)的數(shù)值如表1所示。

圖1 ABB1410型號機械臂的D-H運動模型Figure 1 D-H motion model of ABB1410 manipulator

表1 連桿參數(shù)的數(shù)值Table 1 Number of connecting rod parameters

1.2 正運動學分析

機械臂的正運動學是指在已知機械臂各個連桿的尺寸大小后，給定其關(guān)節(jié)處的轉(zhuǎn)角、速度、加速度等參數(shù)，計算出末端執(zhí)行器處的位姿和運動狀態(tài)[8]。假設(shè)某時刻機械臂六個關(guān)節(jié)的轉(zhuǎn)動角度分別為θ1、θ2、θ3、θ4、θ5、θ6，已知相鄰兩桿間表示位姿的齊次變換矩陣為

Tn=Rot(z,θn)Trans(0,0,dn)Trans(αn,0,0)Rot(x,αn)=

根據(jù)表1中的連桿參數(shù)，可以得到ABB1410型號機械臂六個關(guān)節(jié)間的轉(zhuǎn)換關(guān)系為

式中:c、s分別表示余弦函數(shù)和正弦函數(shù)；n-1Tn表示相鄰兩關(guān)節(jié)間的坐標轉(zhuǎn)換關(guān)系。在求出六個連桿間的轉(zhuǎn)換關(guān)系后，不難求得末端執(zhí)行器與基坐標系之間的轉(zhuǎn)換公式為

1.3 逆運動學分析

逆運動學是指在已知機械臂末端執(zhí)行器坐標系的位姿矩陣后，通過對正運動學變換矩陣的反向求解，得到機械臂各個關(guān)節(jié)處的旋轉(zhuǎn)角度。

采用矩陣逆乘的方法求解逆運動學解的過程為

由于推導過程煩瑣，其具體過程就不在本文中展開敘述。根據(jù)Picper準則可知，機械臂逆運動學的解并不具有唯一性。因此，在實際應用中，應根據(jù)機械臂具體的避障要求以及路徑規(guī)劃的運動軌跡來判斷機械臂所對應的逆解。

2 機械臂碰撞檢測

2.1 機械臂與障礙物模型簡化

圖2 機械臂簡化模型Figure 2 Simplified model of the manipulator

采用長方體包絡(luò)法將機械臂與障礙物抽象簡化。具體做法是將機械臂連桿和工作空間中需要規(guī)避的不規(guī)則形體的障礙物用最小體積的六面體包裹起來。為了簡化運算，將包絡(luò)機械臂連桿的長方體看作一條直線，而將連桿長方體的邊長疊加到障礙物長方體中。機械臂簡化模型如圖2所示。

最終，只需判斷連桿直線與包絡(luò)障礙物長方體的六個平面間的位置關(guān)系，即可完成碰撞檢測。

2.2 碰撞檢測

假設(shè)關(guān)節(jié)i和i+1處的坐標分別為p1(x1,y1,z1)和p2(x2,y2,z2)，那么連桿li所處直線的方向為H=(x2-x1,y2-y1,z2-z1)。長方體某一平面的四個頂點分別為s1(x1,y1,z1)、s2(x2,y2,z2)、s3(x3,y3,z3)、s4(x4,y4,z4)，該平面S的法向量為M(m1,m2,m3)。運用空間幾何學知識可知，當M·H=0時，直線與平面平行；當M·H≠0時，說明直線與平面相交，必定存在著某個交點p0(x0,y0,z0)。交點為碰撞點需要同時滿足兩個條件：其一，交點必須處于連桿線段上；其二，交點要在空間平面的內(nèi)部。連桿線段i若要與平面S相交，需要滿足以下條件：

在完成連桿線段i與平面S的碰撞判斷后，接下來用同樣的方法判斷連桿li與障礙物長方體的其他五個平面是否碰撞，對于機械臂另外五個連桿的判斷也同樣采用這種方法進行求解。

3 三維空間建模

圖3 三維空間規(guī)劃Figure 3 Three-dimensional space planning

三維空間建模的具體方法如下：沿坐標軸方向構(gòu)造包含初始點、目標點及障礙物的三維空間立方體區(qū)域ABCD-EFGH，該立方體區(qū)域即為作業(yè)空間的規(guī)劃區(qū)域。在完成作業(yè)空間的規(guī)劃后，采用等分法對空間進行劃分，得到三維路徑規(guī)劃所需要的路徑節(jié)點。首先沿AE邊線將空間ABCD-EFGH進行等距劃分，得到n個平面Πi(i=1,2,…,n)，然后再沿AB邊線對這n個平面進行m等分，最后沿AD邊線進行m等分，這樣就將平面Πi(i=1,2,…,n)離散成m×m個柵格，三維空間規(guī)劃如圖3所示。

通過上述步驟，將三維規(guī)劃空間離散為一系列三維空間點的集合。

4 基于改進蟻群算法的路徑規(guī)劃

4.1 基本蟻群算法

蟻群算法是模擬蟻群覓食行為的啟發(fā)式智能算法[9-10]，該算法的實現(xiàn)步驟如下。

(1) 初始化算法參數(shù)。在搜索之前，需要給蟻群算法的幾個重要參數(shù)賦予初始值，初始值合理與否直接影響著算法搜索的性能[11]，蟻群算法的重要參數(shù)包括螞蟻數(shù)量、信息素啟發(fā)值、適應度啟發(fā)值及信息素衰減系數(shù)。

(2) 算法搜索過程。螞蟻從當前點移動至下一路徑點時，根據(jù)可視域內(nèi)各個點的選擇概率來決定移動目標，螞蟻k在t時刻從i點移動至j點的概率可以表示為

式中：集合{allowed}表示螞蟻k下一步所能選擇的路徑節(jié)點的集合；τij(t)表示在t時刻從i點至j點路徑上的殘余信息素大小；δij(t)表示在t時刻從i點至j點路徑上的能見度；α表示信息素啟發(fā)值；β表示適應度啟發(fā)值。

(3) 局部信息素更新。每當螞蟻完成一次路徑的搜索，就要按照局部信息素更新策略對經(jīng)過路徑上的信息素進行更新，更新公式為τij=(1-ξ)τij，其中：τij表示以點(i,j)為信息素載體的信息素濃度；ξ表示信息素衰減系數(shù)。

(4) 全局信息素更新。當一次迭代中的螞蟻都完成從出發(fā)點到終點的搜索后，要進行全局信息素的更新，更新公式為τij(t+1)=(1-ρ)τij(t)+Δτij(t)，其中：ρ表示信息素揮發(fā)系數(shù)。

4.2 蟻群算法的改進

4.2.1建立可視域搜索空間 為降低搜索空間的復雜程度，將機械臂末端執(zhí)行器的運動簡化為沿x、y、z三個軸方向上的直線運動，其中沿x軸方向的最大移動距離為Lx,max，沿y軸方向的最大移動距離為Ly,max，沿z軸方向的最大移動距離為Lz,max。這樣，當螞蟻對下一路徑點進行搜索時，就會相對于當前位置形成一個可視域搜索空間。按照此種方法，將螞蟻的搜索限制在一定區(qū)域內(nèi)，簡化了搜索空間，提高了蟻群算法的搜索效率。

4.2.2設(shè)置信息素濃度τij的上、下限 在蟻群算法的尋優(yōu)過程中，路徑上殘余信息素濃度τij的大小嚴重影響著螞蟻的判斷，決定著收斂結(jié)果的質(zhì)量。當信息素濃度過高時，螞蟻搜索將會失去隨機性，算法陷入局部最優(yōu)；反之，當信息素濃度過低時，算法容易陷入早熟收斂。因此，通過對信息素濃度設(shè)置上、下限，避免算法的早熟和局部最優(yōu)，增減算法的正反饋作用，公式如下：

式中：τx表示局部或全局更新計算所得到的信息素濃度大小。

4.2.3揮發(fā)系數(shù)ρ的自適應調(diào)節(jié) 揮發(fā)系數(shù)ρ反映了蟻群的全局搜索能力和算法收斂的速度。當ρ值較大時，信息素的濃度衰減較快，由正反饋機制占據(jù)主導地位，算法隨機性較差；而當ρ值較小時，算法將陷入隨機搜索，正反饋作用降低，進而導致收斂速度減慢。對ρ值采用自適應調(diào)節(jié)策略，公式如下：

式中：ξ∈(0,1)；N表示算法的迭代次數(shù)。

5 仿真結(jié)果與分析

5.1 算法實現(xiàn)流程

為了驗證本文方法實現(xiàn)六自由度機械臂路徑規(guī)劃的可行性，對其進行了仿真驗證。將機械臂工作空間抽象化，使用長方體包絡(luò)法簡化障礙物模型，通過蟻群算法在已知工作環(huán)境中尋找一條合適的規(guī)劃路徑，得到組成路徑的節(jié)點坐標，再將該節(jié)點依次作為機械臂執(zhí)行器的移動目標點，根據(jù)逆運動學計算公式求解每一目標點所對應的關(guān)節(jié)角度，可得到多組關(guān)節(jié)角度解的集合。任選一組解進行碰撞檢測，若不發(fā)生碰撞，則該解滿足路徑規(guī)劃需求，否則進行下一組解的碰撞檢測。若所有的解均不滿足規(guī)劃需求，則將該路徑節(jié)點視為障礙點，重新進行路徑規(guī)劃，直至所有路徑節(jié)點均不發(fā)生碰撞。

圖4 算法實現(xiàn)流程Figure 4 Algorithm implementation flow chart

算法實現(xiàn)流程如圖4所示。

5.2 仿真結(jié)果分析

利用仿真環(huán)境為Matlab 2014b，CPU型號為Inter i5-6500，內(nèi)存為8 GB的計算機分別對優(yōu)化后的蟻群算法和普通蟻群算法進行仿真對比。機械臂工作空間為2 m*2 m*1 m，離散點劃分數(shù)量為20*20*10。初始點坐標為(6,5,2),目標點坐標為(13,6,10)。

根據(jù)文獻[11]中的實驗結(jié)果設(shè)置算法參數(shù)，蟻群數(shù)量m為50，迭代次數(shù)N為100，信息素啟發(fā)值α為1，適應度啟發(fā)值β為5，揮發(fā)系數(shù)ρ為0.7，仿真結(jié)果對比如圖5所示。

觀察仿真結(jié)果可知，在相同的參數(shù)條件下，改進蟻群算法搜索到的路徑長度更短，且迭代速度更快。改進蟻群算法和普通蟻群算法的路徑長度分別為1.941 m和2.119 m，迭代次數(shù)分別為38次和65次，尋優(yōu)時間分別為1.636 s和2.823 s。

圖5 仿真結(jié)果對比Figure 5 Comparison of simulation results

選擇改進蟻群算法路徑規(guī)劃的結(jié)果作為機械臂的運動路徑，機械臂在路徑節(jié)點的位姿坐標和關(guān)節(jié)轉(zhuǎn)角如表2所示。機械臂在沿規(guī)劃路徑運動的過程中，六個關(guān)節(jié)轉(zhuǎn)角隨時間的變化曲線如圖6所示。表2和圖6中關(guān)節(jié)轉(zhuǎn)角的數(shù)據(jù)均為實際轉(zhuǎn)角與π的比值。結(jié)果表明，每個關(guān)節(jié)角的變化都是平滑且連續(xù)的曲線，并未有突變情況發(fā)生，符合機械臂運動學規(guī)律，路徑規(guī)劃有效。

根據(jù)上述路徑規(guī)劃結(jié)果，利用RobotStudio軟件模擬機械臂的避障運動過程，觀察可知，機械臂機身及末端執(zhí)行器均未與障礙物發(fā)生碰撞，證明碰撞檢測算法有效。機械臂運動仿真過程如圖7所示。結(jié)果表明，在采用本文的改進蟻群算法對空間六自由度機械臂進行避障路徑規(guī)劃時，能夠快速、準確地搜尋到一條符合機械臂運動要求且避開碰撞的最優(yōu)路徑。

表2 路徑節(jié)點的位姿坐標和關(guān)節(jié)轉(zhuǎn)角Table 2 Pose coordinates and joint angles of the path nodes

圖6 關(guān)節(jié)轉(zhuǎn)角隨時間的變化曲線Figure 6 Joint angle curve changing with time

圖7 機械臂運動仿真過程Figure 7 Simulation process of manipulater motion

6 結(jié)論

本文以ABB1410型號的六自由度空間機械臂為研究對象，首先對其進行運動學分析，建立D-H運動模型，推導正、逆運動學變換公式，得到從空間坐標到關(guān)節(jié)轉(zhuǎn)角的轉(zhuǎn)換關(guān)系。然后設(shè)計機械臂與障礙物碰撞檢測算法，抽象化三維空間模型。 對蟻群算法進行改進，使用改進后的蟻群算法完成了機械臂在工作空間的路徑規(guī)劃。仿真結(jié)果驗證了該方法的可行性，為空間六自由度機械臂提供了快速、精確的路徑規(guī)劃方法。