張許有，劉有余*,

（1.高端裝備先進(jìn)感知與智能控制教育部重點實驗室，安徽蕪湖 241000；2.安徽工程大學(xué) 機(jī)械工程學(xué)院，安徽蕪湖 241000）

路徑規(guī)劃算法是解決機(jī)械臂避障規(guī)劃問題的核心，為此，學(xué)者們給出了如A*算法[1-2]、人工勢場法[3]、強(qiáng)化學(xué)習(xí)法[4-5]、RRT(Rapidly exploring random tree)[6-7]、PRM[8]等，其中PRM 算法具有規(guī)劃復(fù)雜度受規(guī)劃空間維數(shù)和復(fù)雜度影響小的特點，對解決高自由度、強(qiáng)耦合的機(jī)械臂路徑規(guī)劃問題具有優(yōu)勢。

基于PRM 算法的路徑規(guī)劃缺點：在規(guī)劃空間采點時，落于障礙間狹窄區(qū)域的節(jié)點數(shù)目少，使算法在路徑搜索時，無法通過狹窄區(qū)域，導(dǎo)致結(jié)果大幅偏離最優(yōu)路徑或規(guī)劃失敗。最直接的解決途徑是通過增加采樣點，提高自由空間中采樣點數(shù)目。但該方法存在兩個問題：自由空間節(jié)點的增加，不能有效增加障礙間狹窄區(qū)域內(nèi)的節(jié)點，使規(guī)劃依舊易失敗或大幅偏離最優(yōu)路徑；采樣點的增加，提升了PRM 算法采樣階段、路徑檢查階段和路徑規(guī)劃階段的時間消耗，使算法的時間復(fù)雜度大幅提升。為此，從PRM算法的采樣階段入手，通過提高采樣點的質(zhì)量，增加處于障礙間狹窄區(qū)域的節(jié)點數(shù)目[9-12]。Amato 等[9]提出了基于障礙物的PRM 算法，該算法在采樣階段以均勻分布方式，一次生成兩個節(jié)點，當(dāng)兩個節(jié)點中一個處于自由空間，另一個處于障礙空間時，以二分法，查找兩節(jié)點間恰好處于自由空間的節(jié)點，并將該節(jié)點加入路徑規(guī)劃器。Boor 等[10]提出了基于高斯采樣的PRM 算法，在采樣過程中，該算法先以均勻分布生成一個節(jié)點，再以該節(jié)點為中心，以高斯分布生成另一節(jié)點，當(dāng)上述兩節(jié)點，分別處于自由空間和障礙空間時，將自由空間節(jié)點加入路徑規(guī)劃器。但上述兩種算法，易使采用點落于障礙區(qū)域外側(cè)，狹窄區(qū)域節(jié)點的采樣效率低。Hus 等[11]將橋試驗法引入PRM 算法，該算法需檢測線段的兩端點和中心點，當(dāng)兩端點處于障礙空間，中心點處于自由空間時，將中心點加入路徑規(guī)劃器。鐘建冬等[12]用星形試驗法增加狹窄通道的節(jié)點密度，該算法需檢測2 個正交線段的2 對端點和中心點，當(dāng)中心點處于自由空間，至少1 對端點處于障礙空間時，中心點即為所需。但上述兩種算法使狹窄區(qū)域節(jié)點過多，使開闊區(qū)域的規(guī)劃差。上述4 種改良算法，對機(jī)械臂避障路徑規(guī)劃，由于關(guān)節(jié)空間與笛卡爾空間具有復(fù)雜的映射關(guān)系，故采樣過程會有很大的時間消耗。

本文建立節(jié)點勢能評價指標(biāo)，定義困難區(qū)域節(jié)點，將障礙空間節(jié)點利用Metropolis 準(zhǔn)則向困難區(qū)域調(diào)整，獲得一個性能更優(yōu)良的PRM 算法，將該算法應(yīng)用于機(jī)械臂路徑規(guī)劃，并驗證方法的有效性。

1 引入勢能函數(shù)的PRM 算法

1.1 勢能函數(shù)和困難區(qū)域節(jié)點的定義

對節(jié)點進(jìn)行評價，需建立其勢能評價標(biāo)準(zhǔn)，勢能函數(shù)包含引力勢能和斥力勢能，分別為：

