張崇峰，陳敏花，侯月陽，張文婧

（1.上海航天技術(shù)研究院，上海 201109；2.上海航天控制技術(shù)研究所，上海 201109；3.上海市空間智能控制技術(shù)重點實驗室，上海 201109）

0 引言

冗余空間機(jī)械臂具有比空間維數(shù)更多的自由度，在空間軌跡規(guī)劃和空間避障方面具有更多的選擇和靈活度，適合完成在軌服務(wù)、維護(hù)維修任務(wù)中的目標(biāo)抓捕和精細(xì)操作，具有廣泛的應(yīng)用前景和巨大的研究意義，成為研究的熱點。

根據(jù)發(fā)展時間進(jìn)行劃分，避障算法可分為2 類傳統(tǒng)方法：典型的有基于構(gòu)形空間的幾何法和拓?fù)浞āD搜索法、人工勢場法等；智能方法主要有遺傳算法、人工神經(jīng)網(wǎng)絡(luò)法、模糊邏輯法、梯度投影法和改進(jìn)的梯度投影法，以及這些方法的混合方法等。

LOZANO-PEREZ提出，將機(jī)械臂的工作空間分為自由空間和障礙空間，并在自由空間內(nèi)利用啟發(fā)式搜索算法尋找機(jī)械臂的運動路徑。在眾多啟發(fā)式算法中，快速擴(kuò)展隨機(jī)樹算法特別適合多自由度機(jī)器人的運動規(guī)劃問題。BERTRAM 等提出了一種路徑規(guī)劃過程中的目標(biāo)選擇方法，該方法保證了新生長的節(jié)點不斷向著目標(biāo)點生長。STILMAN對快速擴(kuò)展隨機(jī)樹算法進(jìn)行了改進(jìn)，使得末端軌跡能夠滿足特定的約束條件，但是快速擴(kuò)展隨機(jī)樹算法搜索效率較低，不具有實時性。ZHAO 等已經(jīng)成功地運用基于構(gòu)型空間，即C 空間的幾何法使得機(jī)械手繞開障礙物順利采摘到水果。周芳等提出了一種路徑規(guī)劃算法，是基于C 空間分解路標(biāo)法，將路標(biāo)圖的搜索維數(shù)由六維降為三維，使路標(biāo)圖的搜索空間大大減少，該方法不僅縮短了在線實時規(guī)劃的時間，還提高了路徑規(guī)劃的成功率。

人工勢場法的基本思想是在機(jī)械臂的C 空間中構(gòu)造一個人工勢場，使得在該勢場中運動的機(jī)械臂受到其目標(biāo)位姿點引力場和C 空間障礙周圍斥力場的共同作用，無碰撞軌跡規(guī)劃可以通過搜索函數(shù)的下降方向?qū)崿F(xiàn)。盡管其是一種局部規(guī)劃方法，但由于便于數(shù)學(xué)描述且適用于多自由度機(jī)械臂，使用非常廣泛。

學(xué)者們努力將機(jī)器學(xué)習(xí)功能引入到現(xiàn)在的機(jī)器人中，使機(jī)器人更加智能化，由于環(huán)境復(fù)雜多變，這種方法在路徑規(guī)劃方面通常與其他一些算法結(jié)合使用。GAUTAM 等提出了一種基于遺傳算法和神經(jīng)網(wǎng)絡(luò)算法結(jié)合的方法，該算法能夠使無人機(jī)在三維環(huán)境下準(zhǔn)確避障，具有良好的效果。

人工勢場法存在局部最小問題，更適用于移動機(jī)器人的無碰撞軌跡規(guī)劃問題，而機(jī)器學(xué)習(xí)方法計算量大，不適用于計算資源緊張的空間機(jī)械臂。C空間法是一種全局規(guī)劃方法，通過求解整個C 空間，使得自由空間、障礙空間一目了然，因而可以按任何性能指標(biāo)搜索路徑，且具有完備解。然而在實際應(yīng)用中C 空間法存在2 個問題：1）對于多自由度的機(jī)械臂來說，求解整個C 空間需要很大的計算量，計算時間隨自由度的增加而呈指數(shù)級增長；2）C 空間中的障礙空間無法獲得解析表達(dá)式，只能通過離散化得到。以上這些問題限制了C 空間方法在空間機(jī)械臂中的應(yīng)用，因此，需要針對空間機(jī)械臂的工程要求和實際背景，對現(xiàn)有成果進(jìn)行改進(jìn)，獲得高效實用的無碰撞軌跡規(guī)劃算法。本文首先介紹碰撞檢測算法，進(jìn)一步研究無碰撞目標(biāo)構(gòu)型求解方案，提出無碰撞路徑試探性搜索算法并仿真驗證方法的有效性。

