李磊 吳翔 包祥威 張景濤 方翔 曹毅

（1河南工業(yè)大學(xué)電氣工程學(xué)院，鄭州，450001；2河南工業(yè)大學(xué)機(jī)電工程學(xué)院，鄭州，450001）

0 引言

操作臂的避障路徑規(guī)劃是在操作臂的工作空間內(nèi)完成起止位姿之間安全連續(xù)路徑搜索，一般以路徑最短、搜索時間最短、或者能量消耗最優(yōu)其中之一作為評價標(biāo)準(zhǔn)。目前按照操作臂所處環(huán)境信息已知狀態(tài)，避障路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃?；谌致窂揭?guī)劃的方法主要有自由空間法、構(gòu)型空間法、拓?fù)浞ê蜄鸥穹ǖ??；诰植柯窂揭?guī)劃的方法有人工勢場法、遺傳算法、模糊邏輯算法、神經(jīng)網(wǎng)絡(luò)法、蟻群禁忌搜索算法[1]，粒子群算法[2]等等。這些算法有的適合靜態(tài)環(huán)境，有的適用于動態(tài)環(huán)境，有的搜索速度快但易陷入局部極優(yōu)，若對一些算法進(jìn)行改進(jìn)和結(jié)合，可以使改進(jìn)后的算法達(dá)到更理想的效果。

通過對比國內(nèi)外規(guī)劃算法研究分析發(fā)現(xiàn)[3-4]，遺傳算法、神經(jīng)網(wǎng)絡(luò)等智能仿生學(xué)算法應(yīng)用于三維空間模型避障運(yùn)動規(guī)劃較為困難，且搜索效率較低；人工勢場法等傳統(tǒng)算法容易早熟收斂；多項(xiàng)式軌跡插值擬合計(jì)算量大，耗時長；基于隨機(jī)采樣的避障路徑規(guī)劃算法具有強(qiáng)適應(yīng)性和概率完備性，高維空間避障路徑采用隨機(jī)采樣可以有效減少精確建模時間。

基于隨機(jī)采樣的應(yīng)用最多的算法為概率路標(biāo)法和快速擴(kuò)展隨機(jī)樹法。

概率路標(biāo)法是在操作臂的可行空間內(nèi)進(jìn)行隨機(jī)采樣，連接鄰近點(diǎn)構(gòu)建“路標(biāo)圖”，查詢路標(biāo)圖中起始位置到目標(biāo)位置最短可行路徑[5]。該方法雖實(shí)用性強(qiáng)，但是最終得到的路徑可能不是最優(yōu)路徑。

快速擴(kuò)展隨機(jī)樹法模擬樹枝生長產(chǎn)生枝葉進(jìn)行空間擴(kuò)展路徑搜索，是一種增量式采樣算法[6]，對于在線和離線路徑規(guī)劃都適用，在高維空間路徑規(guī)劃該算法時效性較好，且具有概率完備性，但每次規(guī)劃所得路徑隨機(jī)性大，且得到的路徑存在不光滑現(xiàn)象。

本文針對傳統(tǒng)操作臂避障路徑規(guī)劃算法進(jìn)行分析比較，選擇雙樹快速隨機(jī)擴(kuò)展連接樹RRT-Connect算法進(jìn)行改進(jìn)，并利用Dijkstra[7]算法和B樣條對改進(jìn)算法規(guī)劃路徑進(jìn)行優(yōu)化處理，以達(dá)到減少規(guī)劃路徑長度、時間和平滑路徑的目的。

1 RRT算法

RRT(快速隨機(jī)擴(kuò)展樹)算法是一種簡單采樣算法。為了算法描述方便，引入一些參數(shù)，這些參數(shù)的意義如表1所示。

表1 RRT算法各參數(shù)意義

2 改進(jìn)RRT-Connect算法

2.1 RRT-Connect算法步驟

2.2 RRT-Connect算法的改進(jìn)

3 路徑優(yōu)化

3.1 Dijkstra算法

通過 算法搜索最短路徑時，首先指定起始節(jié)點(diǎn)s，此外引進(jìn)兩個集合S和U。S是用來存放已求出最短路徑節(jié)點(diǎn)的集合，U是用來存放剩余節(jié)點(diǎn)的集合。算法初始時，將起始節(jié)點(diǎn)s加入集合S中，并用鄰近矩陣D記錄各個節(jié)點(diǎn)之間距離。通過不斷循環(huán)尋找U集合中距離起始節(jié)點(diǎn)最近的節(jié)點(diǎn)，將該節(jié)點(diǎn)放入到集合S中，并修改起始節(jié)點(diǎn)經(jīng)過該節(jié)點(diǎn)到其他節(jié)點(diǎn)的鄰近矩陣的值。

