基于高階擾動觀測器的機器人碰撞檢測算法

陳一豪，李 毅，朱明超

（1.中國科學院長春光學精密機械與物理研究所，長春 130033；2.中國科學院大學，北京 100049）

0 引言

機器人可以在復雜環境中替代人類高效地完成許多重復性工作,在其工作過程中,很可能與環境中的人和物體發生碰撞而造成嚴重的后果[1]。在復雜的任務環境中,通常需要機器人與人協作完成特定的工作,在這種情形下機器人的安全性就顯得尤為重要[2]。安全性的提高需要機器人具有感知危險的能力,在發生碰撞的情況下能夠做出規避動作以保證操作人員與機器人自身的安全。最常見的提高機器人安全性的方法是碰撞檢測[3],利用碰撞檢測技術可以在碰撞發生時檢測到由碰撞產生的外力,并執行保護措施避免產生傷害。

最常見的碰撞檢測方法是采用機器人末端安裝的六維力傳感器[4]感知外部碰撞,這種方法可以精確的檢測到傳感器后端的外部碰撞力,無法檢測傳感器前端發生的碰撞；采用關節力矩傳感器[5]可以檢測到所有能對機器人關節力矩產生影響的碰撞,力矩傳感器的安裝增加了關節的質量和體積并且提高了機器人的應用成本；機器人表面覆蓋敏感皮膚進行檢測[6],可以精確的檢測到碰撞發生的位置和碰撞力的大小,但是需要在所有接觸表面覆蓋敏感皮膚,成本較高且會使機器人的布線難度增大。以上有傳感器檢測的方法存在傳感器成本高,需要修改機器人結構等缺點。因此也有學者提出了無需增加額外傳感器的碰撞檢測方法,利用伺服電機返回的關節實時電流、位置和速度等信息,通過選取合適的觀測量也能夠準確判斷碰撞的發生。目前在該方向已有較多的研究成果,趙漢杰[7]提出了基于電機電流變化率的動態閾值檢測方法,通過對不同速度下的電機電流變化率進行標定得到了較好的碰撞檢測效果；李智靖等[8]提出了一種基于卷積力矩觀測器和摩擦補償的碰撞檢測算法,減小了系統的觀測誤差；Xu等[9]利用碰撞中的力會引起速度變化提出了基于優化速度偏差的檢測算法,將碰撞引起的速度變化與目標速度的差值作為觀測量,也能較好的檢測到機器人的碰撞；Han等[10]設計了動量觀測器進行雙臂機器人的碰撞檢測,避免了計算機器人關節的加速度。

機器人在發生碰撞時外力急劇變化,需要碰撞檢測系統具有很好地實時性,也需要碰撞檢測系統具有良好的抵抗噪聲能力。錢琮偉等[11]用基于廣義動量的擾動觀測器檢測碰撞,但系統的實時性不夠理想。吳海彬等[12]設計了基于動量偏差觀測器的碰撞檢測算法,并通過性能調整函數使觀測器的實時性得到了較大提高,系統的實時響應在20ms左右,但是在系統輸入存在噪聲時外力觀測結果受到的干擾較大。本文設計了高階擾動觀測器用于機器人的碰撞檢測,在保證碰撞檢測系統實時性的同時提高了系統的抵抗噪聲能力。通過搭建計算機仿真系統驗證觀測器的實時性和魯棒性,最后在機器人平臺上進行了碰撞實驗。

1 基于擾動觀測器的碰撞檢測

1.1 高階擾動觀測器設計

串聯機器人與外部發生碰撞時機器人的動力學方程可表示如式(1)所示。

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

對機器人廣義動量進行求導得為：

將公式和代入,得到廣義動量的導數為：

機器人的運動由每個關節的旋轉運動組成,由公式可知各關節的廣義動量等于作用在該關節所有力矩之和的積分,機器人關節的廣義動量與關節所受外力矩間具有解耦性,可以基于該性質設計擾動觀測器估計關節所受外力矩的大小。設計的擾動觀測器結構可表示為：

式(6)中：r為觀測器觀測得到的外力值；k1＞0,k2＞0,k3＞0為增益矩陣；為動量估計值,其導數的表示為：

在僅使用上式作為觀測器時觀測結果的實時性較差,為了提升觀測器的動態性能,構造fe作為前饋調整項加入觀測器,如式(9)所示：

調整后的觀測器結構可表示為：

式(11)中k4＞0為前饋調整項增益矩陣。

圖1 高階擾動觀測器結構圖

1.2 碰撞檢測閾值