1 碰撞檢測算法

碰撞檢測算法是無碰撞的目標(biāo)構(gòu)型求解算法和無碰撞的路徑搜索算法運行的基礎(chǔ)。碰撞檢測算法是在工作空間中進(jìn)行的，可以測量得到機(jī)械臂的各個關(guān)節(jié)角，并通過運動學(xué)關(guān)系求得機(jī)械臂上各個點在中心體體坐標(biāo)系中的位置，而工作空間中的障礙物在中心體體坐標(biāo)系中的位置可以通過相關(guān)敏感器測量得到，這樣就具備了在工作空間進(jìn)行碰撞檢測的前提條件。

碰撞檢測算法是近似和多層次的。近似是指為了簡化檢測的計算量，對機(jī)械臂和障礙物進(jìn)行了一定的簡化處理。真實的機(jī)械臂由單自由度關(guān)節(jié)鉸接在一起的7 段圓柱體構(gòu)成，算法將其簡化為連接各個關(guān)節(jié)軸的線段，對舍去的部分，可以通過適當(dāng)擴(kuò)大障礙物的邊界加以修正，這樣的處理方法是方便的，也是符合工程實際和工程要求的。多層次是指這種碰撞檢測算法提出了3 個層次的碰撞檢測問題，并通過對時間或空間離散化的方法，逐步簡化，將高層次的碰撞檢測問題轉(zhuǎn)化為低層次的碰撞檢測問題，符合工程實際和工程要求。

這3 個層次為機(jī)械臂運動的碰撞檢測、機(jī)械臂構(gòu)型的碰撞檢測、機(jī)械臂上一點的碰撞檢測。首先將機(jī)械臂的運動過程進(jìn)行時間上的離散化，即將構(gòu)型連續(xù)變化的機(jī)械臂運動過程，離散為有限多個固定的機(jī)械臂構(gòu)型。在構(gòu)型空間中，將表示運動過程的連續(xù)曲線，離散為有限多個點，從而將運動過程的碰撞檢測問題轉(zhuǎn)化為有限多個機(jī)械臂構(gòu)型的碰撞檢測問題。只要任意一個構(gòu)型的機(jī)械臂與障礙物相碰，則整個運動過程是與障礙物相碰的，在構(gòu)型空間中這個運動的軌跡與構(gòu)型空間障礙物相交。接著對機(jī)械臂進(jìn)行空間上的離散化，將處于某一構(gòu)型的機(jī)械臂用幾條線段來表示，進(jìn)而將這些線段離散化為一系列的有限多個點，將機(jī)械臂構(gòu)型的碰撞檢測問題轉(zhuǎn)化為有限多個點的碰撞檢測問題。如果這些點中存在位于障礙物內(nèi)部的點，則認(rèn)為該機(jī)械臂構(gòu)型是與障礙物碰撞的，在構(gòu)型空間中，表示該構(gòu)型的點位于構(gòu)型空間障礙物內(nèi)部。

由此可知，碰撞檢測算法的基本問題是如何判斷一個點是否在障礙物之內(nèi)。采用凸多面體矢量內(nèi)積檢測算法，對于障礙物，總可以找到一個凸多面體的包絡(luò)，或者把障礙物分成幾部分，每一部分找到一個凸多面體的包絡(luò)，然后用這些凸多面體近似代替障礙物本身。記某個凸多面體的第個面的中心點為P，第個面的外法線矢量為n，記待檢測的點為，點位于這個凸多面體的充分必要條件是：對于凸多面體的所有的面都有

凸多面體判斷如圖1 所示。點只要位于其中一個凸多面體之內(nèi)，就認(rèn)為點位于障礙物之內(nèi)。利用碰撞檢測算法，可以判斷機(jī)械臂的某個運動過程或某個構(gòu)型是否與障礙物相碰。在此基礎(chǔ)上可以運行無碰撞的目標(biāo)構(gòu)型求解算法和無碰撞的路徑搜索算法。

