本發(fā)明屬于機(jī)器人技術(shù)領(lǐng)域，具體的說(shuō)是一種基于scs(sphere-cylinder-sphere，球體-圓柱體-球體)包圍結(jié)構(gòu)的機(jī)器人碰撞檢測(cè)方法。

背景技術(shù)：

機(jī)器人技術(shù)是實(shí)現(xiàn)先進(jìn)制造技術(shù)的重要基礎(chǔ)之一，正不斷向高精度、高效率、高協(xié)調(diào)性方向發(fā)展。機(jī)器人技術(shù)中的多自由度機(jī)器人是工業(yè)機(jī)器人的一種。為了提高機(jī)器人運(yùn)動(dòng)過(guò)程中的安全性，需要設(shè)計(jì)相應(yīng)的碰撞檢測(cè)，避免機(jī)器人與工件之間發(fā)生碰撞，從而使機(jī)器人在各種作業(yè)情況下都能安全快速準(zhǔn)確地到達(dá)給定的位置。

早期的碰撞檢測(cè)大多是基于格子或者bsp樹(shù)的?；诟褡拥呐鲎矙z測(cè)實(shí)現(xiàn)簡(jiǎn)單但精度不夠，不屬于嚴(yán)格意義的3d碰撞檢測(cè)。基于bsp樹(shù)的碰撞檢測(cè)需要很長(zhǎng)的處理時(shí)間，占用較多的資源容量。

當(dāng)前多采用基于包圍體的碰撞檢測(cè)算法。包圍體技術(shù)是在1976年由clark提出的，基本思想是用一個(gè)簡(jiǎn)單的幾何形體將動(dòng)畫(huà)場(chǎng)景中的幾何形體圍住，通過(guò)構(gòu)造樹(shù)狀層次結(jié)構(gòu)來(lái)逼近真實(shí)的包圍體?；趯哟伟鼑w的碰撞檢測(cè)算法根據(jù)包圍體類(lèi)型的不同可以區(qū)分為：包圍球體、aabb軸對(duì)齊包圍體、obb有向包圍體、k-dop包圍體等。obb和k-dop包圍體方法雖然緊密性較好，但復(fù)雜度比較高，旋轉(zhuǎn)更新慢，不適合多關(guān)節(jié)機(jī)器人在運(yùn)動(dòng)過(guò)程中需持續(xù)旋轉(zhuǎn)這一特點(diǎn)。包圍球的方法不需要旋轉(zhuǎn)更新，aabb的旋轉(zhuǎn)更新快，適合工業(yè)機(jī)器人的碰撞檢測(cè)，但二者包裝的緊密性較差，降低了碰撞檢測(cè)的精度。由于機(jī)器人的關(guān)節(jié)以及連桿多為柱形，并且機(jī)器人在運(yùn)動(dòng)過(guò)程中各關(guān)節(jié)和連桿需要不斷旋轉(zhuǎn)，持續(xù)求解包圍體位置比較復(fù)雜，因而直接用于碰撞檢測(cè)的包圍球和aabb軸對(duì)齊包圍體方法有一定的局限性。

技術(shù)實(shí)現(xiàn)要素：

為了克服機(jī)器人現(xiàn)有包圍體碰撞檢測(cè)算法的計(jì)算速度慢，檢測(cè)精度低等缺點(diǎn)，本發(fā)明提供一種基于scs包圍結(jié)構(gòu)的機(jī)器人碰撞檢測(cè)方法，根據(jù)機(jī)器人的結(jié)構(gòu)特點(diǎn)，可以大幅度提高碰撞檢測(cè)效率，同時(shí)在一定程度上提高機(jī)器人工作過(guò)程中的安全性和碰撞檢測(cè)精度。

本發(fā)明為實(shí)現(xiàn)上述目的所采用的技術(shù)方案是：一種基于scs包圍結(jié)構(gòu)的機(jī)器人碰撞檢測(cè)方法，包括以下步驟：

步驟1：采用d-h方法建立的機(jī)器人各桿坐標(biāo)系；