在算法中，采用 表示i節(jié)點(diǎn)與起始節(jié)點(diǎn)s之間的距離，兩個節(jié)點(diǎn)之間若連通，則w(i,j)表示i節(jié)點(diǎn)和j節(jié)點(diǎn)之間的權(quán)值，否則w(i,j)=∞。根據(jù)節(jié)點(diǎn)圖構(gòu)造鄰近矩陣Dn×n，n表示節(jié)點(diǎn)圖中節(jié)點(diǎn)個數(shù)。首先求出鄰近矩陣的第一行除去起始節(jié)點(diǎn)之外的最小值，并記為k，則第k個節(jié)點(diǎn)就是距離起始節(jié)點(diǎn)最近的節(jié)點(diǎn)，將k節(jié)點(diǎn)加入集合S中。然后從集合U中求出距離起始節(jié)點(diǎn)最近的節(jié)點(diǎn)m，在將m節(jié)點(diǎn)加入S集合之前，需要做以下比較：

根據(jù)式（4）計(jì)算出m節(jié)點(diǎn)到起始節(jié)點(diǎn)的最短距離，并對鄰近矩陣D內(nèi)的值進(jìn)行替換。依次循環(huán)，直至U中節(jié)點(diǎn)都加入集合S為止。Dijkstra算法流程圖如下；

3.2 路徑平滑處理

改進(jìn)RRT-Connect算法優(yōu)化后的路徑，在節(jié)點(diǎn)連接處存在折點(diǎn)、不平滑現(xiàn)象。操作臂沿此路徑執(zhí)行任務(wù)時，操作臂會出現(xiàn)快速變向現(xiàn)象，該現(xiàn)象必然會影響執(zhí)行任務(wù)的精度，并減少操作臂的使用周期。為了避免出現(xiàn)這種現(xiàn)象，必須對規(guī)劃路徑進(jìn)行平滑處理。B樣條曲線在路徑平滑方面效果顯著，其優(yōu)點(diǎn)之一便是修改局部路徑而不會對全部路線變動。

一般情況下，K階B樣條曲線方程可表示為：

本文采用三次B樣條對路徑進(jìn)行優(yōu)化處理，為使操作臂的運(yùn)動更加平穩(wěn)，盡可能多地插入平滑路徑點(diǎn)。

4 算法驗(yàn)證

為驗(yàn)證本文算法的有效性，筆者進(jìn)行了算法仿真試驗(yàn)。在每組實(shí)驗(yàn)中，起點(diǎn)均用符號‘S’表示，坐標(biāo)為(10,10)，終點(diǎn)均用符號‘G’表示，坐標(biāo)為(490,490)，坐標(biāo)點(diǎn)單位為cm。實(shí)驗(yàn)中各類算法的路徑規(guī)劃結(jié)果如圖2-圖6所示。

圖2 原始RRT算法仿真結(jié)果

圖6 B 樣條曲線仿真結(jié)果

實(shí)驗(yàn)1：原始RRT算法的路徑規(guī)劃。

實(shí)驗(yàn)2：在實(shí)驗(yàn)1的基礎(chǔ)上引入雙向搜索策略，采用RRT-Connect算法路徑規(guī)劃策略，并與RRT仿真結(jié)果進(jìn)行對比。

圖3中，規(guī)劃路徑分別由1號藍(lán)色路徑和2號紅色路徑拼接組成，3號路徑是去除無效節(jié)點(diǎn)之后完整規(guī)劃路徑。

圖3 RRT-Connect算法仿真結(jié)果

為了避免采樣算法的隨機(jī)性誤差，進(jìn)行40次仿真驗(yàn)證，得到驗(yàn)證結(jié)果如表2所示。

表2 RRT與RRT-Connect算法仿真結(jié)果對比

由表2可知，RRT-Connect算法在平均搜索時間、平均節(jié)點(diǎn)和平均路徑長度上遠(yuǎn)遠(yuǎn)優(yōu)于原始RRT算法。

實(shí)驗(yàn)3：在實(shí)驗(yàn)2的基礎(chǔ)上采用改進(jìn)RRT-Connect算法進(jìn)行路徑規(guī)劃。

圖4中，規(guī)劃路徑分別由1號藍(lán)色路徑和2號紅色路徑拼接組成，3號粉紅色路徑是去除無效節(jié)點(diǎn)之后完整規(guī)劃路徑。

圖4 改進(jìn)RRT-Connect算法仿真結(jié)果

由圖3和圖4對比可知改進(jìn)RRT-Connect算法減少了了無效節(jié)點(diǎn)的出現(xiàn)，且路徑長度優(yōu)于傳統(tǒng)RRT-Connect算法。

實(shí)驗(yàn)4：在實(shí)驗(yàn)3的基礎(chǔ)上引入Dijkstra算法搜索最短路徑。

圖5中，1號紅色路徑表示優(yōu)化之后的路徑，2號粉紅色路徑是改進(jìn)RRT-Connect規(guī)劃路徑。明顯看出，紅色路徑比粉紅色路徑長度更短。

圖5 融合Dijkstra算法仿真結(jié)果

為了避免采樣算法的隨機(jī)性誤差，進(jìn)行40次仿真驗(yàn)證，得到驗(yàn)證結(jié)果如表3所示。

