許曉群，侯忠平，王昌順，杜能，夏衛(wèi)生

（1.湖北中煙工業(yè)有限責(zé)任公司武漢卷煙廠，湖北武漢 430040；2.華中科技大學(xué)材料科學(xué)與工程學(xué)院，湖北武漢 430074）

0 前言

多機(jī)器人是在一個(gè)共同的工作空間中協(xié)作運(yùn)行，彼此之間存在交互和運(yùn)動(dòng)，在此過(guò)程中就不可避免地會(huì)碰撞，一旦發(fā)生碰撞就會(huì)造成嚴(yán)重的操作事故，甚至嚴(yán)重毀壞機(jī)器人的結(jié)構(gòu)，所以系統(tǒng)必須實(shí)時(shí)檢測(cè)到這些機(jī)器人之間的碰撞。碰撞檢測(cè)是多機(jī)器人協(xié)同工作中必不可少的環(huán)節(jié)和安全工作的保障條件。共享空間的多個(gè)幾何物體之間分別存在以下3 種位置關(guān)系：分離、接觸、穿透。碰撞檢測(cè)是對(duì)幾何物體之間的位置關(guān)系進(jìn)行實(shí)時(shí)判斷以檢測(cè)出是否發(fā)生接觸或穿透現(xiàn)象，并向系統(tǒng)返回其發(fā)生的部位［1-2］。所以，碰撞檢測(cè)技術(shù)需要至少三大信息輸出：是否碰撞、碰撞部位以及查詢距離［3-5］。

碰撞檢測(cè)是分別對(duì)兩兩物體的空間位置進(jìn)行檢測(cè)，多機(jī)器人之間的碰撞檢測(cè)也是基于兩兩機(jī)器人之間的碰撞檢測(cè)。但機(jī)器人的結(jié)構(gòu)不是簡(jiǎn)單的幾何模型，直接進(jìn)行碰撞檢測(cè)需要消耗大量的計(jì)算成本，所以在對(duì)復(fù)雜幾何結(jié)構(gòu)進(jìn)行碰撞檢測(cè)前，通常先執(zhí)行物體的包圍體測(cè)試［6-9］。常采用常見(jiàn)的簡(jiǎn)單幾何體去近似替代機(jī)器人的機(jī)械臂、末端執(zhí)行器等，從而得到更簡(jiǎn)略且高效的碰撞檢測(cè)算法，可以快速且實(shí)時(shí)地進(jìn)行多機(jī)器人碰撞檢測(cè)。有些學(xué)者用球體代替機(jī)器人關(guān)節(jié)，也有學(xué)者將球體平分，中間用圓柱體相連的膠囊體結(jié)構(gòu)代替機(jī)器人關(guān)節(jié)和連桿［10-14］。用于機(jī)器人關(guān)節(jié)與末端表示的主要簡(jiǎn)化建模方法有長(zhǎng)方體表示法、圓柱加半球表示法、球體群表示法和橢球表示法。機(jī)器人簡(jiǎn)化建模和距離計(jì)算是相互關(guān)聯(lián)的，每一種包圍盒技術(shù)都有與之對(duì)應(yīng)的碰撞檢測(cè)和距離計(jì)算算法。用球體對(duì)機(jī)器人末端執(zhí)行器進(jìn)行簡(jiǎn)化建模，不僅可以緊密包圍，而且數(shù)學(xué)等式簡(jiǎn)單，并且球體表示不需要考慮方向變化，只考慮位置關(guān)系就可以，只有點(diǎn)-點(diǎn)距離計(jì)算，計(jì)算量最小。用橢球?qū)C(jī)器人關(guān)節(jié)連桿進(jìn)行逼近，相比單個(gè)球體的逼近，不僅對(duì)機(jī)器人連桿包圍更加緊密，而且節(jié)約了大量空間［15］。本文作者采用橢球體直接對(duì)機(jī)器人連桿和關(guān)節(jié)進(jìn)行簡(jiǎn)化建模，從而進(jìn)行基于橢球體的包圍體碰撞檢測(cè)。

1 基于包圍橢球的機(jī)器人建模