步驟2：采用包括兩個(gè)球體和一個(gè)圓柱體的scs包圍結(jié)構(gòu)，對(duì)機(jī)器人連桿和關(guān)節(jié)分別進(jìn)行包圍；

步驟3：求機(jī)器人各個(gè)scs包圍結(jié)構(gòu)的兩個(gè)球心坐標(biāo)；

步驟4：將滾軸工件模型以及各個(gè)scs包圍結(jié)構(gòu)向世界坐標(biāo)系中的xy、yz、xz三個(gè)平面投影，判斷滾軸工件模型與各個(gè)scs包圍結(jié)構(gòu)的投影在三個(gè)平面上是否均相交；

步驟5：如果是，則檢測(cè)到碰撞發(fā)生，重新規(guī)劃?rùn)C(jī)器人路徑，返回步驟3；否則，判斷是否到達(dá)路徑終點(diǎn)；

步驟6：如果已到達(dá)路徑終點(diǎn)，則表明未檢測(cè)到碰撞發(fā)生；如果未到達(dá)路徑終點(diǎn)，則更新機(jī)器人狀態(tài)，返回步驟3。

所述采用d-h方法建立的機(jī)器人各桿坐標(biāo)系，具體為：

坐標(biāo)系{i-1}到坐標(biāo)系{i}的變化矩陣為：

其中，分別為坐標(biāo)系{i}相對(duì)坐標(biāo)系{i-1}的姿態(tài)矩陣和位置矩陣；θi是關(guān)節(jié)i的旋轉(zhuǎn)角；βi是關(guān)節(jié)i和i+1間的扭轉(zhuǎn)角；αi為關(guān)節(jié)i+1的長(zhǎng)度；di是關(guān)節(jié)i的偏距。

所述scs包圍結(jié)構(gòu)具體為：兩個(gè)球體的球心分別與圓柱體底面的兩個(gè)圓心重合，scs包圍結(jié)構(gòu)的球體半徑與圓柱體底面半徑比值大于或者等于1。

所述采用scs包圍結(jié)構(gòu)，對(duì)機(jī)器人連桿和關(guān)節(jié)分別進(jìn)行包圍，使連桿或關(guān)節(jié)的兩端分別在兩個(gè)球體內(nèi)部，連桿或者關(guān)節(jié)的中間部分在圓柱體內(nèi)部。

所述求機(jī)器人各個(gè)scs包圍結(jié)構(gòu)的兩個(gè)球心坐標(biāo)，具體為：

其中，分別為坐標(biāo)系{i}相對(duì)坐標(biāo)系{i-1}的姿態(tài)矩陣和位置矩陣，1≤i≤n，p、w分別為坐標(biāo)系{n}相對(duì)于世界坐標(biāo)系的姿態(tài)矩陣和位置矩陣，球心在世界坐標(biāo)系中的坐標(biāo)即為(px，py，pz)。

所述將滾軸工件模型以及各個(gè)scs包圍結(jié)構(gòu)模型向世界坐標(biāo)系中的xy、yz、xz三個(gè)平面投影，判斷滾軸工件模型與各個(gè)scs包圍結(jié)構(gòu)的投影在三個(gè)平面上是否均相交，包括以下步驟：

將scs包圍結(jié)構(gòu)在任一平面上的投影轉(zhuǎn)化為相互連接的三個(gè)部分，即第一部分為圓形：以第一球體的球心在平面上的投影點(diǎn)為圓心，以第一球體的半徑為半徑的圓；第二部分為矩形：連接scs包圍結(jié)構(gòu)在平面上的兩球心投影點(diǎn)，形成線(xiàn)段，過(guò)兩個(gè)投影點(diǎn)分別做所述線(xiàn)段的垂線(xiàn)，兩投影點(diǎn)分別為兩垂線(xiàn)上線(xiàn)段的中點(diǎn)，其中所述兩垂線(xiàn)上線(xiàn)段的長(zhǎng)度均等于scs包圍結(jié)構(gòu)中圓柱體底面直徑的長(zhǎng)度，所述兩垂線(xiàn)上的線(xiàn)段在平面上平行且相等，為矩形的一組對(duì)邊；第三部分為圓形：以第二球體的球心在平面上的投影點(diǎn)為圓心，以第二球體的半徑為半徑的圓；

