袁 野，白瑞林，邢曉凡

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 動量偏差觀測器

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

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

式(7)對時間求導(dǎo)為：

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

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

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

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

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

圖1 新型觀測器工作原理圖

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

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

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

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

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

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

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

3 碰撞仿真測試

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

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

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

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

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

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

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

4 實驗結(jié)果分析

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

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

4.1 動力學(xué)參數(shù)辨識實驗

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

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

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

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

利用整體實驗的方法辨識出的動力學(xué)模型參數(shù)，在運(yùn)動平穩(wěn)時能夠較好的預(yù)測出關(guān)節(jié)力矩，但是在一些速度換向處會出現(xiàn)較大的偏差。這是因為建模中使用的摩擦模型較為簡單，使得辨識出來的模型參數(shù)與真實的參數(shù)間存在誤差，這樣的誤差會對觀測器的效果產(chǎn)生一定的影響。

4.2 碰撞檢測實驗

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

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

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

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

5 結(jié)語

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