本文作者基于六自由度工業(yè)機(jī)器人PUMA560 進(jìn)行研究。PUMA560 機(jī)器人的三維示意如圖1 所示，其連桿和關(guān)節(jié)具體結(jié)構(gòu)如圖2 所示。PUMA560 是關(guān)節(jié)型機(jī)器人，含有6 個(gè)連桿和6 個(gè)關(guān)節(jié)，6 個(gè)運(yùn)動(dòng)關(guān)節(jié)均為轉(zhuǎn)動(dòng)關(guān)節(jié)，其末端手腕的運(yùn)動(dòng)決定于6 個(gè)運(yùn)動(dòng)關(guān)節(jié)順序運(yùn)動(dòng)或同時(shí)運(yùn)動(dòng)，其中前3 個(gè)運(yùn)動(dòng)關(guān)節(jié)確定末端手腕的空間位置，后3 個(gè)運(yùn)動(dòng)關(guān)節(jié)確定末端手腕的空間姿態(tài)。

圖1 PUMA560 機(jī)器人三維圖Fig.1 3D model of PUMA560 robot

圖2 PUMA560 機(jī)器人的連桿和關(guān)節(jié)結(jié)構(gòu)Fig.2 Linkage and joint structure of PUMA560 robot

PUMA560 機(jī)器人連桿和關(guān)節(jié)結(jié)構(gòu)均為不規(guī)則幾何圖形，并且機(jī)器人的碰撞檢測(cè)中反饋的分離距離是機(jī)器人的空間位置最短距離，只與機(jī)器人前3 個(gè)運(yùn)動(dòng)關(guān)節(jié)有關(guān)，于是將PUMA560 機(jī)器人簡(jiǎn)化為如圖3 所示的三自由度連桿結(jié)構(gòu)。因此本文作者采用橢球體對(duì)PUMA560 機(jī)器人的三自由度連桿結(jié)構(gòu)進(jìn)行包圍，如圖4 所示。由于機(jī)器人的碰撞檢測(cè)在于判斷末端結(jié)構(gòu)的碰撞情況，所以主要判斷機(jī)器人末端包圍橢球體的碰撞情況。

圖3 PUMA560 簡(jiǎn)化模型Fig.3 Simplified model of PUMA560

圖4 橢球體包圍機(jī)器人連桿和關(guān)節(jié)Fig.4 The ellipsoid surrounding the robot connecting rods and joints

橢球體的主軸長(zhǎng)決定于連桿長(zhǎng)，取連桿的中點(diǎn)為橢球體的中心，則每一個(gè)橢球體代表相對(duì)應(yīng)的每一個(gè)連桿。根據(jù)PUMA560 機(jī)器人連桿參數(shù)，設(shè)置第3 個(gè)連桿，即末端連桿的橢球體參數(shù)，橢球體中心為機(jī)器人運(yùn)動(dòng)時(shí)實(shí)時(shí)反饋的連桿中心的位置坐標(biāo)，橢球體的3 個(gè)主軸根據(jù)PUMA560 的參數(shù)設(shè)置為e1＝250 mm，e2＝120 mm，e3＝125 mm。

由于連桿隨關(guān)節(jié)運(yùn)動(dòng)而運(yùn)動(dòng)，將桿和關(guān)節(jié)作為整體被橢球體包圍，不再區(qū)分關(guān)節(jié)和連桿，使包圍盒結(jié)構(gòu)清晰簡(jiǎn)單，不用單獨(dú)考慮連桿和關(guān)節(jié)各自的碰撞情況，使得計(jì)算量減少，提高碰撞檢測(cè)效率。

2 基于橢球坐標(biāo)系的最短距離計(jì)算

2.1 構(gòu)建橢球坐標(biāo)系

橢球坐標(biāo)系是以某個(gè)橢球面為參照面，建立特定的局部坐標(biāo)系，如圖5 所示，橢球體A 的中心為點(diǎn)xCA，橢球體B 的中心為點(diǎn)xCB，以橢球體A 為參照面，建立橢球坐標(biāo)系ε，以點(diǎn)xCA為坐標(biāo)系原點(diǎn)，以橢球體A 的長(zhǎng)軸和短軸分別為坐標(biāo)系的x和y軸。

圖5 基于橢球體A 建立橢球坐標(biāo)系εFig.5 Ellipsoidal coordinate system ε based on an ellipsoid A

橢球坐標(biāo)系中的坐標(biāo)在笛卡爾坐標(biāo)系中均滿足：