由式（1）和式（2）得勢能函數(shù)為

式（1）～（3）中：x為規(guī)劃空間中一點；goal為目標(biāo)點；Oi和Ri為障礙i的中心點和危險距離；n為障礙數(shù)目；l為避障剛體的危險距離；s為安全距離；Dg(x,goal)是關(guān)于x和goal的距離函數(shù)；Dr(x,Oi)是關(guān)于x和Oi的距離函數(shù)；Ug(x)為引力函數(shù)；Ur,i(x)為x相對i的斥力函數(shù)；λ和 η為引力常數(shù)和斥力常數(shù)，且λ ＞＞η ＞0，以保證處于障礙空間的點對應(yīng)的勢能大于處于自由空間的點。

欲實現(xiàn)障礙空間點向障礙間狹窄區(qū)域調(diào)整，需定義x*：對?k,z=1,2,···,n且k≠z，若

則x*為困難區(qū)域節(jié)點。

式中：Cobs為障礙空間；σ為困難區(qū)域節(jié)點的判定距離，σ通過將原有的障礙空間增大，從而尋找出至少離兩個障礙較近的節(jié)點。

1.2 調(diào)整策略

先求得障礙空間節(jié)點集合、困難區(qū)域節(jié)點集合，再將障礙空間節(jié)點集合中所有節(jié)點，隨機(jī)的向困難區(qū)域節(jié)點集合中的某一點調(diào)整，并根據(jù)勢能指標(biāo)，依概率決定調(diào)整是否成功，直至所有障礙空間節(jié)點均完成調(diào)整。節(jié)點位置調(diào)整和概率選擇式分別為：

式中：T是正常數(shù)；t為調(diào)整次數(shù)；ρ為（0,1）上的隨機(jī)數(shù)；xb(t)和xb(t-1)為障礙空間節(jié)點b調(diào)整后和調(diào)整前的坐標(biāo)；為困難區(qū)域節(jié)點a的坐標(biāo)；U為正的勢能常數(shù)，用以防止困難區(qū)域節(jié)點距離過近造成的節(jié)點多樣性喪失；為調(diào)整次數(shù)為t時，調(diào)整成功的概率，操作中，隨機(jī)生成一個（0,1）的隨機(jī)數(shù)p，當(dāng)＞p時，b接受調(diào)整；反之，b不接受，隨機(jī)再選擇一個困難區(qū)域節(jié)點，以b更新后的位置重新調(diào)整。

式（6）是依據(jù)模擬退火算法Metropolis 準(zhǔn)則[13]設(shè)置，當(dāng)調(diào)整后節(jié)點b離a較遠(yuǎn)或依然處于障礙空間時，較大，使得較小，調(diào)整成功率低；當(dāng)調(diào)整后節(jié)點b離a過近時，由于U使得較小，調(diào)整成功率低，從而防止過多的困難區(qū)域節(jié)點聚集在一起；當(dāng)節(jié)點b離a距離合適時，接近0，較大，調(diào)整成功率高。

1.3 勢能代價PRM 算法流程

步驟1 在需規(guī)劃的狀態(tài)空間中隨機(jī)采點，求出障礙空間節(jié)點集合H、自由空間節(jié)點集合I，困難區(qū)域節(jié)點集合G，計算G中元素的勢能，初始化U，令t=0，集合record為空集；

步驟2 判斷H是否為空集，若為空，則執(zhí)行步驟6；否則，計算t=t+1，h=1，M=size(H)（M為集合H中節(jié)點數(shù)），執(zhí)行步驟3；

步驟3 判斷h≤M是否成立，若不成立，則將H中record對應(yīng)的h元素全部刪除，跳轉(zhuǎn)至步驟2；否則，執(zhí)行步驟4；

步驟4 用式（5）將H中元素xh(t-1)向G中隨機(jī)一個元素調(diào)整，計算p，xh(t)，U(xh(t))，

步驟6 安全路徑檢查，設(shè)置距離d，將安全路徑中節(jié)點距離小于d的節(jié)點作為其相鄰節(jié)點，并用A*算法進(jìn)行路徑規(guī)劃；

步驟7 算法結(jié)束。

2 勢能代價PRM 算法機(jī)械臂避障規(guī)劃

