基于動量偏差觀測器的機(jī)器人碰撞檢測算法

吳海彬， 李實懿， 吳國魁

(福州大學(xué)機(jī)械工程及自動化學(xué)院，福建 福州 350108)

基于動量偏差觀測器的機(jī)器人碰撞檢測算法

吳海彬， 李實懿， 吳國魁

(福州大學(xué)機(jī)械工程及自動化學(xué)院，福建 福州 350108)

針對機(jī)器人易與進(jìn)入其工作空間的人或物體發(fā)生碰撞的問題，提出一種基于廣義動量偏差觀測器的碰撞檢測方法來提高機(jī)器人的安全性。根據(jù)機(jī)械手廣義動量與外力矩之間具有解耦性的特點(diǎn)，該檢測算法設(shè)計了動量偏差觀測器來間接獲取碰撞力，并通過性能調(diào)整函數(shù)進(jìn)一步改進(jìn)觀測器動態(tài)響應(yīng)特性，進(jìn)而判定碰撞是否發(fā)生。該碰撞檢測算法無需外加傳感器，也無需計算或檢測加速度信息，可以有效避免加減速過程對碰撞檢測的影響，只需檢測機(jī)器人關(guān)節(jié)電機(jī)的驅(qū)動電流及編碼器反饋的位置信息，即可實現(xiàn)碰撞實時檢測。仿真和實驗表明，該算法在實時性方面達(dá)到了預(yù)期要求，加減速過程對碰撞檢測的擾動很小，而且能提供碰撞發(fā)生的大致位置和方向信息，對進(jìn)一步的安全控制具有重要意義。

機(jī)器人;碰撞檢測;動量;偏差觀測器;碰撞方位

0 引 言

工業(yè)機(jī)器人可以工作在復(fù)雜的工作環(huán)境中，高效、高精度地執(zhí)行重復(fù)性的作業(yè)。在其工作過程中，很可能與周圍環(huán)境發(fā)生碰撞，造成周圍環(huán)境或者工件損壞。此外，在現(xiàn)代生產(chǎn)中，很多復(fù)雜的工作，需要機(jī)器人與工作人員處于同一工作空間中并通常需要在操作上相互配合［1］。在這種情況下，機(jī)器人的安全性將顯得更為重要［2］，若機(jī)器人不采取必要的防撞措施，將對人造成嚴(yán)重傷害，因此，必須首先解決機(jī)器人的碰撞安全性問題。為了保證機(jī)器人的安全性，需要對碰撞進(jìn)行檢測，并及時采取必要的控制策略，避免發(fā)生嚴(yán)重碰撞，并控制碰撞接觸力在完全可承受的范圍內(nèi)［3］。

解決機(jī)器人安全性問題可以從碰撞檢測開始，目前已有一些學(xué)者在這方面做出努力并取得了一定成果。檢測機(jī)器人是否發(fā)生碰撞的方法有添加外部傳感器檢測法［4-7］:如在機(jī)器人手腕添加力傳感器法［4］，該方法雖然能夠精確檢測出碰撞力的大小，但僅限于安裝了外部傳感器的部位;此外，還有在機(jī)器人外表面包裹敏感皮膚檢測法［5-7］，該方法能夠較好地檢測出碰撞力及其碰撞部位，但該方法增加了對外部信息處理的難度，增加了機(jī)器人布線的復(fù)雜度，增加了機(jī)器人的體積、降低了靈活性。也有學(xué)者提出視覺傳感器檢測法［8-9］，該方法可以比較全面的掌握外部環(huán)境信息，但圖像信息量大，難以達(dá)到實時性要求。文獻(xiàn)［10］采用機(jī)器人內(nèi)部傳感器檢測法，該法通過采集機(jī)器人各個關(guān)節(jié)力矩，并采集位置編碼器的值，運(yùn)用動力學(xué)方程計算在此運(yùn)動狀態(tài)下機(jī)器人所需的驅(qū)動力矩，將該力矩與實際采集的力矩對比以確定機(jī)器人是否發(fā)生碰撞。然而該法需要求解位置值的一階、二階導(dǎo)數(shù)，這會引入噪音干擾，影響檢測的準(zhǔn)確性。文獻(xiàn)［11-12］采用基于能量、動量檢測法，該法將碰撞視為系統(tǒng)故障，能較好地檢測出碰撞。但該算法在實時性與跟蹤外力的準(zhǔn)確性方面相互影響，難以同時達(dá)到最好的實時性及準(zhǔn)確性。

