基于末端桿件姿態(tài)約束的串聯(lián)機(jī)器臂避碰策略

王恒升，黃平倫，彭天博（1.中南大學(xué)高性能復(fù)雜制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南長(zhǎng)沙410083；.中南大學(xué)機(jī)電工程學(xué)院，湖南長(zhǎng)沙410083）

?

基于末端桿件姿態(tài)約束的串聯(lián)機(jī)器臂避碰策略

王恒升1，2，黃平倫2，彭天博2
（1.中南大學(xué)高性能復(fù)雜制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南長(zhǎng)沙410083；2.中南大學(xué)機(jī)電工程學(xué)院，湖南長(zhǎng)沙410083）

根據(jù)雙臂鑿巖臺(tái)車(chē)上六自由度串聯(lián)機(jī)械臂的工作特點(diǎn)，為減小機(jī)械臂運(yùn)動(dòng)干涉的可能性，提出了該機(jī)械臂在運(yùn)動(dòng)過(guò)程中保持末端桿件垂直于工作斷面的姿態(tài)約束條件，并給出實(shí)現(xiàn)這一約束條件的運(yùn)動(dòng)學(xué)方法和空間桿件發(fā)生干涉的判斷式.針對(duì)某隧道斷面64個(gè)炮孔設(shè)計(jì)，對(duì)機(jī)械臂運(yùn)動(dòng)進(jìn)行了仿真，結(jié)果表明文中所提方法具有良好的避碰效果.

六自由度串聯(lián)機(jī)械臂；避碰；運(yùn)動(dòng)干涉；姿態(tài)約束

機(jī)器人運(yùn)動(dòng)干涉（避碰）問(wèn)題的解決往往與路徑規(guī)劃聯(lián)系在一起.過(guò)去幾十年來(lái)，這一問(wèn)題的研究范圍包括移動(dòng)機(jī)器人路徑規(guī)劃、多機(jī)器人路徑規(guī)劃、串聯(lián)（并聯(lián)）機(jī)械臂的路徑規(guī)劃等［1－2］.針對(duì)機(jī)器臂來(lái)說(shuō)，路徑規(guī)劃的任務(wù)就是要在機(jī)械臂起點(diǎn)位姿和終點(diǎn)位姿之間尋找一條路徑，沿著這條路徑驅(qū)動(dòng)關(guān)節(jié)變量向目標(biāo)位姿運(yùn)動(dòng)的過(guò)程中，機(jī)械臂的各個(gè)桿件不會(huì)發(fā)生任何形式的碰撞（或稱運(yùn)動(dòng)干涉），包括一個(gè)機(jī)械臂的各桿件之間、幾個(gè)機(jī)械臂的桿件之間、機(jī)械臂與環(huán)境及障礙物之間.

路徑規(guī)劃及避障的研究大致可分為2類(lèi)方法：

一類(lèi)是基于規(guī)劃的方法（或離線方法），一般是對(duì)機(jī)械臂的工作空間進(jìn)行離線規(guī)劃，搜索出一條最優(yōu)路徑.20世紀(jì)70年代，文獻(xiàn)［3－4］做了開(kāi)創(chuàng)性的工作.文獻(xiàn)［4］提出的C－空間法對(duì)路徑規(guī)劃的研究起了非常深遠(yuǎn)的影響，很多研究工作都是基于C－空間提出的.實(shí)際算法在應(yīng)用時(shí)需要對(duì)這一位姿空間進(jìn)行離散化，如將上述平面分割成許多小方格（稱為網(wǎng)格），自由區(qū)域用0表示，有干涉區(qū)域用1表示.典型的基于離散化位姿空間的最短路徑搜索方法有Dijkstra算法和A＊算法.八叉樹(shù)、Voronoi圖、概率地圖等提供了不同的表示位姿空間的方法［2－5］.位姿空間法的明顯不足是，隨著機(jī)械臂自由度數(shù)目的增加，位姿空間會(huì)向高維發(fā)展，所需要的存貯空間、計(jì)算搜索時(shí)間和計(jì)算復(fù)雜度會(huì)迅速增加，無(wú)法實(shí)際應(yīng)用.很多研究工作就是想辦法使位姿空間適合于多自由度機(jī)械臂的應(yīng)用.

另一類(lèi)方法是基于控制提出的，試圖通過(guò)實(shí)時(shí)控制來(lái)實(shí)現(xiàn)避障及搜索最優(yōu)路徑.典型的方法有人工勢(shì)函數(shù)法［6］和反射控制法［7］.人工勢(shì)函數(shù)法建立一個(gè)勢(shì)場(chǎng)，該場(chǎng)吸引機(jī)械臂向其目標(biāo)位姿移動(dòng)而同時(shí)排斥其遠(yuǎn)離障礙物，通過(guò)搜索勢(shì)函數(shù)的梯度方向來(lái)尋找最優(yōu)路徑.反射控制法涉及到快速評(píng)估機(jī)械臂的運(yùn)動(dòng)干涉情況，如有碰撞的可能，就采取措施避開(kāi)障礙物.反射控制法常常通過(guò)安裝傳感器實(shí)現(xiàn)快速準(zhǔn)確判斷障礙物的方位.與基于規(guī)劃的方法相比，基于控制的方法屬于在線的局部尋優(yōu)法，容易陷入局部最優(yōu)而無(wú)法到達(dá)目標(biāo)位姿.為了跳出局部最優(yōu)，基于控制的方法往往需要在控制結(jié)構(gòu)中引入全局路徑規(guī)劃，非常消耗計(jì)算資源.