ε＝｛x∈R ｜（x -xCA）TQ（x -xCA）＝1｝，x∈R，Q∈Rm×m，Q＝RDRT，R∈Rm×m，R為三階單位向量矩陣。

其中：e1、e2、e3為橢球體的3 個(gè)半軸長(zhǎng)。橢球體A 向量矩陣Q乘以比例因子d的平方轉(zhuǎn)換為矩陣P，d為標(biāo)量，橢球體坐標(biāo)系ε中坐標(biāo)的表達(dá)式轉(zhuǎn)化為

其中：P＝d2Q，d表示為橢球坐標(biāo)系ε中任意的點(diǎn)到坐標(biāo)系ε原點(diǎn)的距離。橢球坐標(biāo)系ε在笛卡爾坐標(biāo)系中的幾何意義是同一中心、半軸的單位方向向量相同但長(zhǎng)短不同的一系列橢球面，同一橢球面的d值相同，為等高面。橢球體A 在橢球坐標(biāo)系ε中的d值為1，即為單位橢球。

2.2 等高面求解

橢球坐標(biāo)系是以等高面來(lái)確定兩點(diǎn)之間的距離，求解橢球體A 和橢球體B 之間的最短距離問(wèn)題則轉(zhuǎn)換為求解橢球體B 在橢球坐標(biāo)系ε中所相切的等高面的問(wèn)題。

根據(jù)等高面（x -xCA）TP（x -xCA）＝d2可知，只需找到位于等高面上任意一點(diǎn)x，則可得到距離的值。由圖6 可知，假設(shè)橢球體B 和橢球坐標(biāo)系ε中構(gòu)建的等高面橢球體A′相切于點(diǎn)xt，即切點(diǎn)xt是屬于等高面上的點(diǎn)，求得切點(diǎn)xt的坐標(biāo)即可求得d的值，但橢球體A 的中心點(diǎn)xCA、橢球體B 的中心點(diǎn)xCB和點(diǎn)xt三點(diǎn)不在一條直線上，而且沒(méi)有通用的求解兩橢球體相切的切點(diǎn)的解析式，只能轉(zhuǎn)換為約束優(yōu)化問(wèn)題，目標(biāo)函數(shù)為

圖6 在橢球體坐標(biāo)系ε 中構(gòu)建等高面與橢球體B 相切Fig.6 Contour planes constructed in the ellipsoidal coordinate system ε tangent to the ellipsoid B

同時(shí)切點(diǎn)xt也在橢球體B 上，所以點(diǎn)xt滿足橢球體B 的軌跡方程，則約束條件為

式（2）中：M為橢球體B 的向量矩陣。

由圖6 可知，過(guò)切點(diǎn)xt指向橢球體A′的法向量nA與過(guò)切點(diǎn)xt指向橢球體B 的法向量nB共線但方向相反，則所共線的直線表達(dá)式通過(guò)對(duì)橢球體的向量方程進(jìn)行求導(dǎo)可得：

其中：t為兩個(gè)法向量的比例因子。由式（3）可得到xt的表達(dá)式：

再將式（4）代入到式（1）中，可得到有關(guān)距離d的多項(xiàng)式函數(shù)fd（t）：

通過(guò)解方程fd（t）＝0 可得到比例因子t的兩個(gè)根，但因?yàn)榉ㄏ蛄縩A和法向量nB方向相反，因此負(fù)根才是滿足幾何約束的解。將求解得到的負(fù)值代入式（4），可得到切點(diǎn)xt的坐標(biāo)值，再將切點(diǎn)xt的坐標(biāo)值代入式（5）得到距離d。

2.3 最短歐氏距離求解

基于橢球坐標(biāo)系求解得到的兩橢球體間的最短距離不是笛卡爾坐標(biāo)系下的歐氏距離，需將橢球坐標(biāo)系中的相對(duì)距離轉(zhuǎn)換為笛卡爾坐標(biāo)中的歐氏距離。由于每個(gè)點(diǎn)的坐標(biāo)均是笛卡爾坐標(biāo)系中的坐標(biāo)值，切點(diǎn)位于橢球體B 上，則通過(guò)求解橢球體A 距離橢球體B最近點(diǎn)得到最短歐氏距離。首先求出最短距離所在直線與橢球體A 的交點(diǎn)x0的坐標(biāo)，然后直接計(jì)算切點(diǎn)xt到點(diǎn)x0的歐氏距離。

