未知模型機(jī)器人的碰撞檢測(cè)算法

袁 野，白瑞林，邢曉凡

（1.江南大學(xué) 輕工過(guò)程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室，無(wú)錫 214122；2.無(wú)錫信捷電氣股份有限公司，無(wú)錫 214072）

0 引言

隨著機(jī)器人應(yīng)用的領(lǐng)域不斷增加，機(jī)器人的安全問(wèn)題也受到了越來(lái)越多的關(guān)注。當(dāng)機(jī)器人在運(yùn)動(dòng)過(guò)程中與周?chē)h(huán)境、操作人員發(fā)生碰撞時(shí)，會(huì)造成很大的損失。因此，碰撞檢測(cè)這種能有效提高機(jī)器人安全性的方法受到了很多研究。比較容易實(shí)現(xiàn)的，可以為機(jī)器人每個(gè)關(guān)節(jié)單獨(dú)設(shè)置一個(gè)力矩閾值，當(dāng)發(fā)生碰撞時(shí)，關(guān)節(jié)力矩超過(guò)閾值，發(fā)出報(bào)警[2]。這種方法需要設(shè)置的較大閾值，不能快速有效地檢測(cè)輕微的碰撞。此外也可以通過(guò)在機(jī)器人末端安裝力矩傳感器來(lái)檢測(cè)碰撞[3]。但一般力矩傳感器價(jià)格昂貴，且只能檢測(cè)發(fā)生在末端的碰撞，在工業(yè)領(lǐng)域有一定的局限性。為了能夠檢測(cè)到機(jī)器人各個(gè)位置發(fā)生的碰撞，可以在機(jī)器人每個(gè)關(guān)節(jié)安裝力矩傳感器，并通過(guò)與機(jī)器人動(dòng)力學(xué)估計(jì)的力矩偏差來(lái)實(shí)現(xiàn)檢測(cè)[4]。這種基于動(dòng)力學(xué)模型的方法更加精準(zhǔn)，但計(jì)算估計(jì)力矩時(shí)需要獲得關(guān)節(jié)加速度，且每個(gè)關(guān)節(jié)的力矩傳感器增大了機(jī)器人的成本。機(jī)器人的關(guān)節(jié)伺服驅(qū)動(dòng)器一般都能提供與關(guān)節(jié)電機(jī)輸出力矩相關(guān)的電機(jī)瞬態(tài)電流，可以用來(lái)計(jì)算關(guān)節(jié)力矩。為了避免計(jì)算加速度，引入廣義動(dòng)量[9～11]，并進(jìn)行改進(jìn)，提高了機(jī)器人碰撞檢測(cè)的靈敏度。

以上的基于動(dòng)力學(xué)模型的碰撞檢測(cè)算法都需要機(jī)器人精準(zhǔn)的動(dòng)力學(xué)參數(shù)，模型參數(shù)的偏差會(huì)影響檢測(cè)的準(zhǔn)確性。目前，大多數(shù)的工業(yè)機(jī)器人的控制方式都是基于運(yùn)動(dòng)學(xué)的，出廠只提供基本的連桿長(zhǎng)度等參數(shù)，而沒(méi)有動(dòng)力學(xué)參數(shù)。因此，對(duì)于模型未知的工業(yè)機(jī)器人，在實(shí)驗(yàn)辨識(shí)出動(dòng)力學(xué)參數(shù)的基礎(chǔ)上，才能設(shè)計(jì)基于模型的碰撞檢測(cè)算法。本文針對(duì)機(jī)器人模型未知的情況，提出通過(guò)整體辨識(shí)的方法得到動(dòng)力學(xué)模型，并設(shè)計(jì)了基于廣義動(dòng)量的新型力矩觀測(cè)器。該觀測(cè)器通過(guò)添加高通濾波結(jié)構(gòu)，減小了因模型偏差導(dǎo)致的觀測(cè)值擾動(dòng)，提高了碰撞檢測(cè)算法的魯棒性。本文提出的算法不需要加速度信號(hào)，也不需要額外的力矩傳感器，減小了機(jī)器人系統(tǒng)的成本，能夠在工業(yè)機(jī)器人上廣泛應(yīng)用。實(shí)驗(yàn)表明，該觀測(cè)器可以有效對(duì)模型未知的機(jī)器人實(shí)現(xiàn)碰撞檢測(cè)。

