基于AABB包圍盒的雙機(jī)械臂避碰技術(shù)的研究

干智峰

摘 要

機(jī)械臂避碰技術(shù)能夠保證機(jī)械臂在工作過(guò)程中的安全，對(duì)于增強(qiáng)機(jī)械臂在整個(gè)應(yīng)用領(lǐng)域中的安全性具有重要意義。本文基于雙機(jī)械臂的D-H模型和AABB包圍盒技術(shù)提出了雙機(jī)械臂避碰方法。并通過(guò)MATLAB和Adams聯(lián)合仿真對(duì)雙機(jī)械臂避碰方法進(jìn)行了驗(yàn)證。仿真結(jié)果顯示，該方法實(shí)現(xiàn)了雙機(jī)械臂之間的碰撞檢測(cè)，能夠防止機(jī)械臂在運(yùn)動(dòng)過(guò)程中發(fā)生碰撞。

關(guān)鍵詞

機(jī)械臂;D-H模型;AABB包圍盒;MATLAB Adams

中圖分類號(hào)： TP242 ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼： A

DOI：10.19694/j.cnki.issn2095-2457 . 2020 . 17 . 34

Abstract

The collision avoidance technology of the manipulator is effective enough to ensure the safety of the manipulator in the working process， which is of great significance to enhance the safety of the manipulator in the whole application field. Based on the D-H model of double manipulator and AABB bounding box technology， this paper presents a collision avoidance method of double manipulator. The collision avoidance method of double manipulator is verified by the joint simulation of MATLAB and Adams. The simulation results show that the collision detection between the two manipulators is fully realized by this method， which can prevent the collision of the two manipulators in the process of movement.

Key words

Robotic arm; D-H model; AABB enclosure; MATLAB Adams

0 引言

隨著機(jī)械臂和多機(jī)械臂系統(tǒng)在實(shí)際生產(chǎn)中大量應(yīng)用，人們對(duì)機(jī)械臂在使用過(guò)程中的安全性也提出了更高的要求。其中最重要的就是要防止機(jī)械臂在運(yùn)動(dòng)過(guò)程中發(fā)生碰撞。

機(jī)械臂避碰算法的核心是機(jī)械臂碰撞檢測(cè)算法。通過(guò)碰撞檢測(cè)算法提前對(duì)機(jī)械臂的位置狀態(tài)進(jìn)行預(yù)判，進(jìn)而防止機(jī)械臂發(fā)生碰撞。在碰撞檢測(cè)算法中主要可以分為：空間剖分法和層次包圍盒方法兩大類。其中層次包圍盒法的應(yīng)用非常普遍，且其非常適用于復(fù)雜環(huán)境中的碰撞檢測(cè)。由于機(jī)械臂具有復(fù)雜的幾何結(jié)構(gòu)和運(yùn)動(dòng)，所以在本文中，采用包圍盒技術(shù)對(duì)運(yùn)動(dòng)中的兩個(gè)機(jī)械臂進(jìn)行碰撞檢測(cè)。

1 AABB包圍盒碰撞檢測(cè)方法簡(jiǎn)介

AABB包圍盒的碰撞檢測(cè)，是基于分離軸定理。該包圍盒構(gòu)造方法簡(jiǎn)單，只需要找出其在坐標(biāo)軸上投影的最大值和最小值即可，具有非常好的簡(jiǎn)單性。此外，當(dāng)將AABB包圍盒用于變形物體的時(shí)候，不需要重新構(gòu)造包圍盒。只需將變形后物體中變形的基本集合元素進(jìn)行更新，然后自下向上重新構(gòu)造包圍盒形成父節(jié)點(diǎn)，最后對(duì)整個(gè)包圍盒進(jìn)行更新。

2 基于AABB包圍盒的雙機(jī)械臂避碰技術(shù)

2.1 機(jī)械臂包圍盒構(gòu)建方式

雙機(jī)械臂避碰技術(shù)的核心是雙機(jī)械臂包圍盒的碰撞檢測(cè)。首先需要為機(jī)械臂建立AABB包圍盒。如何用簡(jiǎn)單的方法，建立具有良好緊密性的包圍盒是碰撞檢測(cè)的關(guān)鍵。為了提高包圍盒對(duì)機(jī)械臂包圍的緊密性，結(jié)合機(jī)械臂的幾何特性，本文提出采用分段建模的方式，對(duì)整個(gè)機(jī)械臂進(jìn)行建模。

在對(duì)機(jī)械臂進(jìn)行分段的時(shí)候，充分考慮了機(jī)械臂的特點(diǎn)，對(duì)機(jī)械臂進(jìn)行D-H建模，將D-H模型關(guān)節(jié)坐標(biāo)系作為每段的分割節(jié)點(diǎn)，依次將機(jī)械臂分割成了六段，并將這六段分別基于不同的D-H模型的關(guān)節(jié)坐標(biāo)系進(jìn)行建模。

2.2 機(jī)械臂AABB包圍盒的建立

2.2.1 機(jī)械臂D-H模型

在本文中，采用了KUKA KR16機(jī)器人進(jìn)行D-H建模。