值得一提的是，近來(lái)年，碰撞干涉檢測(cè)問(wèn)題在計(jì)算機(jī)圖形的應(yīng)用方面，如動(dòng)畫(huà)、三維可視化幾何處理、計(jì)算機(jī)游戲等，取得不少的研究成果［8－10］.

本文針對(duì)一種六自由度機(jī)械臂，提出以末端執(zhí)行器的姿態(tài)為約束，不需要建立C－空間.在機(jī)械臂的運(yùn)動(dòng)過(guò)程中，只要保持末端執(zhí)行器姿態(tài)固定，就可以大大減小機(jī)械臂運(yùn)動(dòng)干涉的可能性，同時(shí)大大減小實(shí)時(shí)控制器在避碰方面的計(jì)算壓力.再輔之以障礙檢測(cè)傳感器，可以很好地防止機(jī)械臂的運(yùn)動(dòng)干涉.

這一思路受到冗余機(jī)械臂自運(yùn)動(dòng)的啟發(fā)［11－15］.對(duì)于冗余機(jī)械臂，相應(yīng)于末端執(zhí)行器的某目標(biāo)位姿，關(guān)節(jié)變量有多種（甚至于無(wú)窮多種）配置.這個(gè)特點(diǎn)使冗余機(jī)械臂為達(dá)到末端執(zhí)行器的某種工作位姿，可以克服關(guān)節(jié)的限制、奇異位形或改善動(dòng)力學(xué)性能，也可以繞過(guò)障礙物，同時(shí)還可以用于末端執(zhí)行器需要鎖定在某確定位姿而調(diào)整關(guān)節(jié)以實(shí)現(xiàn)避障、多機(jī)械臂協(xié)作及某特定作業(yè)（如醫(yī)療外科、擰開(kāi)瓶蓋等）的情況.

六自由度機(jī)械臂在關(guān)節(jié)配置的靈活性上不如冗余機(jī)械臂，無(wú)法實(shí)現(xiàn)末端執(zhí)行器位姿固定調(diào)整驅(qū)動(dòng)關(guān)節(jié)位置，但可以放松自運(yùn)動(dòng)中對(duì)末端執(zhí)行器位姿固定的這一約束，僅保持末端執(zhí)行器姿態(tài)不變或者姿態(tài)的某些參數(shù)不變.針對(duì)鑿巖機(jī)械臂的工作特點(diǎn)，本文提出末端執(zhí)行器的姿態(tài)保持垂直工作面的避障運(yùn)動(dòng)約束條件.

1 末端桿件姿態(tài)確定

研究對(duì)象為雙臂鑿巖臺(tái)車(chē)上的六自由度機(jī)器臂.鑿巖臺(tái)車(chē)的末端執(zhí)行器是液壓鑿巖鉆機(jī)，安裝在圖1所示的滑架上.滑架上裝有3m多長(zhǎng)的鉆釬，工作時(shí)要求在巖石斷面上打出一定深度的炮孔.機(jī)械臂的作用就是根據(jù)爆破工藝的設(shè)計(jì)要求，將滑架以某姿態(tài)移動(dòng)到斷面的指定位置.對(duì)于4.8m寬的煤礦運(yùn)煤通道，通道斷面的一次爆破成形需要60～80個(gè)炮孔，2個(gè)機(jī)械臂同時(shí)工作.機(jī)械臂的運(yùn)動(dòng)干涉可能發(fā)生在機(jī)械臂桿件之間，也可能發(fā)生在2個(gè)機(jī)械臂之間，還有可能發(fā)生在機(jī)械臂與隧道壁之間.由于該機(jī)械臂的質(zhì)量和尺寸較大，發(fā)生碰撞后引起的破壞性很大.目前國(guó)內(nèi)使用的液壓鑿巖臺(tái)車(chē)大多是手動(dòng)操作，避碰是由操作人員手動(dòng)掌握的.將自動(dòng)控制技術(shù)與機(jī)器人技術(shù)應(yīng)用于鑿巖臺(tái)車(chē)，實(shí)現(xiàn)某種程度的自動(dòng)化作業(yè)，可以大大減少操作人員的勞動(dòng)強(qiáng)度.

圖1 機(jī)械臂外形Fig.1 Outline of the manipulator

鑿巖臺(tái)車(chē)機(jī)械臂外形如圖1所示，機(jī)械臂機(jī)構(gòu)簡(jiǎn)圖如圖2所示.圖2表達(dá)出了固結(jié)于每個(gè)關(guān)節(jié)的坐標(biāo)系設(shè)置.機(jī)械臂共有6個(gè)關(guān)節(jié)，前5個(gè)關(guān)節(jié)為轉(zhuǎn)動(dòng)關(guān)節(jié)，最后1個(gè)是移動(dòng)關(guān)節(jié).本文提出機(jī)械臂運(yùn)動(dòng)過(guò)程中保持末端桿件（即滑架）的姿態(tài)垂直于工作斷面的方法，主要基于以下原因：

圖2 機(jī)械臂機(jī)構(gòu)簡(jiǎn)圖Fig.2 Mechanism of the manipulator