機(jī)器人發(fā)生碰撞后，碰撞力會瞬間增大［13］，因此，碰撞檢測算法需具備實時性、準(zhǔn)確性以及一定的碰撞方位識別能力，以便更好地控制碰撞力，減小碰撞傷害。本文提出一種基于廣義動量的偏差觀測器方法來檢測碰撞，該方法最主要的優(yōu)點(diǎn)是無需加速度信息，運(yùn)動過程中即使存在加減速的變化，也能及時準(zhǔn)確地檢測出機(jī)器人是否發(fā)生碰撞，并能提供一定的碰撞方位信息。

1 碰撞檢測算法

1.1 碰撞檢測算法

機(jī)器人在正常工作時，有可能與進(jìn)入其工作空間中的人或物體發(fā)生碰撞。當(dāng)一個n關(guān)節(jié)開鏈機(jī)器人與周圍環(huán)境發(fā)生碰撞時，動力學(xué)方程為

M(θ)∈Rn×n，為機(jī)器人的慣性矩陣，且M(θ)為正定矩陣;

C(θ，θ.)∈Rn×n，為機(jī)器人的哥氏矩陣，矢量C(θ，θ.)θ.包含了運(yùn)動方程中的哥氏力和離心力項;

g(θ)∈Rn×1，為作用于各關(guān)節(jié)的重力矩矢量; τ為關(guān)節(jié)的驅(qū)動力矩;

τe為外部作用力等效到各關(guān)節(jié)的力矩。

外部作用力與其在各關(guān)節(jié)的等效力矩可以通過機(jī)器人的雅可比矩陣進(jìn)行換算為

式中:Fe∈R6，作用于機(jī)器人的外部力和力矩;

Jk∈R6×n，力作用位置的機(jī)器人雅可比矩陣。

機(jī)器人在運(yùn)動過程中，一旦與周圍環(huán)境的人或物體發(fā)生碰撞，在碰撞瞬時，機(jī)器人的動量往往會發(fā)生較大的變化，而基于動量的實時變化判定是否發(fā)生碰撞會比直接檢測碰撞更容易和方便。

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

根據(jù)文獻(xiàn)［14］知，.M(θ)-2C(θ，θ.)為一反對稱矩陣，故有

結(jié)合式(1)、式(4)，對式(3)求導(dǎo)可得

由此可以看出方程中含有外力矩分量，并且動量與外力矩間具有解藕性，因此，可以依賴動量設(shè)計外力矩觀測器，通過觀測值判斷機(jī)器人是否發(fā)生碰撞，定義為

式中:r為觀測的外力矩值;K1＞0，K2＞0為增益矩陣;?p為動量估計值，具體有

對式(6)展開，可得

然而，僅采用式(8)作為觀測器時，會產(chǎn)生較大的延遲，并伴有較大的振蕩，影響檢測的實時性及準(zhǔn)確性，為此，構(gòu)造調(diào)整函數(shù)

將式(9)作為式(8)的前饋調(diào)節(jié)，可得如下觀測器為

式中:K1＞0，K2＞0，K3＞0，均為增益矩陣;p(t)= M(θ)θ.為機(jī)械手的廣義動量;τ為給定的關(guān)節(jié)驅(qū)動力矩;fe為性能調(diào)整函數(shù);

觀測器的控制框圖如圖1所示。

圖1 觀測器控制框圖Fig.1 Schematic of the observer

1.2 碰撞檢測算法的性能分析

該觀測器能有效地觀測出外力矩的變化，并能大致提供碰撞的位置和碰撞力的方向，對機(jī)器人識別碰撞具有重要意義。

根據(jù)式(5)，對式(10)求導(dǎo)并將式(5)、式(7)代入可得

對其進(jìn)行拉氏變換可得傳遞函數(shù)