KR16機(jī)器人共有六個(gè)串聯(lián)旋轉(zhuǎn)關(guān)節(jié)。前三個(gè)關(guān)節(jié)用來(lái)控制機(jī)器人末端執(zhí)行器的位置，后三個(gè)關(guān)節(jié)用來(lái)確定末端執(zhí)行器的姿態(tài)。建立的模型如圖1所示。

2.2.2 建立包圍盒層次樹(shù)

機(jī)械臂包圍盒層次樹(shù)構(gòu)造過(guò)程如下：

（1）根據(jù)機(jī)械臂的幾何模型建立D-H模型，根據(jù)模型對(duì)機(jī)械臂進(jìn)行分段;

（2）對(duì)某一分段的物體建立包圍盒;將該包圍盒作為層次包圍盒樹(shù)的根;

（3）根據(jù)最長(zhǎng)軸方法，確定分段包圍盒的最長(zhǎng)軸，將此軸作為分割軸;

（4）根據(jù)包圍盒中值法確定分割軸上的分割點(diǎn)，定位分割平面;

（5）應(yīng)用劃分出來(lái)的分割平面，將分段包圍盒劃分為了兩個(gè)子集;

（6）把步驟5的子集作為根節(jié)點(diǎn)，返回（3）直到包圍盒樹(shù)達(dá)到要求的深度;

（7）返回步驟2直到所有分段的包圍盒層次樹(shù)都構(gòu)建完成。

根據(jù)這種方法做出的層次包圍盒樹(shù)，是一棵完全二叉樹(shù)。如圖2為機(jī)器人末端執(zhí)行器的包圍盒層次樹(shù)示意圖。

2.3 雙機(jī)械臂碰撞檢測(cè)方法的實(shí)現(xiàn)

包圍盒的碰撞檢測(cè)將采用全局搜索和局部檢索的碰撞檢測(cè)方式。全局搜索是指通過(guò)包圍盒快速排除多數(shù)明顯不相交的對(duì)象;而局部檢索是全局搜索的后續(xù)階段，采用局部檢索對(duì)可能相交的物體進(jìn)行進(jìn)一步檢測(cè)。

2.3.1 AABB包圍盒全局搜索

雙機(jī)械臂碰撞檢測(cè)改進(jìn)的全局搜索的主要包含以下幾個(gè)步驟，首先為機(jī)器人A、B計(jì)算AABB分段包圍盒，分別建立包圍盒在全局坐標(biāo)系下的X、Y、Z軸上的投影列表XA、YA、ZA、XB、YB、ZB，并對(duì)列表進(jìn)行第一次排序。之后對(duì)X、Y、Z軸三個(gè)軸進(jìn)行劃分，得到機(jī)器人A、B在X、Y、Z軸三個(gè)軸投影到子序列。將對(duì)應(yīng)區(qū)域的子序列進(jìn)行二次排序后，將子列表進(jìn)行重新組合得到列表XO、YO、ZO。檢查在三個(gè)列表中是否有重疊區(qū)域。當(dāng)在三個(gè)列表上都重疊的時(shí)候，就說(shuō)明包圍盒重疊了。利用全局搜索可以快速地排除大量明顯不相交的包圍盒。

2.3.2 運(yùn)用分離軸定理逐步求精

根據(jù)全局搜索確定了進(jìn)一步碰撞檢測(cè)的對(duì)象后，將采用分離軸定理對(duì)包圍盒進(jìn)行碰撞檢測(cè)。

對(duì)于AABB包圍盒，在選取分離軸的時(shí)候，只需要分別取第一個(gè)包圍盒中三個(gè)相互垂直的邊，第二個(gè)包圍盒中三個(gè)相互垂直的邊，以及第一個(gè)包圍盒三個(gè)邊中的任意一個(gè)邊和第二個(gè)包圍盒三個(gè)邊中的任意一個(gè)邊所決定的平面的法線。按照這種做法，一共能夠計(jì)算出9個(gè)分離軸。所以，一共需要檢測(cè)15個(gè)分離軸。

首先任選一個(gè)分離軸，將機(jī)器人A的包圍盒頂點(diǎn)投影到選定分離軸，然后求出這六個(gè)值的最大值和最小值。同理，用同樣的方法求出機(jī)器人B的包圍盒投影到分離軸上的最大值和最小值，通過(guò)計(jì)算兩個(gè)包圍盒在分離軸上的投影范圍，記錄包圍盒在這個(gè)分離軸上的投影是否重疊。重復(fù)上述過(guò)程，直到將15個(gè)分離軸全部檢測(cè)。根據(jù)投影重疊進(jìn)而判斷包圍盒是否發(fā)生碰撞。

3 雙機(jī)械臂避碰仿真實(shí)驗(yàn)

在本節(jié)中，采用MATLAB和Adams聯(lián)合仿真來(lái)驗(yàn)證雙機(jī)械臂避碰方法。