在以外力矩信息為依據判斷碰撞發生時,通常對外力矩設置大于模型誤差固定閾值即可判斷是否有碰撞發生,但是當模型誤差較大時檢測碰撞的實時性受影響較大。實際碰撞發生時,通常外力矩變化速度較快,通過對外力矩的變化速度設置閾值也能檢測到碰撞發生,但是在機器人以較慢速度接觸彈性物體時無法準確檢測碰撞。

為了提高碰撞檢測的實時性而又不漏檢碰撞的發生,本節以外力矩和外力矩變化速度為基礎提出了接觸危險系數用于判斷是否有碰撞發生。

關節i的接觸危險系數定義為：

式(12)中：ri為觀測器得到的關節i的外力矩大??；為外力矩變化速度大小,由ri差分得到；KEti、KEvi分別為關節i的力矩危險系數和力矩變化速度危險系數,單位為Nm-1和(Nm/s)-1。

無碰撞發生時,關節i的接觸危險系數存在如式的關系,其中Eilim為關節i的碰撞檢測閾值,即不滿足該條件時判斷碰撞發生。

2 碰撞檢測算法仿真驗證

為了驗證該高階擾動觀測器的性能,通過在Simulink中搭建仿真環境進行仿真驗證,并與一階擾動觀測器進行對比。仿真系統組成如圖2所示,運動軌跡為已知的任意軌跡,機器人控制器負責將輸入的位置指令轉化為關節實際的驅動力大小,機器人模型在關節電機驅動和外力的作用下運動,外力矩觀測器采集機器人的運動信息并輸出外力矩的觀測結果,其中觀測器采集到的運動信息有一定的噪聲干擾。

圖2 仿真系統

2.1 觀測器仿真結果對比

仿真條件設置為,關節1受到幅值為15Nm頻率為5rad/s的正弦規律變化的外力,關節2受到大小為±20Nm持續時間為1s的階躍型外力。通過對實際機器人系統傳感器噪聲統計特性的分析,設定關節位置噪聲幅值為0.02°,速度噪聲幅值為0.3°/s,關節輸出力矩噪聲幅值為2Nm。

在以上設定的外力及噪聲條件下,高階擾動觀測器的仿真結果如圖3所示,圖中,τ1、τ2為關節1和關節2所受實際外力大小,r1、r2為高階擾動觀測器的觀測結果,r1a、r2a為一階擾動觀測器的觀測結果。由圖3中的仿真結果可知,本文的高階擾動觀測器的觀測結果與實際外力間的延遲明顯優于一階擾動觀測器,并且觀測結果中噪聲的幅值也有明顯的降低,表明設計的高階擾動觀測器與一階擾動觀測器相比具有一定的性能提升。

圖3 擾動觀測器仿真結果

2.2 碰撞檢測閾值仿真驗證

前文提出了接觸危險系數,并在接觸危險系數的基礎上設定了碰撞檢測條件,本節在仿真系統中進行碰撞檢測閾值的對比測試,確定其是否具有比簡單的外力矩閾值更好的檢測效果。

以關節2為例在仿真系統中進行測試,模擬實際環境中機器人與外界的不同碰撞情況,對比接觸危險系數與固定閾值的檢測結果。仿真過程中在2s、4s、5.5s、和8.5s共發生四次外部接觸,其中1s至3s期間模擬機器人由于模型存在的觀測誤差,在5.5s時的接觸為預期的主動接觸。結果如圖4所示,圖4(a)為關節1所受外力矩,圖4(b)為關節1的接觸危險系數。表1展示了兩種檢測閾值在仿真的四次碰撞下檢測到碰撞的時間,未標出時間的即為未檢測到碰撞發生。由表中數據可知,在2s的第一次碰撞時,由于碰撞的力較小外力矩閾值并未成功檢測到碰撞發生,第三次主動的預期接觸在外力矩閾值下被誤判為碰撞,其余兩次二者均能正常檢測到碰撞發生,在這兩次碰撞中接觸危險系數檢測到碰撞的時間均早于外力矩閾值給出碰撞檢測結果。

圖4 關節1碰撞檢測閾值仿真結果

表1 碰撞檢測結果

由以上仿真結果可知,本文提出的接觸危險系數在檢測碰撞的過程中表現出更好的碰撞檢測實時性和準確性,具有預期的碰撞檢測效果。

3 碰撞實驗

3.1 實驗平臺