機(jī)械臂關(guān)節(jié)空間搜索的優(yōu)點：1）無需考慮機(jī)械臂逆運(yùn)動學(xué)解[14]的存在性和唯一性；2）可避免機(jī)械臂的奇異位形[15]；3）避免笛卡爾空間中機(jī)械臂末端姿態(tài)難以測量的問題。選擇在q1q2,···,qN（N為機(jī)械臂自由度）的關(guān)節(jié)空間中進(jìn)行路徑搜索。

2.1 連桿勢能函數(shù)的建立

機(jī)械臂的規(guī)劃空間中，障礙和其連桿的形狀不規(guī)則，難以建立精確的勢能表達(dá)式，因此需對連桿和障礙簡化。對連桿作圓柱包圍體簡化，障礙作球包圍體簡化。∠Okαj(q)βj(q)與∠Okβj(q)αj(q)均小于90°，如圖1 所示，∠Okαj(q)βj(q) 與∠Okβj(q)αj(q)有一角大于90°，如圖2 所示。

圖1 ∠Okαj(q)βj(q) 與∠Okβj(q)αj(q)均小于90°

圖2 ∠Okαj(q)βj(q) 與∠Okβj(q)αj(q)有一角大于90°

令圖1 和圖2 中∠Okαj(q)βj(q) 為φ，∠Okβj(q)αj(q)為θ，建立連桿的勢能函數(shù)。

1）如圖1 所示，當(dāng)φ與θ 均小于90°時

式中：q=[q1,q2,···,qN]；αj(q)和βj(q)分別為連桿兩端中心的位置矢量，均是q的函數(shù)，由機(jī)械臂正向運(yùn)動學(xué)[16]求出。

2）如圖2 所示，當(dāng)φ 與θ 有一個角大于90°時

綜合圖1 和圖2，連桿j與障礙k的距離distk,j(q)定義為：

由式（1）～式（3），連桿勢能函數(shù)U(q)為：

式中：rj為連桿j的圓柱半徑；Rk為障礙k的半徑。

2.2 安全路徑檢查

構(gòu)建算法路線圖時，需要判斷兩個構(gòu)型qA，qB之間是否經(jīng)過障礙。對機(jī)械臂，兩個不同構(gòu)形的同一連桿的空間關(guān)系存在異面、相交、平行、重合，難以用連續(xù)的方式判斷，因此用離散方式，具體步驟如下：

步驟1 令m=1，初始化閾值E，a=‖qB-qA‖。

步驟3 計算K=0,1,···,2m-1-1，qtext=qA+，判斷distk,j(qtext)＞Rk+rj+s是否全部成立，若成立，m=m+1，執(zhí)行步驟2，否則，執(zhí)行步驟4。

步驟4 算法結(jié)束，qA，qB間經(jīng)過障礙。

步驟5 算法結(jié)束，qA，qB間不經(jīng)過障礙。

上述方法，依次將qA與qB在N維狀態(tài)空間中連線2m(m=1,2,···)等分，并依次判斷所有2m等分點處的碰撞情況，當(dāng)發(fā)現(xiàn)等分點處存在碰撞時，認(rèn)為qA，qB間經(jīng)過障礙；當(dāng)≤E且所有等分點均不碰撞時，認(rèn)為qA，qB間不經(jīng)過障礙。

2.3 機(jī)械臂避障路徑規(guī)劃算法流程

步驟1 在q1,q2,···,qN空間中隨機(jī)采點，求出障礙空間節(jié)點集合H、自由空間節(jié)點集合I，困難區(qū)域節(jié)點集合G，計算G中元素的勢能，初始化U，令t=0，集合record為空集。

步驟2 判斷H是否為空集，若為空，則執(zhí)行步驟6；否則，計算t=t+1，h=1，M=size(H) (M為集合H中節(jié)點數(shù)目)，執(zhí)行步驟3。

步驟3 判斷h≤M是否成立，若不成立，則將H中record對應(yīng)的h元素全部刪除，跳轉(zhuǎn)至步驟2；否則，執(zhí)行步驟4。

步驟4 用式（5）將H中元素qh(t-1)向G中隨機(jī)一個元素調(diào)整，計算p=rand，由式（5）計算qh(t)，由式（13）計算U(qh(t))，由式（6）計算

