陳一豪，李 毅，朱明超

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

0 引言

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

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

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

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

1.1 高階擾動觀測器設(shè)計

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

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

對機器人廣義動量進行求導(dǎo)得為：

將公式和代入,得到廣義動量的導(dǎo)數(shù)為：

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

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

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

調(diào)整后的觀測器結(jié)構(gòu)可表示為：

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

圖1 高階擾動觀測器結(jié)構(gòu)圖

1.2 碰撞檢測閾值

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

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

關(guān)節(jié)i的接觸危險系數(shù)定義為：

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

無碰撞發(fā)生時,關(guān)節(jié)i的接觸危險系數(shù)存在如式的關(guān)系,其中Eilim為關(guān)節(jié)i的碰撞檢測閾值,即不滿足該條件時判斷碰撞發(fā)生。

2 碰撞檢測算法仿真驗證

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

圖2 仿真系統(tǒng)

2.1 觀測器仿真結(jié)果對比

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

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

圖3 擾動觀測器仿真結(jié)果

2.2 碰撞檢測閾值仿真驗證

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

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

圖4 關(guān)節(jié)1碰撞檢測閾值仿真結(jié)果

表1 碰撞檢測結(jié)果

由以上仿真結(jié)果可知,本文提出的接觸危險系數(shù)在檢測碰撞的過程中表現(xiàn)出更好的碰撞檢測實時性和準(zhǔn)確性,具有預(yù)期的碰撞檢測效果。

3 碰撞實驗

3.1 實驗平臺

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

圖5 碰撞檢測實驗平臺

3.2 高階擾動觀測器實驗

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

圖6 機器人關(guān)節(jié)運行軌跡

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

圖7 高階擾動觀測器實驗

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

3.3 實際碰撞檢測實驗

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

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

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

圖8 碰撞檢測實驗

以上實驗結(jié)果表明,本文提出的高階擾動觀測器在實際碰撞中能夠?qū)崟r檢測碰撞發(fā)生,在增加對應(yīng)碰撞響應(yīng)后能保證機器人在碰撞過程中不會對人體產(chǎn)生傷害,提高了機器人的人機協(xié)作安全性。

4 結(jié)語

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