判斷滾軸工件模型與各個(gè)scs包圍結(jié)構(gòu)的投影在三個(gè)平面上是否相交，簡(jiǎn)化為判斷在三個(gè)平面上，滾軸工件模型的投影與scs包圍結(jié)構(gòu)投影轉(zhuǎn)化后的三個(gè)部分是否相交，若在任一平面上兩模型的投影不相交，則被檢測(cè)的結(jié)構(gòu)沒(méi)有發(fā)生碰撞，當(dāng)且僅當(dāng)三個(gè)平面上兩模型的投影均相交時(shí)，被檢測(cè)的結(jié)構(gòu)認(rèn)定為發(fā)生了碰撞。

本發(fā)明具有以下優(yōu)點(diǎn)及有益效果：

一是碰撞檢測(cè)速度快。scs包圍結(jié)構(gòu)普遍適用于在三維空間中連續(xù)不定向旋轉(zhuǎn)的柱形結(jié)構(gòu)，機(jī)器人的結(jié)構(gòu)特點(diǎn)和運(yùn)動(dòng)特點(diǎn)符合這一需求。在碰撞檢測(cè)階段，只需定位球心的坐標(biāo)就可得出碰撞檢測(cè)結(jié)果。

二是檢測(cè)精度較高。采用scs層次包圍結(jié)構(gòu)與大部分工業(yè)機(jī)器人的結(jié)構(gòu)相吻合，可以比較緊密將機(jī)器人包圍，提高檢測(cè)精度。

附圖說(shuō)明

圖1為scs包圍結(jié)構(gòu)截面示意圖；

圖2采用d-h方法建立的motoman-mh6機(jī)器人運(yùn)動(dòng)學(xué)坐標(biāo)系；

圖3為scs包圍結(jié)構(gòu)在平面上的投影轉(zhuǎn)化圖；

圖4基于scs包圍結(jié)構(gòu)的機(jī)器人碰撞檢測(cè)流程圖。

具體實(shí)施方式

下面結(jié)合附圖及實(shí)施例對(duì)本發(fā)明做進(jìn)一步的詳細(xì)說(shuō)明，基于scs包圍結(jié)構(gòu)的機(jī)器人碰撞檢測(cè)方法流程如圖4所示。

1.在初始階段，以motoman-mh6機(jī)器人為對(duì)象，如圖2所示，采用d-h方法建立的機(jī)器人各桿坐標(biāo)系，其中，坐標(biāo)系{0}三個(gè)坐標(biāo)軸方向與世界坐標(biāo)系中相應(yīng)的x、y、z坐標(biāo)軸方向一致，坐標(biāo)系{i-1}到坐標(biāo)系{i}的變化矩陣為：

其中，分別為坐標(biāo)系{i}相對(duì)坐標(biāo)系{i-1}的姿態(tài)矩陣和位置矩陣；θi是關(guān)節(jié)i的旋轉(zhuǎn)角；βi是關(guān)節(jié)i和i+1間的扭轉(zhuǎn)角；αi為關(guān)節(jié)i+1的長(zhǎng)度；di是關(guān)節(jié)i的偏距。

2.采用sphere-cylinder-sphere(球體-圓柱體-球體)包圍結(jié)構(gòu)，對(duì)機(jī)器人連桿和關(guān)節(jié)分別進(jìn)行包圍。如圖1所示，其中：scs包圍結(jié)構(gòu)由球體s1，圓柱體c，以及球體s2組成；兩個(gè)球體的球心分別與圓柱體底面的兩個(gè)圓心重合；scs包圍結(jié)構(gòu)的球體半徑與圓柱體底面半徑比值大于或者等于1；采用scs包圍空間對(duì)機(jī)器人各個(gè)連桿和關(guān)節(jié)進(jìn)行包圍，并且保證機(jī)器人各個(gè)連桿和關(guān)節(jié)在scs包圍結(jié)構(gòu)內(nèi)部。包圍特點(diǎn)為：連桿或者關(guān)節(jié)的兩端分別在兩個(gè)球體內(nèi)部，連桿或者關(guān)節(jié)的中間主體部分在圓柱體內(nèi)部。

