陳肇星，徐向榮，查文斌，楊 陽(yáng)

（安徽工業(yè)大學(xué) 機(jī)械工程學(xué)院，馬鞍山 243000）

0 引言

機(jī)器人是現(xiàn)代制造業(yè)的關(guān)鍵裝備，也是智能時(shí)代將數(shù)據(jù)融入生活的重要設(shè)備。七自由度機(jī)械臂由于具備獨(dú)特的冗余運(yùn)動(dòng)特性和近似于人手臂的靈活性，被廣泛應(yīng)用。為了開(kāi)發(fā)冗余機(jī)器人的工作潛力，工程師們對(duì)冗余機(jī)械臂的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性進(jìn)行研究，機(jī)械臂的路徑規(guī)劃是一個(gè)重點(diǎn)方向。而路徑規(guī)劃有兩種研究方向，在關(guān)節(jié)空間規(guī)劃和在任務(wù)空間進(jìn)行規(guī)劃。本文中就采用在機(jī)械臂的關(guān)節(jié)空間規(guī)劃，避免在工作空間進(jìn)行路徑規(guī)劃時(shí)，需要大量的冗余臂逆運(yùn)動(dòng)求解而影響路徑規(guī)劃速度。

串聯(lián)機(jī)械臂主流的路徑規(guī)劃方法有三種：基于地圖構(gòu)建法[1]，人工勢(shì)場(chǎng)法[2]和基于隨機(jī)采樣方法。人工勢(shì)場(chǎng)法是在目標(biāo)的引力和障礙物的斥力作用下驅(qū)動(dòng)。機(jī)械臂，得到運(yùn)行路徑。該方法容易在新環(huán)境擴(kuò)展，但是機(jī)器人可能會(huì)陷入到局部最小值，同時(shí)當(dāng)面對(duì)較多障礙物時(shí)，計(jì)算量會(huì)變得很大?；诘貓D構(gòu)建法是將機(jī)器人的工作空間劃分為不同的網(wǎng)格空間，然后在劃分的網(wǎng)格空間中按照一定的規(guī)則找到可行路徑，主要分為路標(biāo)法[3]和柵格法[4]?；陔S機(jī)采樣的方法主要分為兩種方法：概率地圖法（Probability Road Map，PRM）[5]和快速擴(kuò)展隨機(jī)樹(shù)法（Rapidlyexploring Random Tree，RRT）[6]。兩者之間的主要差別是RRT算法中的新節(jié)點(diǎn)是在隨機(jī)樹(shù)中距離該節(jié)點(diǎn)最近的節(jié)點(diǎn)的相連接；而概率地圖法在查詢階段中產(chǎn)生的新節(jié)點(diǎn)是與一定范圍內(nèi)所有的可連接節(jié)點(diǎn)相連接。因此PRM算法產(chǎn)生的地圖會(huì)更加復(fù)雜。

RRT算法的工作流程比較簡(jiǎn)單。以自由空間中一個(gè)初始點(diǎn)為根節(jié)點(diǎn)，采用隨機(jī)采樣的方法進(jìn)行擴(kuò)展，生成無(wú)碰撞的隨機(jī)擴(kuò)展樹(shù)。當(dāng)擴(kuò)展樹(shù)不停生長(zhǎng)，在自由空間中逐步覆蓋，最終會(huì)得到足夠靠近終點(diǎn)的節(jié)點(diǎn)，從而生成從初始到目標(biāo)的路徑。RRT算法自身有計(jì)算簡(jiǎn)單，適用于多維空間的優(yōu)點(diǎn)。很多文章證明利用該算法可以實(shí)現(xiàn)在二維空間甚至高維空間的路徑規(guī)劃，因此這種方法可以應(yīng)用于機(jī)械臂的路徑規(guī)劃。當(dāng)機(jī)械臂為冗余機(jī)械臂時(shí)，其逆運(yùn)算過(guò)程會(huì)很復(fù)雜，消耗大量時(shí)間。因此在更高維度的關(guān)節(jié)空間內(nèi)進(jìn)行路徑規(guī)劃是一種需求。利用RRT算法及改進(jìn)算法可以在機(jī)械臂的關(guān)節(jié)空間中進(jìn)行機(jī)械臂的路徑規(guī)劃，避免了大量的逆運(yùn)算，提高了工作效率。

