基于改進(jìn)蟻群算法的六自由度機(jī)械臂路徑規(guī)劃

趙華東， 雷超帆， 江 南

(鄭州大學(xué) 機(jī)械工程學(xué)院 河南 鄭州 450001)

0 引言

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

1 機(jī)械臂運(yùn)動(dòng)學(xué)研究

1.1 D-H運(yùn)動(dòng)模型的建立

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

以ABB1410型號(hào)機(jī)械臂為例，建立的D-H運(yùn)動(dòng)模型如圖1所示，連桿參數(shù)的數(shù)值如表1所示。

圖1 ABB1410型號(hào)機(jī)械臂的D-H運(yùn)動(dòng)模型Figure 1 D-H motion model of ABB1410 manipulator

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

1.2 正運(yùn)動(dòng)學(xué)分析

機(jī)械臂的正運(yùn)動(dòng)學(xué)是指在已知機(jī)械臂各個(gè)連桿的尺寸大小后，給定其關(guān)節(jié)處的轉(zhuǎn)角、速度、加速度等參數(shù)，計(jì)算出末端執(zhí)行器處的位姿和運(yùn)動(dòng)狀態(tài)[8]。假設(shè)某時(shí)刻機(jī)械臂六個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度分別為θ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型號(hào)機(jī)械臂六個(gè)關(guān)節(jié)間的轉(zhuǎn)換關(guān)系為

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

1.3 逆運(yùn)動(dòng)學(xué)分析

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

采用矩陣逆乘的方法求解逆運(yùn)動(dòng)學(xué)解的過(guò)程為

由于推導(dǎo)過(guò)程煩瑣，其具體過(guò)程就不在本文中展開(kāi)敘述。根據(jù)Picper準(zhǔn)則可知，機(jī)械臂逆運(yùn)動(dòng)學(xué)的解并不具有唯一性。因此，在實(shí)際應(yīng)用中，應(yīng)根據(jù)機(jī)械臂具體的避障要求以及路徑規(guī)劃的運(yùn)動(dòng)軌跡來(lái)判斷機(jī)械臂所對(duì)應(yīng)的逆解。

2 機(jī)械臂碰撞檢測(cè)

2.1 機(jī)械臂與障礙物模型簡(jiǎn)化

圖2 機(jī)械臂簡(jiǎn)化模型Figure 2 Simplified model of the manipulator

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

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

2.2 碰撞檢測(cè)

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

在完成連桿線段i與平面S的碰撞判斷后，接下來(lái)用同樣的方法判斷連桿li與障礙物長(zhǎng)方體的其他五個(gè)平面是否碰撞，對(duì)于機(jī)械臂另外五個(gè)連桿的判斷也同樣采用這種方法進(jìn)行求解。

3 三維空間建模

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

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

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

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

4.1 基本蟻群算法

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

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

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

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

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

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

4.2 蟻群算法的改進(jìn)

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

4.2.2設(shè)置信息素濃度τij的上、下限 在蟻群算法的尋優(yōu)過(guò)程中，路徑上殘余信息素濃度τij的大小嚴(yán)重影響著螞蟻的判斷，決定著收斂結(jié)果的質(zhì)量。當(dāng)信息素濃度過(guò)高時(shí)，螞蟻搜索將會(huì)失去隨機(jī)性，算法陷入局部最優(yōu)；反之，當(dāng)信息素濃度過(guò)低時(shí)，算法容易陷入早熟收斂。因此，通過(guò)對(duì)信息素濃度設(shè)置上、下限，避免算法的早熟和局部最優(yōu)，增減算法的正反饋?zhàn)饔茫饺缦拢?/p>

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

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

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

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

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

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

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

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

5.2 仿真結(jié)果分析

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

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

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

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

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

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

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

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

圖7 機(jī)械臂運(yùn)動(dòng)仿真過(guò)程Figure 7 Simulation process of manipulater motion

6 結(jié)論

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