本發(fā)明涉及一種工業(yè)機(jī)器人碰撞預(yù)測(cè)方法，尤其是一種兩個(gè)工業(yè)機(jī)器人在同一工作空間運(yùn)動(dòng)時(shí)的碰撞預(yù)測(cè)方法。
背景技術(shù)：
近十幾年工業(yè)機(jī)器人的制造和控制技術(shù)發(fā)展迅速，已經(jīng)廣泛地應(yīng)用于汽車制造業(yè)并能達(dá)到很高的控制精度。隨著我國(guó)勞動(dòng)力成本增加，在“機(jī)器換人”和產(chǎn)業(yè)升級(jí)的要求下各行業(yè)的自動(dòng)化生產(chǎn)水平亟需提高。應(yīng)用工業(yè)機(jī)器人能夠避免重復(fù)繁瑣的人工勞動(dòng)、實(shí)現(xiàn)高穩(wěn)定、高精度生產(chǎn)，從而使工作效率得到很大提升。對(duì)單個(gè)機(jī)器人的路徑、軌跡規(guī)劃大多能通過(guò)人工示教完成，所以已經(jīng)有越來(lái)越多的工廠使用可編程的工業(yè)機(jī)器人代替生產(chǎn)線上的人工。但是對(duì)于一類需要兩個(gè)機(jī)器人互相協(xié)調(diào)配合才能完成的任務(wù)，如何讓機(jī)器人既能夠自主避免可能出現(xiàn)的干涉和碰撞又能快速準(zhǔn)確地完成任務(wù)，這便要求機(jī)器人在協(xié)作運(yùn)動(dòng)的同時(shí)能夠?qū)赡艹霈F(xiàn)的碰撞作出可靠的預(yù)測(cè)。
技術(shù)實(shí)現(xiàn)要素：
為了克服已有工業(yè)機(jī)器人軌跡規(guī)劃方法中無(wú)法有效預(yù)測(cè)碰撞、安全性較低的不足，本發(fā)明提供了一種控制精度較高、有效實(shí)現(xiàn)碰撞預(yù)測(cè)、安全性較高的基于投影和距離判斷的工業(yè)機(jī)器人碰撞預(yù)測(cè)方法。
本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案是：
一種基于投影和距離判斷的工業(yè)機(jī)器人碰撞預(yù)測(cè)方法，對(duì)于運(yùn)行過(guò)程中可能發(fā)的碰撞，采用一種檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)距離的方法進(jìn)行碰撞檢測(cè)：將機(jī)器人關(guān)節(jié)向速度向量的法平面投影，若關(guān)節(jié)投影發(fā)生干涉則對(duì)關(guān)節(jié)軸線上各關(guān)鍵點(diǎn)最短距離進(jìn)行判斷，若小于運(yùn)行時(shí)的安全距離則認(rèn)為存在碰撞的風(fēng)險(xiǎn)。
進(jìn)一步，所述碰撞預(yù)測(cè)方法包括以下步驟：
步驟1：機(jī)器人關(guān)節(jié)標(biāo)準(zhǔn)化處理
對(duì)于兩個(gè)機(jī)器人同時(shí)作業(yè)，按照關(guān)節(jié)幾何形狀，采用半球端面圓柱體包絡(luò)兩個(gè)關(guān)節(jié)從而得到標(biāo)準(zhǔn)化后的關(guān)節(jié)邊界。
步驟2：求解運(yùn)動(dòng)過(guò)程中的安全距離
將兩球面端點(diǎn)連線得到關(guān)節(jié)軸線，計(jì)其長(zhǎng)度為li，i＝1,2；并得到圓柱體半徑為ri，在各軸線按等間距取
根據(jù)dhit設(shè)定碰撞檢測(cè)的極限距離dlim＝k1*dhit以及安全距離dsafe＝k2*dhit，k1,k2為安全系數(shù)；
步驟3：碰撞預(yù)測(cè)
根據(jù)關(guān)節(jié)角位移以及瞬時(shí)角速度可由正運(yùn)動(dòng)學(xué)求解得各關(guān)鍵點(diǎn)的笛卡爾坐標(biāo)
則說(shuō)明兩個(gè)關(guān)節(jié)在速度向量法平面的投影發(fā)生干涉，則進(jìn)一步判斷各關(guān)節(jié)關(guān)鍵點(diǎn)之間最短距離d，若有：
d＝min{‖p11-p21‖,‖p11-p22‖,‖p11-p23‖…‖p11-p24‖}<dsafe
則判定為有可能碰撞的危險(xiǎn)狀態(tài)，否則認(rèn)為是安全運(yùn)行。
本發(fā)明的技術(shù)構(gòu)思為：通過(guò)判斷關(guān)節(jié)在運(yùn)動(dòng)方向向量法平面上投影的干涉情況以及關(guān)節(jié)上所選點(diǎn)的距離完成碰撞檢測(cè)。
本發(fā)明的有益效果主要表現(xiàn)在：提出了一種檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)距離的方法進(jìn)行碰撞檢測(cè)，相較于單純的最短距離檢測(cè)能夠更有效地完成機(jī)器人關(guān)節(jié)運(yùn)動(dòng)過(guò)程的碰撞檢測(cè)；
附圖說(shuō)明
圖1為碰撞檢測(cè)流程圖。
具體實(shí)施方式
下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步描述。
參照?qǐng)D1，一種基于投影和距離判斷的工業(yè)機(jī)器人碰撞預(yù)測(cè)方法，對(duì)于運(yùn)行過(guò)程中可能發(fā)的碰撞，采用一種檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)距離的方法進(jìn)行碰撞檢測(cè)：將機(jī)器人關(guān)節(jié)向速度向量的法平面投影，若關(guān)節(jié)投影發(fā)生干涉則對(duì)關(guān)節(jié)軸線上各關(guān)鍵點(diǎn)最短距離進(jìn)行判斷，若小于運(yùn)行時(shí)的安全距離則認(rèn)為存在碰撞的風(fēng)險(xiǎn)。
所述工業(yè)機(jī)器人碰撞預(yù)測(cè)方法的過(guò)程為：首先，對(duì)機(jī)器人關(guān)節(jié)進(jìn)行標(biāo)準(zhǔn)化處理，然后檢測(cè)關(guān)節(jié)投影干涉情況通過(guò)判斷關(guān)鍵點(diǎn)距離進(jìn)行碰撞檢測(cè)：將機(jī)器人關(guān)節(jié)向速度向量的法平面投影，若關(guān)節(jié)投影發(fā)生干涉則對(duì)關(guān)節(jié)軸線上各關(guān)鍵點(diǎn)最短距離進(jìn)行判斷，若小于運(yùn)行時(shí)的安全距離則認(rèn)為存在碰撞的風(fēng)險(xiǎn)。
對(duì)于兩個(gè)機(jī)器人同時(shí)作業(yè)，其各關(guān)節(jié)為復(fù)雜的圓周運(yùn)動(dòng)，在這樣的運(yùn)動(dòng)狀態(tài)下單純的根據(jù)關(guān)節(jié)間距離并不能準(zhǔn)確地預(yù)判碰撞的發(fā)生。本發(fā)明采用一種先檢測(cè)關(guān)節(jié)投影干涉情況然后判斷關(guān)鍵點(diǎn)之間最短距離的方法完成完成運(yùn)動(dòng)過(guò)程中的碰撞檢測(cè)。以兩個(gè)機(jī)器人末端關(guān)節(jié)為例進(jìn)行說(shuō)明：
所述碰撞預(yù)測(cè)方法包括以下步驟：
步驟1：機(jī)器人關(guān)節(jié)標(biāo)準(zhǔn)化處理
按照關(guān)節(jié)幾何形狀，采用半球端面圓柱體包絡(luò)兩個(gè)關(guān)節(jié)從而得到標(biāo)準(zhǔn)化后的關(guān)節(jié)邊界。
步驟2：關(guān)鍵點(diǎn)選取以及碰撞預(yù)測(cè)安全距離計(jì)算
由步驟2標(biāo)準(zhǔn)化處理后，連接兩球面端點(diǎn)連線即得到關(guān)節(jié)軸線，長(zhǎng)度為li(i＝1,2)；對(duì)于圓柱體半徑為ri。在各軸線按等間距取
根據(jù)dhit設(shè)定碰撞檢測(cè)的極限距離dlim＝k1*dhit以及安全距離dsafe＝k2*dhit，k1,k2為安全系數(shù)可根據(jù)實(shí)際選取大于1的常數(shù)。
步驟3：碰撞預(yù)測(cè)
處于運(yùn)動(dòng)狀態(tài)的工業(yè)機(jī)器人，根據(jù)其關(guān)節(jié)角位移以及瞬時(shí)角速度可由正運(yùn)動(dòng)學(xué)求解得各關(guān)鍵點(diǎn)的笛卡爾坐標(biāo)
則說(shuō)明兩個(gè)關(guān)節(jié)在速度向量法平面的投影發(fā)生干涉，則進(jìn)一步判斷各關(guān)節(jié)關(guān)鍵點(diǎn)之間最短距離d，若有：
d＝min{‖p11-p21‖,‖p11-p22‖,‖p11-p23‖…‖p11-p24‖}<dsafe
則判定為有可能碰撞的危險(xiǎn)狀態(tài)否則認(rèn)為是安全運(yùn)行。