由此可知，該觀測器相當(dāng)于對τe引入了一個二階系統(tǒng)，觀測值r將跟隨τe的變化而變化，在穩(wěn)態(tài)時r≈τe。若忽略摩擦力的作用，在無碰撞時，r=0;當(dāng)機(jī)器人發(fā)生碰撞時，r將會迅速增大;當(dāng)碰撞消失后，r將會迅速趨于0。由于實際系統(tǒng)中會存在摩擦且對于給定系統(tǒng)，各關(guān)節(jié)摩擦力將在一確定的區(qū)域內(nèi)變動，因此，可設(shè)定一碰撞力閥值rth＞0，當(dāng)|r|＞rth或者向量中的某一元素|rj|＞rth，j時，說明機(jī)器人發(fā)生了碰撞。

對于典型的二階系統(tǒng)，其快速性越好，則振蕩往往也越嚴(yán)重，若使其振蕩較小，則快速性就會變差。為了使系統(tǒng)具有快速性又能減小系統(tǒng)振蕩，在觀測器中加入調(diào)整函數(shù)fe，專門用于改善二階系統(tǒng)的性能。fe的作用相當(dāng)于在二階系統(tǒng)的前向通道中加入了一PD調(diào)節(jié)器。PD調(diào)節(jié)器可以減小二階系統(tǒng)的超調(diào)量，改善其穩(wěn)定性和快速性［15］。因此，適當(dāng)選取K1，K2，K3的值可以使碰撞檢測算法達(dá)到實時性和準(zhǔn)確性要求。

當(dāng)機(jī)器人的第i個連桿發(fā)生碰撞時，第i連桿的雅可比矩陣Ji為

式中:Ji∈R6×n;Ai∈R6×i。

根據(jù)式(2)可知，外力矩

式中:a1j(1≤j≤i-1)為不全為零的數(shù)，a1i≠0，由此可以看出第i連桿關(guān)節(jié)等效外力矩不為零，第i連桿以前的關(guān)節(jié)等效外力矩不全為零，第i連桿以后的關(guān)節(jié)等效外力矩均為零。依據(jù)τe的這一性質(zhì)，可以判斷出機(jī)器人的碰撞發(fā)生在哪一連桿上。然而，以典型垂直結(jié)構(gòu)6自由度工業(yè)機(jī)器人而言，當(dāng)碰撞作用力垂直于第二和第三關(guān)節(jié)軸線所組成的平面時，不論力的作用位置在哪，均只能在第一關(guān)節(jié)處檢測到外力矩。當(dāng)只有第一關(guān)節(jié)有外力矩時，不能確定碰撞力作用在機(jī)器人哪個連桿上。這種情況的處理對策是，只要轉(zhuǎn)動第一關(guān)節(jié)就可以避開碰撞，故可以把力等效為作用在第一關(guān)節(jié)上。此外，分析τe的各個分量a1k(1≤k≤i)的方向及機(jī)器人的幾何結(jié)構(gòu)可以大致確定發(fā)生碰撞的碰撞力方向。因此，考察τe就可以大致確定發(fā)生碰撞的連桿及力的大致方向。

該檢測算法在機(jī)器人靜止時也有效，但是在機(jī)器人啟動的時候仍有可能會產(chǎn)生誤判斷。當(dāng)機(jī)械手從靜止到開始運(yùn)動這一瞬間，由于機(jī)械手的慣性作用，機(jī)械手的速度保持為零，因此其廣義動量也為零。然而機(jī)械手的關(guān)節(jié)力矩卻可以在瞬間增大，此時，從式(10)可以看出r將瞬間變大，其效果相當(dāng)于機(jī)器人靜止時發(fā)生了碰撞，但實際上機(jī)械手卻并未與周圍環(huán)境發(fā)生碰撞。

為了避免在機(jī)械手從靜止到運(yùn)動時刻的誤檢測，采用式(14)對式(10)做修正。

式中:K4＞0，為一調(diào)整矩陣，通常選對角陣;Ma為一與機(jī)器人慣性相關(guān)的參數(shù);K5為用于判斷是否為機(jī)械手從靜止到運(yùn)動瞬間的參數(shù)，具有形式

由此便可避免機(jī)械手從靜止到運(yùn)動時刻誤判定為碰撞，同時可以減小碰撞檢測的閥值rth，這對減小機(jī)器人碰撞造成的損害具有重要意義。結(jié)合式(10)和式(14)則可得到一較好的碰撞檢測算法。