（1）根據(jù)爆破工藝，炮孔的姿態(tài)設(shè)計(jì)要求打孔時(shí)滑架的姿態(tài)基本接近于與斷面相垂直，只有掏槽孔和外沿孔有一定的偏角要求，但偏轉(zhuǎn)的角度不大，一般在30°以內(nèi).因此，滑架在運(yùn)動(dòng)過(guò)程中保持與斷面垂直，在接近目標(biāo)點(diǎn)時(shí)再調(diào)整最終姿態(tài)不會(huì)造成機(jī)械臂運(yùn)動(dòng)的不平穩(wěn).

（2）機(jī)械臂運(yùn)動(dòng)過(guò)程中末端桿件保持與斷面垂直，可以減小機(jī)械臂的空間活動(dòng)范圍與空間占用.圖3b和3c分別為滑架垂直斷面和不垂直斷面時(shí)機(jī)械臂在斷面上的投影示意圖，很明顯滑架垂直斷面時(shí)機(jī)械臂與隧道壁以及2個(gè)機(jī)械臂之間碰撞的可能性大為減小.

（3）滑架與斷面垂直，可以有效減少單個(gè)機(jī)械臂自身干涉的可能性，如圖4所示.圖4 機(jī)器臂兩桿件間的自干涉

圖3 鑿巖臺(tái)車(chē)在隧道內(nèi)示意圖Fig.3 Sketch of tunnelling rig in workplace

Fig.4 Self－interference of the manipulator between two links

2 滑架保持垂直于工作斷面的實(shí)現(xiàn)

用四元數(shù)法［16－20］建立Oi-XiYiZi（i＝1～6）坐標(biāo)系，如圖2所示.這里的桿件坐標(biāo)系全部采用同一姿態(tài)，可以更直觀地理解關(guān)節(jié)的運(yùn)動(dòng).

5個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)軸在相應(yīng)關(guān)節(jié)坐標(biāo)系中的矢量表達(dá)分別為e2、e1、e3、e1、e2，其中e1、e2、e3分別表示相應(yīng)坐標(biāo)系X、Y、Z軸方向的單位矢量.旋轉(zhuǎn)變量為θ1、θ2、θ3、θ4、θ5.在機(jī)械臂運(yùn)動(dòng)學(xué)逆解完成以后，各個(gè)炮孔所對(duì)應(yīng)的關(guān)節(jié)變量值均已確定.接下來(lái)的任務(wù)是確定角度θ4、θ5，使機(jī)械臂從一個(gè)孔到另一個(gè)孔的運(yùn)動(dòng)過(guò)程中始終保持滑架與斷面垂直，也即在實(shí)際控制中根據(jù)關(guān)節(jié)角度傳感器測(cè)量得到θ1、θ2、θ3的值，然后確定θ4、θ5的跟隨值，其約束條件就是保證滑架垂直于工作斷面.當(dāng)機(jī)械臂θ1、θ2、θ3接近目標(biāo)

從機(jī)械臂的運(yùn)動(dòng)學(xué)特性可知（見(jiàn)圖1和2），末端執(zhí)行器目標(biāo)位置的驅(qū)動(dòng)主要由關(guān)節(jié)1和2來(lái)實(shí)現(xiàn)，關(guān)節(jié)3～5主要用于調(diào)節(jié)末端執(zhí)行器的姿態(tài)，其中關(guān)節(jié)3的作用是在打周邊孔時(shí)防止機(jī)械臂上附帶的油路管道對(duì)鉆臂運(yùn)動(dòng)產(chǎn)生影響.在運(yùn)動(dòng)中為實(shí)現(xiàn)滑架姿態(tài)始終垂直工作斷面，需要按關(guān)節(jié)1～3的角度調(diào)整關(guān)節(jié)4和5的角度，下文詳細(xì)推導(dǎo)這一約束關(guān)系.位姿時(shí)，再調(diào)整θ4、θ5，達(dá)到目標(biāo)位姿.

在下面的推導(dǎo)中用到的幾個(gè)標(biāo)志矢量所對(duì)應(yīng)的四元數(shù)定義為：①工作斷面法向量的四元數(shù)，N＝（0，e3）＝（0，0，0，1）；②關(guān)節(jié)4的虛擬法向量四元數(shù)，N4＝（0，e2）＝（0，0，1，0）；③關(guān)節(jié)5的虛擬法向量四元數(shù)，N5＝（0，e1）＝（0，1，0，0）.關(guān)節(jié)1～5的旋轉(zhuǎn)運(yùn)動(dòng)用四元數(shù)可以分別表示為

式中：ci表示cos（θi／2），si表示sin（θi／2），i＝1～5.

由于僅涉及到桿件的姿態(tài)，未涉及位置量，所以推導(dǎo)過(guò)程只運(yùn)用坐標(biāo)的旋轉(zhuǎn)變換.

關(guān)節(jié)1～3的連續(xù)旋轉(zhuǎn)變換的四元數(shù)運(yùn)算為

其結(jié)果Q3亦為單位四元數(shù)，并記為

其中a、b、f、r為實(shí)數(shù)，而且滿足關(guān)系

加入關(guān)節(jié)4的旋轉(zhuǎn)變換為

將式（1）和（3）代入式（5），有

