吳長征，岳 義，，韋寶琛，劉明芳

（1.上海交通大學 機械系統與振動國家重點實驗室，上海 200240；2.上海航天設備制造總廠，上海 200245）

0 引言

冗余機械臂是指關節(jié)空間維數大于任務空間維數的機械臂。由于具有冗余的自由度，機械臂在避障、避關節(jié)奇異、避關節(jié)角極限、增加特定方向的可操作度、最小化能量消耗、最小化關節(jié)力矩以及容錯能力上都有優(yōu)異的性能［1］。這些優(yōu)點促進了冗余機械臂的廣泛應用與研究。

避障規(guī)劃是機器人運動規(guī)劃的重要部分，也是機械臂進行自主運動的關鍵環(huán)節(jié)。姜力等［2］基于冗余機械臂的零空間自運動特性，利用臂型角并結合人工勢場法給出了一種避障控制方法。同樣是利用冗余機械臂的自運動特性，申浩宇等［3］基于協同操作雅可比矩陣提出協調避障算法。Khatib［4］于1986年最早提出了人工勢場法，并將其用于移動機器人以及機械臂的避障規(guī)劃，設定與目標位姿的引力場，設定與障礙物之間的斥力場，使機器人沿著勢場的負梯度方向運動。Park等［5］改進了人工勢場法，將其用于動態(tài)環(huán)境下的避障規(guī)劃?；诹憧臻g的方法，利用了雅可比數值迭代，需要考慮數值迭代的穩(wěn)定性，人工勢場法及其改進在使用過程中需要避免局部極小值帶來的問題。

依托于計算機硬件性能的規(guī)劃方法也受到了學者們的廣泛關注。從基于搜索的規(guī)劃方法，如A*方法、D*方法以及它們的改進算法ARA*（Anytime RePairing A*）等［6］方法，到基于采樣的規(guī)劃方法，如RRT（Rapidly Exploring Random Tree）方法、PRM（Probabilistic Roadmap）方法以及它們的改進算法T-RRT（Transition-based RRT）等［7］方法。這些方法具有很突出的規(guī)劃能力，但都需要進行大量的運算?；谒阉鞯腁*方法、D*方法適合低維度機器人，如移動機器人等，對于高維度的機械臂規(guī)劃難以勝任；基于采樣的方法，如RRT-CONNECT（RRT Connect）方法、PRM 方法能很好地解決高維空間的問題，但隨機性很強，且同一個問題能得到多個可行解，不能保證最優(yōu)軌跡；基于采樣規(guī)劃的改進優(yōu)化方法，如RRT*［8］方法、PRM*方法能夠優(yōu)化得到最優(yōu)軌跡，但處理效率低，甚至在指定的規(guī)劃時間內難以完成求解［9］。

上述避障規(guī)劃方法都有其各自適應的領域，但缺乏廣泛的通用性。針對當前廣泛采用的正交冗余機械臂的避障規(guī)劃問題，本文利用機械臂自身的構型特點，提出一種基于S-R-S（球副-轉動副-球副）構型的避障規(guī)劃新方法，避免采用現有數值迭代和隨機采樣方法帶來的問題。

1 冗余機械臂構型分析及碰撞檢測模型

1.1 機械臂構型及其等效

現有的機器人廠商推出的協作型機器人大多采用正交型7 自由度冗余機械臂，如KUKA 的iiwa、ABB 的yumi、YASIKAWA 的SDA5、SCHUNK 的Lwa 4D等。本文以德國SCHUNK 公司的Lwa4D機械臂為例說明。此類型機械臂的關節(jié)配置模式如圖1 所示。

圖1 Schunk-Lwa4D 機械臂的關節(jié)配置模式Fig.1 The joint configuration of the Schunk-Lwa4D manipulator

在圖1中，此類型關節(jié)配置的特點為：前3 個關節(jié)（J1-J2-J3）、后3 個關節(jié)（J5-J6-J7）軸線相互垂直，配合形成球運動副。上述7 自由度正交冗余機械臂可以等效為S-R-S 構型，如圖2 所示。由于存在兩個等效球副，機械臂的自運動性能優(yōu)越。