2 碰撞檢測算法在機(jī)器人系統(tǒng)上的實現(xiàn)

從前述的碰撞檢測算法可以看出，需要實時提取機(jī)器人的關(guān)節(jié)驅(qū)動力矩，各關(guān)節(jié)位置、速度等參數(shù)，并且需要判斷機(jī)器人是否處于開始啟動階段。目前，工業(yè)機(jī)器人較多地采用交流伺服電機(jī)來驅(qū)動機(jī)械手的各個關(guān)節(jié)，由于交流伺服電機(jī)驅(qū)動器一般都具有瞬態(tài)扭矩輸出功能，這為碰撞檢測算法的實施提供了方便。伺服系統(tǒng)的簡要控制框圖如圖2所示。

圖2 伺服系統(tǒng)簡要控制框圖Fig.2 Brief Schematic of servo system

可以從伺服驅(qū)動器中讀取出各個關(guān)節(jié)的反饋累積脈沖、瞬時輸出轉(zhuǎn)矩及伺服電機(jī)的轉(zhuǎn)速，對所讀取出的參數(shù)進(jìn)行適當(dāng)換算便可分別得到某一瞬時機(jī)械手各關(guān)節(jié)的位置，關(guān)節(jié)驅(qū)動力矩和關(guān)節(jié)轉(zhuǎn)速。如此便可用碰撞檢測算法對機(jī)器人進(jìn)行碰撞檢測。此過程只需從伺服驅(qū)動器中讀取適當(dāng)參數(shù)而無需外加傳感器，也無需計算加速度。

此外，還需判斷機(jī)械手是否處于靜止到運(yùn)動時刻，消除機(jī)械手開始運(yùn)行時刻的擾動，以更準(zhǔn)確地檢測機(jī)械手是否發(fā)生碰撞。圖3為機(jī)械手從靜止到運(yùn)行時刻采集的滯留脈沖、電機(jī)轉(zhuǎn)速及碰撞檢測算法外力矩觀測值，圖3中r表示碰撞檢測算法觀測值，v為關(guān)節(jié)速度，z為脈沖數(shù)，所對應(yīng)的縱軸單位分別為N.m，(°)/s，100個脈沖單位。

如圖3所示，滯留脈沖開始逐漸增大時電機(jī)轉(zhuǎn)速為零(t1時刻)，滯留脈沖開始增大后經(jīng)過一段時間，電機(jī)速度開始從零逐漸增大(t2時刻)，在此段時間內(nèi)，觀測值會產(chǎn)生一個峰值。產(chǎn)生這一現(xiàn)象的主要原因是機(jī)械手運(yùn)行之前，伺服驅(qū)動器接收到來自上位機(jī)的指令脈沖，但由于機(jī)械結(jié)構(gòu)的慣性作用，機(jī)械手并沒動作，因此電機(jī)的速度為零，編碼器的計數(shù)也不會變化，指令脈沖與編碼器反饋回的脈沖便會有一差值，該差值即為滯留脈沖。在實際運(yùn)用中可據(jù)此原理來判斷機(jī)械手是否處于開始運(yùn)行狀態(tài)，具體如下:

若滯留脈沖不為0且電機(jī)速度為0，則為機(jī)械手啟動狀態(tài)，否則為非啟動狀態(tài)。

此后用式(14)對式(10)修正，可使碰撞檢測算法更為精確。

圖3 機(jī)械手啟動時段觀測外力矩、關(guān)節(jié)速度、滯留脈沖的變化Fig.3 Change of the observed external torque，joint velocity，and detained pulse during starting status of robotmanipulator

3 實驗

為驗證碰撞檢測算法能夠?qū)ν饬剡M(jìn)行跟蹤并有較好的實時性，本文在Matlab上先進(jìn)行仿真實驗，以確定該算法是否有良好的動態(tài)性能，并確定其在理想狀態(tài)下(忽略摩擦及其它一些不確定干擾)是否能實時跟蹤外力矩。最后在一6自由度機(jī)械手實驗平臺上檢測碰撞檢測算法在實際狀況下的碰撞辨識能力。

3.1 M atlab仿真實驗