若要機(jī)械臂在運(yùn)動(dòng)過(guò)程中保持滑架姿態(tài)垂直于工作斷面，則關(guān)節(jié)4的虛擬法向量與斷面法向量必須正交.利用四元數(shù)法，關(guān)節(jié)4的虛擬法向量（四元數(shù)）N4經(jīng)過(guò)前面4個(gè)關(guān)節(jié)的旋轉(zhuǎn)變換后得到的四元數(shù)為Q4N4Q－14，該四元數(shù)是一個(gè)純矢量，應(yīng)與斷面法向量相垂直.按照此約束條件，可以得到

記

式（7）可簡(jiǎn)化為

根據(jù)幾何意義，可以得到

其中，

下面說(shuō)明θ5的求解.經(jīng)過(guò)前面4個(gè)關(guān)節(jié)的旋轉(zhuǎn)后，為保持滑架垂直斷面，關(guān)節(jié)5的虛擬法向量N5與斷面法向量之間應(yīng)該成直角，如果不是直角可通過(guò)關(guān)節(jié)5調(diào)成直角，這就是關(guān)節(jié)5需要調(diào)整的角度.

設(shè)關(guān)節(jié)5的虛擬法向量（四元數(shù)）N5經(jīng)過(guò)前面4個(gè)關(guān)節(jié)旋轉(zhuǎn)后變換為N′5，根據(jù)四元數(shù)運(yùn)算法則，有

其中Q4由式（5）得到.關(guān)節(jié)5的調(diào)整角度

為簡(jiǎn)化符號(hào)表達(dá)，式（12）中直接用四元數(shù)的符號(hào)來(lái)表示其對(duì)應(yīng)的矢量運(yùn)算.式（10）和（12）即為保持滑架垂直所需要的θ4、θ5跟隨θ1、θ2、θ3變化的關(guān)系式.

3 滑架保持垂直于作業(yè)斷面方法的仿真證明

按照上述方法，直觀上能使機(jī)械臂桿件運(yùn)動(dòng)所占用的空間小，有利于防止運(yùn)動(dòng)干涉.下面定量計(jì)算按照這種控制策略運(yùn)動(dòng)時(shí)機(jī)械臂桿件之間的距離，用于檢測(cè)桿件運(yùn)動(dòng)過(guò)程中發(fā)生干涉的可能性.這里將空間桿件簡(jiǎn)化為空間線段，如圖5中的AB和CD表示2個(gè)空間桿件.

3.1 干涉的判斷

設(shè)2個(gè)桿件所在直線分別為L(zhǎng)1和L2，2條直線的公垂線為GG′，如圖5所示.為便于后文求解G、G′的坐標(biāo)，這里引入直線的Plücker坐標(biāo)［19］.在某坐標(biāo)系下，Plücker坐標(biāo)用2個(gè)向量表達(dá)1條直線，將直線L表達(dá)為形如

的1個(gè)6維向量.其中，d為直線的單位方向向量；m為直線相對(duì)于坐標(biāo)原點(diǎn)的矩（向量），由m＝p×d確定，p為坐標(biāo)原點(diǎn)連接該直線上任意一點(diǎn)構(gòu)成的向量；d和m正交，即d·m＝0.如圖5所示，2個(gè)桿件AB和CD所在的2條直線L1和L2的Plücker坐標(biāo)分別為L(zhǎng)1（p1×d1，d1）、L2（p2×d2，d2）.其中，p1、p2分別為向量，P1和P2為直線L1和L2上的任意點(diǎn)，d1和d2為直線L1和L2的單位方向向量.令g、g′分別表示向量，則公垂線GG′所在直線的Plücker坐標(biāo)為

垂足G、G′的坐標(biāo)計(jì)算可參照?qǐng)D5列出的向量運(yùn)算式得到，如下所示：

其中的實(shí)系數(shù)u、l可由式（16）給出的向量約束方程求解，P1和P2為直線L1和L2上的任意點(diǎn)，可分別選已知坐標(biāo)的A點(diǎn)和C點(diǎn).由圖5的幾何關(guān)系可以寫(xiě)出式（16），其中h為一實(shí)數(shù).

圖5 Plücker坐標(biāo)及空間2條直線的距離Fig.5 Plücker coordinate and the distance between two lines in space

至此，垂足G、G′的坐標(biāo)可以通過(guò)式（15）和（16）輕松求得.接下來(lái)討論3類(lèi)9種距離.

3.1.1 第1類(lèi)距離

傳統(tǒng)的成本管理過(guò)分強(qiáng)調(diào)“省”和“節(jié)流”，努力追求成本最小化，結(jié)果使成本管理僅僅限制在生產(chǎn)領(lǐng)域當(dāng)中，這種方式把直接材料、直接人工，和制造費(fèi)用當(dāng)作減少成本費(fèi)用的主要技巧，因此成本管理陷入了一個(gè)簡(jiǎn)單的循環(huán)，成本的減少并不能夠提供最終的決策所需要的有效信息，無(wú)法準(zhǔn)確的表現(xiàn)出整個(gè)經(jīng)營(yíng)活動(dòng)過(guò)程，更不能具體的表現(xiàn)出每一個(gè)環(huán)節(jié)的成本信息。反而更多的關(guān)注點(diǎn)放在了生產(chǎn)過(guò)程中的節(jié)省，這樣造成了在過(guò)程當(dāng)中核算的比較多，但是前期的預(yù)測(cè)和準(zhǔn)備較少，成本管理效果非常弱。

