陳一豪，李 毅，朱明超

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

0 引言

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

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

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

1 基于擾動(dòng)觀測(cè)器的碰撞檢測(cè)

1.1 高階擾動(dòng)觀測(cè)器設(shè)計(jì)

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

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

對(duì)機(jī)器人廣義動(dòng)量進(jìn)行求導(dǎo)得為：

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

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

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

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

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

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

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

1.2 碰撞檢測(cè)閾值

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

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

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

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

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

2 碰撞檢測(cè)算法仿真驗(yàn)證

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

圖2 仿真系統(tǒng)

2.1 觀測(cè)器仿真結(jié)果對(duì)比

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

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

圖3 擾動(dòng)觀測(cè)器仿真結(jié)果

2.2 碰撞檢測(cè)閾值仿真驗(yàn)證

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

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

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

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

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

3 碰撞實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺(tái)

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

圖5 碰撞檢測(cè)實(shí)驗(yàn)平臺(tái)

3.2 高階擾動(dòng)觀測(cè)器實(shí)驗(yàn)

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

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

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

圖7 高階擾動(dòng)觀測(cè)器實(shí)驗(yàn)

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

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

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

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

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

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

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

4 結(jié)語

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