圖1 凸多面體Fig.1 Schematic diagram of the convex polyhedron

2 無碰撞目標(biāo)構(gòu)型求解算法

無碰撞目標(biāo)構(gòu)型求解算法的主要思路是：首先，由任意初始構(gòu)型通過運動學(xué)軌跡規(guī)劃算法，得到一個滿足機(jī)械臂末端位置姿態(tài)要求的目標(biāo)構(gòu)型；然后，對目標(biāo)構(gòu)型進(jìn)行碰撞檢測，如果沒有碰撞，則將這個目標(biāo)構(gòu)型作為最終的無碰撞目標(biāo)構(gòu)型；如果發(fā)生碰撞，則在這個目標(biāo)構(gòu)型附近對各個關(guān)節(jié)角進(jìn)行微小調(diào)整，并對調(diào)整之后的目標(biāo)構(gòu)型反復(fù)進(jìn)行碰撞檢測，直到不發(fā)生碰撞為止，將得到的目標(biāo)構(gòu)型作為最終的無碰撞目標(biāo)構(gòu)型。

在對關(guān)節(jié)角進(jìn)行微調(diào)時，采取的方法為：在碰撞構(gòu)型下，改變某個關(guān)節(jié)角，得到新的構(gòu)型；將新的構(gòu)型作為假想的初始構(gòu)型，通過運動學(xué)軌跡規(guī)劃算法得到新的目標(biāo)構(gòu)型，并對其進(jìn)行碰撞檢測；如發(fā)生碰撞，則繼續(xù)改變該關(guān)節(jié)角的大小，直到得到的目標(biāo)構(gòu)型不與障礙物碰撞為止。在構(gòu)型空間中，該方法就是沿某個坐標(biāo)軸的方向進(jìn)行搜索的算法。

一般全方位機(jī)械臂為6 自由度，在需要避障的場景下需要增加自由度，即7 自由度機(jī)械臂，也可稱為冗余機(jī)械臂。

2.1 機(jī)械臂運動學(xué)算法

7 自由度機(jī)械臂速度級正運動學(xué)方程如下：

7 自由度機(jī)械臂速度級逆運動學(xué)方程如下：

式中：()為雅克比矩陣的轉(zhuǎn)置。

7 自由度機(jī)械臂有無數(shù)多組解，固定7 自由度機(jī)械臂的某個關(guān)節(jié)，使機(jī)械臂暫時退化為6 自由度，逆運動學(xué)求解方法簡化為

如果對固定的關(guān)節(jié)角進(jìn)行搜索，可以通過6 自由度逆運動學(xué)求解算法對機(jī)械臂7 自由度逆運動學(xué)的無數(shù)個解進(jìn)行搜索，這是無碰撞目標(biāo)構(gòu)型求解算法的基本思路。

2.2 無碰撞目標(biāo)構(gòu)型求解算法

無碰撞目標(biāo)構(gòu)型求解算法流程如圖2 所示。算法左側(cè)部分只運行一次，如果求解成功，則算法運行結(jié)束；如果失敗則轉(zhuǎn)至運行右側(cè)部分，右側(cè)為一個循環(huán)，不斷搜索滿足末端作用器位置和姿態(tài)的機(jī)械臂構(gòu)型，直至找到無碰撞的目標(biāo)構(gòu)型作為最終結(jié)果輸出。若算法由左側(cè)部分轉(zhuǎn)至了右側(cè)部分，則將左側(cè)部分的積分結(jié)果作為搜索的起始構(gòu)型，不斷改變其第一個關(guān)節(jié)角的角度，作為7 自由度逆運動學(xué)求解算法的積分初值構(gòu)型，直至找到一個與障礙物不碰撞，且積分過程中無奇異的目標(biāo)構(gòu)型。如果算法在右側(cè)部分循環(huán)了很多次仍沒有找到合乎要求的目標(biāo)構(gòu)型，則可以改變算法最初的積分初值，重新運行該算法。

圖2 無碰撞構(gòu)型求解算法Fig.2 Configuration algorithm without obstacles