2個(gè)桿件所在直線L1和L2之間公垂線的距離，即圖5中公垂線GG′的長(zhǎng)度.可分以下2種情況確定公垂線的長(zhǎng)度：

（1）垂足G、G′都落在對(duì)應(yīng)線段內(nèi)部，且d1× d2≠0時(shí)，根據(jù)公垂線段距離公式計(jì)算

（2）除去（1）的情況，即d1×d2＝0時(shí)或者至少有1個(gè)垂足點(diǎn)沒(méi)有落在對(duì)應(yīng)線段內(nèi)部時(shí)，式（17）計(jì)算出的公垂線長(zhǎng)度對(duì)判斷干涉沒(méi)有意義，此時(shí)取GG′＝＋∞.線段之間的最短距離可能是以下幾種情況之一：線段端點(diǎn)向另一線段的投影垂直距離（本文稱為第2類(lèi)距離），即AA′、BB′、CC′、DD′之一；兩線段端點(diǎn)之間的連線距離（本文稱為第3類(lèi)距離），即AC、AD、BC、BD之一.

3.1.2 第2類(lèi)距離

線段端點(diǎn)向?qū)?yīng)線段的投影點(diǎn)在對(duì)應(yīng)線段內(nèi)，投影垂直距離有AA′、BB′、CC′、DD′4種，其中符號(hào)A′表示A點(diǎn)在線段CD上的投影點(diǎn)，其他類(lèi)推.先分析端點(diǎn)A投影到CD所在直線A′點(diǎn)的距離AA′的計(jì)算，如圖6所示，按照投影點(diǎn)的位置分為3種情況.判斷此3種情況可概括為下面判別式的計(jì)算，根據(jù)3個(gè)端點(diǎn)的坐標(biāo)可得到

圖6 線段端點(diǎn)向另一線段投影情況Fig.6 Cases of projection to another line from one line in space

（3）δ＜0，即圖6c的情況.

對(duì)于第2類(lèi)距離的（1）、（3）2種情況，A′點(diǎn)不在CD線段內(nèi)，此時(shí)距離AA′對(duì)干涉的判斷沒(méi)有影響，此距離可不計(jì)算在內(nèi)，判斷時(shí)可取AA′＝＋∞.對(duì)于情況（2），A′在線段CD段內(nèi)，對(duì)桿件的干涉判斷有影響，距離AA′＝

3.1.3 第3類(lèi)距離

線段端點(diǎn)向另一線段的投影點(diǎn)不在對(duì)應(yīng)線段內(nèi)時(shí)，計(jì)算2條線段端點(diǎn)之間的連線距離，即AC、AD、BC、BD4種，根據(jù)4個(gè)端點(diǎn)的坐標(biāo)，利用歐拉距離公式可計(jì)算出這些距離.

按照以上分析計(jì)算出2個(gè)桿件的3類(lèi)9種距離，取其中的最短距離

來(lái)判斷2個(gè)桿件的干涉情況.由于tmin是桿件中心軸之間的距離，因此還應(yīng)該考慮桿件的尺寸.將2個(gè)桿件考慮為半徑分別為R1、R2的2個(gè)圓柱體，判斷2個(gè)桿件的碰撞干涉可用如下的判斷式：

Δ≤0時(shí)，兩桿件有碰撞干涉發(fā)生；Δ＞0時(shí)，無(wú)碰撞發(fā)生；Δ值越大，發(fā)生碰撞干涉的可能性越小.

3.2 仿真

圖7所示為臺(tái)車(chē)應(yīng)用的某煤礦運(yùn)煤隧道斷面的64個(gè)炮孔位置設(shè)計(jì)，用虛線給出了一種左右臂鉆孔的孔序規(guī)劃.右臂從58號(hào)孔開(kāi)始，沿著虛線指示的路徑，最后到63號(hào)孔結(jié)束；左臂從11號(hào)孔開(kāi)始，到1號(hào)孔結(jié)束.

圖7 斷面炮孔布置及左右臂作業(yè)孔序路徑圖Fig.7 Borehole design pattern on working section and boring sequence paths of right and left manipulators