圖2 等效的S-R-S 構型的機構示意圖Fig.2 Schematic diagram of the equivalent S-R-S configuration

1.2 機器人桿件及環(huán)境障礙物模型

機械臂與環(huán)境障礙物的碰撞檢測是進行避障規(guī)劃的基礎，為提升環(huán)境障礙物檢測的效率，需要對環(huán)境障礙物進行預處理建模。根據包圍盒原理［10］，可將環(huán)境障礙物簡化為圓柱體或球體兩種類型，如圖3 所示。這兩種處理形式對計算機內存及計算資源的消耗小，同時，在碰撞檢測過程中的計算量也小。相比于球體，圓柱體在空間中等效占據的體積與實物更為接近。機器人桿件以及外部障礙物一般較為規(guī)則，在本文的研究中，將機器人桿件及外部障礙物均處理成圓柱體模型。

圖3 機器人桿件及環(huán)境障礙物簡化模型處理Fig.3 Simplified models for the robot bars and the environmental obstacles

對機器人桿件及環(huán)境障礙物均處理為圓柱體凸體對象，多個凸體之間可以找到一個分割面，且凸體對象上兩點間的距離既是局部最小值，也是全局最小值。這些性質使得圓柱體簡化模型更適合作碰撞檢測。

通過上述簡化后，機械臂的碰撞檢測等效為2個圓柱體之間的碰撞檢測。2 個圓柱體上各點之間的幾何關系如圖4 所示。

圖4 碰撞檢測幾何關系Fig.4 Geometric relation for the collision detection

MN表示AB與CD的公垂線段，且有

式中：{λ|λ＜0或0 ≤λ≤1或λ＞1}，{μ|μ＜0或0 ≤μ≤1或μ＞1}，根據λ與μ的9 種不同取值［11］，得到桿件之間9 種不同的碰撞模式。

2 基于S-R-S構型特點的避障規(guī)劃方法

2.1 避障規(guī)劃模型

根據圖2等效S-R-S 構型，可知肘部關節(jié)的位置在一個球面上，且球面區(qū)域為單連通區(qū)域。這種單連通性也能很好地映射到機械臂關節(jié)空間，球面上的連續(xù)曲線對應到關節(jié)空間同樣也為連續(xù)區(qū)域。

圖5為機械臂等效避障規(guī)劃示意圖。單臂在障礙環(huán)境中的避障規(guī)劃等效于E點，在球面上的規(guī)劃加上關節(jié)4（q4）的規(guī)劃以及末端工具位置T的規(guī)劃。可知在S點和W點均等效于一個球關節(jié)，因此，在規(guī)劃完E點和q4之后，T點的規(guī)劃與E點的規(guī)劃類似，均為在球面上的規(guī)劃。

圖5 機械臂等效避障規(guī)劃示意圖Fig.5 Schematic diagram of the equivalence obstacle avoidance planning for the manipulator

機械臂的構型為串聯結構形式，前面靠近基座的關節(jié)的運動會對后面關節(jié)的狀態(tài)產生影響。為排除這種強耦合的影響，將單臂的避障規(guī)劃分成肘部笛卡爾位置規(guī)劃、第2 根臂桿規(guī)劃、末端工具規(guī)劃這3 個部分依次進行。即先進行桿件SE的無碰撞規(guī)劃，再進行桿件EW的無碰撞規(guī)劃，最后再進行末端工具桿件WT的規(guī)劃。

2.2 肘部笛卡爾位置規(guī)劃

當機械臂臂桿SE與障礙物碰撞時，可以分為3種情況：1）障礙物完全在E點的可達球面內；2）障礙物一端在E點的可達球面內另一端，在可達球面外（包括在球面上）；3）障礙物的兩端均在可達球面外（包括在球面上）。如圖6 所示為桿件SE與障礙物碰撞的3 種情況。在圖6中，FG為障礙物，且S、F、G3 點必定構成一個平面。過S點作SFG的垂直平面，該垂平面與FG的交點為H點。

圖6 桿件SE 與障礙物碰撞的3 種情況Fig.6 Three collision cases of the bar SE and the obstacle