如圖7 所示，先將橢球體A 縮放為單位球A0，沿3 個(gè)坐標(biāo)軸方向?qū)E球體A 進(jìn)行縮放，縮放系數(shù)分別為1／e1、1／e2、1／e3，得到縮放矩陣T為

圖7 等高面縮放變換Fig.7 Contour scaling

縮放變化不改變幾何關(guān)系，故縮放后的切點(diǎn)坐標(biāo)xtF仍在最短距離直線上，由于在笛卡爾坐標(biāo)系中進(jìn)行坐標(biāo)計(jì)算，橢球體A 的中心點(diǎn)xCA不再為原點(diǎn)，所以縮放后的切點(diǎn)xtF的坐標(biāo)通過(guò)縮放變換求解：

再求點(diǎn)x′t的單位向量x0：

該單位向量的一端端點(diǎn)為單位球A0的中心，因此單位向量x0位于單位球A0上，又因?yàn)閱挝磺駻0是由橢球體A 進(jìn)行縮放變換得到的，對(duì)單位球A0進(jìn)行縮放變換的逆變換又可得到橢球體A，故單位向量x0也在橢球體A 上，這時(shí)可得到縮放后的最短距離向量dF＝xtF-x0，將最短距離向量dF進(jìn)行縮放變換的逆變換可得到實(shí)際距離向量d＝T-1dF。橢球體A 上距離橢球體B 最短距離的點(diǎn)xA的坐標(biāo)通過(guò)單位向量x0縮放變換的逆變換和橢球體坐標(biāo)系相對(duì)笛卡爾坐標(biāo)系的平移變換向量xCA的組合變換得到：

最后對(duì)實(shí)際最短距離向量d求二范數(shù)即可得到兩橢球體間的最短歐氏距離：

3 多機(jī)器人碰撞檢測(cè)算法

由于機(jī)器人的運(yùn)動(dòng)軌跡是由機(jī)器人末端結(jié)構(gòu)的運(yùn)動(dòng)情況決定，因此文中所研究的多機(jī)器人碰撞檢測(cè)算法主要針對(duì)機(jī)器人末端結(jié)構(gòu)之間的碰撞檢測(cè)。所要實(shí)現(xiàn)的功能是：已知當(dāng)前時(shí)刻多個(gè)機(jī)器人末端結(jié)構(gòu)的位置坐標(biāo)，計(jì)算出多個(gè)機(jī)器人末端結(jié)構(gòu)兩兩之間的最短距離，并與其接觸距離進(jìn)行對(duì)比，最終判斷多個(gè)機(jī)器人運(yùn)動(dòng)過(guò)程是否發(fā)生碰撞，如果發(fā)生碰撞則確定發(fā)生碰撞的運(yùn)動(dòng)軌跡范圍。

基于橢球體建模的多機(jī)器人碰撞檢測(cè)算法是先根據(jù)機(jī)器人的結(jié)構(gòu)參數(shù)進(jìn)行建模。作者通過(guò)DH 參數(shù)法對(duì)機(jī)器人構(gòu)建固定連桿坐標(biāo)系；然后對(duì)機(jī)器人運(yùn)動(dòng)路徑進(jìn)行規(guī)劃，依據(jù)機(jī)器人運(yùn)動(dòng)學(xué)仿真來(lái)設(shè)定機(jī)器人末端連桿的初始位置和目標(biāo)位置得到規(guī)劃路徑；然后在機(jī)器人沿規(guī)劃路徑運(yùn)動(dòng)過(guò)程中調(diào)用最短距離求解函數(shù)進(jìn)行分離距離的求解，最后根據(jù)分離距離和接觸判斷是否發(fā)生碰撞。