3.2.1 仿真1：2個(gè)孔位之間機(jī)械臂的移動(dòng)仿真本文所討論的機(jī)械臂的末端執(zhí)行器（鑿巖機(jī)及鉆釬）安裝在末端桿件（即滑架）上，工作時(shí)，機(jī)械臂根據(jù)爆破工藝設(shè)計(jì)的炮孔的位置和方向，將滑架移送到目標(biāo)位置和姿態(tài).如果在移動(dòng)過(guò)程中沒(méi)有對(duì)末端執(zhí)行器的姿態(tài)進(jìn)行約束，這4m多長(zhǎng)的桿件在空中擺動(dòng)起來(lái)所占用的空間是很大的，極易與隧道壁發(fā)生碰撞.由于機(jī)械臂的質(zhì)量大，運(yùn)動(dòng)慣性很大，碰撞產(chǎn)生的后果是極其嚴(yán)重的.對(duì)實(shí)際臺(tái)車(chē)進(jìn)行試驗(yàn)調(diào)試時(shí)，開(kāi)始沒(méi)有對(duì)末端桿件姿態(tài)進(jìn)行約束，機(jī)械臂的末端桿件擺動(dòng)很大；由于慣性、桿件及關(guān)節(jié)連接件的非絕對(duì)剛性、多個(gè)油缸同時(shí)運(yùn)動(dòng)時(shí)液壓系統(tǒng)的不均勻性等原因，末端桿件的運(yùn)動(dòng)非常不平穩(wěn)，效果很差.在本仿真中，選取右臂從30號(hào)孔到41號(hào)孔的運(yùn)動(dòng)過(guò)程來(lái)分析，仿真中忽略了機(jī)械臂的質(zhì)量、彈性、阻尼及液壓系統(tǒng)等動(dòng)力學(xué)要素，僅在理想情況下考慮其運(yùn)動(dòng)學(xué)關(guān)系.30號(hào)孔的末端桿件的位姿數(shù)據(jù)為：位置坐標(biāo)（－1 600，545，4 900）mm（參考坐標(biāo)系見(jiàn)圖2），姿態(tài)數(shù)據(jù)（0°，0°，0°），運(yùn)動(dòng)學(xué)反解后得到的5個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)數(shù)據(jù)（17.398°，－13.647°，85.772°，－16.891°，－14.275°）.41號(hào)孔的末端桿件的位姿數(shù)據(jù)為：位置坐標(biāo)（－400，3 425，4 900）mm（參考坐標(biāo)系見(jiàn)圖2），姿態(tài)數(shù)據(jù)（0°，－3°，0°），運(yùn)動(dòng)學(xué)反解后得到的5個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)數(shù)據(jù)（3.304°，36.546°，0.207°，－33.551°，－13.185°）.在考慮末端桿件姿態(tài)約束時(shí)，關(guān)節(jié)1、2、3的角度均勻變化，關(guān)節(jié)4和5按照第2節(jié)式（10）和（12）跟隨變化，保證末端桿件的姿態(tài)與工作斷面垂直，仿真過(guò)程未考慮關(guān)節(jié)6的運(yùn)動(dòng).仿真過(guò)程中末端桿件的運(yùn)動(dòng)過(guò)程如圖8a所示，圖8b所示為未對(duì)末端桿件姿態(tài)進(jìn)行約束時(shí)的仿真結(jié)果，5個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)都假設(shè)為均勻變化.圖8a、b的結(jié)果對(duì)比可見(jiàn)，考慮末端桿件姿態(tài)約束的機(jī)械臂運(yùn)動(dòng)過(guò)程，滑架的空間占用小，與隧道壁發(fā)生碰撞干涉的可能性小.如果考慮動(dòng)力學(xué)因素的話，結(jié)果的對(duì)比將更為明顯.

為了定量表示機(jī)械臂與隧道壁碰撞的可能，圖9示出了仿真過(guò)程中末端桿件（滑架）與隧道壁之間的最短距離.圖中A曲線表示運(yùn)用約束機(jī)械臂末端姿態(tài)時(shí)的曲線，B曲線表示未約束機(jī)械臂末端姿態(tài)時(shí)的曲線.2條曲線對(duì)比明顯，運(yùn)用約束機(jī)械臂末端桿件姿態(tài)方法可以有效地減少滑架與環(huán)境發(fā)生碰撞的可能性.

圖9 滑架與隧道壁最短距離Fig.9 The shortest distance between sliding frame and tunnel walls

3.2.2 仿真2：連續(xù)多孔機(jī)械臂定位過(guò)程仿真

利用左右臂的連續(xù)運(yùn)動(dòng)過(guò)程仿真來(lái)分析機(jī)械臂桿件間的自干涉與互干涉情況.同樣不考慮實(shí)際工作中2個(gè)臂的進(jìn)度可能不同、液壓系統(tǒng)的不對(duì)稱等情況，按照?qǐng)D7所示的鉆孔任務(wù)分配孔序，2個(gè)臂分別從各自的起始孔位，經(jīng)過(guò)中間孔位，最后運(yùn)動(dòng)到終點(diǎn)孔位.2個(gè)臂的起始孔和終止孔的數(shù)據(jù)見(jiàn)表1.仿真過(guò)程與仿真1類(lèi)似，前3個(gè)關(guān)節(jié)均勻變化，同時(shí)到達(dá)目標(biāo)值，關(guān)節(jié)4和5按照式（10）和（12）進(jìn)行垂直姿態(tài)時(shí)角度變化的跟蹤.對(duì)于單臂的自干涉，僅可能發(fā)生在滑架與大臂之間，按照第3節(jié)公式計(jì)算最短距離，圖10為單臂自干涉的仿真結(jié)果.

圖10 機(jī)械臂滑架與大臂間最短距離（自干涉）Fig.10 The shortest distance between sliding frame andboom of the manipulator（self interference）

表1 2個(gè)臂的起始孔和終止孔數(shù)據(jù)Tab.1_Data of starting and ending holes of two manipulators

