譚 燕

(重慶三峽職業(yè)學(xué)院機(jī)械與電子工程系重慶404155)

機(jī)械臂正朝著靈活、控制精確、高效的方向發(fā)展，可以越來越多地完成人類難以完成或具有危險的工作。機(jī)械臂工作軌跡直接決定了機(jī)械臂工作效率[1-2］，因此研究機(jī)械臂軌跡規(guī)劃問題對于提高經(jīng)濟(jì)效益意義明顯。

機(jī)械臂軌跡規(guī)劃的常用方法是以邊界約束為限定條件，使用多項式函數(shù)[3］、拋物線過渡的線性函數(shù)、B樣條曲線[4］等進(jìn)行擬合，使用待定系數(shù)法確定軌跡函數(shù)。文獻(xiàn)[5］以6自由度機(jī)械臂為研究對象，使用多項式插值法進(jìn)行軌跡規(guī)劃，使用雜交算法尋優(yōu)，得到了速度約束下的時間最優(yōu)軌跡；文獻(xiàn)[6］使用牛頓下山法求出了軌跡關(guān)鍵點(diǎn)，使用三次樣條插值擬合了軌跡，實現(xiàn)了能量消耗最優(yōu)；文獻(xiàn)[7］以距離最優(yōu)為目標(biāo)選擇了空間梯形軌跡關(guān)鍵點(diǎn)，使用5次多項式插值法進(jìn)行擬合，實現(xiàn)了預(yù)期目標(biāo)；文獻(xiàn)[8］在速度約束下，提出了粒子群優(yōu)化的多項式插值軌跡規(guī)劃方法，得到了時間最優(yōu)軌跡。以上研究成果都達(dá)到了各自的優(yōu)化目標(biāo)，但是目前沒有成熟統(tǒng)一的軌跡規(guī)劃方法。本文研究了避障同時的多目標(biāo)優(yōu)化軌跡規(guī)劃方法。

本文研究機(jī)械臂軌跡規(guī)劃問題，設(shè)置了3個目標(biāo):避障、軌跡長度最短、關(guān)節(jié)角變化量最小。給出了簡單高效的碰撞檢測策略，當(dāng)發(fā)生碰撞時，使用改進(jìn)遺傳算法尋得最優(yōu)中間點(diǎn)，而后使用軌跡基元規(guī)劃起點(diǎn)至中間點(diǎn)、中間點(diǎn)至終點(diǎn)的軌跡，最終達(dá)到避障同時保證軌跡最短和關(guān)節(jié)角變化量最小的目的。

1 機(jī)械臂運(yùn)動學(xué)建模

1.1 仿人手臂機(jī)械臂

本文研究對象為如圖1所示的仿人手臂機(jī)械臂，圖中L1＝120 mm為大臂長度，L2＝100 mm為小臂長度。肩關(guān)節(jié)具有兩個旋轉(zhuǎn)自由度。肘關(guān)節(jié)具有1個旋轉(zhuǎn)自由度，腕關(guān)節(jié)具有3個旋轉(zhuǎn)自由度，圖中為每個旋轉(zhuǎn)自由度建立了坐標(biāo)系，z軸為其旋轉(zhuǎn)軸，按照圖中各坐標(biāo)系的腳標(biāo)，將坐標(biāo)系記為坐標(biāo)系1～6。

1.2 運(yùn)動學(xué)建模

機(jī)械臂運(yùn)動學(xué)模型是機(jī)械臂各關(guān)節(jié)角與末端執(zhí)行器位姿互相求解問題。當(dāng)各關(guān)節(jié)角已知，求解末端執(zhí)行器位姿時稱為運(yùn)動學(xué)正解；當(dāng)末端執(zhí)行器位姿已知，求解各機(jī)械臂關(guān)節(jié)角時稱為運(yùn)動學(xué)逆解。

本文使用D-H法建立運(yùn)動學(xué)正解模型，記坐標(biāo)系n到坐標(biāo)系n＋1的齊次變換矩陣為，經(jīng)過兩次旋轉(zhuǎn)和兩次平移可由坐標(biāo)系n轉(zhuǎn)換到坐標(biāo)系n＋1，得矩陣形式為:

那么從基座坐標(biāo)系(記為0坐標(biāo)系)到末端執(zhí)行器的齊次變換矩陣為:

式(2)即為機(jī)械臂運(yùn)動學(xué)正解模型，當(dāng)機(jī)械臂各關(guān)節(jié)角確定時，可以通過式(2)確定末端執(zhí)行器的位姿。

機(jī)械臂運(yùn)動學(xué)逆解與正解過程恰好相反，在式(2)兩端分別左乘()-1、()-1、…、()-1，再使用反正切函數(shù)可以求得θi，i＝ 1，2，…，6。 運(yùn)動學(xué)逆解存在多解問題，一般需根據(jù)實際情況進(jìn)一步確定。

2 插入最優(yōu)中間點(diǎn)的軌跡規(guī)劃

2.1 插入最優(yōu)中間點(diǎn)軌跡規(guī)劃原理

本文對機(jī)械臂軌跡規(guī)劃的目標(biāo)包括3個:避障、軌跡最短和關(guān)節(jié)角變化量最小。軌跡規(guī)劃的整體思路是:根據(jù)機(jī)械臂任務(wù)確定軌跡起點(diǎn)和終點(diǎn)，使用軌跡基元進(jìn)行軌跡規(guī)劃，然后對規(guī)劃出的軌跡進(jìn)行碰撞檢測，若發(fā)生碰撞則插入最優(yōu)中間點(diǎn)，再使用軌跡基元規(guī)劃從起點(diǎn)至中間點(diǎn)、中間點(diǎn)至終點(diǎn)的軌跡，在保證軌跡不發(fā)生碰撞的同時，保證軌跡最短且關(guān)節(jié)角變化量最小。機(jī)械臂避障軌跡規(guī)劃原理如圖2所示。

由圖2可知，還有3個問題需要明確:一是確定軌跡基元，二是碰撞檢測如何實現(xiàn)，三是最優(yōu)中間點(diǎn)如何選取。軌跡基元為點(diǎn)與點(diǎn)之間擬合使用的曲線函數(shù)。

常用函數(shù)包括多項式函數(shù)、B樣條曲線等。本文選擇使用5次多項式作為軌跡基元。

2.2 碰撞檢測方法

機(jī)械臂軌跡規(guī)劃的首要要求是安全，即避開障礙物，因此必須提出機(jī)械臂與障礙物的碰撞檢測策略。為了提出簡單高效、利于推廣的碰撞檢測方法，本文將障礙物模型簡化為球形，球心為障礙物幾何中心，球半徑為障礙物最大半徑。將機(jī)械臂模型簡化為線段，線段長度與機(jī)械臂長度一致，為了實現(xiàn)避障，將機(jī)械臂包絡(luò)半徑疊加到障礙物半徑中，最終得到障礙物球形模型的膨脹半徑為R＝Rob＋Rlink，式中Rob為障礙物半徑，Rlink為連桿包絡(luò)半徑。

通過障礙物與機(jī)械臂簡化模型可以看出，機(jī)械臂與障礙物之間碰撞檢測問題可以簡化為球模型與線段的位置關(guān)系判斷問題。確定連桿線段的位姿，只需用到關(guān)節(jié)2的位姿、關(guān)節(jié)3的位姿、關(guān)節(jié)4的位姿，則兩連桿在三維空間的矢量表達(dá)式為:

式中:為大臂的矢量表達(dá)式；為小臂的矢量表達(dá)式；(1，4)表示矩陣第1行、第4列的元素。

記障礙物圓球模型的球心坐標(biāo)為P＝[px，py，pz］，選取連桿線段上任意一點(diǎn)到球心的矢量表達(dá)為，則連桿與球心距離為:

式中:d1為大臂到球心的距離；d2為小臂到球心的距離。若di＞R，i＝1，2則機(jī)械臂與障礙物不會發(fā)生碰撞，否則會發(fā)生碰撞。

本文提出的碰撞檢測策略在一定程度上犧牲了部分機(jī)械臂工作空間，但是其簡單高效的特點(diǎn)使這種方法易于推廣使用。

3 最優(yōu)中間點(diǎn)選取方法

3.1 建立適應(yīng)度函數(shù)

本文規(guī)劃機(jī)械臂軌跡時，在避障的同時保證軌跡最短且各關(guān)節(jié)角變化量最小，因此需要從軌跡長度和關(guān)節(jié)角變化量兩個方面建立適應(yīng)度函數(shù)。

記fQ為關(guān)節(jié)角變化量，fL為機(jī)械臂運(yùn)動軌跡長度，使用插值法對這兩個量進(jìn)行計算。在運(yùn)動軌跡中插入n-1個點(diǎn)，從而將運(yùn)動軌跡劃分為n段，通過求得相鄰插入點(diǎn)的關(guān)節(jié)角變化量和軌跡長度，再通過累加得到整個軌跡的關(guān)節(jié)角變化量和軌跡長度，為:

式中:fOB為表征碰撞檢測結(jié)果的二值變量，若機(jī)械臂與障礙物不發(fā)生碰撞則fOB＝1，若機(jī)械臂與障礙物發(fā)生碰撞則fOB＝0；η1、η2為權(quán)重因子，角度變化量與軌跡長度數(shù)量級相差較大，約為1 000倍，為保證兩個因素對適應(yīng)度函數(shù)具有相同的影響程度，取η1＝1、η2＝0.001。

對于多障礙物環(huán)境時，需要設(shè)置與障礙物相同數(shù)量的中間點(diǎn)，此時適應(yīng)度函數(shù)為各中間點(diǎn)適應(yīng)度函數(shù)的累加和。

3.2 改進(jìn)遺傳算法的尋優(yōu)方法

遺傳算法模擬自然界遺傳和優(yōu)勝劣汰的自然選擇過程，使種群適應(yīng)度不斷提高，尋得最優(yōu)解。遺傳算法包括染色體編碼、初始種群生成、遺傳操作(選擇、交叉、變異)、解碼等過程[9］。其中初始種群生成方法、交叉概率、變異概率對算法尋優(yōu)效果影響極大[10］。本文對此3個方面進(jìn)行改進(jìn)。

(1)染色體編碼。使用實數(shù)編碼方式，由于1個中間點(diǎn)對應(yīng)6個關(guān)節(jié)角，那么對于需要設(shè)置n個中間點(diǎn)的規(guī)劃問題，其需要6n個關(guān)節(jié)角。即編碼方式為(θ1，θ2，…，θ6n)。

(2)初始種群生成。在傳統(tǒng)遺傳算法中使用隨機(jī)方法產(chǎn)生初始種群，這種方法產(chǎn)生的初始種群適應(yīng)度不高，沒有優(yōu)勢。本文提出以均勻方式產(chǎn)生初始種群，將機(jī)械臂工作空間分成z個區(qū)域，在每個區(qū)域中使用均勻數(shù)值產(chǎn)生p個染色體，這樣就得到了z×p個染色體，依據(jù)適應(yīng)度函數(shù)選擇最大的前N個個體作為初始種群。這種初始種群產(chǎn)生方式保證了初始種群在解空間的分散性，有利于全局搜索。

(3)選擇操作。以個體適應(yīng)度為選擇依據(jù)，每個個體被選概率為f(i)/∑f(i)，式中f(i)為個體i的適應(yīng)度值，∑f(i)為所有個體適應(yīng)度之和。這種選擇方

式中:qij為第i段軌跡第j個關(guān)節(jié)角；fQ為整個運(yùn)動軌跡的 6 個關(guān)節(jié)角變化量之和；(pi，x，pi，y，pi，z)為第i個插入點(diǎn)的三維坐標(biāo)；fL為整個運(yùn)動軌跡長度。

以軌跡長度和各關(guān)節(jié)角變化量為基礎(chǔ)，建立適應(yīng)度函數(shù)fG為:式既保證了優(yōu)秀個體被選可能性較大，也保證了任意個體被選可能性，保持了種群多樣性。

(4)交叉算子。交叉算子可以提高物種多樣性，提高物種進(jìn)化能力。那么，對于物種多樣性較差的種群使用較大的交叉概率，提高物種多樣性；對于物種多樣性較好的種群使用較小的交叉概率，保持物種多樣性。本文使用適應(yīng)度值方差和分布熵衡量物種多樣性，并提出相應(yīng)的交叉概率。記種群規(guī)模為N，第i個個體適應(yīng)度為fi，種群平均適應(yīng)度為，則第j代種群適應(yīng)度方差Gj為:

種群分布熵計算方法為:將可行解空間劃分為z個子空間，種群個體落在第i個子空間數(shù)量記為Ai，那么第j代種群的分布熵Hj為:

式中:pi＝Ai/N。

適應(yīng)度值方差、種群分布熵越大，表示種群空間分布和適應(yīng)度值分布越均勻，進(jìn)化能力越強(qiáng)，此時使用較小交叉概率。由此提出交叉概率為:

(5)變異算子。變異算子可以破壞優(yōu)良基因，也可以使劣質(zhì)基因變異為優(yōu)良個體，因此對于優(yōu)良基因應(yīng)使用較小的變異概率，對于劣質(zhì)基因應(yīng)使用較大的變異概率。記第i個個體適應(yīng)度值與平均值之差為θ，則第i個個體的變異概率為:

式中:pi＝1/(1＋e-θ)。 分析式(8)可知，當(dāng)個體適應(yīng)度值較大時，變異概率較小，保護(hù)了優(yōu)良基因；當(dāng)個體適應(yīng)度值較小時，變異概率較大，劣質(zhì)基因具有更大機(jī)會變異為優(yōu)良基因。

(6)解碼。解碼方法與式(2)一致，即通過6個關(guān)節(jié)角確定中間點(diǎn)的位姿。

4 仿真驗證

本文設(shè)置了兩組仿真，一是確定中間點(diǎn)數(shù)量的仿真實驗，二是為了驗證本文提出方法的路徑規(guī)劃效果，同時比較改進(jìn)遺傳算法與傳統(tǒng)遺傳算法搜索最優(yōu)中間點(diǎn)的性能。

4.1 中間點(diǎn)數(shù)量的選取

本文中提到，當(dāng)機(jī)械臂軌跡與障礙物發(fā)生碰撞時，插入最優(yōu)中間點(diǎn)，而后使用軌跡基元分別規(guī)劃起點(diǎn)至中間點(diǎn)、中間點(diǎn)至終點(diǎn)的軌跡，但是中間點(diǎn)數(shù)量問題一直未得到明確或驗證。

首先設(shè)置單障礙物情況，障礙物中心為P＝(50，100，50)，膨脹半徑為R＝30 mm，使用軌跡基元直接規(guī)劃機(jī)械臂軌跡會發(fā)生碰撞，因此需要插入中間點(diǎn)。當(dāng)插入一個中間點(diǎn)時，插入中間點(diǎn)為θ＝[0.229，0.748，-0.841，0，0.407，0.383］，軌跡如圖 3a 所示。 當(dāng)插入兩個中間點(diǎn)時，尋優(yōu)得到的兩個中間點(diǎn)分別為:

插入兩個中間點(diǎn)時的軌跡如圖3b所示。

為了對比兩條機(jī)械臂軌跡的性能，計算兩條軌跡的適應(yīng)度函數(shù)值如表1所示。

表1 單障礙物時不同數(shù)量中間點(diǎn)對比

表1中迭代次數(shù)是指最早出現(xiàn)最優(yōu)解時的迭代次數(shù)。由表1可以看出，當(dāng)插入兩個中間點(diǎn)時，適應(yīng)度值相差-0.000 9，下降百分比為1.29%，說明增加一個中間點(diǎn)的優(yōu)化效果不明顯，但是增加一個中間點(diǎn)時計算量和迭代次數(shù)明顯增加，因此存在一個障礙物時應(yīng)當(dāng)插入一個中間點(diǎn)。

接下來研究兩個障礙物的情況，障礙物中心分別為P1＝(150，50，30)、P2＝(50，100，50)，膨脹半徑為R＝30 mm。分別插入一個中間點(diǎn)和兩個中間點(diǎn)進(jìn)行軌跡規(guī)劃，插入一個中間點(diǎn)時中間點(diǎn)為θ＝[0.493，0.996，-0.438，0，0.1472，0.716］， 插 入 兩 個中間點(diǎn)時，兩個中間點(diǎn)分別為:

這里不再給出軌跡規(guī)劃結(jié)果，計算兩條軌跡的適應(yīng)度值，結(jié)果如表2所示。

表2 兩個障礙物時不同數(shù)量中間點(diǎn)對比

由表2可知，當(dāng)工作環(huán)境中存在兩個障礙物需要躲避時，與插入一個中間點(diǎn)相比，插入兩個中間點(diǎn)時適應(yīng)度值增加了0.006 4，增加百分比為10.4%，優(yōu)化效果非常明顯，因此當(dāng)存在兩個障礙物時應(yīng)當(dāng)插入兩個中間點(diǎn)。

基于對單個障礙物和兩個障礙物的情況分析，本文粗略地規(guī)定:當(dāng)工作空間中存在N≥0個障礙物時，考慮到軌跡優(yōu)劣和計算量大小，最佳的避障策略是插入相同數(shù)量的中間點(diǎn)。

4.2 改進(jìn)遺傳算法與傳統(tǒng)算法對比

本節(jié)設(shè)置了包含兩個障礙物的工作環(huán)境，用于對比改進(jìn)遺傳算法與傳統(tǒng)算法在中間點(diǎn)選取上的性能優(yōu)劣。 兩個障礙物位置分別為P1＝(150，50，30)、P2＝(50，100，50)，膨脹半徑為R＝30 mm。 由于環(huán)境中包含2個障礙物，因此需要優(yōu)化出2個中間點(diǎn)，遺傳算法編碼時需設(shè)置12個角度量。

使用改進(jìn)遺傳算法尋優(yōu)得到的中間點(diǎn)見式(9)，使用傳統(tǒng)遺傳算法尋優(yōu)得到的兩個中間點(diǎn)為:

而后使用5次多項式分別規(guī)劃起點(diǎn)至中間點(diǎn)、中間點(diǎn)至中間點(diǎn)、中間點(diǎn)至終點(diǎn)的軌跡，計算兩條軌跡各自的適應(yīng)度值，結(jié)果如表3所示。

表3 兩種遺傳算法的軌跡規(guī)劃結(jié)果

由表2可知，改進(jìn)遺傳算法在迭代131次時出現(xiàn)最優(yōu)個體，而傳統(tǒng)算法在196次時才出現(xiàn)；與傳統(tǒng)遺傳算法規(guī)劃的軌跡相比，改進(jìn)遺傳算法尋得的中間點(diǎn)用于軌跡規(guī)劃時，軌跡長度減少了12.1%，關(guān)節(jié)角變化量減少了24.4%，適應(yīng)度值增加了23.6%，軌跡優(yōu)勢非常明顯。這是因為改進(jìn)遺傳算法使用均勻方式產(chǎn)生初始種群，非常有利于全局搜索，而且交叉概率和變異概率的設(shè)置方法非常有利于種群進(jìn)化，使得改進(jìn)遺傳算法出現(xiàn)最優(yōu)個體的迭代次數(shù)少，且軌跡適應(yīng)度值更大。

對于本節(jié)設(shè)置的兩個障礙物工作環(huán)境，規(guī)劃出的最優(yōu)軌跡如圖4所示，圖中實線表示實際軌跡，虛線表示點(diǎn)與點(diǎn)之間的連線。以上分析及仿真結(jié)果充分證明了插入最優(yōu)點(diǎn)的機(jī)械臂軌跡規(guī)劃方法，在實現(xiàn)避障的同時，能夠規(guī)劃出長度最短、關(guān)節(jié)角變化量最小的工作軌跡。

5 結(jié)語

本文研究了仿人手臂機(jī)械臂避障軌跡規(guī)劃問題，在避障的同時，保證了關(guān)節(jié)角變化量最小和軌跡最短。得出了以下結(jié)論:(1)插入最優(yōu)中間點(diǎn)避障算法能夠準(zhǔn)確規(guī)劃出一條避障軌跡，且關(guān)節(jié)角變化量最小和軌跡最短；(2)通過改進(jìn)遺傳算法的初始種群生成方法、交叉變異概率，有利于算法全局尋優(yōu)能力，且加強(qiáng)了種群進(jìn)化能力，與傳統(tǒng)遺傳算法相比，不僅尋優(yōu)速度快，而且結(jié)果更優(yōu)。