1 基本RRT算法

RRT算法作為一種基于隨機(jī)采樣的空間搜索算法，最大的特點(diǎn)就是簡(jiǎn)潔，高效。它是在確定初始狀態(tài)和終點(diǎn)狀態(tài)的基礎(chǔ)上，隨機(jī)樹(shù)在自由空間中通過(guò)采樣、擴(kuò)展，最終完成搜索，得到一條從初始狀態(tài)到目標(biāo)狀態(tài)的安全可行路徑。RRT算法實(shí)現(xiàn)簡(jiǎn)單，不需要對(duì)障礙物進(jìn)行精確建模，在高維空間中任有良好的工作效果。因此，該算法被廣泛應(yīng)用于移動(dòng)機(jī)器人和多自由度串聯(lián)機(jī)械臂的路徑規(guī)劃。其偽代碼如算法1所示。

在算法中，qs是起始點(diǎn)，qG是終點(diǎn)，λ是生長(zhǎng)步長(zhǎng)，T是隨機(jī)樹(shù)。

在算法1中的幾個(gè)主要函數(shù)，是RRT算法的重要組成，而且在后面介紹的改進(jìn)算法中仍有利用。下面對(duì)這幾個(gè)函數(shù)進(jìn)行說(shuō)明：

GetRandomNode()，采樣函數(shù)：在自由空間中隨機(jī)選取一個(gè)采樣節(jié)點(diǎn)qrand。采樣點(diǎn)之間的生成是相互獨(dú)立的，一般情況下是在空間中進(jìn)行均勻分布采樣。

NearNeighbor()，最近點(diǎn)函數(shù)：對(duì)于已經(jīng)產(chǎn)生搜索隨機(jī)樹(shù)，求取隨機(jī)樹(shù)中離函數(shù)輸入節(jié)點(diǎn)q最近的節(jié)點(diǎn)qclosest。利用以下公式：

StearNode()，擴(kuò)展函數(shù)：是指從自由空間中的節(jié)點(diǎn)q1向另一節(jié)點(diǎn)q2方向擴(kuò)展步長(zhǎng)λ，生成新的節(jié)點(diǎn)。其公式為：

ColionFree()，碰撞檢測(cè)函數(shù)：對(duì)兩個(gè)節(jié)點(diǎn)之間生成的路徑進(jìn)行碰撞檢測(cè)，以確定兩點(diǎn)之間的路徑是安全的。若不產(chǎn)生碰撞，兩節(jié)點(diǎn)之間可以產(chǎn)生路徑并返回可行邏輯值。

GettheGoal，到達(dá)判定：當(dāng)隨機(jī)樹(shù)的新節(jié)點(diǎn)生長(zhǎng)到目標(biāo)節(jié)點(diǎn)附近時(shí)，則判定到達(dá)目標(biāo)，否則繼續(xù)繼續(xù)進(jìn)行循環(huán)操作。

2 改進(jìn)的快速搜索隨機(jī)樹(shù)算法

基于RRT算法的改進(jìn)算法有很多，相當(dāng)一部分應(yīng)用于平面的移動(dòng)機(jī)器人，但很多對(duì)于串聯(lián)機(jī)器人有著很高的參考價(jià)值。Frazzoli等人利用對(duì)路徑重構(gòu)，提出了RRT*算法，使得得到的路徑具有漸進(jìn)最優(yōu)特性[7]。并且，在RRT*算法的基礎(chǔ)之上，Nasir等人提出了具有智能采樣和路徑優(yōu)化功能的RRT*-smart算法[8]，解決了RRT*算法中收斂速度慢的問(wèn)題。還有為了改進(jìn)在 狹窄復(fù)雜環(huán)境下規(guī)劃效率而采用的多向隨機(jī)樹(shù)擴(kuò)展改進(jìn)[9]。隨著機(jī)器語(yǔ)言的發(fā)展，也出現(xiàn)了與智能算法相結(jié)合的采樣改進(jìn)。Shiarlis等人提出的RLT*（Rapidly-Exploring Learing Trees）算法[10]，降低了反向強(qiáng)化學(xué)習(xí)計(jì)算開(kāi)銷。同時(shí)還有一些利用神經(jīng)網(wǎng)絡(luò)的學(xué)習(xí)能力與快速搜索隨機(jī)樹(shù)算法相結(jié)合，進(jìn)行路徑規(guī)劃的算法[11]。