圖10中橫坐標(biāo)為機(jī)械臂運(yùn)動(dòng)仿真過(guò)程的采樣時(shí)間點(diǎn)，在曲線上標(biāo)出了發(fā)生干涉前后的作業(yè)炮孔序號(hào)（鉆臂按圖7所示順序移動(dòng)），縱坐標(biāo)為大臂與滑架的最短距離Δ，當(dāng)Δ≤0時(shí)有自干涉發(fā)生.多數(shù)情況下大臂與滑架間最短距離為一固定值（未畫(huà)出），該值就是圖1關(guān)節(jié)5軸線上大臂端點(diǎn)與滑架固連的一小段垂直桿件的長(zhǎng)度.只有當(dāng)左臂（見(jiàn)圖10a）從5號(hào)孔向2號(hào)孔、從2號(hào)孔向1號(hào)孔移動(dòng)過(guò)程中會(huì)發(fā)生自干涉.當(dāng)右臂（見(jiàn)圖10b）從58號(hào)孔向59號(hào)孔移動(dòng)過(guò)程中會(huì)發(fā)生自干涉.這與實(shí)際情況是相符的，因?yàn)榇藭r(shí)機(jī)械臂出現(xiàn)如圖4b所示的干涉情況，即滑架的后部已經(jīng)與大臂緊靠在一起.要避免這種自干涉的發(fā)生，只需要對(duì)關(guān)節(jié)4的轉(zhuǎn)動(dòng)角度θ4進(jìn)行一個(gè)極小值的限制即可，在實(shí)際控制中給定這個(gè)極小值角度為7°.

圖11為兩臂同時(shí)按圖7給出的孔序運(yùn)動(dòng)時(shí)，2個(gè)臂之間的最短距離計(jì)算結(jié)果.由于鑿巖臺(tái)車(chē)機(jī)械結(jié)構(gòu)的空間限制，2個(gè)機(jī)械臂的安裝位置距離很近，在2個(gè)臂同時(shí)運(yùn)動(dòng)時(shí)，如不小心，機(jī)械臂間碰撞的可能性很大.采取本文提出的機(jī)械臂移動(dòng)過(guò)程中保持末端桿件姿態(tài)垂直于工作斷面的方法，能夠有效地防止互干涉的發(fā)生.圖11a顯示機(jī)械臂運(yùn)動(dòng)過(guò)程中2個(gè)臂之間的最短距離基本大于200mm，只有某個(gè)點(diǎn)時(shí)，2個(gè)臂之間的最短距離小于100mm，總體無(wú)碰撞發(fā)生.圖11b為機(jī)械臂移動(dòng)過(guò)程中未采取末端桿件姿態(tài)垂直斷面的方法，在2個(gè)臂移動(dòng)過(guò)程中，多處發(fā)生碰撞.在實(shí)際情況下，機(jī)械臂鑿巖任務(wù)完成的進(jìn)度不同，發(fā)生互干涉的危險(xiǎn)性更為嚴(yán)重.只對(duì)末端桿件姿態(tài)進(jìn)行約束會(huì)減少干涉的可能性，但還不能完全避免干涉.實(shí)際控制時(shí)，在移動(dòng)機(jī)械臂前，先利用本文第3.1節(jié)的方法，計(jì)算移動(dòng)過(guò)程中的兩臂間最短距離進(jìn)行干涉預(yù)測(cè)，根據(jù)預(yù)測(cè)結(jié)果對(duì)鉆孔順序進(jìn)行調(diào)整.此時(shí)可定義一個(gè)機(jī)械臂為主臂另一個(gè)為從臂，如有干涉的危險(xiǎn)時(shí)，從臂避讓主臂.運(yùn)用這種策略，可以有效避免機(jī)械臂間發(fā)生互干涉的風(fēng)險(xiǎn).

圖11 兩機(jī)械臂的互干涉情況Fig.11 Interference between two manipulators

圖12為仿真過(guò)程中左臂滑架與隧道壁的最短距離計(jì)算結(jié)果.兩圖對(duì)比可以看出，采用約束機(jī)械臂末端桿件姿態(tài)垂直于工作斷面的方法有效地避免了運(yùn)動(dòng)過(guò)程中機(jī)械臂與環(huán)境的干涉.

圖12 機(jī)械臂與隧道壁間最短距離Fig.12 The shortest distance between manipulator and tunnel walls

4 結(jié)論

（1）考慮臺(tái)車(chē)機(jī)械臂處在鉆孔工位時(shí)，針對(duì)滑架偏離垂直斷面的姿態(tài)方向一般不大這一實(shí)際情況，提出在移動(dòng)過(guò)程中保持末端執(zhí)行器（滑架）垂直于斷面的位姿路徑，等到機(jī)械臂基本到達(dá)目標(biāo)位置后再將關(guān)節(jié)調(diào)整至目標(biāo)姿態(tài)，即所謂的基于末端姿態(tài)約束的避障策略.這一策略通過(guò)文中的理論推導(dǎo)及仿真計(jì)算，證明是有效的.這種方法已經(jīng)應(yīng)用于某鑿巖臺(tái)車(chē)自動(dòng)化改造中六自由度機(jī)械臂的實(shí)時(shí)控制.

（2）運(yùn)用四元數(shù)方法對(duì)機(jī)械臂的運(yùn)動(dòng)關(guān)系進(jìn)行描述與分析，推導(dǎo)出了關(guān)節(jié)4和5跟隨前面3個(gè)關(guān)節(jié)的計(jì)算公式.

（3）針對(duì)空間桿件的干涉判斷，提出3類(lèi)9種距離的計(jì)算方法，得到桿件線段在空間的最短距離.運(yùn)用這種方法，對(duì)某鑿巖機(jī)械臂運(yùn)動(dòng)過(guò)程的干涉進(jìn)行判斷，運(yùn)算結(jié)果可靠.

［1］Lahouar S，Zeghloul S，Romdhane L.Collision free path planning for multi－DoF manipulators［M］／／Industrial Robotics：Theory，Modeling and Control.［S.l.］：Pro Literatur Verlag，2006：349－378.