由于臂桿SE碰撞只可能發(fā)生在可達區(qū)域球面內，上述3 種情況均可轉化為圖7。在圖7（a）中，從球心S點向障礙物FG作射線，分別與球面相交于E′和E″點，障礙物在球面上的投影為弧段?所在的區(qū)域。因此，E點在球面的運動只要不經過弧段?，則臂桿SE便不會與障礙物發(fā)生碰撞，如圖7（b）所示。則為等效的障礙物桿件。可認為桿件處于SE′和SE″均為臨界碰撞狀態(tài)（桿件和障礙物均已作安全距離處理），因此，只需要調整SE在SFG平面內的位置便能避開障礙物。將E′和E″點記作關鍵點，當∠ESE'≤∠ESE''時，避障調整的方向為朝向E′點運動避障（調整量?。粗畡t朝向E″點運動避障。

圖7 桿件SE 碰撞情況的等效轉化Fig.7 Equivalent transformation of the collision cases of the bar SE

由上得知避障調整的方向后，以過S點以FG為法線的SFG的垂平面（記為SHM）為界，僅對障礙物的其中一端作出分析（保留HG段，另一端分析方法相同）。

現在針對圖8 對SE桿的避障規(guī)劃作出說明。桿SE與障礙物發(fā)生碰撞的情況下才作出調整。在對障礙物和機器人桿件完成簡化的基礎上，障礙物的軸心線段與球心總能構成一個平面。發(fā)生碰撞時，桿件與障礙物的軸心線在此平面中相交于一點，即如圖8（a）所示。由于在運動過程中，桿件SE必將會經過此平面，因此，避障需要保證桿件軸心線段SE與障礙物HG無交點。

如圖8（b）所示，將實際障礙物通過相似變換為等效障礙物。不管何種情況下，總能找到等效障礙物與球面的交點，此處的交點可認為是臨界無碰撞狀態(tài)下的肘部的位置E′，這也是在運動過程中最有可能發(fā)生碰撞的位置，因此，將其設為關鍵危險點。

根據向量幾何關系可得E′點的向量表達式為

圖8 桿件SE 的避障規(guī)劃示意圖Fig.8 Schematic diagram of the obstacle avoidance planning for the bar SE

式中：norm為對向量取模操作；S為球心位置向量；G為障礙物端點位置向量；E為肘部關節(jié)點位置向量。

由于在規(guī)劃過程中起始狀態(tài)和終止狀態(tài)均為已知，可以認為該肘部的臨界無碰撞軌跡為經過球面3 點的球面曲線。在球面上進行規(guī)劃，可以參考和借鑒在空間中進行多點規(guī)劃，但又略有不同，其不同在于規(guī)劃后的曲線上所有路徑點需要滿足均在球面上的約束，即肘部的桿長約束。

設定肘部的起始位置為Estart，設定肘部的終止位置為Egoal，如圖9 所示。對肘部的規(guī)劃轉化為在球面上尋找一條經過Estart、Egoal和E'的光滑球面曲線。經過空間3 點的曲線求解有Bézier 曲線、B 樣條曲線、Hermitte 插值曲線等方法。但由于肘部滿足桿長約束條件，這些常用的方法不能直接使用。

圖9 球面Bézier 曲線在軸平面中的投影Fig.9 Projection of the spherical Bézier curve onto the axis plane

Bézier 曲線因其具有良好的幾何特性，廣泛應用于計算機圖形等方面。一條n次Bézier 曲線的定義為

式中：Bi，n(u)為基函數，且采用n次Bernstein 多 項式［12］，其定義為

式（3）中的幾何系數Qi稱為控制點，用戶根據需求定義，用來控制曲線的走向。式（3）和式（4）共同描述了經過空間相關點的連續(xù)曲線。本文采用3次Bézier 曲線，含有｛Q0，Q1，Q2，Q3｝4 個控制點。且其具有較好的性質，如其端點的切線方向平行于向量Q1?Q0和Q3?Q2，并且該曲線包含在這4 個控制點形成的凸包內。3 次Bézier 曲線的形式為