利用本實驗室自主研發的模塊化機器人的兩個關節搭建了兩自由度碰撞檢測實驗平臺進行算法的驗證,實驗平臺組成如圖5所示。兩個關節均為轉動關節,關節1為水平轉動關節,直接固定在實驗平臺上；關節2為垂直轉動關節,通過連桿與關節1的末端相連；各關節由永磁同步電機驅動,關節內置諧波減速器,減速比均為160。實驗裝置末端采用六維力/力矩傳感器反饋末端所受力和力矩信號,通過雅可比矩陣可以將機器人末端的六維力信息轉換為各關節受到的等效外力矩。

圖5 碰撞檢測實驗平臺

3.2 高階擾動觀測器實驗

碰撞實驗基于前文所述實驗平臺進行,機器人按圖6中所示軌跡由初始位置開始在-85°到80°內作周期性運動,運動速度從-49°/s到50°/s,加速度從-30°/s2到31°/s2,實驗條件覆蓋較廣,可以充分地證明該觀測器的實際觀測結果的可靠性。

圖6 機器人關節運行軌跡

首先在試驗平臺上驗證提出的高階擾動觀測器性能,機器人按照設定運行軌跡持續運行,在機器人運行過程中記錄觀測力矩與六維力感知的等效外力矩的數據。實驗過程中采集的數據如圖7所示,圖中τ1,τ2是關節的輸出力矩,r1,r2為觀測器輸出的觀測外力矩,τext1、τext2為末端六維力傳感器獲取的關節等效外力矩。

圖7 高階擾動觀測器實驗

圖7為擾動觀測器實驗結果,在實驗過程中在機器人末端施加多次隨機外力模擬實際環境中的碰撞。觀察六維力傳感器給出的關節實際外力矩和高階擾動觀測器給出的觀測外力矩數據曲線,可知在有外部接觸時高階擾動觀測器能快速檢測到實際外力矩并給出實際外力矩的大小,且無外部接觸時僅有很小的觀測誤差。由以上實驗結果可以確定本文提出的高階擾動觀測器在實際系統發生碰撞時能快速檢測到外力變化且外力觀測的數值與實際外力矩一致,僅存在較小的模型誤差,且當噪聲較大時觀測結果基本不受影響,因此該擾動觀測器的實時性和魯棒性較高。

3.3 實際碰撞檢測實驗

碰撞檢測是為了在機器人發生碰撞時快速檢測到碰撞發生并使機器人做出相應的碰撞響應以減小由碰撞造成的損失,因此本節在該實驗平臺上進行碰撞實驗驗證本文提出的碰撞檢測算法能提高機器人的安全性。

實驗過程中機器人仍按照圖6中的給定軌跡運行,在機器人末端的運行軌跡上放置障礙物,機器人運動到該點時與障礙物發生碰撞,觀察機器人在碰撞后是否能準確檢測到碰撞發生,并采取相應措施。本實驗中機器人在檢測到碰撞后向碰撞力的方向運動以遠離碰撞發生點作為碰撞保護措施。

試驗結果如圖8所示,圖中為關節2在碰撞過程中的外力矩變化曲線。通過圖中外力矩的變化曲線可知,在0.5s時機器人與障礙物發生接觸,約0.4s后檢測到碰撞發生,開始向遠離碰撞的方向運動,由碰撞引起的外力逐步減小至零。在碰撞過程中末端力傳感器的實際接觸力最大為18.2N。參照協作機器人安全規范ISO/TS 15066-2016中的人體模型,對外部接觸耐受程度最低的臉部在接觸力為65N時會受到傷害,在碰撞過程中的最大接觸力遠小于能對人體造成傷害的水平。

圖8 碰撞檢測實驗

以上實驗結果表明,本文提出的高階擾動觀測器在實際碰撞中能夠實時檢測碰撞發生,在增加對應碰撞響應后能保證機器人在碰撞過程中不會對人體產生傷害,提高了機器人的人機協作安全性。

4 結語

為了提高機器人在人機協作環境中的安全性,提出了一種基于高階擾動觀測器的碰撞檢測算法。高階擾動觀測器在保證觀測結果實時性的同時能提供較好的魯棒性,減小了系統噪聲對觀測結果的影響。且提出的基于接觸危險系數的碰撞檢測閾值能夠比外力矩閾值更快的檢測碰撞。通過數值仿真驗證了提出的高階擾動觀測器和碰撞檢測閾值具有更好的檢測效果。利用自主研發的模塊化機器人搭建了實驗平臺完成了碰撞實驗,實驗結果表明高階擾動觀測器具有良好的實時性和魯棒性,在使用接觸危險系數判斷碰撞時能實時檢測到碰撞發生,使機器人碰撞的接觸力遠小于能對人體造成傷害的標準,提高了機器人的人機協作安全性。