步驟6 用2.2 節(jié)的方法進(jìn)行安全路徑檢查，設(shè)置距離d，將安全路徑中與節(jié)點距離小于d的節(jié)點作為其相鄰節(jié)點，并用A*算法進(jìn)行路徑規(guī)劃。

步驟7 算法結(jié)束。

3 仿真驗證

3.1 勢能代價PRM 算法性能分析

為了測試勢能代價PRM 算法（提出算法）的性能，本文將其與PRM、OBPRM[9]、高斯采樣PRM 算法[10]進(jìn)行對比測試。在自由空間節(jié)點數(shù)目相同的條件下，規(guī)劃結(jié)果優(yōu)劣的評價指標(biāo)：成功率SUC和規(guī)劃路程代價S，表達(dá)式為：

式中：M為算法獨立運(yùn)行的次數(shù)；Msuc為在M次獨立運(yùn)行中，算法規(guī)劃成功的次數(shù)；Sγ為第 γ次路徑規(guī)劃成功時，規(guī)劃路程代價；mγ為第 γ次路徑規(guī)劃成功時，相鄰節(jié)點的間隔總數(shù)。

規(guī)劃的時間消耗也是算法的重要性能指標(biāo)，如果一個算法優(yōu)化結(jié)果質(zhì)量的提升，是以大幅增加其時間復(fù)雜度為代價，該算法往往不能被接受。本文在自由空間采用點數(shù)目相同條件下，比較各算法的規(guī)劃時間。

為了直觀體現(xiàn)所有算法在規(guī)劃空間的采點情況，先將其用于3 種不同環(huán)境下的質(zhì)點避障規(guī)劃；同時為了提高結(jié)果的可信度，將所有算法對同一問題獨立運(yùn)行20 次。所有算法的參數(shù)設(shè)置見表1；所有算法的成功率SUC、規(guī)劃路程代價S和優(yōu)化時間t指標(biāo)的均值見表2；所有算法的某次采樣及規(guī)劃情況如圖3～ 圖5 所示（圖中黃線表示規(guī)劃路徑，紅點表示自由空間采樣點，圓表示障礙空間，圖3b）、圖4b）和圖5b）中綠點表示障礙空間節(jié)點向困難區(qū)域節(jié)點調(diào)整成功后的位置)。

表1 算法的參數(shù)設(shè)置

由表2 可知：環(huán)境1 下，提出算法的SUC比PRM、高斯采樣PRM、OBPRM 算法分別多75%、15%、15%，其S相對PRM、高斯采樣PRM、OBPRM 算法分別降低了7.19%、4.28%、2.84%，其t相對PRM、高斯采樣PRM、OBPRM 算法分別降低了4.33%、12.82%、6.91%；環(huán)境2 下，提出算法的SUC比PRM、高斯采樣PRM、OBPRM 算法分別多80%、20%、25%，其S相對PRM、高斯采樣PRM、OBPRM 算法分別降低了8.98%、5.17%、4.31%，其t相對PRM、高斯采樣PRM、OBPRM 算法分別降低了8.11%、15.44%、10.55%；環(huán)境3 下，提出算法的SUC比PRM、高斯采樣PRM、OBPRM 算法分別多30%、0%、5%，其S相對PRM、高斯采樣PRM、OBPRM 算法分別降低了8.04%、3.95%、4.88%，其t相對PRM、高斯采樣PRM、OBPRM 算法分別降低了4.33%、10.39%、10.29%?？梢?，勢能代價PRM算法的規(guī)劃結(jié)果優(yōu)于其他3 種算法，且規(guī)劃時間少。由圖3b），圖4b)和圖5b）知，障礙空間節(jié)點調(diào)整成功后，其中絕大部分節(jié)點落在了障礙間狹窄區(qū)域，提高了PRM 算法通過狹窄區(qū)域的能力。

表2 算法的SUC、S 和t

圖3 環(huán)境1 下4 種算法的采樣及規(guī)劃情況

圖4 環(huán)境2 下4 種算法的采樣及規(guī)劃情況

圖5 環(huán)境3 下4 種算法的采樣及規(guī)劃情況

3.2 機(jī)械臂避障規(guī)劃問題的結(jié)果分析

