一種工業(yè)機(jī)器人碰撞檢測(cè)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種工業(yè)機(jī)器人的檢測(cè)方法，具體說是一種用于工業(yè)機(jī)器人碰撞檢測(cè) 的方法。
【背景技術(shù)】
[0002] 工業(yè)機(jī)器人在應(yīng)用過程中，因各種原因?qū)е聶C(jī)器人與外界物體或人員發(fā)生碰撞， 經(jīng)常產(chǎn)生機(jī)器人本體損壞和/或與之碰撞物的損壞或損傷。若不對(duì)此情況加以檢測(cè)和適當(dāng) 處理，在碰撞發(fā)生時(shí)，其產(chǎn)生的危害是很難控制的，安全也不能得到保障。
[0003] 常見的碰撞檢測(cè)方法主要有基于路徑規(guī)劃和力矩傳感器的檢測(cè)方法。基于路徑規(guī) 劃的檢測(cè)方法計(jì)算量往往很大，且需要針對(duì)特定的工作空間；基于力矩傳感器的檢測(cè)方法， 由于需要額外增加傳感器，增加了碰撞檢測(cè)成本，而且由于安裝的需要有可能需要改變機(jī) 器人的結(jié)構(gòu)，操作較為不便。
[0004] 中國(guó)專利申請(qǐng)CN101719173A公開的一種面向?qū)ο蟮牟⑿信鲎矙z測(cè)方法、裝置及 系統(tǒng)，公開了一種面向?qū)ο蟮牟⑿信鲎矙z測(cè)方法、裝置及系統(tǒng)，由主控制結(jié)點(diǎn)根據(jù)運(yùn)動(dòng)物體 列表，生成所有需做碰撞檢測(cè)的物體對(duì)的列表，空閑結(jié)點(diǎn)向主控制結(jié)點(diǎn)發(fā)出請(qǐng)求做碰撞檢 測(cè)的信息，主控制結(jié)點(diǎn)收到上述請(qǐng)求做碰撞檢測(cè)的信息后，將需做碰撞檢測(cè)的物體對(duì)列表 中的物體對(duì)分配給各空閑結(jié)點(diǎn)進(jìn)行并行碰撞檢測(cè)。該方法、裝置及系統(tǒng)在保證碰撞響應(yīng)唯 一性、完全性的同時(shí)，還能降低算法的復(fù)雜性，進(jìn)一步提高碰撞檢測(cè)速度。但是此方法通過 檢測(cè)被碰撞物有否發(fā)生移動(dòng)來判斷是否發(fā)生碰撞，這種方法需要提供一個(gè)無干擾的環(huán)境， 也就是說其無法對(duì)未知環(huán)境的碰撞進(jìn)行判斷。
[0005] 中國(guó)專利CN201110260907公開一種通過計(jì)算一個(gè)碰撞標(biāo)量來衡量機(jī)器人在實(shí)時(shí) 運(yùn)作中是否發(fā)生碰撞，這種檢測(cè)方法涉及兩個(gè)變量，一個(gè)是位置誤差，一個(gè)是電機(jī)的電流 值。由于電機(jī)的電流值無法獲得，需要在電機(jī)上安裝電流傳感器，這無疑增加了碰撞檢測(cè)的 成本。

【發(fā)明內(nèi)容】