1 機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)

當(dāng)機(jī)器人與外界發(fā)生碰撞時(shí)，用牛頓-歐拉法可以得到關(guān)節(jié)空間坐標(biāo)系下的n自由度串聯(lián)機(jī)器人的動(dòng)力學(xué)方程：

不采用額外的關(guān)節(jié)力矩傳感器下，可以根據(jù)關(guān)節(jié)電機(jī)電流和控制力矩之間的關(guān)系間接獲取力矩值。通常，機(jī)器人的關(guān)節(jié)由伺服電機(jī)和減速機(jī)驅(qū)動(dòng)，這種動(dòng)力傳輸方式下控制力矩與電機(jī)電流之間成正比。關(guān)節(jié)控制力矩計(jì)算公式為：

式中，Im為關(guān)節(jié)電機(jī)電流，T為電機(jī)的轉(zhuǎn)矩系數(shù)，N為減速機(jī)的減速比。

為了實(shí)現(xiàn)碰撞檢測(cè)，需要能夠?qū)崟r(shí)觀測(cè)碰撞力的等效力矩extτ，由式(1)和式(2)可知：

因此，觀測(cè)碰撞力矩需要機(jī)器人的動(dòng)力學(xué)參數(shù)以及運(yùn)行中的狀態(tài)參數(shù)。為了獲取精準(zhǔn)的動(dòng)力學(xué)參數(shù)，本文基于整體實(shí)驗(yàn)的方法，辨識(shí)機(jī)器人的動(dòng)力學(xué)參數(shù)[1]。

機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)一般包含動(dòng)力學(xué)建模及線性化、激勵(lì)軌跡設(shè)計(jì)、數(shù)據(jù)采樣與處理、參數(shù)估計(jì)和模型驗(yàn)證5個(gè)步驟[1]。

未發(fā)生碰撞時(shí)，使用修改的牛頓-歐拉參數(shù)[5]可以將式(1)表示為慣性參數(shù)的線性形式：

式中，fc是庫(kù)倫摩擦系統(tǒng)，fv是粘滯摩擦系數(shù)。

組合式(4)和式(5)，得到完整的動(dòng)力學(xué)線性化方程：

機(jī)器人的慣性參數(shù)并不是都對(duì)力矩有影響，式6中的觀測(cè)矩陣并不是列滿秩矩陣，所以無(wú)法通過(guò)辨識(shí)計(jì)算動(dòng)力學(xué)參數(shù)。通過(guò)線性關(guān)系消去、重組慣性參數(shù)可以得到一組最小參數(shù)集

[5,6]。被消去、重組的慣性參數(shù)其值可定義為零，將Y中對(duì)應(yīng)的列消去就可以得到對(duì)應(yīng)的觀測(cè)矩陣Ymin。

為了減小測(cè)量噪聲的影響，調(diào)高參數(shù)辨識(shí)的精度，需要精心設(shè)計(jì)激勵(lì)軌跡。采用改進(jìn)傅里葉級(jí)數(shù)作為激勵(lì)軌跡[7]，驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)。通過(guò)一定的采樣頻率，采集在多個(gè)時(shí)刻的關(guān)節(jié)角度和力矩，構(gòu)成觀測(cè)矩陣和力矩向量。采用觀測(cè)矩陣條件數(shù)作為優(yōu)化目標(biāo)，使用遺傳算法優(yōu)化傅里葉級(jí)數(shù)中的參數(shù)。

2 力矩觀測(cè)器設(shè)計(jì)

2.1 動(dòng)量偏差觀測(cè)器

機(jī)器人關(guān)節(jié)的伺服電機(jī)都帶有編碼器，可以反饋運(yùn)行中的角度信號(hào)，關(guān)節(jié)角速度和角加速度則無(wú)法直接測(cè)量。直接的數(shù)值微分會(huì)放大測(cè)量誤差，二次差分后的角加速度信號(hào)無(wú)法使用。為避免引入角加速度，本文基于廣義動(dòng)量，利用動(dòng)量偏差觀察碰撞力矩。