關于控制點的取法成了曲線的關鍵，現給出該曲線的求解方法。圖9為球面Bézier 曲線在軸平面中的投影。圖9中，Q0=Estart，Q3=E'，Q6=Egoal。該曲線在過S點的與平面SFG和SHM的公垂平面中的投影仍然為Bézier 曲線。由于兩端弧線具有一個共同點E′，此曲線共有7 個控制點。為保證曲線的光滑，需要保證在E′左右的切線方向一致，即

為保證求得的投影曲線始終在投影圓內，需要保證所有的控制點均在圓內。即

為保證曲線變化的均勻性，可以設定Q1Q2在線段EstartE′上的投影K1K2、Q4Q5，在線段EgoalE′上的投影K3K4均為1/k（k＞2）處（注：圖9中取在1/3 處），且Q2Q4與坐標軸平行（具體根據投影平面給出）。若在區(qū)域外，則取其與外圓的交點為相應控制點?，F僅以投影面為XZ、曲線EstartE′為例說明，根據幾何關系可以得到

式（3）～式（9）即為肘部運動軌跡在投影面的計算公式，此時C(u)為二維坐標(C(u)x，C(u)z)。為得到球面上的三維曲線，還需作如下處理：

式中：sgn(Estart_Y)為運動軌跡點的Y軸坐標與起始點的Y軸坐標符號相同。

通過式（2）～式（10）便完成了對肘部笛卡爾位置的規(guī)劃。

2.3 第2 根臂桿規(guī)劃

對第2 根臂桿EW規(guī)劃時，設定桿件EW發(fā)生碰撞且其他桿件不發(fā)生碰撞，此時桿SE在障礙物過球心的平面內。為使得其他桿件的運動軌跡盡量不變，可只通過調整關節(jié)角q4來避障。為使得桿件EW避開障礙物的調節(jié)最為迅速有效，此時E處的肘部（關節(jié)4）的軸線垂直于障礙物過球心的平面SHG可得到此效果。

根據圖10 的避障規(guī)劃示意圖可知，在E處的肘部（關節(jié)4）的軸線垂直于障礙物過球心的平面時，調整桿件EW遠離碰撞區(qū)域運動到臨界無碰撞狀態(tài)EW′，可以實現桿件EW的避障規(guī)劃。桿件EW的調整方向沿著向量HG的方向。

圖10 桿件EW 避障規(guī)劃示意圖Fig.10 Schematic diagram of the obstacle avoidance panning for the bar EW

臨界無碰撞狀態(tài)關節(jié)4 的關節(jié)角，即為

根據q4的角度值以及其軸線的方向，便能夠得到機械臂在EW處于臨界碰撞狀態(tài)的構型。若SE桿不發(fā)生碰撞則肘部的笛卡爾軌跡無需調整，只需要調整E點姿態(tài)，使得肘部關節(jié)q4軸線為平面EHG的法線方向。

2.4 末端工具桿件規(guī)劃

對桿件WT的調整規(guī)劃，由于桿件WT同樣等效在一個球形關節(jié)上，桿WT與SE的運動類似，均為在球面的運動，唯一不同的是WT對應的球關節(jié)在笛卡爾空間中的位置是浮動的。由于上述的避障方法是從基座開始向外擴展計算調整量的，因此，WT桿件對應的球關節(jié)在笛卡爾空間中的位置只取決于前面4 個關節(jié)的關節(jié)角。根據前面的內容可以知道，當桿件WT開始調整時，前面4 個關節(jié)角是已知的，因此，WT調整時W點的位置也是已知的，WT的調整也唯一確定。

2.5 避障規(guī)劃算法總結

結合上述基于S-R-S 構型特點的避障規(guī)劃3 部分，總結得到正交冗余機械臂的避障規(guī)劃新方法如下（注：i表示桿件的順序取值1、2、3）。

算法：基于S-R-S 構型特點的正交冗余機械臂避障規(guī)劃方法

1）利用碰撞檢測計算方法：式（1），計算危險桿件集合Φ0

2）求取i=arg min｛Φ0｝

3）Ifi=1