下面選取一個算例，驗證無碰撞目標(biāo)構(gòu)型求解算法的有效性。給出機(jī)械臂的構(gòu)型及自由度配置，各節(jié)機(jī)械臂的旋轉(zhuǎn)軸指向（箭頭所指為右手定則正向）如圖3 所示，圖中的構(gòu)型為機(jī)械臂轉(zhuǎn)角均為零時的狀態(tài)。各節(jié)機(jī)械臂的幾何尺寸如圖4 所示，圖中各尺寸的基準(zhǔn)為機(jī)械臂各軸的軸線。系統(tǒng)中各節(jié)臂的安裝點位置參數(shù)見表1。

表1 機(jī)械臂桿件參數(shù)Tab.1 Position parameters of the manipulator bars

圖3 機(jī)械臂各軸旋轉(zhuǎn)方向Fig.3 Schematic diagram of the circumvolve directions of the manipulator

圖4 機(jī)械臂幾何尺寸Fig.4 Geometrical dimensions of the manipulator

目標(biāo)位姿：（2.0 m，0.3 m，0.5 m，-0.2，0.5，0）。障礙物參數(shù)：立方體頂點坐標(biāo)為（1.5，-0.2，0）、（1.5，-0.2，0.5）、（1.5，0.3，0）、（1.5，0.3，0.5）、（2，-0.2，0）、（2，-0.2，0.5）、（2，0.3，0）、（2，0.3，0.5）。初始積分關(guān)節(jié) 角：-1.570 7、3.141 4、1、0、0、0、0。積分時間：60 s。

右側(cè)部分積分初值的搜索方法為在左側(cè)部分積分結(jié)果的周圍以0.03為步長改變第一個關(guān)節(jié)角，作為右側(cè)部分的積分初值，即′=+0.03，=0，1，-1，2，-2，3，-3，…，當(dāng)=0 時即是左側(cè)部分的積分結(jié)果，算法的搜索過程見表2。

表2 搜索結(jié)果Tab.2 Search results

由表1 可知，算法左側(cè)的積分結(jié)果是與障礙物相碰的，然后算法轉(zhuǎn)至右側(cè)部分，右側(cè)部分在左側(cè)的積分結(jié)果周圍搜索，最終當(dāng)=3，即=-0.026 7時，求得了一個不與障礙物相碰且積分過程無奇異的構(gòu)型。得到無碰撞目標(biāo)構(gòu)型后，可在構(gòu)型空間中進(jìn)行無碰撞的路徑搜索。

3 無碰撞路徑試探性搜索算法

對于空間機(jī)器人的中央控制器來說，由于需要求解的構(gòu)型空間是7 個自由度的，計算負(fù)載相當(dāng)大，所以需要一種不需求解整個構(gòu)型空間的路徑搜索算法。本文提出一種試探性搜索算法，由起始構(gòu)型按照某一規(guī)律不斷向目標(biāo)構(gòu)型運動，如果運動過程中發(fā)生碰撞，則調(diào)整前進(jìn)的方向，直到找到一條不與障礙物發(fā)生碰撞的路徑。這樣，就不用求解整個構(gòu)型空間，而只需進(jìn)行碰撞檢測。于是，路徑搜索算法轉(zhuǎn)變?yōu)榱? 個子問題：1）如何設(shè)計在構(gòu)型空間中的試探規(guī)律；2）碰撞檢測；3）發(fā)生碰撞后如何調(diào)整。碰撞檢測問題已解決，介紹另外2 個問題的解決方案。

在構(gòu)型空間中，任意兩點之間都可以用若干條平行于坐標(biāo)軸的折線來連結(jié)。在有障礙物時，如果存在無碰撞的路徑，則該路徑一定也可以用若干條平行于坐標(biāo)軸的折線來近似，且近似之后的折線路徑也不與障礙物發(fā)生碰撞。一個二維構(gòu)型空間中的例子，如圖5 所示。S 為初始構(gòu)型，E 為目標(biāo)構(gòu)型，C1、C2、C3 均為障礙物。

圖5 二維構(gòu)型空間路徑Fig.5 Path in two-dimensional configuration space