機(jī)器人的廣義動(dòng)量定義為：

式(7)對(duì)時(shí)間求導(dǎo)為：

通過(guò)對(duì)式(10)做卡普拉斯變換，可以分析出殘差r觀測(cè)碰撞力矩相當(dāng)于是一個(gè)低通濾波器[10]，可通過(guò)改變K值來(lái)調(diào)節(jié)觀測(cè)器的抗擾能力以及響應(yīng)速度。

2.2 改進(jìn)的觀測(cè)器

工業(yè)機(jī)器人一般使用減速比較大的RV減速機(jī)，通過(guò)章節(jié)1中電機(jī)電流估計(jì)力矩實(shí)驗(yàn)辨識(shí)出來(lái)的動(dòng)力學(xué)參數(shù)會(huì)有誤差。此時(shí)，殘差觀測(cè)值r中不僅包含了碰撞力矩，也包含了參數(shù)誤差產(chǎn)生的力矩。參數(shù)誤差信號(hào)通常集中在低頻區(qū)域[11,12]，低通濾波器并不能濾除參數(shù)誤差的干擾，因此，本文將觀測(cè)器擴(kuò)展為二階系統(tǒng)，設(shè)計(jì)了新型的機(jī)器人碰撞力矩觀測(cè)器：

將式(11)積分并代入式(12)中可得：

式中K1，K2為對(duì)大于零的對(duì)角矩陣，其工作原理如圖1所示。

圖1 新型觀測(cè)器工作原理圖

將式(12)在時(shí)間上求兩次導(dǎo)可得：

對(duì)式(14)同樣做拉普拉斯變化，可以得到碰撞力矩觀測(cè)器的傳遞函數(shù)為：

式(15)表明二階系統(tǒng)的觀測(cè)器可以等效為一個(gè)帶通濾波器，并可以進(jìn)一步推導(dǎo)為：

因此，本文設(shè)計(jì)的新型碰撞力矩觀測(cè)器可以簡(jiǎn)化為一個(gè)一階的低通濾波器和一個(gè)一階的高通濾波器?？梢酝ㄟ^(guò)合理設(shè)置高通濾波器的參數(shù)，濾除參數(shù)誤差產(chǎn)生的力矩影響，提高碰撞檢測(cè)的品質(zhì)。

當(dāng)機(jī)器人與外界發(fā)生碰撞時(shí)，可通過(guò)對(duì)應(yīng)的觀測(cè)器值判定碰撞位置以及碰撞方向，判定公式如下：

式中，Ni是每個(gè)關(guān)節(jié)的閾值，可以通過(guò)實(shí)驗(yàn)獲得。從最末端關(guān)節(jié)到關(guān)節(jié)1，依次根據(jù)式(17)，計(jì)算Sign值，若為0，說(shuō)明對(duì)應(yīng)連桿沒(méi)有發(fā)生碰撞；若為1，對(duì)應(yīng)連桿發(fā)生正方向碰撞；若為-1，對(duì)應(yīng)連桿發(fā)生反方向的碰撞。

另外，在機(jī)器人啟動(dòng)過(guò)程中，關(guān)節(jié)電機(jī)的輸出控制力矩實(shí)時(shí)變化后，機(jī)器人由于慣性因素等位置和速度會(huì)發(fā)生滯后，碰撞力矩觀測(cè)器會(huì)出現(xiàn)誤檢測(cè)。本文采用式(18)對(duì)觀測(cè)器值r*進(jìn)行修正，t0是機(jī)器人啟動(dòng)時(shí)間：

3 碰撞仿真測(cè)試

為了驗(yàn)證本文提出的新型碰撞檢測(cè)觀測(cè)器的有效性，并確保機(jī)器人本體實(shí)驗(yàn)時(shí)的安裝，首先在MATLAB中借助Robotics工具箱搭建機(jī)器人動(dòng)力學(xué)模型，同時(shí)文獻(xiàn)[10]中的低通濾波觀測(cè)器以及本文提出的新型觀測(cè)器，通過(guò)數(shù)值仿真來(lái)驗(yàn)證觀測(cè)器檢測(cè)碰撞力矩的效果。

