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

背景技術(shù)：

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

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

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

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

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

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

步驟1：采用d-h方法建立的機器人各桿坐標系；

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

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

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

步驟5：如果是，則檢測到碰撞發(fā)生，重新規(guī)劃機器人路徑，返回步驟3；否則，判斷是否到達路徑終點；

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

所述采用d-h方法建立的機器人各桿坐標系，具體為：

坐標系{i-1}到坐標系{i}的變化矩陣為：

其中，分別為坐標系{i}相對坐標系{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的長度；di是關(guān)節(jié)i的偏距。

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

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

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

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

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

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

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

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

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

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

附圖說明

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

圖2采用d-h方法建立的motoman-mh6機器人運動學坐標系；

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

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

具體實施方式

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

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

其中，分別為坐標系{i}相對坐標系{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的長度；di是關(guān)節(jié)i的偏距。

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

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

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

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

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

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

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

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

5.在碰撞檢測階段三：如果檢測到碰撞發(fā)生，重新規(guī)劃機器人路徑，進行碰撞檢測；如果未檢測到碰撞發(fā)生，判斷機器人是否到達路徑規(guī)劃終點，若未達到終點，更新機器人狀態(tài)，進行碰撞檢測。

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