本文提出的Gm_RRT算法不僅適用于二維空間，同樣在高維空間內(nèi)有著較快的運(yùn)行速度與運(yùn)算效果。該算法的改進(jìn)點(diǎn)主要有以下三個(gè)部分。

首先，本文提出的改進(jìn)算法和常見(jiàn)的幾種改進(jìn)快速搜索隨機(jī)樹(shù)算法一樣，采用了概率偏置策略，即會(huì)以一定的概率將目標(biāo)點(diǎn)設(shè)置為隨機(jī)采樣節(jié)點(diǎn)qrand，加速搜索隨機(jī)樹(shù)向目標(biāo)點(diǎn)擴(kuò)展，提高效率。

其次，由于每次產(chǎn)生新節(jié)點(diǎn)都會(huì)進(jìn)行碰撞檢測(cè)，如果發(fā)生碰撞，會(huì)舍棄新節(jié)點(diǎn)并選取新的隨機(jī)采樣節(jié)點(diǎn)qrand，需要重新計(jì)算隨機(jī)樹(shù)中各個(gè)節(jié)點(diǎn)與新隨機(jī)采樣節(jié)點(diǎn)的距離。此時(shí)產(chǎn)生新節(jié)點(diǎn)的過(guò)程基本上是隨機(jī)的。而本文中可以對(duì)隨機(jī)點(diǎn)產(chǎn)生過(guò)程進(jìn)行干涉處理，保持了隨機(jī)樹(shù)生長(zhǎng)的隨機(jī)性與生長(zhǎng)的合理性。采用的方式是在保證采樣過(guò)程是在隨機(jī)的基礎(chǔ)上，一次采樣一組隨機(jī)采樣節(jié)點(diǎn)，并以采樣節(jié)點(diǎn)到目標(biāo)點(diǎn)的距離由小到大來(lái)進(jìn)行排序，如下面圖1所示。當(dāng)發(fā)生碰撞而隨機(jī)樹(shù)無(wú)法進(jìn)行擴(kuò)展，就以次距離的采樣節(jié)點(diǎn)為方向進(jìn)行擴(kuò)展，由此來(lái)重復(fù)以上工作。當(dāng)能夠擴(kuò)展新的節(jié)點(diǎn)或者隨機(jī)采樣節(jié)點(diǎn)組為空時(shí)，會(huì)重新開(kāi)始新的采樣。

圖1 Gm-RRT算法中隨機(jī)點(diǎn)數(shù)組的產(chǎn)生和排序

最后就是引入自適應(yīng)的引力勢(shì)場(chǎng)[12]。改進(jìn)算法是結(jié)合了RRT算法和人工勢(shì)場(chǎng)法的優(yōu)缺點(diǎn)后，提出了在目標(biāo)點(diǎn)處設(shè)置引力勢(shì)場(chǎng)。通過(guò)在目標(biāo)點(diǎn)處設(shè)置虛擬引力勢(shì)場(chǎng)，由此來(lái)引導(dǎo)隨機(jī)樹(shù)趨向目標(biāo)節(jié)點(diǎn)生長(zhǎng)，生長(zhǎng)的過(guò)程如圖2所示。而影響生成節(jié)點(diǎn)的引力勢(shì)場(chǎng)大小也會(huì)隨節(jié)點(diǎn)的生長(zhǎng)條件發(fā)生改變，主要是機(jī)械臂末端位姿和目標(biāo)位姿的差距和機(jī)械臂和障礙物之間的碰撞情況。該算法也因此具有自適應(yīng)的特點(diǎn)。

圖2 Gm-RRT算法節(jié)點(diǎn)的產(chǎn)生

算法中向目標(biāo)點(diǎn)生長(zhǎng)的引力比例系數(shù)為kd，當(dāng)搜索樹(shù)生長(zhǎng)發(fā)生碰撞時(shí)，會(huì)采用次距離的采樣點(diǎn)，同時(shí)改變引力比例系數(shù)的大小，使隨機(jī)樹(shù)的生長(zhǎng)偏向當(dāng)前隨機(jī)采樣點(diǎn)的方向。這樣，能夠讓隨機(jī)擴(kuò)展樹(shù)的生長(zhǎng)過(guò)程仍保持著向自由空間探索的趨勢(shì)。