機(jī)器人模型誤差產(chǎn)生的低頻偏差信號(hào)與關(guān)節(jié)速度有關(guān)[8,9]，而碰撞力矩的頻率較高，因此可以用本文的新型碰撞力矩觀測(cè)器來(lái)過(guò)濾掉模型誤差引入的擾動(dòng)。機(jī)器人的位姿與自由度決定了模型偏差頻率特性[12]：

機(jī)器人運(yùn)動(dòng)過(guò)程中控制速度的最大值為20°/s，根據(jù)式(19)，并考慮測(cè)量誤差、抖動(dòng)等因素，可以將高通濾波器的系統(tǒng)K2設(shè)置成值為1的對(duì)角矩陣。按照文獻(xiàn)[9]，設(shè)置低通濾波器系統(tǒng)K1=20，來(lái)過(guò)濾系統(tǒng)中的高頻噪聲。

圖2 機(jī)器人碰撞仿真測(cè)試結(jié)果

從圖中2可以看出，當(dāng)模型沒(méi)有偏差時(shí)，文獻(xiàn)[10]和本文的觀測(cè)器都能夠很好的觀測(cè)碰撞力矩。在沒(méi)有發(fā)生碰撞時(shí)，觀測(cè)值幾乎為零。發(fā)生碰撞后，觀測(cè)值迅速增大并超過(guò)閾值，觸發(fā)碰撞發(fā)生信號(hào)。碰撞結(jié)束后，觀測(cè)值又重新恢復(fù)到零，解除碰撞發(fā)生信號(hào)。

圖3 機(jī)器人模型偏差觀測(cè)結(jié)果

從圖3中可以發(fā)現(xiàn)，由于模型有一定的偏差，文獻(xiàn)[10]中的觀測(cè)器只能過(guò)濾高頻的噪聲，對(duì)于模型偏差導(dǎo)致的低頻波動(dòng)并沒(méi)有效果，導(dǎo)致觀測(cè)值不能很好的跟蹤碰撞力矩，甚至?xí)恢贝笥陂撝?，產(chǎn)生誤檢測(cè)。而本文提出的新型觀測(cè)器，通過(guò)設(shè)置合理的高通濾波器參數(shù)，能夠比較準(zhǔn)確的過(guò)濾模型誤差，跟蹤碰撞力矩。

4 實(shí)驗(yàn)結(jié)果分析

圖4 機(jī)器人系統(tǒng)

為了驗(yàn)證觀測(cè)器的有效性，在信捷電氣的串聯(lián)六軸機(jī)器人上進(jìn)行實(shí)驗(yàn)，該機(jī)器人整體的系統(tǒng)結(jié)構(gòu)如圖所示。運(yùn)動(dòng)控制器采用信捷電氣的XG2型號(hào)PLC，該型號(hào)PLC與伺服驅(qū)動(dòng)器之間可以通過(guò)EtherCAT總線進(jìn)行通訊，通訊周期為2ms，并可以通過(guò)上位機(jī)采集所有電機(jī)中的位置信號(hào)和電流信號(hào)。

4.1 動(dòng)力學(xué)參數(shù)辨識(shí)實(shí)驗(yàn)

根據(jù)章節(jié)1的流程建立式(6)形式的動(dòng)力學(xué)線性化方程，并利用改進(jìn)的遺傳算法設(shè)計(jì)出激勵(lì)軌跡。

圖5 六關(guān)節(jié)激勵(lì)軌跡

按照規(guī)劃好的激勵(lì)軌跡，驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)重復(fù)運(yùn)動(dòng)50次，采集運(yùn)動(dòng)過(guò)程中的位置信號(hào)和電流信號(hào)，并進(jìn)行數(shù)據(jù)預(yù)處理。預(yù)處理后的數(shù)據(jù)利用最小二乘法辨識(shí)出動(dòng)力學(xué)參數(shù)值，并擬合關(guān)節(jié)的力矩輸出。依舊以關(guān)節(jié)2和關(guān)節(jié)5為例，擬合效果如圖6所示。

圖6 機(jī)器人力矩預(yù)測(cè)