在聯(lián)合仿真實(shí)驗(yàn)中，設(shè)置多個(gè)場(chǎng)景來(lái)驗(yàn)證算法的有效性。圖3（a）是機(jī)械臂的包圍盒沒(méi)有發(fā)生碰撞的場(chǎng)景。圖3（b）顯示的是機(jī)械臂的包圍盒在空間中的位置映射。如圖3所示，雙機(jī)械臂的包圍盒沒(méi)有發(fā)生碰撞，雙機(jī)械臂保持運(yùn)動(dòng)。

如圖4所示，機(jī)械臂分段包圍盒層次樹(shù)的根包圍盒發(fā)生了碰撞。

如圖5（a）所示，當(dāng)機(jī)械臂分段包圍盒層次樹(shù)的根包圍盒發(fā)生碰撞，此時(shí)，如圖5（b）所示，碰撞的包圍盒層次樹(shù)會(huì)向下遍歷，對(duì)子包圍盒進(jìn)行碰撞檢測(cè)。如果子包圍盒沒(méi)有發(fā)生碰撞，即說(shuō)明機(jī)械臂運(yùn)行安全，機(jī)械臂會(huì)繼續(xù)運(yùn)動(dòng)。如果葉子結(jié)點(diǎn)包圍盒發(fā)生碰撞，則會(huì)給出碰撞信號(hào)，使機(jī)械臂停止運(yùn)動(dòng)。

圖6（a）表示的是計(jì)算得到的機(jī)械臂間包圍盒最小距離與時(shí)間的函數(shù)圖像，圖6（b）表示的是碰撞檢測(cè)狀態(tài)圖像。從圖6可以看出，在機(jī)械臂運(yùn)動(dòng)的過(guò)程中，包圍盒發(fā)生了碰撞，且包圍盒的葉子結(jié)點(diǎn)發(fā)生了碰撞，系統(tǒng)給出了碰撞信號(hào)。

圖7是層次包圍盒樹(shù)的碰撞檢測(cè)對(duì)比圖。如圖7（a）所示，在圖中共有兩處的包圍盒有重疊，在第一處的包圍盒層次樹(shù)的葉子結(jié)點(diǎn)包圍盒發(fā)生了碰撞，而在第二處，葉子節(jié)點(diǎn)包圍盒并沒(méi)有發(fā)生碰撞。從圖7（b）可以看出，只有第一處包圍盒距離為零時(shí)給出了碰撞信號(hào)，而第二處并沒(méi)有給出信號(hào)。

根據(jù)上述的仿真試驗(yàn)，可以根據(jù)AABB包圍盒對(duì)雙機(jī)械臂進(jìn)行碰撞檢測(cè)，當(dāng)包圍盒的葉子結(jié)點(diǎn)發(fā)生碰撞的時(shí)候，算法會(huì)給出碰撞信號(hào)。在實(shí)際應(yīng)用中，可以根據(jù)碰撞信號(hào)，提前對(duì)機(jī)械臂采取制動(dòng)措施，防止機(jī)械臂發(fā)生碰撞。

4 小結(jié)

本文重點(diǎn)介紹了應(yīng)用在雙機(jī)械臂避碰中的基于AABB包圍盒的雙機(jī)械臂避碰算法。重點(diǎn)研究了避碰算法中核心的基于AABB包圍盒的碰撞檢測(cè)算法。剖析了AABB包圍盒的建立方法，并結(jié)合機(jī)械臂的結(jié)構(gòu)特性，提出采用分段建模的方式對(duì)機(jī)械臂包圍盒的構(gòu)造方法。此外，還詳細(xì)研究了雙機(jī)械臂基于AABB包圍盒的碰撞檢測(cè)方法。最后通過(guò)MATLAB和Adams聯(lián)合仿真對(duì)雙機(jī)械臂的避碰算法進(jìn)行了驗(yàn)證。仿真結(jié)果顯示，基于AABB包圍盒的雙機(jī)械臂避碰方法能夠有效地檢測(cè)到雙機(jī)械臂包圍盒的碰撞，并給出避碰信號(hào)。

參考文獻(xiàn)

[1]黃可.層次包圍盒的碰撞檢測(cè)算法研究[J].中國(guó)科技信息， 2010（17）：97-98.

[2]Barnes，Noble.Websters New Universal Unabridged Dictionary[J]，New York，1989：P1-2.

[3]Zhao W， Wen R. The Algorithm of Fast Collision Detection Based on Hybrid Bounding Box[C].International Conference on Computer Science and Electronics Engineering. IEEE， 2012：547-551.

[4]熊玉梅.虛擬環(huán)境中物體碰撞檢測(cè)技術(shù)的研究[D].上海大學(xué)， 2011.

[5]Ning T， Guo C. Optimization of collision detection method using hybrid bounding box[J]. Computer Engineering & Applications， 2011， 47（1）：1-3.

[6]謝斌斌.基于包圍盒的碰撞檢測(cè)技術(shù)的研究[D].哈爾濱工程大學(xué)，2015.

[7]王曉榮，王萌，李春貴.基于AABB包圍盒的碰撞檢測(cè)算法的研究[J].計(jì)算機(jī)工程與科學(xué)，2010，32（4）：59-61.