表3 RRT-Connect與融合算法仿真結(jié)果對比

由表3可知，改進(jìn)RRT-Connect與Dijkstra融合算法IRRT-Connect在平均搜索時間、平均節(jié)點(diǎn)和平均路徑長度優(yōu)于改進(jìn)RRT-Connect算法。

實(shí)驗(yàn)5：在實(shí)驗(yàn)4的基礎(chǔ)上采用B樣條曲線對路徑進(jìn)行平滑處理。

圖6中，1號粉紅色路徑表示首次規(guī)劃路徑，2號紅色路徑表示Dijkstra算法優(yōu)化路徑，3號黑色路徑表示B樣條函數(shù)平滑處理后路徑。

由圖6中三條路徑對比可知，平滑處理后的黑色路徑在節(jié)點(diǎn)連接處比紅色路徑曲率更小。

5 仿真實(shí)驗(yàn)

仿真實(shí)驗(yàn)以越疆科技有限公司生產(chǎn)的Dobot魔術(shù)師為機(jī)械臂模型進(jìn)行三維空間路徑規(guī)劃，該機(jī)械臂三個自由度?？臻g環(huán)境包括操作臂模型和障礙物模型。

為易于觀察，圖7所示的規(guī)劃場景去掉障礙物上面一部分。在X方向上，障礙物中心點(diǎn)與機(jī)械臂的距離為25，在Y方向上為25，在Z方向上為0，長度單位均為cm。

圖7 操作臂規(guī)劃場景

在操作臂規(guī)劃場景內(nèi)應(yīng)用改進(jìn)RRT-Connect算法進(jìn)行避障路徑規(guī)劃，確定操作臂的起始坐標(biāo)和目標(biāo)坐標(biāo)分別為(20，10，18)和(25，10，20)并在空間中用“start”和“goal”標(biāo)注。

操作臂完成從起始目標(biāo)點(diǎn)向目標(biāo)節(jié)點(diǎn)的運(yùn)動路徑規(guī)劃如下。

首次規(guī)劃路徑如圖8中紅色路徑所示。

圖8 操作臂首次規(guī)劃路徑

首次規(guī)劃路徑呈鋸齒狀，存在多余路徑節(jié)點(diǎn)，利用Dijkstra算法進(jìn)行二次路徑優(yōu)化。優(yōu)化路徑如圖9中綠色路徑所示。

圖9 操作臂規(guī)劃優(yōu)化路徑

雖然優(yōu)化路徑與首次規(guī)劃路徑相比，減少了路徑長度，但路徑在節(jié)點(diǎn)連接處存在折角。采用B樣條曲線對路徑進(jìn)行光滑處理，平滑后路徑如圖10中3號粉紅色路徑所示。

圖1 Dijkstra算法流程圖

圖10 操作臂規(guī)劃平滑路徑

選取相同的操作臂避障空間環(huán)境，分別進(jìn)行原算法和改進(jìn)的融合算法路徑規(guī)劃。路徑規(guī)劃結(jié)果如圖11所示。

圖11 算法規(guī)劃路徑對比

在操作臂三維空間配置同樣的規(guī)劃場景，進(jìn)行RRT算法、RRT-Connect算法和改進(jìn)RRT-Connect多次仿真驗(yàn)證，取驗(yàn)證平均結(jié)果作為對比標(biāo)準(zhǔn)，仿真結(jié)果如表4所示。

通過表4可以發(fā)現(xiàn)，本文提出的規(guī)劃算法對串聯(lián)機(jī)械臂的實(shí)際使用具有參考價值。

表4 三維空間算法仿真結(jié)果對比

6 結(jié)論

針對機(jī)械臂在抓取目標(biāo)物體的路徑規(guī)劃問題,本文提出了一種改進(jìn)RRT-Connect和Dijkstra融合算法的路徑規(guī)劃策略。引入雙樹目標(biāo)偏執(zhí)、極致貪婪策略提高路徑搜索速率，減少了傳統(tǒng)RRT-Connect算法中無效節(jié)點(diǎn)數(shù)量、搜索時間和路徑長度，再利用Dijkstra算法進(jìn)行首次路徑優(yōu)化，得到最短路徑。為了避免優(yōu)化后的路徑在節(jié)點(diǎn)連接處存在折點(diǎn)、不平滑現(xiàn)象，必須對規(guī)劃路徑進(jìn)行平滑處理，其中利用B樣條差值的方法得到一條光滑平緩的完整運(yùn)動曲線。仿真表明，該算法更加適合機(jī)械臂的實(shí)際運(yùn)行與操作，進(jìn)而證明該算法的可行性與實(shí)用性。

本文是在障礙物位置信息已知的情況下，對空間中機(jī)械臂的避障軌跡規(guī)劃進(jìn)行研究，但在實(shí)際應(yīng)用環(huán)境中，障礙物環(huán)境位置往往處于未知狀態(tài)，因此對復(fù)雜環(huán)境下的機(jī)械臂軌跡規(guī)劃有待進(jìn)一步研究和改善。