偽代碼中，kp是目標(biāo)引力源對(duì)節(jié)點(diǎn)產(chǎn)生引力的比例系數(shù)。結(jié)合式(1)，可得新算法中的節(jié)點(diǎn)擴(kuò)展公式為：

其中：

在經(jīng)過(guò)Gm-RRT算法對(duì)機(jī)器人的路徑點(diǎn)進(jìn)行規(guī)劃后得到的路徑點(diǎn)，由于新算法是依賴于RRT算法進(jìn)行的，在計(jì)算過(guò)程中會(huì)出現(xiàn)隨機(jī)的路徑情況，在局部的過(guò)程中也可能會(huì)出現(xiàn)路徑點(diǎn)波折的情況。因此，在算法中加入對(duì)機(jī)器人的路徑一定的優(yōu)化是不可或缺的一部分。

在常見(jiàn)的路徑優(yōu)化方法中，有一個(gè)方法是利用三角形兩邊之和大于第三邊的規(guī)律，由路徑的終點(diǎn)逐步向前尋找合適的路徑。在該過(guò)程中，得到的路徑可能會(huì)出現(xiàn)較小的轉(zhuǎn)角，這對(duì)機(jī)器人的運(yùn)行并不是友好的。在本文中，優(yōu)化的過(guò)程是選用彈性帶理論，對(duì)得到的路徑進(jìn)行優(yōu)化處理。而且利用彈性帶原理在計(jì)算中需要的計(jì)算量是比較少的，運(yùn)算的效率會(huì)比較高。

彈性帶理論最早的提出是為了解決旅行商問(wèn)題的一種方法[13]。在這里，我們可以將得到的路徑看成是一條具備彈性的帶子，路徑點(diǎn)是受到了整條帶子能量的影響而產(chǎn)生形變。

彈性帶原理中的能量函數(shù)為：

而彈性帶上點(diǎn)的受到的力的迭代公式為：

在本文中已經(jīng)利用RRT算法運(yùn)行得到了機(jī)器人的可行路徑，在當(dāng)我們接下來(lái)彈性帶原理對(duì)得到的路徑進(jìn)行優(yōu)化處理，此時(shí)為了加快運(yùn)算的效率，我們這里可以將路徑節(jié)點(diǎn)的“受力”情況分析為：

那么可以設(shè)計(jì)當(dāng)路徑節(jié)點(diǎn)受到的力超出一定范圍時(shí)，就可以認(rèn)為節(jié)點(diǎn)是受到周圍節(jié)點(diǎn)的影響的，在受到彈性力的基礎(chǔ)上，節(jié)點(diǎn)會(huì)發(fā)生移動(dòng)，描述方程為：

在算法的最后對(duì)機(jī)器人運(yùn)行路徑進(jìn)行優(yōu)化的時(shí)候仍舊需要對(duì)機(jī)器人在運(yùn)行過(guò)程中的可能產(chǎn)生的碰撞進(jìn)行檢測(cè)。以保證機(jī)器人運(yùn)行的安全。

經(jīng)過(guò)前文的分析，可以得到新算法Gm-RRT算法的偽代碼如下算法2所示：

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

本節(jié)中，我們將會(huì)利用新算法進(jìn)行路徑規(guī)劃仿真實(shí)驗(yàn)，以驗(yàn)證新算法的可行性。

3.1 在二維平面的仿真分析

本文中實(shí)驗(yàn)的搭建基于MATLAB平臺(tái)，文中分別利用RRT算法，RRT*算法和改進(jìn)Gm-RRT算法在平面中進(jìn)行路徑規(guī)劃的仿真分析，驗(yàn)證了算法的有效性。仿真環(huán)境硬件信息：計(jì)算機(jī)CPU為Intel Core i7-7700，GPU為GTX1060，內(nèi)存容量為8G。實(shí)驗(yàn)參數(shù)設(shè)置：偏置概率為P=0.2，步長(zhǎng)設(shè)置為15。對(duì)于改進(jìn)算法中，將采樣點(diǎn)個(gè)數(shù)設(shè)置為3，初始kd值設(shè)置為0.3，其值依照算法依次遞減0.1。為了使算法的運(yùn)行環(huán)境更加客觀，本文中采用的仿真地圖有兩個(gè)，分別為簡(jiǎn)單環(huán)境地圖和復(fù)雜環(huán)境地圖。

