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

吳海彬， 李實懿， 吳國魁

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

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

吳海彬， 李實懿， 吳國魁

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

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

機器人;碰撞檢測;動量;偏差觀測器;碰撞方位

0 引 言

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

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

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

1 碰撞檢測算法

1.1 碰撞檢測算法

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

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

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

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

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

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

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

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

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

機器人的廣義動量為

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

結合式(1)、式(4)，對式(3)求導可得

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

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

對式(6)展開，可得

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 實驗

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

3.1 M atlab仿真實驗

上述碰撞檢測算法是一個二階系統(tǒng)，fe可以調(diào)整系統(tǒng)的性能，仿真實驗分別對有調(diào)整函數(shù)fe和無調(diào)整函數(shù)fe作用進行了仿真，以研究fe對系統(tǒng)性能的影響，并考察算法能否實時跟蹤外力矩。為研究方便，本實驗對兩自由度機械手建模(圖4中的pd_ Rob)，并在第一關節(jié)上作用階躍信號的外力矩，第二關節(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曲線表示實際作用在關節(jié)上的外力矩值。比較圖5(a)和圖5(b)可以看出在調(diào)整函數(shù)fe作用下碰撞檢測算法對外力矩的跟蹤在響應的快速性和振蕩的消減方面具有很好的效果，且相對于無調(diào)整函數(shù)fe的系統(tǒng)具有更好的動態(tài)性能。同時從圖5(b)中可以看出曲線r可以快速地跟蹤曲線step，且其超調(diào)量較小并逐漸地趨于step。由此可以說明該碰撞檢測算法具有很好的實時性，能較好地跟蹤外力矩。

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

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

式中:τN為電機的額定轉矩;I為各關節(jié)的減速比。

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

圖6 機械手實驗平臺Fig.6 Robotmanipulator experiment platform

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

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

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

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

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

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

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

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

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

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

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

4 結 論

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

［1］ 吳海彬，楊劍鳴.機器人在人機交互過程中的安全性研究進展［J］.中國安全科學學報，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］ 理查德.摩雷，李澤湘，夏恩卡.薩斯特，等.機器人操作的數(shù)學導論［M］.北京:機械工業(yè)出版社，1998:11-102.

［15］ 王良勇，楊梟.帶有前饋和神經(jīng)網(wǎng)絡補償?shù)臋C械手系統(tǒng)軌跡跟蹤控制［J］.電機與控制學報，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

國家自然科學基金(51175084)

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

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

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

吳海彬