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

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

0 引言

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

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

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

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

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

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

1 碰撞檢測算法

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

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

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

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

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

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

2 無碰撞目標構型求解算法

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

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

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

2.1 機械臂運動學算法

7 自由度機械臂速度級正運動學方程如下：

7 自由度機械臂速度級逆運動學方程如下：

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

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

如果對固定的關節(jié)角進行搜索，可以通過6 自由度逆運動學求解算法對機械臂7 自由度逆運動學的無數個解進行搜索，這是無碰撞目標構型求解算法的基本思路。

2.2 無碰撞目標構型求解算法

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

圖2 無碰撞構型求解算法Fig.2 Configuration algorithm without obstacles

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

表1 機械臂桿件參數Tab.1 Position parameters of the manipulator bars

圖3 機械臂各軸旋轉方向Fig.3 Schematic diagram of the circumvolve directions of the manipulator

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

目標位姿：（2.0 m，0.3 m，0.5 m，-0.2，0.5，0）。障礙物參數：立方體頂點坐標為（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）。初始積分關節(jié) 角：-1.570 7、3.141 4、1、0、0、0、0。積分時間：60 s。

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

表2 搜索結果Tab.2 Search results

由表1 可知，算法左側的積分結果是與障礙物相碰的，然后算法轉至右側部分，右側部分在左側的積分結果周圍搜索，最終當=3，即=-0.026 7時，求得了一個不與障礙物相碰且積分過程無奇異的構型。得到無碰撞目標構型后，可在構型空間中進行無碰撞的路徑搜索。

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

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

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

圖5 二維構型空間路徑Fig.5 Path in two-dimensional configuration space

對于一個存在障礙的構型空間以及確定的路徑起點S 和終點E，總可以找到一個足夠大的正整數，能夠將路徑分割得足夠細，使得由該路徑分割常數構造的·個子運動，經過一定的順序安排后，可以組成一條連接點S 和E 點的無碰撞軌跡。因此，試探性路徑搜索算法的關鍵就是通過試探的方法找到一個子運動的順序。即相當于已知有個運動方向，以及沿每個方向上的個子運動，要求以某種順序將這些子運動進行排列，使得這些子運動連起來形成的路徑不與障礙物相碰。

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

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

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

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

圖7 初始構型和目標構型Fig.7 Schematic diagram of the initial and target configurations

表3 初始構型和目標構型Tab.3 Initial and target configurations

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

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

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

4 結束語

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