多機(jī)器人碰撞檢測(cè)算法的流程如圖8 所示：首先初始化機(jī)器人的DH 參數(shù)；通過(guò)機(jī)器人的DH 參數(shù)構(gòu)建機(jī)器人連桿坐標(biāo)系，獲得機(jī)器人的簡(jiǎn)化模型；根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)規(guī)劃?rùn)C(jī)器人運(yùn)動(dòng)路徑；在已規(guī)劃的路徑上選取合適的時(shí)間步長(zhǎng)，在軌跡上取樣；在軌跡每一個(gè)離散時(shí)間點(diǎn)上調(diào)用基于橢球坐標(biāo)系最短距離求解算法，獲得當(dāng)前時(shí)刻機(jī)器人之間的最短距離；輸出機(jī)器人沿規(guī)劃路徑運(yùn)動(dòng)過(guò)程中最短距離隨時(shí)間變化的曲線；最后將最短距離變化曲線與接觸距離曲線結(jié)合，判斷是否發(fā)生碰撞。

圖8 多機(jī)器人碰撞檢測(cè)算法流程Fig.8 Flow of multi-robot collision detection algorithm

針對(duì)三機(jī)器人進(jìn)行碰撞檢測(cè)仿真。三機(jī)器人碰撞檢測(cè)是以雙機(jī)器人碰撞檢測(cè)仿真為基礎(chǔ)，同時(shí)給3 個(gè)機(jī)器人A、B、C 規(guī)劃運(yùn)行軌跡，機(jī)器人A 和機(jī)器人B 沿相交軌跡運(yùn)動(dòng)，機(jī)器人B 和機(jī)器人C 沿逐漸靠近的軌跡運(yùn)動(dòng)，機(jī)器人A 和機(jī)器人C 沿交叉軌跡運(yùn)動(dòng)?？梢酝茰y(cè)，機(jī)器人A 和機(jī)器人B 之間的最短距離是是先變小后變大，機(jī)器人B 和機(jī)器人C 之間的最短距離是逐漸變小，機(jī)器人A 和機(jī)器人C 之間的最短距離是先變小后變大。在3 個(gè)機(jī)器人同時(shí)運(yùn)動(dòng)過(guò)程中，對(duì)兩兩機(jī)器人之間的最短距離進(jìn)行求解并判斷是否發(fā)生碰撞，因此同時(shí)輸出3 條最短距離變化曲線。

三機(jī)器人同時(shí)運(yùn)動(dòng)的路徑規(guī)劃如圖9 所示，3 個(gè)機(jī)器人A、B、C 分別沿箭頭方向同時(shí)運(yùn)動(dòng)，其中藍(lán)線和黃線在一個(gè)平面內(nèi)且發(fā)生相交，紅線在藍(lán)線、黃線的平面之外。在3 個(gè)機(jī)器人同時(shí)運(yùn)動(dòng)過(guò)程中進(jìn)行碰撞檢測(cè)仿真，仿真結(jié)果如圖10 所示，同時(shí)輸出機(jī)器人A 和B、機(jī)器人B 和C、機(jī)器人A 和C 之間的最短距離變化曲線，均符合推測(cè)情況。再和接觸距離變化曲線（綠色曲線）相比，判斷是否發(fā)生碰撞，由仿真結(jié)果可知僅機(jī)器人A 和B 發(fā)生碰撞，符合實(shí)際情況。

圖9 三機(jī)器人運(yùn)動(dòng)路徑規(guī)劃Fig.9 Three robot motion paths planning

圖10 三機(jī)器人間最短距離變化曲線與碰撞檢測(cè)Fig.10 The shortest distance curves between the three machines and collision detection

4 結(jié)論

文中針對(duì)基于橢球體建模的多機(jī)器人碰撞檢測(cè)方法進(jìn)行研究，采取離散碰撞檢測(cè)的方法，在已規(guī)劃好的路徑上劃分步長(zhǎng)，每一個(gè)步長(zhǎng)上反復(fù)地調(diào)用靜態(tài)最短距離求解算法，從而在每一個(gè)步長(zhǎng)上進(jìn)行碰撞檢測(cè)。文中從反饋物體間的最短距離的角度出發(fā)，獲取機(jī)器人運(yùn)動(dòng)時(shí)彼此之間的最短距離并由此對(duì)多機(jī)器人進(jìn)行碰撞檢測(cè)。又由于橢球的拉伸性可以較為緊密地包圍模型，所以采用橢球體對(duì)機(jī)器人的連桿和關(guān)節(jié)進(jìn)行建模，利用空間幾何的相關(guān)知識(shí)，求解橢球體曲面間的最短距離，從而實(shí)現(xiàn)基于距離的多機(jī)器人碰撞檢測(cè)。