上述碰撞檢測算法是一個二階系統(tǒng)，fe可以調(diào)整系統(tǒng)的性能，仿真實驗分別對有調(diào)整函數(shù)fe和無調(diào)整函數(shù)fe作用進(jìn)行了仿真，以研究fe對系統(tǒng)性能的影響，并考察算法能否實時跟蹤外力矩。為研究方便，本實驗對兩自由度機(jī)械手建模(圖4中的pd_ Rob)，并在第一關(guān)節(jié)上作用階躍信號的外力矩，第二關(guān)節(jié)上不作用外力矩，其控制框圖如圖4所示。其中圖4(a)和圖4(b)分別為不加fe和加入fe的碰撞檢測算法的控制框圖。經(jīng)Matlab仿真后得到的外力矩跟蹤曲線如圖5所示，其中圖5(a)和圖5(b)分別為無fe和有fe作用時的曲線。

圖4 碰撞檢測算法控制框圖Fig.4 Control block diagram of collision detection algorithm

圖5中的r曲線代表根據(jù)碰撞檢測算法觀測出的外力矩值，step曲線表示實際作用在關(guān)節(jié)上的外力矩值。比較圖5(a)和圖5(b)可以看出在調(diào)整函數(shù)fe作用下碰撞檢測算法對外力矩的跟蹤在響應(yīng)的快速性和振蕩的消減方面具有很好的效果，且相對于無調(diào)整函數(shù)fe的系統(tǒng)具有更好的動態(tài)性能。同時從圖5(b)中可以看出曲線r可以快速地跟蹤曲線step，且其超調(diào)量較小并逐漸地趨于step。由此可以說明該碰撞檢測算法具有很好的實時性，能較好地跟蹤外力矩。

3.2 六自由度機(jī)械手實驗平臺上的碰撞實驗

為進(jìn)一步驗證該碰撞檢測算法在實際情況下能夠及時有效地檢測出機(jī)器人是否與周圍環(huán)境或人發(fā)生碰撞，本文在一型號為REBot-V-6R-650的6自由度機(jī)械手實驗平臺上對該算法進(jìn)行了驗證，該實驗平臺如圖6所示。該機(jī)械手的6個關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)，由交流伺服電機(jī)驅(qū)動，采用諧波減速器，減速比分別為134、135、99、96、85、64。關(guān)節(jié)的輸入轉(zhuǎn)矩與電機(jī)的瞬時轉(zhuǎn)矩百分比q的關(guān)系為

式中:τN為電機(jī)的額定轉(zhuǎn)矩;I為各關(guān)節(jié)的減速比。

圖5 fe對系統(tǒng)性能的影響Fig.5 Impact of feon the performance of system

圖6 機(jī)械手實驗平臺Fig.6 Robotmanipulator experiment platform

為驗證碰撞檢測算法的實時性及準(zhǔn)確性，設(shè)計實驗如下:讓機(jī)械手以30°/s的速度從θstart=［90°120°20°0°0°10°］運(yùn)動到θend=［90°50°-40°0°0°10°］，同時在機(jī)械手第四連桿上特別安裝電阻應(yīng)變式力傳感器，在機(jī)械手運(yùn)行過程中，用手對傳感器施加碰撞力，如圖7所示。

力傳感器可以直接檢測碰撞力，這里專門用于驗證本文提出的碰撞檢測算法的有效性。碰撞過程中實時采集力傳感器數(shù)據(jù)，并實時讀取伺服驅(qū)動器的電機(jī)扭矩、轉(zhuǎn)速、及位置脈沖數(shù)據(jù)，整理后得如圖8所示碰撞曲線。圖8中r1、r2表示由碰撞檢測算法算出的第二、第三關(guān)節(jié)等效外力矩值，f1表示傳感器測得的碰撞力，f2表示通過式Fe=(JT)-1τe得出的Fe的模，rth表示碰撞檢測閥值。從圖8中可以看出通過碰撞檢測算法算出的力與從傳感器檢測出的碰撞力其上升的趨勢是基本相同的，只是由于摩擦力等因素比實際力要大，在t1時刻，系統(tǒng)可以檢測到碰撞，此時的碰撞力約為85 N，處于人體可容忍范圍［16］，圖8中的碰撞閥值還有較大余量，可進(jìn)一步減小，以使檢測到碰撞時的碰撞力減小。由此可以說明該碰撞檢測算法是有效的，使用該碰撞檢測算法能夠保證較好的安全性。

