未知模型機器人的碰撞檢測算法

袁 野，白瑞林，邢曉凡

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

0 引言

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

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

1 機器人動力學參數辨識

當機器人與外界發生碰撞時，用牛頓-歐拉法可以得到關節空間坐標系下的n自由度串聯機器人的動力學方程：

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

式中，Im為關節電機電流，T為電機的轉矩系數，N為減速機的減速比。

為了實現碰撞檢測，需要能夠實時觀測碰撞力的等效力矩extτ，由式(1)和式(2)可知：

因此，觀測碰撞力矩需要機器人的動力學參數以及運行中的狀態參數。為了獲取精準的動力學參數，本文基于整體實驗的方法，辨識機器人的動力學參數[1]。

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

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

式中，fc是庫倫摩擦系統，fv是粘滯摩擦系數。

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

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

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

為了減小測量噪聲的影響，調高參數辨識的精度，需要精心設計激勵軌跡。采用改進傅里葉級數作為激勵軌跡[7]，驅動機器人運動。通過一定的采樣頻率，采集在多個時刻的關節角度和力矩，構成觀測矩陣和力矩向量。采用觀測矩陣條件數作為優化目標，使用遺傳算法優化傅里葉級數中的參數。

2 力矩觀測器設計

2.1 動量偏差觀測器

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

機器人的廣義動量定義為：

式(7)對時間求導為：

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

2.2 改進的觀測器

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

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

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

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

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

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

式(15)表明二階系統的觀測器可以等效為一個帶通濾波器，并可以進一步推導為：

因此，本文設計的新型碰撞力矩觀測器可以簡化為一個一階的低通濾波器和一個一階的高通濾波器。可以通過合理設置高通濾波器的參數，濾除參數誤差產生的力矩影響，提高碰撞檢測的品質。

當機器人與外界發生碰撞時，可通過對應的觀測器值判定碰撞位置以及碰撞方向，判定公式如下：

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

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

3 碰撞仿真測試

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

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

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

圖2 機器人碰撞仿真測試結果

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

圖3 機器人模型偏差觀測結果

從圖3中可以發現，由于模型有一定的偏差，文獻[10]中的觀測器只能過濾高頻的噪聲，對于模型偏差導致的低頻波動并沒有效果，導致觀測值不能很好的跟蹤碰撞力矩，甚至會一直大于閾值，產生誤檢測。而本文提出的新型觀測器，通過設置合理的高通濾波器參數，能夠比較準確的過濾模型誤差，跟蹤碰撞力矩。

4 實驗結果分析

圖4 機器人系統

為了驗證觀測器的有效性，在信捷電氣的串聯六軸機器人上進行實驗，該機器人整體的系統結構如圖所示。運動控制器采用信捷電氣的XG2型號PLC，該型號PLC與伺服驅動器之間可以通過EtherCAT總線進行通訊，通訊周期為2ms，并可以通過上位機采集所有電機中的位置信號和電流信號。

4.1 動力學參數辨識實驗

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

圖5 六關節激勵軌跡

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

圖6 機器人力矩預測

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

4.2 碰撞檢測實驗

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

圖7 機器人碰撞力矩觀測結果

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

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

5 結語

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