［2］Lavalle S M.Planning algorithms［M］.Cambridge，Eng.：Cambridge University Press，2006.

［3］Udupa S M.Collision detection and avoidance in computer controlled manipulators［D］.Pasadena：California Institute of Technology，1977.

［4］Lozano－Pérez T，Wesley M A.An algorithm for planning collision－free paths among polyhedral obstacles［J］.Communications of the ACM，1979，22（10）：560.

［5］Tournassoud P，Planification et contr?le en robotique：application aux robots mobiles et manipulateurs traite des no （French）［M］.Paris：Hermes Science，1992.

［6］Khatib O.Real－time obstacle avoidance for manipulators and mobile robots［J］.The International Journal of Robotics Research，1986，5（1）：90.

［7］Newman W S.High－speed robot control in complex environments［D］.Boston：Massachusetts Institute of Technology，1987.

［8］Wu J，Dick C，Westermann R.Efficient collision detection for composite finite element simulation of cuts in deformable bodies ［J］.The Visual Computer，2013，29（6）：739

［9］Glondu L，Schvartzman S C，Marchal M，et al.Fast collision detection for fracturing rigid bodies［J］.IEEE Transactions on Visualization and Computer Graphics，2014，20（1）：30

［10］Mujika A，Oyarzun D，Arrieta A，et al.Real time accurate collision detection for virtual characters［C］／／WSCG 2010 Communication Papers.Plzen：The Press of the University of West Bohemia，2010：123－130.

［11］Burdick J W.On the inverse kinematics of redundant manipulators：characterization of the self－motion manifolds ［M］／／Advanced Robotics.Beilin：Springer，1989：25－34.

［12］Oriolo G.Stabilization of self－motions in redundant robots ［C］／／Robotics and Automation，1994 IEEE International Conference on.［S.l.］.IEEE，1994：704－709.

［13］王連圭，馬保離.冗余機(jī)器人系統(tǒng)的自運(yùn)動(dòng)控制［J］.控制與決策，2003，18（2）：199.WANG Liangui，MA Baoli.Self－motion control of redundant manipulators［J］.Control and Decision，2003，18（2）：199.

［14］馬保離.冗余機(jī)器人的雙向自由運(yùn)動(dòng)路徑規(guī)劃［J］.控制理論與應(yīng)用，2005，22（4）：547.MA Baoli.Bi－directional self－motion path planning of redundant manipulators［J］.Control Theory ＆Applications，2005，22（4）：547.

［15］Zhang Y，Zhu H，Tan Z，et al.Self－motion planning of redundant robot manipulators based on quadratic program and shown via PA10 example［C］／／Systems and Control in Aerospace and Astronautics.［S.l.］.IEEE，2008：1－6.

［16］Hamilton W R.Elements of quaternions（Vol.I ＆II）［M］.New York：Chelsea，1869.

［17］Yang A T，F(xiàn)reudenstein F.Application of dual－number quaternion algebra to the analysis of spatial mechanisms［J］.Journal of Applied Mechanics，1964，31（2）：300.

［18］Gouasmi M.Robot kinematics，using dual Quaternions［J］.IAES International Journal of Robotics and Automation（IJRA），2012，1（1）：13.

［19］Sariyildiz E，Temeltas H.Solution of inverse kinematic problem for serial robot using dual quaterninons and plücker coordinates［C］／／Advanced Intelligent Mechatronics，IEEE／ASME International Conference on.［S.l.］：IEEE，2009：338－343.

［20］張榮輝，賈宏光，陳濤，等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算［J］.光學(xué)精密工程，2008，16（10）：1963.ZHANG Ronghui，JIA Hongguang，CHEN Tao，et al.Attitude solution for strapdown inertial navigation system based on quaternion algprithm［J］.Optics and Precision Engineering，2008，16（10）：1963.

Collision Avoidance Based on Pose Constraint of End Link for Serial Manipulator

WANG Hengsheng1，2，HUANG Pinglun2，PENG Tianbo2
（1.State Key Laboratory for High Performance Complex Manufacturing，Central South University，Changsha 410083，China；2.College of Mechanical and Electrical Engineering，Central South University，Changsha 410083，China）

To reduce the possibility of motion interference of manipulators，apose constraint was proposed according to the mechanism characteristics of manipulators for a hydraulic tunneling rig with two 6－DoF（degree of freedom）manipulators.The pose constraint is that the end link of manipulator keeps perpendicular to working cross section during whole motion process.Then，kinematic methods for the realization of the constraint and the minimum distance between spatial links were given.Finally，motion simulation of manipulators was made for a tunneling cross section with 64－borehole design pattern.The results show that the pose constraint of end link perpendicular to working section during the motion of manipulators is effective for collision avoidance.

6－DoF（degree of freedom）manipulator；collision avoidance；motion interference；pose constraint

TP242.3

A

0253－374X（2016）01－0128－08

10.11908／j.issn.0253－374x.2016.01.019

2015-01-08

國(guó)家“九七三”重點(diǎn)基礎(chǔ)研究發(fā)展計(jì)劃（2013CB035504）

王恒升（1963—），男，教授，博士生導(dǎo)師，工學(xué)博士，主要研究方向?yàn)橹悄軝C(jī)器人.E－mail：whsheng＠csu.edu.cn