圖7 碰撞實驗Fig.7 Graph of collision experiment

圖8 機(jī)械手實際碰撞時的實測碰撞力與觀測外力矩的對比Fig.8 Curves of im pact force got by external force sensor and im pact force got by observer when collision occurs

從圖5(b)中可以看出理論上觀測外力矩對實際外力矩的響應(yīng)速度比較快，調(diào)整觀測器參數(shù)，可使性能進(jìn)一步提升，經(jīng)Matlab仿真可知其響應(yīng)速度可以在10ms以內(nèi)。在實際實驗中，如圖8所示，從曲線f2來可以看出，發(fā)生碰撞后，曲線迅速爬升，觀測器響應(yīng)迅速，從碰撞開始(t0)到檢測到發(fā)生碰撞(t1)，只需20ms左右，此時碰撞力還處于人體可容忍范圍，因此，該碰撞檢測算法具有較好的實時性。

此外，為驗證該碰撞檢測算法在加減速時是否會發(fā)生誤檢測并為碰撞檢測閥值的選取提供依據(jù)，這里讓機(jī)械手分別以加速度θ¨=176°/s2和θ¨=23°/s2從靜止加速到θ.=44°/s，最后分別以同樣的減速度減少到速度為0，并實時計算r值，如圖9所示。圖中r已由式(14)修正，濾去啟動時刻峰值。比較圖9(a)和圖9(b)，雖然圖9(a)的加速度比圖9(b)大很多，但觀測出的外力矩大小卻沒多大變化，由此可得出，加速度對該碰撞檢測的算法的影響很小，在機(jī)械手加減速的時候也能準(zhǔn)確地檢測碰撞。此外，從圖9中可以看出，雖然機(jī)械手沒有發(fā)生碰撞，但是r≠0，并且在某一值上下波動，主要原因是機(jī)械手在運(yùn)行時關(guān)節(jié)間存在摩擦力矩τf的作用。因此，在選擇碰撞閥值時需保證rth＞|τf|，τf可以從實驗中得出。由于在沒有碰撞時r的值維持在某一值附近，因此，只需選取合適的rth，則不會產(chǎn)生誤檢測。

圖9 未發(fā)生碰撞時機(jī)械手運(yùn)行的觀測外力矩曲線Fig.9 Curves of observed external torque w ithout collision

該碰撞檢測算法能夠判斷碰撞發(fā)生在機(jī)器人哪個連桿上，在機(jī)器人靜止時算法也可以檢測出碰撞。這里在機(jī)器人靜止時分別用手對機(jī)器人的第二和第三根桿件作用碰撞力，并實時采集伺服電機(jī)參數(shù)，計算碰撞力矩值，力矩曲線如圖10所示。

圖10中的r1和r2分別表示通過碰撞檢測算法觀測出的作用于機(jī)器人第二和第三關(guān)節(jié)的外力矩。圖10(a)代表碰撞力作用在第二根桿上的情況，從圖10(a)中可以看出只在第二關(guān)節(jié)上有外力矩的明顯變化，圖10(b)代表碰撞力作用在第三根桿上的情況，從圖10(b)可以看出第二和第三關(guān)機(jī)的外力矩均有很大的變化。實驗證實了碰撞力作用于第i桿件，則第i關(guān)節(jié)及其前面的關(guān)節(jié)等效外力矩不為零，而第i關(guān)節(jié)之后的等效外力矩為0。依據(jù)此原理可以得出圖10(a)中碰撞力發(fā)生于第二關(guān)節(jié)，而圖10(b)中碰撞發(fā)生在第三關(guān)節(jié)，與實際情況相符。

本文所提出的碰撞檢測系統(tǒng)，從傳遞函數(shù)看為一個二階系統(tǒng)。相對于文獻(xiàn)［11］和文獻(xiàn)［12］的一階系統(tǒng)來說，本文所提碰撞檢測方法對系統(tǒng)中產(chǎn)生的高頻噪聲具有更好的濾波效果，使檢測結(jié)果更為準(zhǔn)確。此外，該碰撞檢測算法的性能調(diào)整參數(shù)有3個，相比于文獻(xiàn)［11］和文獻(xiàn)［12］的單一調(diào)整參數(shù)，可以使系統(tǒng)表現(xiàn)出更加多樣的性能要求，以適應(yīng)不同的場合，同時，可以協(xié)調(diào)各個參數(shù)以使某一參數(shù)不致過大，從而避免對某些噪聲信號過度放大。