3.在碰撞檢測(cè)階段一，利用代數(shù)解析法求機(jī)器人各個(gè)scs包圍結(jié)構(gòu)的兩個(gè)球心坐標(biāo)。

求各個(gè)scs包圍結(jié)構(gòu)的兩個(gè)球心坐標(biāo)，求解過(guò)程為：

其中，分別為坐標(biāo)系{i}相對(duì)坐標(biāo)系{i-1}的姿態(tài)矩陣和位置矩陣，p、w分別為坐標(biāo)系{n}相對(duì)于世界坐標(biāo)系的姿態(tài)矩陣和位置矩陣，球心在世界坐標(biāo)系中的坐標(biāo)即為(px，py，pz)。

4.在碰撞檢測(cè)階段二，將滾軸工件模型以及各個(gè)scs包圍結(jié)構(gòu)向世界坐標(biāo)系中的xy、yz、xz三個(gè)平面投影。

采用常見(jiàn)的包圍結(jié)構(gòu)對(duì)滾軸工件模型進(jìn)行包圍，之后向世界坐標(biāo)系中的xy、yz、xz三個(gè)平面投影。

scs包圍結(jié)構(gòu)在任一平面上的投影可以轉(zhuǎn)化為相互連接的三個(gè)部分。如圖示3所示，第一部分為圓形：以s1的球心在平面上的投影點(diǎn)為圓心，以s1的半徑為半徑的圓。第二部分為矩形：連接scs包圍結(jié)構(gòu)在平面上的兩球心投影點(diǎn)，形成線(xiàn)段o1o2，過(guò)兩個(gè)投影點(diǎn)分別做o1o2的垂線(xiàn)l1，l2，其中o1為l1上線(xiàn)段ab的中點(diǎn)，o2為l2上線(xiàn)段cd的中點(diǎn)，其中ab、cd的長(zhǎng)度均等于圓柱體底面直徑的長(zhǎng)度，矩形abdc即為所求。第三部分為圓形:以s2的球心在平面上的投影點(diǎn)為圓心，以s2的半徑為半徑的圓。

判斷滾軸工件模型與各個(gè)scs包圍結(jié)構(gòu)的投影在三個(gè)平面上是否相交，可以簡(jiǎn)化為判斷在三個(gè)平面上，滾軸工件模型的投影與scs包圍結(jié)構(gòu)模型投影轉(zhuǎn)化后的三個(gè)部分是否相交，若在任一平面上兩模型的投影不相交，則被檢測(cè)的結(jié)構(gòu)沒(méi)有發(fā)生碰撞，當(dāng)且僅當(dāng)三個(gè)平面上兩模型的投影均相交時(shí)，被檢測(cè)的結(jié)構(gòu)認(rèn)定為發(fā)生了碰撞。

5.在碰撞檢測(cè)階段三：如果檢測(cè)到碰撞發(fā)生，重新規(guī)劃?rùn)C(jī)器人路徑，進(jìn)行碰撞檢測(cè)；如果未檢測(cè)到碰撞發(fā)生，判斷機(jī)器人是否到達(dá)路徑規(guī)劃終點(diǎn)，若未達(dá)到終點(diǎn)，更新機(jī)器人狀態(tài)，進(jìn)行碰撞檢測(cè)。

在結(jié)束階段：如果在機(jī)器人運(yùn)動(dòng)路徑中始終未檢測(cè)到碰撞發(fā)生，則認(rèn)定規(guī)劃路徑安全，碰撞檢測(cè)結(jié)束。