利用整體實(shí)驗(yàn)的方法辨識(shí)出的動(dòng)力學(xué)模型參數(shù)，在運(yùn)動(dòng)平穩(wěn)時(shí)能夠較好的預(yù)測(cè)出關(guān)節(jié)力矩，但是在一些速度換向處會(huì)出現(xiàn)較大的偏差。這是因?yàn)榻Ｖ惺褂玫哪Σ聊Ｐ洼^為簡(jiǎn)單，使得辨識(shí)出來(lái)的模型參數(shù)與真實(shí)的參數(shù)間存在誤差，這樣的誤差會(huì)對(duì)觀測(cè)器的效果產(chǎn)生一定的影響。

4.2 碰撞檢測(cè)實(shí)驗(yàn)

辨識(shí)出模型的動(dòng)力學(xué)參數(shù)后，將碰撞檢測(cè)算法寫(xiě)入控制器軟件中進(jìn)行碰撞檢測(cè)實(shí)驗(yàn)。驅(qū)動(dòng)機(jī)器人執(zhí)行簡(jiǎn)單的點(diǎn)到點(diǎn)運(yùn)行模式，設(shè)置運(yùn)行過(guò)程中各關(guān)節(jié)的最大速度為20°/s，采用仿真測(cè)試時(shí)設(shè)計(jì)的觀測(cè)器參數(shù)值。在機(jī)器人運(yùn)行過(guò)程中，人為在連桿5上施加正向碰撞力，在連桿2上施加反向碰撞力，實(shí)驗(yàn)觀測(cè)結(jié)果如圖7所示。

圖7 機(jī)器人碰撞力矩觀測(cè)結(jié)果

從圖中可知，由于實(shí)驗(yàn)辨識(shí)出的動(dòng)力學(xué)參數(shù)有誤差，文獻(xiàn)[10]的觀測(cè)器，不能過(guò)濾掉低頻的模型偏差，使得觀測(cè)器的值一直在大范圍波動(dòng)。在發(fā)生碰撞時(shí)，碰撞力矩和模型偏差并不能很好區(qū)分出來(lái)。而本文采用的新型觀測(cè)器，在沒(méi)有發(fā)生碰撞時(shí)，輸出較為平穩(wěn)，能夠有效地過(guò)濾掉模型偏差，因此，可以設(shè)置一個(gè)較小的閾值。當(dāng)發(fā)生碰撞時(shí)，新型觀測(cè)器的輸出值迅速增大，超過(guò)閾值，觸發(fā)碰撞報(bào)警信號(hào)。

以上實(shí)驗(yàn)結(jié)果表明，本文提出的新型觀測(cè)器，相對(duì)于低通濾波器形式的觀測(cè)器，可以使用在動(dòng)力學(xué)模型不夠精準(zhǔn)的情況下。

5 結(jié)語(yǔ)

本文提出了一種適用于未知模型機(jī)器人的參數(shù)辨識(shí)方法，以及基于廣義動(dòng)量的新型碰撞檢測(cè)算法。該算法無(wú)需機(jī)器人的關(guān)節(jié)加速度信號(hào)，也不需要額外的關(guān)節(jié)力矩傳感器，只需要關(guān)節(jié)電機(jī)的位置信號(hào)以及驅(qū)動(dòng)電流，有利于在常規(guī)機(jī)器人系統(tǒng)中推廣應(yīng)用。在未知模型參數(shù)的情況下，通過(guò)整體的實(shí)驗(yàn)辨識(shí)可以得出一組比較精準(zhǔn)的動(dòng)力學(xué)參數(shù)，減小了機(jī)器人的模型誤差。利用觀測(cè)器等效為帶通濾波器的性質(zhì)，進(jìn)一步過(guò)濾掉模型偏差導(dǎo)致的擾動(dòng)，實(shí)現(xiàn)了工業(yè)機(jī)器人系統(tǒng)的碰撞檢測(cè)。后續(xù)工作將研究更加精準(zhǔn)的摩擦模型，提高動(dòng)力學(xué)參數(shù)辨識(shí)的精度，進(jìn)一步提高算法的碰撞檢測(cè)性能。