圖10 機(jī)械手不同位置碰撞時的碰撞力觀測曲線Fig.10 Different curve of im pact force when robot collides at different position

4 結(jié) 論

為提高機(jī)械手作業(yè)時的安全性，根據(jù)機(jī)械手廣義動量與外力矩之間具有解藕性，設(shè)計了一種動量偏差觀測法來間接獲取關(guān)節(jié)外力矩，并與所選取的適當(dāng)?shù)呐鲎擦﹂y值進(jìn)行對比，可判斷機(jī)械手是否發(fā)生碰撞。該碰撞檢測算法僅需讀取關(guān)節(jié)電機(jī)的電流及編碼器的位置信息就可以檢測出碰撞，無需添加外部傳感器，也無需計算或檢測加速度信息，可以有效避免加減速過程對碰撞檢測的影響。當(dāng)機(jī)械手發(fā)生碰撞時，觀測值迅速增加，待碰撞消失后，觀測值迅速下降到碰撞力閥值以下。通過Matlab仿真平臺驗證了該碰撞檢測算法能實時檢測碰撞，并能準(zhǔn)確跟蹤碰撞力的變化。此外在一6自由度機(jī)械手實驗平臺上驗證了該碰撞檢測算法在實際情況下也能實時地并能在機(jī)械手對人造成傷害之前檢測出碰撞，并準(zhǔn)確地檢測出發(fā)生碰撞的桿件。在機(jī)器人發(fā)生危險前檢測出碰撞后，需要對其采取適當(dāng)控制算法以真正提高機(jī)器人的安全性，這也是后續(xù)所要研究的內(nèi)容。

［1］ 吳海彬，楊劍鳴.機(jī)器人在人機(jī)交互過程中的安全性研究進(jìn)展［J］.中國安全科學(xué)學(xué)報，2011，21(11):70-86. WU Haibin，YANG Jianming.Progress in robot safety research during human-robot interaction［J］.China Safety Science Journal，2011，21(11):70-86.

［2］ ANTONIO Bicchi，MICHELE Bavaro，GIANLUCA Boccadamo，et al.Physical human-robot interaction:Dependability，safety，and performance［C］//Proceedings of the 10th IEEE International Workshop on Advanced Motion Control，March 26-28，2008，Trento，Italy.2008:9-14.

［3］ SAMIHaddadin，ALIN Albu-Schaffer，MIRKO Frommberger，et al.The“DLR crash report”:towards a standard crash-testing protocol for robot safety-part II:discussions［C］//Proceedings of the 2009 IEEE International Conference on Robotics and Automation，May12-17，2009，Kobe，Japan.2009:280-287.

［4］ LU Shujun，JAE H Chung，STEVEN A Velinsky.Human-robot collision detection and identification based on wristand base force/ torque sensors［C］//Proceedings of the 2005 IEEE International Conference on Robotics and Automation，April 18-22，2005，Barcelona，Spain.2005:3796-3801.

［5］ VLADIMIRr J Lumelsky，EDWARD Cheung.Real-time collision avoidance in teleoperated whole-sensitive robot arm manipulators［J］.IEEE Transactions on Systems，Man and Cybernetics，1993，23(1):194-203.

［6］ YOJIYamada，KAZUHISA Shin，NUIO Tsuchida，etal.A tactile sensor system for universal joint sections of manipulators［J］. IEEE Transactions on Robotics and Automation，1993，9(4):512-517.

［7］ SAMSON Phan，ZHAN Fan Quek，PREYASShah，etal.Capacitive skin sensors for robot impactmonitoring［C］//IEEE/RSJ International Conference on Intelligent Robotsand Systems，Sept.25 -30，2011，San Francisco，California.2011:2992-2997.

［8］ DIRKM Ebert，DOMINIK D Henrich.Safe human-robot-cooperation:Image-based collision detection for industrial robots［C］// IEEE/RSJ International Conference on Intelligent Robots and Systems，Sept.30-Oct.4，2002，Lausanne，Switzerland.2002，2: 1826-1831.