如圖4所示，地圖中的黑色區(qū)域?yàn)檎系K物，地圖的大小為，兩張地圖中，起始點(diǎn)坐標(biāo)為(50，50)，終點(diǎn)坐標(biāo)為(950，950)。其中，紅色線條即為算法得到的軌跡。

圖4 RRT算法與其改進(jìn)算法在平面環(huán)境的仿真分析

為了將得到的結(jié)果與數(shù)據(jù)顯示的更加清晰，本文在經(jīng)過(guò)多次重復(fù)試驗(yàn)后將相關(guān)數(shù)據(jù)統(tǒng)計(jì)和整理，得到表的結(jié)果，這樣可以顯得更加直觀。

表1 RRT算法與改進(jìn)算法在場(chǎng)景一的仿真統(tǒng)計(jì)

表2 RRT算法與改進(jìn)R算RT法 在場(chǎng)景R二R的T*仿 真統(tǒng)G計(jì)m-RRT

從統(tǒng)計(jì)的結(jié)果可以看出，改進(jìn)算法在機(jī)器人的路徑規(guī)劃中的表現(xiàn)是比較符合預(yù)期的。Gm-RRT算法符合RRT算法的優(yōu)點(diǎn)，并在一定程度上體現(xiàn)了自己能對(duì)不同環(huán)境進(jìn)行適應(yīng)的特點(diǎn)。

3.2 冗余機(jī)械臂的路徑規(guī)劃仿真

本文中進(jìn)行路徑規(guī)劃仿真的機(jī)器人為庫(kù)卡的LBR iiwa14工業(yè)機(jī)器人。該款機(jī)器人為七自由度冗余串聯(lián)機(jī)器人，該款機(jī)器人在工業(yè)上具有較為廣泛的應(yīng)用。該機(jī)器人具體的DH參數(shù)將在表3中給出。圖5中顯示了機(jī)器人的模型與關(guān)節(jié)的坐標(biāo)。

圖5 機(jī)器人iiwa14的模型與關(guān)節(jié)坐標(biāo)信息

表3 冗余機(jī)械臂iiwa14的DH參數(shù)

本文進(jìn)行實(shí)驗(yàn)的平臺(tái)是基于MATALB平臺(tái)的Robotics System Toolbox工具箱，該工具箱提供設(shè)計(jì)、模擬和測(cè)試機(jī)械手、移動(dòng)機(jī)器人和仿人機(jī)器人的工具和算法。對(duì)于機(jī)械手和仿人機(jī)器人，工具箱包括碰撞檢查、軌跡生成、正向和反向運(yùn)動(dòng)學(xué)以及使用剛體樹(shù)表示的動(dòng)力學(xué)的算法。對(duì)于移動(dòng)機(jī)器人，它包括映射、定位、路徑規(guī)劃、路徑跟蹤和運(yùn)動(dòng)控制的算法。工具箱提供了常見(jiàn)工業(yè)機(jī)器人應(yīng)用的參考示例。它還包括一個(gè)商用工業(yè)機(jī)器人模型庫(kù)，工程師可以導(dǎo)入、可視化和模擬這些模型。

路徑規(guī)劃的計(jì)算過(guò)程中，部分過(guò)程是需要利用到機(jī)械臂的運(yùn)動(dòng)學(xué)，以計(jì)算機(jī)械臂的末端和各個(gè)臂桿在工作空間中的位姿。由表3給出的機(jī)器人DH參數(shù)，進(jìn)行計(jì)算，得到各個(gè)剛體連桿的位姿。式(6)中nTn+1是n+1關(guān)節(jié)相對(duì)于n關(guān)節(jié)的齊次變換矩陣。

對(duì)機(jī)械臂相鄰的坐標(biāo)系進(jìn)行連續(xù)的坐標(biāo)變換，可以得到末端桿的相對(duì)于機(jī)器人基坐標(biāo)系的變換矩陣為：

