一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)和工業(yè)機(jī)器人的制作方法
【專利摘要】本申請(qǐng)公開了一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)和工業(yè)機(jī)器人，該系統(tǒng)應(yīng)用于具有多個(gè)關(guān)節(jié)的工業(yè)機(jī)器人，具體為根據(jù)工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目標(biāo)狀態(tài)數(shù)據(jù)計(jì)算該關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度，實(shí)時(shí)狀態(tài)數(shù)據(jù)包括關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度和實(shí)時(shí)時(shí)間，目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè)的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng)時(shí)間；然后在所述關(guān)節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算關(guān)節(jié)的允許位置和允許速度；最后根據(jù)目標(biāo)運(yùn)動(dòng)速度和允許速度對(duì)關(guān)節(jié)進(jìn)行減速保護(hù)，即當(dāng)關(guān)節(jié)的運(yùn)動(dòng)速度在奇異點(diǎn)過渡區(qū)域內(nèi)的運(yùn)動(dòng)速度超出允許速度后及時(shí)控制器進(jìn)行減速，以避免超速的發(fā)生，從而能夠解決工業(yè)機(jī)器人因?yàn)樵谄娈慄c(diǎn)區(qū)域因速度變大而導(dǎo)致的停機(jī)或生產(chǎn)安全的問題。
【專利說(shuō)明】
-種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)和工業(yè)機(jī)器人
技術(shù)領(lǐng)域
[0001] 本申請(qǐng)?jiān)O(shè)及機(jī)器人技術(shù)領(lǐng)域，更具體地說(shuō)，設(shè)及一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)和 工業(yè)機(jī)器人。
【背景技術(shù)】
[0002] 工業(yè)機(jī)器人是工業(yè)自動(dòng)化系統(tǒng)中重要的部件之一，當(dāng)前，多個(gè)國(guó)家在工業(yè)機(jī)器人 技術(shù)上已經(jīng)樹立了一定的領(lǐng)先優(yōu)勢(shì)，尤其是在機(jī)器人本體的設(shè)計(jì)上，相關(guān)的基礎(chǔ)技術(shù)已經(jīng) 比較成熟，因而眾多的機(jī)器人廠家在機(jī)器人應(yīng)用上投入了大量精力。對(duì)工業(yè)機(jī)器人的奇異 點(diǎn)處理方法是機(jī)器人應(yīng)用技術(shù)中一個(gè)主要的技術(shù)難點(diǎn)，是工業(yè)機(jī)器人必然遇到的技術(shù)問 題，工業(yè)機(jī)器人一旦遇到或接近奇異點(diǎn)，就會(huì)發(fā)生關(guān)節(jié)運(yùn)動(dòng)速度變大的現(xiàn)象，從而導(dǎo)致機(jī)器 人停機(jī)甚至帶來(lái)生產(chǎn)安全問題。 【實(shí)用新型內(nèi)容】
[0003] 有鑒于此，本申請(qǐng)?zhí)峁┮环N奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)和工業(yè)機(jī)器人，用于解決工 業(yè)機(jī)器人因?yàn)樵谄娈慄c(diǎn)區(qū)域因速度變大導(dǎo)致停機(jī)或生產(chǎn)安全的問題。
[0004] 為了實(shí)現(xiàn)上述目的，現(xiàn)提出的方案如下：
[0005] -種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，應(yīng)用于具有多個(gè)關(guān)節(jié)的工業(yè)機(jī)器人，包括：
[0006] 實(shí)時(shí)檢測(cè)模塊，用于根據(jù)所述工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目標(biāo)狀態(tài)數(shù)據(jù) 計(jì)算所述關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度，所述實(shí)時(shí)狀態(tài)數(shù)據(jù)包括所述關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度和實(shí)時(shí)時(shí) 間，所述目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè)的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng)時(shí)間；
[0007] 奇異點(diǎn)區(qū)域計(jì)算模塊，用于當(dāng)所述關(guān)節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算所 述關(guān)節(jié)的允許速度；
[000引奇異點(diǎn)處理模塊，用于根據(jù)所述目標(biāo)運(yùn)動(dòng)速度和所述允許速度對(duì)所述關(guān)節(jié)進(jìn)行減 速保護(hù)。
[0009] 可選的，所述實(shí)時(shí)檢測(cè)模塊包括：
[0010] 本體信息單元，用于獲取所述實(shí)時(shí)運(yùn)動(dòng)角度和所述實(shí)時(shí)時(shí)間；
[0011] 模型處理單元，用于根據(jù)所述實(shí)時(shí)運(yùn)動(dòng)角度、所述實(shí)時(shí)時(shí)間、所述目標(biāo)運(yùn)動(dòng)角度和 所述目標(biāo)運(yùn)動(dòng)時(shí)間計(jì)算所述目標(biāo)運(yùn)動(dòng)速度。
[0012] 可選的，所述模型處理單元包括：
[0013] 正逆解子單元，用于根據(jù)所述運(yùn)動(dòng)任務(wù)計(jì)算所述關(guān)節(jié)的目標(biāo)位置和目標(biāo)姿態(tài)，并 根據(jù)所述目標(biāo)位置和目標(biāo)姿態(tài)計(jì)算出所述目標(biāo)運(yùn)動(dòng)角度和所述目標(biāo)運(yùn)動(dòng)時(shí)間；
[0014] 速度計(jì)算子單元，用于根據(jù)所述實(shí)時(shí)運(yùn)動(dòng)角度、所述實(shí)時(shí)時(shí)間、所述目標(biāo)運(yùn)動(dòng)角度 和所述目標(biāo)運(yùn)動(dòng)時(shí)間計(jì)算所述目標(biāo)運(yùn)動(dòng)速度。
[0015] 可選的，所述實(shí)時(shí)檢測(cè)模塊還包括：
[0016] 警報(bào)單元，用于當(dāng)所述關(guān)節(jié)的運(yùn)動(dòng)速度和位置處于異常狀態(tài)時(shí)發(fā)出預(yù)設(shè)的控制指 令。
[0017] 可選的，所述警報(bào)單元包括：
[0018] 超速警報(bào)子單元，用于當(dāng)所述運(yùn)動(dòng)速度超出預(yù)設(shè)的速度闊值時(shí)發(fā)出第一控制指 令，所述第一控制指令用于控制所述工業(yè)機(jī)器人停機(jī)；
[0019] 工作區(qū)域警報(bào)子單元，用于當(dāng)所述工業(yè)機(jī)器人的末端的位置超出正常工作區(qū)域時(shí) 發(fā)出第二控制指令，所述第二控制指令用于控制所述工業(yè)機(jī)器人停機(jī)；
[0020] 奇異點(diǎn)區(qū)域警報(bào)子單元，用于根據(jù)對(duì)所述工業(yè)機(jī)器人的運(yùn)動(dòng)位置的判斷發(fā)出運(yùn)動(dòng) 狀態(tài)提不?目息。
[0021] -種工業(yè)機(jī)器人，設(shè)置有如上所述的奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)。
[0022] 從上述的技術(shù)方案可W看出，本申請(qǐng)公開了一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)和工業(yè) 機(jī)器人，該系統(tǒng)應(yīng)用于具有多個(gè)關(guān)節(jié)的工業(yè)機(jī)器人，具體為根據(jù)工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí) 狀態(tài)數(shù)據(jù)和目標(biāo)狀態(tài)數(shù)據(jù)計(jì)算該關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度，實(shí)時(shí)狀態(tài)數(shù)據(jù)包括關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng) 角度和實(shí)時(shí)時(shí)間，目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè)的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng) 時(shí)間；然后在所述關(guān)節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算關(guān)節(jié)的允許位置和允許速度； 最后根據(jù)目標(biāo)運(yùn)動(dòng)速度和允許速度對(duì)關(guān)節(jié)進(jìn)行減速保護(hù)，即當(dāng)關(guān)節(jié)的運(yùn)動(dòng)速度在奇異點(diǎn)過 渡區(qū)域內(nèi)的運(yùn)動(dòng)速度超出允許速度后及時(shí)控制器進(jìn)行減速，W避免超速的發(fā)生，從而能夠 解決工業(yè)機(jī)器人因?yàn)樵谄娈慄c(diǎn)區(qū)域因速度變大而導(dǎo)致的停機(jī)或生產(chǎn)安全的問題。
【附圖說(shuō)明】
[0023] 為了更清楚地說(shuō)明本申請(qǐng)實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn) 有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本 申請(qǐng)的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可W 根據(jù)運(yùn)些附圖獲得其他的附圖。
[0024] 圖1為本申請(qǐng)實(shí)施例提供的一種奇異點(diǎn)區(qū)域減速保護(hù)方法的流程圖
[0025] 圖2為本申請(qǐng)?zhí)峁┑囊环N工業(yè)機(jī)器人的D-H模型示意圖；
[0026] 圖3為本申請(qǐng)另一實(shí)施例提供的一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)的結(jié)構(gòu)框圖；
[0027] 圖4為本申請(qǐng)例提供另一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)的結(jié)構(gòu)框圖；
[0028] 圖5為本申請(qǐng)例提供又一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)的結(jié)構(gòu)框圖。
【具體實(shí)施方式】
[0029] 下面將結(jié)合本申請(qǐng)實(shí)施例中的附圖，對(duì)本申請(qǐng)實(shí)施例中的技術(shù)方案進(jìn)行清楚、完 整地描述，顯然，所描述的實(shí)施例僅僅是本申請(qǐng)一部分實(shí)施例，而不是全部的實(shí)施例。基于 本申請(qǐng)中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有作出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他 實(shí)施例，都屬于本申請(qǐng)保護(hù)的范圍。
[0030] 實(shí)施例一
[0031] 圖1為本申請(qǐng)實(shí)施例提供的一種奇異點(diǎn)區(qū)域減速保護(hù)方法的流程圖。
[0032] 如圖1所示，本實(shí)施例提供的奇異點(diǎn)區(qū)域減速保護(hù)方法應(yīng)用于具有多個(gè)關(guān)節(jié)的工 業(yè)機(jī)器人，為了方便描述，本實(shí)施例所選用的工業(yè)機(jī)器人具有6個(gè)關(guān)節(jié)。
[0033] 工業(yè)機(jī)器人通常是用D-H模型進(jìn)行機(jī)器人的結(jié)構(gòu)建模，如圖2所示，每?jī)蓚€(gè)關(guān)節(jié)之 間的位置及姿態(tài)變換都可W使用四個(gè)參數(shù)的D-H矩陣進(jìn)行模擬計(jì)算。最常見的6關(guān)節(jié)的工業(yè) 機(jī)器人結(jié)構(gòu)可W用六個(gè)D-H關(guān)節(jié)變換矩陣來(lái)表示，分別為：
[0036] 其中，ci = cos(0i) ,si = sin(0i) ,1 = 1,2, . . .，5,6，θ?為第i關(guān)節(jié)的關(guān)節(jié)轉(zhuǎn)動(dòng)角度， 曰1為關(guān)節(jié)i到關(guān)節(jié)i+1之間的連桿長(zhǎng)度，di為關(guān)節(jié)i-巧Ij關(guān)節(jié)i之間的連桿偏距。
[0037] 通過計(jì)算矩陣乘法可W得到機(jī)器人基座到機(jī)器人末端的變換矩陣：
[00；3 引
[0039] 基于上述分析，本實(shí)施例提供的奇異點(diǎn)區(qū)域減速保護(hù)方法包括下列步驟：
[0040] S101:根據(jù)工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目標(biāo)狀態(tài)數(shù)據(jù)計(jì)算關(guān)節(jié)的目標(biāo)運(yùn) 動(dòng)速度。
[0041] 其中實(shí)時(shí)狀態(tài)數(shù)據(jù)包括關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度和實(shí)時(shí)時(shí)間，目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè) 的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng)時(shí)間。目標(biāo)運(yùn)動(dòng)速度的計(jì)算過程包括：
[0042] 步驟1:獲取機(jī)器人關(guān)節(jié)的實(shí)時(shí)關(guān)節(jié)轉(zhuǎn)動(dòng)角度θιΚ及實(shí)時(shí)時(shí)間t。
[0043] 步驟2:將運(yùn)動(dòng)任務(wù)的目標(biāo)位置和目標(biāo)姿態(tài)采用封閉形式解法進(jìn)行計(jì)算，解出關(guān)節(jié) 需要轉(zhuǎn)動(dòng)的目標(biāo)運(yùn)動(dòng)角度θ'ι，并根據(jù)插補(bǔ)時(shí)間計(jì)算出運(yùn)動(dòng)任務(wù)的目標(biāo)運(yùn)動(dòng)時(shí)間t'。然根 據(jù)后關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度θιΚ及需要轉(zhuǎn)動(dòng)的目標(biāo)運(yùn)動(dòng)角度θ'ι，實(shí)時(shí)時(shí)間tW及運(yùn)動(dòng)任務(wù)的 目標(biāo)運(yùn)動(dòng)時(shí)間t'，求解出運(yùn)動(dòng)任務(wù)的關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度：
[0044]
[0045] S102:當(dāng)關(guān)節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算關(guān)節(jié)的允許速度。
[0046] 首先，采用化S(阻尼最小方差)算法進(jìn)行奇異值控制：
[0047]
[004引其中，J為機(jī)器人雅克比矩陣J'為加入阻尼因子后的雅克比矩陣，P2為阻尼因子。
[0049] 對(duì)阻尼因子的控制中加入奇異值控制：
[(K)加]
[0051]其中，δ為雅克比矩陣J的最小奇異值，ε為δ的最大闊值。
[0052] 當(dāng)Tw2《I目日I <Twi時(shí)，設(shè)定運(yùn)動(dòng)速度闊值k。
[0化3]計(jì)算機(jī)器人在闊值Twi處的運(yùn)動(dòng)速度作為運(yùn)動(dòng)速度最大值(? ,1)然后對(duì)闊值 Twl處的運(yùn)動(dòng)速度進(jìn)行歸一化處理，未使用減速算法的運(yùn)動(dòng)速度與使用減速算法的運(yùn)動(dòng)速度
之比為 5.
[0054]使用關(guān)節(jié)5角度值作為減速算法的變量，計(jì)算得到的距離變量為
[0化5]經(jīng)過減速算法修正過后的關(guān)節(jié)的允許速度為：
[0化6]
[0057] S103:根據(jù)目標(biāo)運(yùn)動(dòng)速度和允許速度對(duì)關(guān)節(jié)進(jìn)行減速保護(hù)。
[005引當(dāng)關(guān)節(jié)為執(zhí)行運(yùn)動(dòng)任務(wù)而達(dá)到的目標(biāo)運(yùn)動(dòng)速度超出允許速度時(shí)，及時(shí)控制該關(guān)節(jié) 降低速度，使之不超出允許速度，從而避免出現(xiàn)超速。
[0059] 從上述技術(shù)方案可W看出，本實(shí)施例提供了一種奇異點(diǎn)區(qū)域減速保護(hù)方法，該方 法應(yīng)用于具有多個(gè)關(guān)節(jié)的工業(yè)機(jī)器人，具體為根據(jù)工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目 標(biāo)狀態(tài)數(shù)據(jù)計(jì)算該關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度，實(shí)時(shí)狀態(tài)數(shù)據(jù)包括關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度和實(shí)時(shí)時(shí) 間，目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè)的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng)時(shí)間；然后在關(guān) 節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算關(guān)節(jié)的允許位置和允許速度;最后根據(jù)目標(biāo)運(yùn)動(dòng) 速度和允許速度對(duì)關(guān)節(jié)進(jìn)行減速保護(hù)，即當(dāng)關(guān)節(jié)的運(yùn)動(dòng)速度在奇異點(diǎn)過渡區(qū)域內(nèi)的運(yùn)動(dòng)速 度超出允許速度后及時(shí)控制器進(jìn)行減速，W避免超速的發(fā)生，從而能夠解決工業(yè)機(jī)器人因 為在奇異點(diǎn)區(qū)域因速度變大而導(dǎo)致的停機(jī)或生產(chǎn)安全的問題。
[0060] 本實(shí)施例中的根據(jù)工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目標(biāo)狀態(tài)數(shù)據(jù)計(jì)算關(guān)節(jié) 的目標(biāo)運(yùn)動(dòng)速度的過程中，還包括步驟3,該步驟為警報(bào)步驟，用于在工業(yè)機(jī)器人的關(guān)節(jié)的 運(yùn)動(dòng)速度和位置處于異常狀態(tài)時(shí)發(fā)出預(yù)設(shè)的控制指令。
[0061] 首先，針對(duì)6個(gè)關(guān)節(jié)的運(yùn)動(dòng)速度分別選取闊值Ti，i = l，2, . . .，5,6,進(jìn)行判斷：
[0062]
[0063] 對(duì)flagi的值件檢測(cè)，當(dāng)flagi = 0時(shí)，機(jī)器人運(yùn)動(dòng)超速，設(shè)備發(fā)出第一控制指令，該 第一控制指令用于控制工業(yè)機(jī)器人停機(jī)。
[0064] 同時(shí)，設(shè)定機(jī)器人的工作區(qū)域zone,然后對(duì)工業(yè)機(jī)器人的末端的位置p(x，y，z)進(jìn) 行工作區(qū)域判斷。
[00 化]
[0066] 對(duì)flagzone的值進(jìn)行檢測(cè)，當(dāng)flagzone = 0時(shí)，機(jī)器人超出正常工作區(qū)域，運(yùn)時(shí)發(fā)出第 二控制指令，該第二控制指令同樣用于控制工業(yè)機(jī)器人停機(jī)，W避免因超出正常工作區(qū)域 造成安全事故。
[0067]最后，還對(duì)工業(yè)機(jī)器人的關(guān)節(jié)信號(hào)進(jìn)行闊值判斷，首先獲得腕部奇異點(diǎn)判斷值05。 針對(duì)奇異點(diǎn)分別選取奇異點(diǎn)闊值Tw，那么進(jìn)行判斷：
[006引
[0069] 對(duì)flagw的值進(jìn)行檢測(cè)，當(dāng)flagw為0時(shí)此時(shí)，機(jī)器人運(yùn)動(dòng)至奇異點(diǎn)過渡區(qū)域，設(shè)備發(fā) 出奇異點(diǎn)過渡警報(bào)信息；當(dāng)flagw為1時(shí)此時(shí)，機(jī)器人運(yùn)動(dòng)至奇異點(diǎn)減速區(qū)域，設(shè)備發(fā)出奇異 點(diǎn)減速警報(bào)信息。當(dāng)flagw為2時(shí)此時(shí)，機(jī)器人正常運(yùn)動(dòng)。
[0070] 實(shí)施例二
[0071] 圖3為本申請(qǐng)實(shí)施例提供的一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)的結(jié)構(gòu)框圖。
[0072] 如圖3所示，本實(shí)施例提供的奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)應(yīng)用于具有多個(gè)關(guān)節(jié)的工 業(yè)機(jī)器人，同樣為了方便描述，本實(shí)施例所選用的工業(yè)機(jī)器人具有6個(gè)關(guān)節(jié)，該系統(tǒng)包括實(shí) 時(shí)檢測(cè)模塊10、奇異點(diǎn)區(qū)域計(jì)算模塊20和奇異點(diǎn)處理模塊30。
[0073] 實(shí)時(shí)檢測(cè)模塊10用于根據(jù)工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目標(biāo)狀態(tài)數(shù)據(jù)計(jì) 算關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度。
[0074] 其中實(shí)時(shí)狀態(tài)數(shù)據(jù)包括關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度和實(shí)時(shí)時(shí)間，目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè) 的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng)時(shí)間。實(shí)時(shí)檢測(cè)模塊10包括本體信息單元11 和模型處理單元12,如圖4所示。
[0075] 本體信息單元11用于獲取機(jī)器人關(guān)節(jié)的實(shí)時(shí)關(guān)節(jié)轉(zhuǎn)動(dòng)角度θιΚ及實(shí)時(shí)時(shí)間t。
[0076] 模型處理單元12包括正逆解子單元121和速度計(jì)算子單元122。
[0077] 正逆解子單元121用于將運(yùn)動(dòng)任務(wù)的目標(biāo)位置和目標(biāo)姿態(tài)采用封閉形式解法進(jìn)行 計(jì)算，解出關(guān)節(jié)需要轉(zhuǎn)動(dòng)的目標(biāo)運(yùn)動(dòng)角度θ'ι，并根據(jù)插補(bǔ)時(shí)間計(jì)算出運(yùn)動(dòng)任務(wù)的目標(biāo)運(yùn)動(dòng) 時(shí)間t'。
[0078] 速度計(jì)算子單元122用于根據(jù)關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度θιΚ及需要轉(zhuǎn)動(dòng)的目標(biāo)運(yùn)動(dòng)角 度9 ' i，實(shí)時(shí)時(shí)間t W及運(yùn)動(dòng)任務(wù)的目標(biāo)運(yùn)動(dòng)時(shí)間t '，求解出運(yùn)動(dòng)任務(wù)的關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速 度：
[0079]
[0080] 奇異點(diǎn)區(qū)域計(jì)算模塊20用于當(dāng)關(guān)節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算關(guān)節(jié)的 允許速度。
[0081] 首先，采用化S(阻尼最小方差)算法進(jìn)行奇異值控制：
[0082] J'=jT(jjT+p2。-!
[0083] 其中，J為機(jī)器人雅克比矩陣，J'為加入阻尼因子后的雅克比矩陣，P2為阻尼因子。
[0084] 對(duì)阻尼因子的控制中加入奇異值控制：
[0085]
[0086] 其中，δ為雅克比矩陣J的最小奇異值，ε為δ的最大闊值。
[0087] 當(dāng)Tw2《I目日I <Twi時(shí)，設(shè)定運(yùn)動(dòng)速度闊值k。
[008引計(jì)算機(jī)器人在闊值Twi處的運(yùn)動(dòng)速度作為運(yùn)動(dòng)速度最大值1然后對(duì)闊值 Twl處的運(yùn)動(dòng)速度進(jìn)行歸一化處理，未使用減速算法的運(yùn)動(dòng)速度與使用減速算法的運(yùn)動(dòng)速度 之比天
[0089] 使用關(guān)節(jié)5角度值作為減速算法的變量，計(jì)算得到的距離變量為
[0090] 經(jīng)過減速算法修正過后的關(guān)節(jié)的允許速度為：
[0091]
'
[0092] 奇異點(diǎn)處理模塊30用于根據(jù)目標(biāo)運(yùn)動(dòng)速度和允許速度對(duì)關(guān)節(jié)進(jìn)行減速保護(hù)。
[0093] 當(dāng)關(guān)節(jié)為執(zhí)行運(yùn)動(dòng)任務(wù)而達(dá)到的目標(biāo)運(yùn)動(dòng)速度超出允許速度時(shí)，及時(shí)控制該關(guān)節(jié) 降低速度，使之不超出允許速度，從而避免出現(xiàn)超速。
[0094] 從上述技術(shù)方案可W看出，本實(shí)施例提供了一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，該系 統(tǒng)應(yīng)用于具有多個(gè)關(guān)節(jié)的工業(yè)機(jī)器人，具體為根據(jù)工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目 標(biāo)狀態(tài)數(shù)據(jù)計(jì)算該關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度，實(shí)時(shí)狀態(tài)數(shù)據(jù)包括關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度和實(shí)時(shí)時(shí) 間，目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè)的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng)時(shí)間；然后在關(guān) 節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算關(guān)節(jié)的允許位置和允許速度;最后根據(jù)目標(biāo)運(yùn)動(dòng) 速度和允許速度對(duì)關(guān)節(jié)進(jìn)行減速保護(hù)，即當(dāng)關(guān)節(jié)的運(yùn)動(dòng)速度在奇異點(diǎn)過渡區(qū)域內(nèi)的運(yùn)動(dòng)速 度超出允許速度后及時(shí)控制器進(jìn)行減速，W避免超速的發(fā)生，從而能夠解決工業(yè)機(jī)器人因 為在奇異點(diǎn)區(qū)域因速度變大而導(dǎo)致的停機(jī)或生產(chǎn)安全的問題。
[00M]本實(shí)施例中的實(shí)時(shí)檢測(cè)模塊10中還包括警報(bào)單元13,該警報(bào)單元用于在工業(yè)機(jī)器 人的關(guān)節(jié)的運(yùn)動(dòng)速度和位置處于異常狀態(tài)時(shí)發(fā)出預(yù)設(shè)的控制指令，具體包括超速警報(bào)子單 元131、工作區(qū)域警報(bào)子單元132和奇異點(diǎn)區(qū)域警報(bào)子單元133,如圖5所示。
[0096] 超速警報(bào)子單元131針對(duì)6個(gè)關(guān)節(jié)的運(yùn)動(dòng)速度分別選取闊值Ti，i = l，2，. . .，5,6， 進(jìn)行判斷：
[0097]
[0098] 然后對(duì)flagi的值件檢測(cè)，當(dāng)flagi = 0時(shí)，機(jī)器人運(yùn)動(dòng)超速，發(fā)出第一控制指令，該 第一控制指令用于控制工業(yè)機(jī)器人停機(jī)。
[0099] 工作區(qū)域警報(bào)子單元132用于設(shè)定機(jī)器人的工作區(qū)域zone,然后對(duì)工業(yè)機(jī)器人的 末端的位置P (X，y，Z)進(jìn)行工作區(qū)域判斷。
[0100]
[0101] 對(duì)flagzone的值進(jìn)行檢測(cè)，當(dāng)flagzone = 0時(shí)，機(jī)器人超出正常工作區(qū)域，運(yùn)時(shí)發(fā)出第 二控制指令，該第二控制指令同樣用于控制工業(yè)機(jī)器人停機(jī)，W避免因超出正常工作區(qū)域 造成安全事故。
[0102] 奇異點(diǎn)區(qū)域警報(bào)子單元133用于對(duì)工業(yè)機(jī)器人的關(guān)節(jié)信號(hào)進(jìn)行闊值判斷，首先獲 得腕部奇異點(diǎn)判斷值θ5。針對(duì)奇異點(diǎn)分別選取奇異點(diǎn)闊值Tw進(jìn)行判斷：
[0103]
[0104] 對(duì)flagw的值進(jìn)行檢測(cè)，當(dāng)flagw為0時(shí)此時(shí)，機(jī)器人運(yùn)動(dòng)至奇異點(diǎn)過渡區(qū)域，設(shè)備發(fā) 出奇異點(diǎn)過渡警報(bào)信息；當(dāng)flagw為1時(shí)此時(shí)，機(jī)器人運(yùn)動(dòng)至奇異點(diǎn)減速區(qū)域，設(shè)備發(fā)出奇異 點(diǎn)減速警報(bào)信息。當(dāng)flagw為2時(shí)此時(shí)，機(jī)器人正常運(yùn)動(dòng)。
[0105] 實(shí)施例Ξ
[0106] 本申請(qǐng)還提供了一種工業(yè)機(jī)器人，該工業(yè)機(jī)器人設(shè)置有上面實(shí)施例所提供的奇異 點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，用于使該工業(yè)機(jī)器人在奇異點(diǎn)區(qū)域不會(huì)出現(xiàn)超速現(xiàn)象，能夠避免因 超速出現(xiàn)的停機(jī)或者安全事故。
[0107] 本說(shuō)明書中各個(gè)實(shí)施例采用遞進(jìn)的方式描述，每個(gè)實(shí)施例重點(diǎn)說(shuō)明的都是與其他 實(shí)施例的不同之處，各個(gè)實(shí)施例之間相同相似部分互相參見即可。對(duì)所公開的實(shí)施例的上 述說(shuō)明，使本領(lǐng)域?qū)I(yè)技術(shù)人員能夠?qū)崿F(xiàn)或使用本申請(qǐng)。對(duì)運(yùn)些實(shí)施例的多種修改對(duì)本領(lǐng) 域的專業(yè)技術(shù)人員來(lái)說(shuō)將是顯而易見的，本文中所定義的一般原理可W在不脫離本申請(qǐng)的 精神或范圍的情況下，在其它實(shí)施例中實(shí)現(xiàn)。因此，本申請(qǐng)將不會(huì)被限制于本文所示的運(yùn)些 實(shí)施例，而是要符合與本文所公開的原理和新穎特點(diǎn)相一致的最寬的范圍。
【主權(quán)項(xiàng)】
1. 一種奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，應(yīng)用于具有多個(gè)關(guān)節(jié)的工業(yè)機(jī)器人，其特征在于，包 括： 實(shí)時(shí)檢測(cè)模塊，用于根據(jù)所述工業(yè)機(jī)器人的關(guān)節(jié)的實(shí)時(shí)狀態(tài)數(shù)據(jù)和目標(biāo)狀態(tài)數(shù)據(jù)計(jì)算 所述關(guān)節(jié)的目標(biāo)運(yùn)動(dòng)速度，所述實(shí)時(shí)狀態(tài)數(shù)據(jù)包括所述關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)角度和實(shí)時(shí)時(shí)間， 所述目標(biāo)狀態(tài)數(shù)據(jù)包括預(yù)設(shè)的運(yùn)動(dòng)任務(wù)所對(duì)應(yīng)的目標(biāo)運(yùn)動(dòng)角度和目標(biāo)運(yùn)動(dòng)時(shí)間； 奇異點(diǎn)區(qū)域計(jì)算模塊，用于當(dāng)所述關(guān)節(jié)的位置處于奇異點(diǎn)過渡區(qū)域內(nèi)時(shí)，計(jì)算所述關(guān) 節(jié)的允許速度； 奇異點(diǎn)處理模塊，用于根據(jù)所述目標(biāo)運(yùn)動(dòng)速度、所述允許速度對(duì)所述關(guān)節(jié)進(jìn)行減速保 護(hù)。2. 如權(quán)利要求1所述的奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，其特征在于，所述實(shí)時(shí)檢測(cè)模塊包 括： 本體信息單元，用于獲取所述實(shí)時(shí)運(yùn)動(dòng)角度和所述實(shí)時(shí)時(shí)間； 模型處理單元，用于根據(jù)所述實(shí)時(shí)運(yùn)動(dòng)角度、所述實(shí)時(shí)時(shí)間、所述目標(biāo)運(yùn)動(dòng)角度和所述 目標(biāo)運(yùn)動(dòng)時(shí)間計(jì)算所述目標(biāo)運(yùn)動(dòng)速度。3. 如權(quán)利要求2所述的奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，其特征在于，所述模型處理單元包 括： 正逆解子單元，用于根據(jù)所述運(yùn)動(dòng)任務(wù)計(jì)算所述關(guān)節(jié)的目標(biāo)位置和目標(biāo)姿態(tài)，并根據(jù) 所述目標(biāo)位置和目標(biāo)姿態(tài)計(jì)算出所述目標(biāo)運(yùn)動(dòng)角度和所述目標(biāo)運(yùn)動(dòng)時(shí)間； 速度計(jì)算子單元，用于根據(jù)所述實(shí)時(shí)運(yùn)動(dòng)角度、所述實(shí)時(shí)時(shí)間、所述目標(biāo)運(yùn)動(dòng)角度和所 述目標(biāo)運(yùn)動(dòng)時(shí)間計(jì)算所述目標(biāo)運(yùn)動(dòng)速度。4. 如權(quán)利要求2或3所述的奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，其特征在于，所述實(shí)時(shí)檢測(cè)模塊 還包括： 警報(bào)單元，用于當(dāng)所述關(guān)節(jié)的運(yùn)動(dòng)速度和位置處于異常狀態(tài)時(shí)發(fā)出預(yù)設(shè)的控制指令。5. 如權(quán)利要求4所述的奇異點(diǎn)區(qū)域減速保護(hù)系統(tǒng)，其特征在于，所述警報(bào)單元包括： 超速警報(bào)子單元，用于當(dāng)所述運(yùn)動(dòng)速度超出預(yù)設(shè)的速度閾值時(shí)發(fā)出第一控制指令，所 述第一控制指令用于控制所述工業(yè)機(jī)器人停機(jī)； 工作區(qū)域警報(bào)子單元，用于當(dāng)所述工業(yè)機(jī)器人的末端的位置超出正常工作區(qū)域時(shí)發(fā)出 第二控制指令，所述第二控制指令用于控制所述工業(yè)機(jī)器人停機(jī)； 奇異點(diǎn)區(qū)域警報(bào)子單元，用于根據(jù)對(duì)所述工業(yè)機(jī)器人的運(yùn)動(dòng)位置的判斷發(fā)出運(yùn)動(dòng)狀態(tài) 提不?目息。6. -種工業(yè)機(jī)器人，其特征在于，設(shè)置有如權(quán)利要求1~5任一項(xiàng)所述的奇異點(diǎn)區(qū)域減 速保護(hù)系統(tǒng)。
【文檔編號(hào)】B25J9/16GK205614678SQ201620076120
【公開日】2016年10月5日
【申請(qǐng)日】2016年1月25日
【發(fā)明人】王業(yè)率, 邊慧杰, 趙天光
【申請(qǐng)人】珠海格力電器股份有限公司