[0006] 本發(fā)明所要解決的技術(shù)問題，在于克服現(xiàn)有技術(shù)存在的缺陷，提出了一種工業(yè)機(jī) 器人碰撞檢測(cè)方法，采用基于力矩差的碰撞檢測(cè)方法，無需附加傳感器，對(duì)機(jī)器人的結(jié)構(gòu)無 需修改，適用于機(jī)器人的任何運(yùn)行工況，對(duì)工況沒有任何限制。
[0007] 本發(fā)明所提出的碰撞檢測(cè)方法是基于力矩差的檢測(cè)方法，主要原理如附圖1所 示：在機(jī)器人運(yùn)行過程中，根據(jù)運(yùn)動(dòng)軌跡實(shí)時(shí)地預(yù)測(cè)出各關(guān)節(jié)的理論力矩值，并實(shí)時(shí)地計(jì)算 關(guān)節(jié)理論力矩值與關(guān)節(jié)實(shí)際采樣力矩值之間的差值，當(dāng)力矩差值超出碰撞閾值時(shí)，視為發(fā) 生碰撞，電機(jī)立即停止運(yùn)轉(zhuǎn)，機(jī)器人停止運(yùn)動(dòng)。
[0008] 具體步驟如下：
關(guān)節(jié)預(yù)測(cè)力矩！由空載時(shí)的關(guān)節(jié)預(yù)測(cè)力矩和負(fù)載所引起的關(guān)節(jié)預(yù)測(cè)力矩 組成。
[0009] 采用機(jī)器人參數(shù)辨識(shí)的方法預(yù)測(cè)空載時(shí)的關(guān)節(jié)預(yù)測(cè)力矩riw<.，根據(jù)參數(shù)辨識(shí)，得 到機(jī)器人各個(gè)關(guān)節(jié)的動(dòng)力學(xué)模型：
式中，#(q)為慣性量，€'<屮初為科氏力和離心力，以q)為重力項(xiàng)，^為關(guān)節(jié)角位移，(6) 為關(guān)節(jié)角速度，#為關(guān)節(jié)角加速度，參數(shù)辨識(shí)流程圖如附圖2所示，按照特定的激勵(lì)軌跡激 勵(lì)機(jī)器人做周期性運(yùn)動(dòng)，在運(yùn)動(dòng)過程中運(yùn)用相關(guān)軟件采樣出實(shí)際關(guān)節(jié)位置和實(shí)際力矩值， 通過數(shù)據(jù)處理得到信噪比較高的力矩值Y、關(guān)節(jié)位置$、關(guān)節(jié)速度j和關(guān)節(jié)加速度$，根據(jù) 動(dòng)力學(xué)建模所得到的模型，采用加權(quán)最小二乘法辨識(shí)出關(guān)節(jié)預(yù)測(cè)力矩。
[0010] 負(fù)載所引起的關(guān)節(jié)預(yù)測(cè)力矩的預(yù)測(cè)與機(jī)器人末端所帶負(fù)載有關(guān)，按照下 列步驟預(yù)測(cè)： 運(yùn)用AutoCAD軟件得到負(fù)載的慣性參數(shù)：質(zhì)量m，質(zhì)心位置r以及慣性張量I; 根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)模型（D-H參數(shù)）推導(dǎo)出負(fù)載所引起的關(guān)節(jié)預(yù)測(cè)力矩。
其中，D-H參數(shù)表示的為機(jī)器人每個(gè)連桿與相鄰連桿之間的相對(duì)位置關(guān)系。
[0012] 2、通過編碼器數(shù)據(jù)實(shí)時(shí)獲得關(guān)節(jié)實(shí)際采樣力矩^^; 3、確定關(guān)節(jié)預(yù)測(cè)力矩值相對(duì)于實(shí)際力矩值在相位上超前的周期數(shù)關(guān)節(jié)預(yù)測(cè)力矩 值相對(duì)于實(shí)際力矩值在相位上超前的周期數(shù)，即為伺服控制系統(tǒng)的滯后周期數(shù)，可以通過 實(shí)驗(yàn)，對(duì)理論位移和實(shí)際位移進(jìn)行數(shù)據(jù)采集，然后分析實(shí)際位移相對(duì)于理論位移在相位上 滯后的周期數(shù)，該值即為這里所說的周期數(shù)#。
[0013] 4、將關(guān)節(jié)預(yù)測(cè)力矩向后推遲#周期，獲得與實(shí)際采樣力矩相位同步的 關(guān)節(jié)預(yù)測(cè)力矩Awlr. .Y); 5、 計(jì)算關(guān)節(jié)力矩差ir,
6、設(shè)定碰撞閾值的上限和下限; 在理想情況下，如果力矩預(yù)測(cè)精度足夠高，在無碰撞情況下力矩差即為〇,那么碰撞閾 值即可以設(shè)置為〇,因此，對(duì)于碰撞基準(zhǔn)閾值的設(shè)定，我們需要驅(qū)動(dòng)機(jī)器人運(yùn)行在預(yù)測(cè)力矩 精度較差的工況下。
[0014] 確定碰撞閾值的上限和下限，可以采用以下的方法：通過對(duì)力矩預(yù)測(cè) 算法的分析，確定出閾值設(shè)定所需要的運(yùn)行工況，在正常工作情況下(無碰撞發(fā)生的情況） 通過實(shí)時(shí)地計(jì)算與實(shí)際采樣力矩相位同步的關(guān)節(jié)預(yù)測(cè)力矩與實(shí)際采樣力矩指間的關(guān)節(jié)力 矩差，得出涵蓋所有情況下的最大正向關(guān)節(jié)力矩差和反向最大關(guān)節(jié)力矩差在 力矩誤差的基礎(chǔ)上放寬(ir> 〇極小整數(shù))，設(shè)為碰撞閾值的上限和下限。 7、碰撞檢測(cè)
時(shí)，視為發(fā)生碰撞，控制系統(tǒng)給機(jī)器人發(fā)停止信號(hào)，機(jī)器人立即停止運(yùn)動(dòng)。
[0015] 本發(fā)明進(jìn)一步優(yōu)化方案是：對(duì)關(guān)節(jié)實(shí)際采樣力矩進(jìn)行平均值濾波，去除其 中噪聲的影響，得到信噪比較高的去噪關(guān)節(jié)采樣力矩? ;采用去噪關(guān)節(jié)采樣力矩 ,: +?計(jì)算關(guān)節(jié)力矩差& :
[0016] 本發(fā)明所提出的基于力矩差的碰撞檢測(cè)方法，無需附加任何傳感器，對(duì)機(jī)器人的 結(jié)構(gòu)無需任何修改，適用于機(jī)器人的任何運(yùn)行工況，對(duì)工況沒有任何限制，成本低。
[0017] 本發(fā)明所提出的基于力矩差的碰撞檢測(cè)方法，首先根據(jù)機(jī)器人參數(shù)辨識(shí)，得到精 確的動(dòng)力學(xué)模型，以用于空載力矩的預(yù)測(cè)；通過對(duì)負(fù)載模型的分析，得到負(fù)載所引起的關(guān)節(jié) 力矩值，整個(gè)過程均是離線進(jìn)行的，僅需要將預(yù)測(cè)的結(jié)果，即公式（1)用于碰撞檢測(cè)的在線 程序中，程序簡(jiǎn)單，執(zhí)行效率高。
[0018] 本發(fā)明的關(guān)鍵點(diǎn)在于基于機(jī)器人參數(shù)辨識(shí)和負(fù)載模型獲得動(dòng)力學(xué)模型，進(jìn)而得到 各個(gè)關(guān)節(jié)的力矩表達(dá)式，以用于根據(jù)運(yùn)動(dòng)軌跡來預(yù)測(cè)關(guān)節(jié)力矩。