機(jī)械臂末端的位姿表示為x=[rT，ψT]T∈R6，其中表示末端相對(duì)于慣性參考系的位置，ψ是末端對(duì)應(yīng)于慣性參考系的姿態(tài)，這里ψ是用x-y-z歐拉角的順序進(jìn)行表示的。在這里對(duì)機(jī)器人的運(yùn)動(dòng)規(guī)劃仿真設(shè)置如下：機(jī)械臂的基坐標(biāo)系與參考坐標(biāo)系重合，且起始狀態(tài)末端點(diǎn)的位姿是 x1=[-1.40，0.04，-3.05，0.5，0.61，0.18]，最終狀態(tài)的末端點(diǎn)位姿是x2=[1.02，0.58，-1.06，-0.61，0.68，0.58]，障礙物為藍(lán)色立方體，大小為0.2×0.4×0.6。在路徑規(guī)劃過(guò)程中的步長(zhǎng)設(shè)置為0.8。

在已知起始狀態(tài)和終點(diǎn)狀態(tài)末端點(diǎn)位姿的情況下，先利用MATLAB中Robotics System工具箱求逆解，得到這兩個(gè)狀態(tài)下機(jī)械臂的關(guān)節(jié)值。將求得的關(guān)節(jié)值作為關(guān)節(jié)空間的起始點(diǎn)和終點(diǎn)。機(jī)器人關(guān)節(jié)起點(diǎn)為Q1=[0.95，1.36，-0.2，-0.6，0.18，1.1，-0.9]，終點(diǎn)是Q2=[2.3，1.38，-0.16，0，0.17，-0.23，-0.94]。利用Gm-RRT算法在關(guān)節(jié)空間對(duì)機(jī)械臂進(jìn)行軌跡規(guī)劃，主要的運(yùn)行步驟即如第二小節(jié)所示，最后得到了機(jī)器人在設(shè)置環(huán)境中規(guī)劃的路徑。

機(jī)器人在運(yùn)行過(guò)程中，在對(duì)機(jī)器人規(guī)劃得到的路徑如果不夠柔順，機(jī)器人在工作過(guò)程中關(guān)節(jié)會(huì)出現(xiàn)明顯的頓挫，那么機(jī)器人在經(jīng)過(guò)長(zhǎng)時(shí)間的工作后會(huì)對(duì)關(guān)節(jié)的機(jī)械結(jié)構(gòu)造成較大的影響。若將得到的機(jī)器人路徑在不進(jìn)行柔順處理的基礎(chǔ)上直接連接運(yùn)行，那么從數(shù)學(xué)上看其導(dǎo)數(shù)是不連續(xù)的，也就是運(yùn)行的速度和加速度都是不連續(xù)的。因此，在本文可以利用B樣條曲線對(duì)機(jī)器人的運(yùn)行軌跡進(jìn)行擬合，使得機(jī)器人軌跡更加柔順，運(yùn)行更加安全。

樣條曲線最早源自于在工程制圖中利用可變形的條狀工具繪制光滑的曲線。而B(niǎo)樣條函數(shù)曲線可以利用函數(shù)進(jìn)行準(zhǔn)確描述的一種樣條曲線，具備局部支撐性和集合不變性等優(yōu)點(diǎn)，因此在工程中會(huì)被廣泛地應(yīng)用。將其應(yīng)用到機(jī)器人運(yùn)動(dòng)規(guī)劃的最大優(yōu)勢(shì)是對(duì)于K次的B樣條曲線，其導(dǎo)函數(shù)是K-1次連續(xù)的。這也就是指機(jī)器人的軌跡在利用較高階B樣條函數(shù)擬合后，其速度和加速度函數(shù)可以是連續(xù)且可導(dǎo)的。以此來(lái)延續(xù)機(jī)器人結(jié)構(gòu)的使用壽命。K次B樣條的曲線的描述公式為：

式中，Nj，k(u)(i=0，1，…，n)表示的是樣條曲線的基函數(shù)，dj(j=0，1，…，n表示的是樣條曲線的控制頂點(diǎn)?；瘮?shù)由德布爾-考克斯（Cox-de Boor）遞推公式求得。自變量u是經(jīng)過(guò)歸一化處理的時(shí)間向量節(jié)點(diǎn)。在這里為了保證樣條曲線是經(jīng)過(guò)路徑的起始點(diǎn)和終點(diǎn)，需要在自變量的首尾均設(shè)置k+1的重復(fù)度，即u0=u1=…=uk=0；un+k=un+k+1=…=un+2k=1?；瘮?shù)的推導(dǎo)公式為：