對于一個存在障礙的構(gòu)型空間以及確定的路徑起點S 和終點E，總可以找到一個足夠大的正整數(shù)，能夠?qū)⒙窂椒指畹米銐蚣?xì)，使得由該路徑分割常數(shù)構(gòu)造的·個子運動，經(jīng)過一定的順序安排后，可以組成一條連接點S 和E 點的無碰撞軌跡。因此，試探性路徑搜索算法的關(guān)鍵就是通過試探的方法找到一個子運動的順序。即相當(dāng)于已知有個運動方向，以及沿每個方向上的個子運動，要求以某種順序?qū)⑦@些子運動進(jìn)行排列，使得這些子運動連起來形成的路徑不與障礙物相碰。

假設(shè)以1，2，…，分別為沿個坐標(biāo)軸方向上的子運動，試探規(guī)律為沿坐標(biāo)軸方向依次進(jìn)行試探，即子運動的試探順序為1，2，…，，1，2，…，，…，1，2，…，。如果第步試探發(fā)生碰撞，則將這一步的子運動調(diào)整到序列末尾，并沿下一個坐標(biāo)方向進(jìn)行試探。如果所有方向的試探結(jié)果均為碰撞，則返回上一步，并認(rèn)為-1 步發(fā)生碰撞（雖然該步碰撞檢測結(jié)果為不碰撞），并沿-1 步的下一個坐標(biāo)方向進(jìn)行試探。如此循環(huán)，直到整個序列不發(fā)生碰撞為止。如果無法得到無碰撞序列，則應(yīng)取更大的，重新進(jìn)行試探。算法流程如圖6 所示。

圖6 試探性搜索算法Fig.6 Tentative search algorithm

下面驗證路徑搜索算法的有效性和正確性。初始構(gòu)型和目標(biāo)構(gòu)型的參數(shù)見表3。障礙物中心點位置坐標(biāo)為[1.2 -0.8 1.7]，初始構(gòu)型和目標(biāo)構(gòu)型示意圖如圖7 所示。路徑分割常數(shù)=10。下面給出算法的搜索結(jié)果，試探次數(shù)為70 次，輸出的子運動序列見表4。

表4 子運動序列Tab.4 Child motion sequence

圖7 初始構(gòu)型和目標(biāo)構(gòu)型Fig.7 Schematic diagram of the initial and target configurations

表3 初始構(gòu)型和目標(biāo)構(gòu)型Tab.3 Initial and target configurations

可以發(fā)現(xiàn)，運動的前14 步?jīng)]有發(fā)生調(diào)整，子運動的順序為默認(rèn)的遞增順序。之后，默認(rèn)的試探路徑與障礙物發(fā)生了碰撞，按算法邏輯進(jìn)行了調(diào)整。最終，算法經(jīng)過70 次試探，找到了一條無碰撞路徑，與默認(rèn)的試探路徑相比，把4 個關(guān)于第3 關(guān)節(jié)角的子運動調(diào)到了路徑的末尾。

路徑搜索算法的三維仿真結(jié)果如圖8 所示。圖中可見，藍(lán)色立方體障礙物在機(jī)械臂附近，黑色桿件模擬機(jī)械臂末端桿件，各個桿件逐漸從一個構(gòu)型變化為另一構(gòu)型，機(jī)械臂運動過程中，在中間2 個構(gòu)型附近，最為接近障礙物，但是通過路徑搜索算法的調(diào)整，并沒有與障礙物發(fā)生碰撞，驗證了無碰撞路徑搜索算法的有效性和正確性。采用i5 處理器，當(dāng)前工況下，試探性搜索算法搜索時間為19.32 s，構(gòu)型空間避障算法為23.53 s，算法效率提升了17.89%。

圖8 路徑搜索算法仿真Fig.8 Simulation of path search arithmetic

4 結(jié)束語

針對空間機(jī)械臂操作服務(wù)的需求，本文提出了一種基于構(gòu)型空間的無碰撞軌跡規(guī)劃算法。該算法包含了碰撞檢測算法、無碰撞的目標(biāo)構(gòu)型求解算法、無碰撞的路徑搜索算法等3 個子算法，并通過仿真算例，驗證算法的有效性。與常規(guī)構(gòu)型空間避障算法相比，本文提出的試探性搜索算法效率提升了17.89%。目前在已知環(huán)境下進(jìn)行仿真，后續(xù)將研究考慮變結(jié)構(gòu)化環(huán)境下的避障優(yōu)化問題。