基于橢球體建模的多機器人碰撞檢測方法研究

許曉群，侯忠平，王昌順，杜能，夏衛生

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

0 前言

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

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

1 基于包圍橢球的機器人建模

本文作者基于六自由度工業機器人PUMA560 進行研究。PUMA560 機器人的三維示意如圖1 所示，其連桿和關節具體結構如圖2 所示。PUMA560 是關節型機器人，含有6 個連桿和6 個關節，6 個運動關節均為轉動關節，其末端手腕的運動決定于6 個運動關節順序運動或同時運動，其中前3 個運動關節確定末端手腕的空間位置，后3 個運動關節確定末端手腕的空間姿態。

圖1 PUMA560 機器人三維圖Fig.1 3D model of PUMA560 robot

圖2 PUMA560 機器人的連桿和關節結構Fig.2 Linkage and joint structure of PUMA560 robot

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

圖3 PUMA560 簡化模型Fig.3 Simplified model of PUMA560

圖4 橢球體包圍機器人連桿和關節Fig.4 The ellipsoid surrounding the robot connecting rods and joints

橢球體的主軸長決定于連桿長，取連桿的中點為橢球體的中心，則每一個橢球體代表相對應的每一個連桿。根據PUMA560 機器人連桿參數，設置第3 個連桿，即末端連桿的橢球體參數，橢球體中心為機器人運動時實時反饋的連桿中心的位置坐標，橢球體的3 個主軸根據PUMA560 的參數設置為e1＝250 mm，e2＝120 mm，e3＝125 mm。

由于連桿隨關節運動而運動，將桿和關節作為整體被橢球體包圍，不再區分關節和連桿，使包圍盒結構清晰簡單，不用單獨考慮連桿和關節各自的碰撞情況，使得計算量減少，提高碰撞檢測效率。

2 基于橢球坐標系的最短距離計算

2.1 構建橢球坐標系

橢球坐標系是以某個橢球面為參照面，建立特定的局部坐標系，如圖5 所示，橢球體A 的中心為點xCA，橢球體B 的中心為點xCB，以橢球體A 為參照面，建立橢球坐標系ε，以點xCA為坐標系原點，以橢球體A 的長軸和短軸分別為坐標系的x和y軸。

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

橢球坐標系中的坐標在笛卡爾坐標系中均滿足：

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

其中：e1、e2、e3為橢球體的3 個半軸長。橢球體A 向量矩陣Q乘以比例因子d的平方轉換為矩陣P，d為標量，橢球體坐標系ε中坐標的表達式轉化為

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

2.2 等高面求解

橢球坐標系是以等高面來確定兩點之間的距離，求解橢球體A 和橢球體B 之間的最短距離問題則轉換為求解橢球體B 在橢球坐標系ε中所相切的等高面的問題。

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

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

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

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

由圖6 可知，過切點xt指向橢球體A′的法向量nA與過切點xt指向橢球體B 的法向量nB共線但方向相反，則所共線的直線表達式通過對橢球體的向量方程進行求導可得：

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

再將式（4）代入到式（1）中，可得到有關距離d的多項式函數fd（t）：

通過解方程fd（t）＝0 可得到比例因子t的兩個根，但因為法向量nA和法向量nB方向相反，因此負根才是滿足幾何約束的解。將求解得到的負值代入式（4），可得到切點xt的坐標值，再將切點xt的坐標值代入式（5）得到距離d。

2.3 最短歐氏距離求解

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

如圖7 所示，先將橢球體A 縮放為單位球A0，沿3 個坐標軸方向對橢球體A 進行縮放，縮放系數分別為1／e1、1／e2、1／e3，得到縮放矩陣T為

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

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

再求點x′t的單位向量x0：

該單位向量的一端端點為單位球A0的中心，因此單位向量x0位于單位球A0上，又因為單位球A0是由橢球體A 進行縮放變換得到的，對單位球A0進行縮放變換的逆變換又可得到橢球體A，故單位向量x0也在橢球體A 上，這時可得到縮放后的最短距離向量dF＝xtF-x0，將最短距離向量dF進行縮放變換的逆變換可得到實際距離向量d＝T-1dF。橢球體A 上距離橢球體B 最短距離的點xA的坐標通過單位向量x0縮放變換的逆變換和橢球體坐標系相對笛卡爾坐標系的平移變換向量xCA的組合變換得到：

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

3 多機器人碰撞檢測算法

由于機器人的運動軌跡是由機器人末端結構的運動情況決定，因此文中所研究的多機器人碰撞檢測算法主要針對機器人末端結構之間的碰撞檢測。所要實現的功能是：已知當前時刻多個機器人末端結構的位置坐標，計算出多個機器人末端結構兩兩之間的最短距離，并與其接觸距離進行對比，最終判斷多個機器人運動過程是否發生碰撞，如果發生碰撞則確定發生碰撞的運動軌跡范圍。

基于橢球體建模的多機器人碰撞檢測算法是先根據機器人的結構參數進行建模。作者通過DH 參數法對機器人構建固定連桿坐標系；然后對機器人運動路徑進行規劃，依據機器人運動學仿真來設定機器人末端連桿的初始位置和目標位置得到規劃路徑；然后在機器人沿規劃路徑運動過程中調用最短距離求解函數進行分離距離的求解，最后根據分離距離和接觸判斷是否發生碰撞。

多機器人碰撞檢測算法的流程如圖8 所示：首先初始化機器人的DH 參數；通過機器人的DH 參數構建機器人連桿坐標系，獲得機器人的簡化模型；根據機器人運動學規劃機器人運動路徑；在已規劃的路徑上選取合適的時間步長，在軌跡上取樣；在軌跡每一個離散時間點上調用基于橢球坐標系最短距離求解算法，獲得當前時刻機器人之間的最短距離；輸出機器人沿規劃路徑運動過程中最短距離隨時間變化的曲線；最后將最短距離變化曲線與接觸距離曲線結合，判斷是否發生碰撞。

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

針對三機器人進行碰撞檢測仿真。三機器人碰撞檢測是以雙機器人碰撞檢測仿真為基礎，同時給3 個機器人A、B、C 規劃運行軌跡，機器人A 和機器人B 沿相交軌跡運動，機器人B 和機器人C 沿逐漸靠近的軌跡運動，機器人A 和機器人C 沿交叉軌跡運動。可以推測，機器人A 和機器人B 之間的最短距離是是先變小后變大，機器人B 和機器人C 之間的最短距離是逐漸變小，機器人A 和機器人C 之間的最短距離是先變小后變大。在3 個機器人同時運動過程中，對兩兩機器人之間的最短距離進行求解并判斷是否發生碰撞，因此同時輸出3 條最短距離變化曲線。

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

圖9 三機器人運動路徑規劃Fig.9 Three robot motion paths planning

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

4 結論

文中針對基于橢球體建模的多機器人碰撞檢測方法進行研究，采取離散碰撞檢測的方法，在已規劃好的路徑上劃分步長，每一個步長上反復地調用靜態最短距離求解算法，從而在每一個步長上進行碰撞檢測。文中從反饋物體間的最短距離的角度出發，獲取機器人運動時彼此之間的最短距離并由此對多機器人進行碰撞檢測。又由于橢球的拉伸性可以較為緊密地包圍模型，所以采用橢球體對機器人的連桿和關節進行建模，利用空間幾何的相關知識，求解橢球體曲面間的最短距離，從而實現基于距離的多機器人碰撞檢測。