之前已經(jīng)利用改進(jìn)RRT算法對(duì)機(jī)器人的路徑進(jìn)行了運(yùn)動(dòng)規(guī)劃，已經(jīng)得到了期望的一些路徑點(diǎn)，此時(shí)利用已知的路徑點(diǎn)反求對(duì)應(yīng)B樣條曲線的控制點(diǎn)，這樣不會(huì)錯(cuò)過(guò)已設(shè)好的路徑點(diǎn)。

由B樣條曲線函數(shù)可以得到，若設(shè)已知曲線路徑對(duì)應(yīng)節(jié)點(diǎn)個(gè)數(shù)為m+1，其控制頂點(diǎn)的個(gè)數(shù)為d+1，那么兩者之間的關(guān)系為：

此時(shí)，我們已經(jīng)利用RRT算法求得了機(jī)器人關(guān)節(jié)路徑點(diǎn)，此時(shí)求取曲線控制點(diǎn)仍缺少k-1個(gè)方程，此時(shí)，需要加入關(guān)節(jié)首尾速度與加速度的約束條件即可。可以寫(xiě)為：

在這里，根據(jù)實(shí)際的情況，可以將機(jī)器人初始和終止?fàn)顟B(tài)的速度和加速度都設(shè)置為零。即v0=ve=0；a0=ae=0。此時(shí)就可以得到足夠數(shù)目的約束方程，就可以求出對(duì)應(yīng)的控制點(diǎn)。

當(dāng)機(jī)器人在有障礙物的環(huán)境中基于改進(jìn)RRT算法進(jìn)行路徑規(guī)劃后，在利用B樣條曲線進(jìn)行擬合，得到了對(duì)應(yīng)的機(jī)器人的柔順關(guān)節(jié)路徑，其結(jié)果如圖6所示，機(jī)器人關(guān)節(jié)按照樣條函數(shù)進(jìn)行運(yùn)動(dòng)控制，那么在運(yùn)行過(guò)程中就可以減少運(yùn)動(dòng)中抖動(dòng)對(duì)其產(chǎn)生的傷害，做到延長(zhǎng)其壽命的目的。

圖6 機(jī)器人運(yùn)動(dòng)柔順處理后的關(guān)節(jié)曲線

仿真運(yùn)動(dòng)過(guò)程如圖3所示。從一系列圖中，我們可以明顯看到，機(jī)械臂從初始位置開(kāi)始，沿著新算法得到的軌跡，繞過(guò)藍(lán)色障礙物，最終到達(dá)指定位置。平均耗時(shí)約7.6s，說(shuō)明了新算法在高維空間進(jìn)行路徑規(guī)劃的可行性。

圖3 彈性帶節(jié)點(diǎn)受力分析

圖7 冗余機(jī)械臂在有障礙物環(huán)境中的運(yùn)動(dòng)過(guò)程

在基于MATLAB平臺(tái)對(duì)冗余自由度機(jī)器人iiwa14進(jìn)行運(yùn)動(dòng)規(guī)劃仿真，采用了基于改進(jìn)的快速搜索隨機(jī)樹(shù)算法，并且利用了B樣條曲線函數(shù)對(duì)得到的路徑進(jìn)行了柔順處理。在經(jīng)過(guò)仿真實(shí)驗(yàn)后可以看到新算法在機(jī)器人的關(guān)節(jié)空間中進(jìn)行運(yùn)動(dòng)規(guī)劃的可行性，且得到的運(yùn)行路徑也是比較符合預(yù)期的效果。

4 結(jié)語(yǔ)

本文研究了冗余機(jī)械臂的路徑規(guī)劃算法，提出了基于快速搜索隨機(jī)樹(shù)算法的改進(jìn)算法Gm-RRT算法。首先該算法能夠在平面環(huán)境下適應(yīng)不同的環(huán)境，體現(xiàn)了自身的特點(diǎn)和優(yōu)點(diǎn)。同時(shí)在機(jī)械臂的避障路徑規(guī)劃中，在關(guān)節(jié)空間中進(jìn)行了路徑規(guī)劃，得到了較好的路徑軌跡。證明了新算法能適用于七自由度機(jī)械臂路徑規(guī)劃的特點(diǎn)。該算法在實(shí)時(shí)性有所不足，也是下一步的改進(jìn)重點(diǎn)。