將上述4 種算法應(yīng)用于機(jī)械臂避障路徑規(guī)劃問題，測試環(huán)境同3.1 節(jié)，且每種算法均對同一問題獨立運(yùn)行20 次。選取的KR5 機(jī)械臂的D-H 參數(shù)[16]如表3 所示。算法的參數(shù)設(shè)置如表4 所示。所有算法的SUC、S和t的均值如表5 所示。提出算法的規(guī)劃情況如圖6～ 圖9所示（圖中綠線、黑線、粉線、藍(lán)線、紅虛線分別表示關(guān)節(jié)2、3、4、5、6 在笛卡爾空間的運(yùn)動軌跡，左側(cè)機(jī)械臂為起始位姿）。

表3 KR5 機(jī)械臂D-H 參數(shù)

表4 算法的參數(shù)設(shè)置

由表5 可知，在環(huán)境1～ 2 下，雖PRM 算法的規(guī)劃時間顯著低于勢能代價PRM 算法，但由于其SUC在兩種環(huán)境下分別為5%和15%（即20 次運(yùn)行只有1 次和3 次規(guī)劃成功），且在環(huán)境3～ 4 下，PRM 算法沒有規(guī)劃成功，而提出算法在4 種環(huán)境中SUC均為100%，因此提出算法的規(guī)劃性能優(yōu)于PRM 算法；在環(huán)境1～ 4 下，提出算法的SUC比OBPRM 算法分別多70%、45%、100%、100%，在環(huán)境1～2 下，其S相對OBPRM 算法分別降低了4.67%和3.61%，其t相對OBPRM 算法分別降低了32.00%和32.50%，因此提出算法的規(guī)劃結(jié)果和時間均優(yōu)于OBPRM 算法；在環(huán)境1～ 4 下，提出算法的SUC比高斯采樣PRM 算法分別多0%、0%、10%、0%，其S相對高斯采樣PRM 算法分別降低了6.63%、4.79%、11.03%、7.67%，其t相對高斯采樣PRM 算法分別降低了57.12%、56.44%、63.03%、57.91%，因此提出算法的規(guī)劃結(jié)果略優(yōu)于高斯采樣PRM 算法，規(guī)劃時間優(yōu)于高斯采樣PRM 算法。

表5 算法的SUC、S 和t 均值

由圖6～ 圖9 可知，提出算法能使機(jī)械臂躲避所有障礙，并到達(dá)目標(biāo)位姿。

圖6 提出算法在環(huán)境1 下的規(guī)劃情況

圖7 提出算法在環(huán)境2 下的規(guī)劃情況

圖8 提出算法在環(huán)境3 下的規(guī)劃情況

圖9 提出算法在環(huán)境4 下的規(guī)劃情況

4 結(jié)論

1）在算法的性能分析中，勢能代價PRM 算法的SUC相對PRM、高斯采樣PRM、OBPRM 算法分別至少多30%、0%、5%，其S相對PRM、高斯采樣PRM、OBPRM 算法分別至少降低了7.19%、3.95%、2.84%，其t相對PRM、高斯采樣PRM、OBPRM 算法分別至少降低了4.33%、10.39%、6.91%，由此可見在性能上，提出算法在規(guī)劃結(jié)果和時間上均優(yōu)于PRM、高斯采樣PRM 和OBPRM 算法；且障礙空間節(jié)點調(diào)整成功后能落在了障礙間狹窄區(qū)域。

2）在機(jī)械臂避障路徑規(guī)劃問題上，勢能代價PRM 算法的規(guī)劃時間雖多于PRM 算法，但提出算法的SUC至少比PRM 算法多85%，且其SUC分別至少比高斯采樣PRM、OBPRM 算法多0%、45%，其S分別至少比高斯采樣PRM、OBPRM 算法降低了4.79%、3.91%，其t分別至少比高斯采樣PRM、OBPRM 算法降低了56.44%、32.00%，由此可見在該問題上，提出算法的規(guī)劃性能優(yōu)于PRM、高斯采樣PRM 和OBPRM 算法。

3）勢能代價PRM 算法能使機(jī)械臂在規(guī)劃中避開所有障礙，并到達(dá)目標(biāo)構(gòu)型。