［9］ FABRIZIO Flacco，TORSTEN Kroger，ALESSANDRO De Luca，et al.A depth space approach to human-robot collision avoidance［C］//Proceedings of the 2012 IEEE International Conference on Robotics and Automation，May 14-18，2012，St.Paul，MN，USA.2012:338-345.

［10］ YOJIYamada，YASUHIRO Hirasawa，HUANG Shengyang，et al.Human-robot contact in the safeguarding space［J］.IEEE/ ASME Transactions on Mechatronics，1997，2(4):230-236.

［11］ ALESSANDRO De Luca，RAFFAEKKA Mattone.Sensorless robot collision detection and hybrid force/motion control［C］// Proceedingsof the2005 IEEE International Conference on Robotics and Automation，April 18-22，2005，Barcelona，Spain. 2005:999-1004.

［12］ ALESSANDRODe Luca，ALIN Albu-Schaffer，SAMIHaddadin， et al.Collision detection and safe reaction with the DLR-III lightweightmanipulator arm［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems，Oct.9-15，2006，Beijing，China.2006:1623-1630.

［13］ YOJIYamada，KAZUTSUGU Suitaa，KOJI Imaia，et al.A failure-to-safety robot system for human-robot coexistence［J］.Robotics and Autonomous systems，1996，18(1):283-291.

［14］ 理查德.摩雷，李澤湘，夏恩卡.薩斯特，等.機(jī)器人操作的數(shù)學(xué)導(dǎo)論［M］.北京:機(jī)械工業(yè)出版社，1998:11-102.

［15］ 王良勇，楊梟.帶有前饋和神經(jīng)網(wǎng)絡(luò)補(bǔ)償?shù)臋C(jī)械手系統(tǒng)軌跡跟蹤控制［J］.電機(jī)與控制學(xué)報，2013，17(8):113-118. WANG Liangyong，YANG Xiao.Trajectory tracking control for robotic manipulators endowed with feedforward and neural networks［J］.Electric Machines and Control，2013，17(8):113 -118.

［16］ KAZUTSUGU Sutia，YOJIYamada，NUIO Tsuchida，et al.A failure-to-safety“Kyozon”system with simple contact detection and stop capabilities for safe human-autonomous robot coexistence［C］//Proceedings of the 1995 IEEE International Conference on Robotics and Automation，May 21-28，Nagoya，Aichi，Japan.1995，3:3089-3096.

(編輯:劉琳琳)

Collision detection algorithm for robotmanipulator based on momentum deviation observer

WU Hai-bin， LIShi-yi， WU Guo-kui
(College of Mechanical Engineering and Automation，F(xiàn)uzhou University，F(xiàn)uzhou 350108，China)

In view of the fact that robots are easy to collide with humans or obstacles which go into its workspace，a collision detection algorithm based on generalized momentum deviation was proposed to improve the safety of robots.According to the decoupling performance between generalized momentum and external torque ofmanipulator，amomentum deviation observer was firstly designed to obtain the impact force.Then，a performance regulating function was introduced to improve its dynamic response characteristics.Once impact force was detected，it is possible to judge whether the collision occurred.With this algorithm，collision can be detected in real time，and can effectively avoid the influence of acceleration or deceleration，without any additional external sensors or acceleration of themanipulator，simply bymeasuring themotor current and the position got by encoders of each joint.Simulation and experiment results show that the algorithm has reached the expected requirements in real-time performance.And the disturbances caused by accelerating and decelerating ofmanipulator are also very small.Moreover，it provides an approximate position and direction of the impact force which has important significance for further security reaction control.

robotmanipulator;collision detection;momentum;deviation observer;position of the impact force

10.15938/j.emc.2015.05.014

TP 24

A

1007-449X(2015)05-0097-08

2013-12-03

國家自然科學(xué)基金(51175084)

吳海彬(1973—)，男，博士，教授，博士生導(dǎo)師，研究方向為機(jī)器人技術(shù)、機(jī)電系統(tǒng)自動化研究;

李實懿(1990—)，男，碩士研究生，研究方向為機(jī)器人技術(shù);

吳國魁(1988—)，男，碩士，研究方向為機(jī)器人技術(shù)。

吳海彬