吳長(zhǎng)征，岳 義，，韋寶琛，劉明芳

（1.上海交通大學(xué) 機(jī)械系統(tǒng)與振動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，上海 200240；2.上海航天設(shè)備制造總廠，上海 200245）

0 引言

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

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

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

上述避障規(guī)劃方法都有其各自適應(yīng)的領(lǐng)域，但缺乏廣泛的通用性。針對(duì)當(dāng)前廣泛采用的正交冗余機(jī)械臂的避障規(guī)劃問(wèn)題，本文利用機(jī)械臂自身的構(gòu)型特點(diǎn)，提出一種基于S-R-S（球副-轉(zhuǎn)動(dòng)副-球副）構(gòu)型的避障規(guī)劃新方法，避免采用現(xiàn)有數(shù)值迭代和隨機(jī)采樣方法帶來(lái)的問(wèn)題。

1 冗余機(jī)械臂構(gòu)型分析及碰撞檢測(cè)模型

1.1 機(jī)械臂構(gòu)型及其等效

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

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

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

圖2 等效的S-R-S 構(gòu)型的機(jī)構(gòu)示意圖Fig.2 Schematic diagram of the equivalent S-R-S configuration

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

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

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

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

通過(guò)上述簡(jiǎn)化后，機(jī)械臂的碰撞檢測(cè)等效為2個(gè)圓柱體之間的碰撞檢測(cè)。2 個(gè)圓柱體上各點(diǎn)之間的幾何關(guān)系如圖4 所示。

圖4 碰撞檢測(cè)幾何關(guān)系Fig.4 Geometric relation for the collision detection

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

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

2 基于S-R-S構(gòu)型特點(diǎn)的避障規(guī)劃方法

2.1 避障規(guī)劃模型

根據(jù)圖2等效S-R-S 構(gòu)型，可知肘部關(guān)節(jié)的位置在一個(gè)球面上，且球面區(qū)域?yàn)閱芜B通區(qū)域。這種單連通性也能很好地映射到機(jī)械臂關(guān)節(jié)空間，球面上的連續(xù)曲線對(duì)應(yīng)到關(guān)節(jié)空間同樣也為連續(xù)區(qū)域。

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

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

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

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

當(dāng)機(jī)械臂臂桿SE與障礙物碰撞時(shí)，可以分為3種情況：1）障礙物完全在E點(diǎn)的可達(dá)球面內(nèi)；2）障礙物一端在E點(diǎn)的可達(dá)球面內(nèi)另一端，在可達(dá)球面外（包括在球面上）；3）障礙物的兩端均在可達(dá)球面外（包括在球面上）。如圖6 所示為桿件SE與障礙物碰撞的3 種情況。在圖6中，F(xiàn)G為障礙物，且S、F、G3 點(diǎn)必定構(gòu)成一個(gè)平面。過(guò)S點(diǎn)作SFG的垂直平面，該垂平面與FG的交點(diǎn)為H點(diǎn)。

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

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

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

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

現(xiàn)在針對(duì)圖8 對(duì)SE桿的避障規(guī)劃作出說(shuō)明。桿SE與障礙物發(fā)生碰撞的情況下才作出調(diào)整。在對(duì)障礙物和機(jī)器人桿件完成簡(jiǎn)化的基礎(chǔ)上，障礙物的軸心線段與球心總能構(gòu)成一個(gè)平面。發(fā)生碰撞時(shí)，桿件與障礙物的軸心線在此平面中相交于一點(diǎn)，即如圖8（a）所示。由于在運(yùn)動(dòng)過(guò)程中，桿件SE必將會(huì)經(jīng)過(guò)此平面，因此，避障需要保證桿件軸心線段SE與障礙物HG無(wú)交點(diǎn)。

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

根據(jù)向量幾何關(guān)系可得E′點(diǎn)的向量表達(dá)式為

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

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

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

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

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

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

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

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

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

為保證求得的投影曲線始終在投影圓內(nèi)，需要保證所有的控制點(diǎn)均在圓內(nèi)。即

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

式（3）～式（9）即為肘部運(yùn)動(dòng)軌跡在投影面的計(jì)算公式，此時(shí)C(u)為二維坐標(biāo)(C(u)x，C(u)z)。為得到球面上的三維曲線，還需作如下處理：

式中：sgn(Estart_Y)為運(yùn)動(dòng)軌跡點(diǎn)的Y軸坐標(biāo)與起始點(diǎn)的Y軸坐標(biāo)符號(hào)相同。

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

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

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

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

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

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

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

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

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

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

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

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

1）利用碰撞檢測(cè)計(jì)算方法：式（1），計(jì)算危險(xiǎn)桿件集合Φ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）關(guān)節(jié)4 獨(dú)立規(guī)劃：式（11）；從Φ1中剔除i=2→得Φ2

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

9）Ifi=3

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

Endif

3 實(shí)驗(yàn)驗(yàn)證

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

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

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

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

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

圖13 基于S-R-S 構(gòu)型特點(diǎn)的避障規(guī)劃新方法的關(guān)節(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 得到的關(guān)節(jié)運(yùn)動(dòng)結(jié)果不一致，且關(guān)節(jié)角變化無(wú)規(guī)律，說(shuō)明了現(xiàn)有的基于采樣的方法具有很強(qiáng)的隨機(jī)性。

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

4 結(jié)束語(yǔ)

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