4）肘部位置規(guī)劃：式（2）～式（10）；從Φ0中剔除i=1→得Φ1

5）求取i=arg min｛Φ1｝

6）Ifi=2

7）關節(jié)4 獨立規(guī)劃：式（11）；從Φ1中剔除i=2→得Φ2

8）求取i=arg min｛Φ2｝

9）Ifi=3

10）腕部位置規(guī)劃：式（2）～式（10）；

Endif

3 實驗驗證

結合Schunk-Lwa4D 機械臂，對上述提出的基于S-R-S 構型特點的避障規(guī)劃新方法進行實驗驗證。實驗時設定機械臂經過的障礙物為長方體立柱。該長方體立柱的尺寸為0.07 m×0.07 m×0.61 m。該障礙物與機械臂對應的安裝位置為處于機械臂坐標系的X方位為0.45 m，Y和Z方位均為0。圖11 給出了機械臂的相關尺寸以及基于S-R-S 構型特點的避障規(guī)劃算法的避障過程。

圖11中起始時雄克機械臂的關節(jié)角為（0.52，?1.57，0，0.09，0，0.75，0）（單位為弧度，下同），終止時的雄克機械臂的關節(jié)角為（?0.52，?1.57，0，0.09，0，0.75，0）。機械臂的肘部處會發(fā)生碰撞，此時第1 根和第2 根桿件均會會發(fā)生碰撞。

分別采用基于采樣的避障規(guī)劃算法RRT-con?nect 和基于本文提出的基于S-R-S 構型特點的避障規(guī)劃算法進行試驗，得到兩種算法避障過程中7 個關節(jié)的關節(jié)角變化曲線。針對同一障礙物采用RRT-connect 算法進行2 次避障規(guī)劃實驗，得到的關節(jié)角度值變化曲線如圖12 所示

采用基于S-R-S 構型特點的避障規(guī)劃新方法進行避障實驗，實驗中雄克機械臂有效地避開了障礙物，繪制機械臂避障過程中關節(jié)角變化過程如圖13所示。通過觀察實驗過程和記錄的數據驗證了基于S-R-S 構型的避障規(guī)劃算法的有效性。

圖12 基于RRT-connect 避障規(guī)劃方法的關節(jié)角變化曲線Fig.12 Variation curves of the joint angles obtained by the obstacle avoidance planning method based on the RRTconnection

圖13 基于S-R-S 構型特點的避障規(guī)劃新方法的關節(jié)角變化曲線Fig.13 Variation curves of the joint angles obtained by the novel obstacle avoidance planning method based on the S-R-S configuration

由圖12 可知：2 次采用RRT-connect 得到的關節(jié)運動結果不一致，且關節(jié)角變化無規(guī)律，說明了現有的基于采樣的方法具有很強的隨機性。

比較圖12 和圖13 可知：新的避障規(guī)劃方法能夠減少關節(jié)不必要的運動，如圖13中關節(jié)3、5、7 均保持不變；采用該規(guī)劃方法，機械臂有效避開了障礙物，運動唯一確定，不存在上述的隨機性問題。同時圖13 也驗證了基于S-R-S 構型特點的避障規(guī)劃新方法的有效性和可行性。

4 結束語

本文將正交7 自由度冗余機械臂構型等效為SR-S 構型，通過分析該構型的特點將機械臂的避障規(guī)劃分為肘部笛卡爾位置規(guī)劃、第2 根臂桿規(guī)劃、末端工具規(guī)劃3 個部分。利用Schunk-Lwa4D 機械臂進行了現有RRT-connect 方法和新提出的基于SR-S 特點的避障規(guī)劃方法的避障實驗，通過對比分析實驗得到了機械臂關節(jié)角變化曲線，得到新的避障規(guī)劃方法解決了基于采樣規(guī)劃方法的隨機性問題，也驗證了基于S-R-S 構型特點的避障規(guī)劃新方法的有效性和可行性。后續(xù)將對擴展基于S-R-S 構型特點的避障規(guī)劃新方法研究復雜環(huán)境障礙物的適用范圍，并優(yōu)化規(guī)劃所得的軌跡。