王志軍 劉 璐 李占賢

(1.華北理工大學(xué)機械工程學(xué)院， 唐山 063210； 2.河北省工業(yè)機器人產(chǎn)業(yè)技術(shù)研究院， 唐山 063210)

0 引言

共融機器人是下一代機器人的發(fā)展目標，實現(xiàn)人機共融的首要問題是安全[1-4]。在機器人作業(yè)過程中，為了保證人、機、物的安全，需要對作業(yè)環(huán)境信息進行感知，而力信息和機器人碰撞點的位置檢測是其中的重要部分。為此，越來越多的傳感器被集成到機器人系統(tǒng)中[5-9]；這些傳感器可協(xié)助機器人獲取環(huán)境信息，有助于機器人作出正確判定[10-14]。視覺傳感器協(xié)助機器人獲取環(huán)境的外觀信息，用于實現(xiàn)目標跟蹤和物體捕獲等[15-17]。碰撞感測也是一種重要的環(huán)境感測方法，通過該方法機器人感測碰撞點和碰撞方向，以免再次發(fā)生碰撞，并實現(xiàn)對機器人的反饋控制。在包含人和機器人的環(huán)境中，碰撞感知對避免嚴重碰撞至關(guān)重要[18-21]。貼在機器人整個表面上的皮膚力傳感器(如人體皮膚系統(tǒng))可以感應(yīng)力作用點、方向和大小。麻省理工學(xué)院研究了一種稱為QTC(量子隧道復(fù)合材料)的特殊復(fù)合材料，該材料受到金屬和非金屬的擠壓時[22-23]，傳感器可通過電阻變化測量力信息。與之相類似的還有由并五苯分子和電子皮膚組成的有機薄膜等。皮膚力傳感器價格昂貴、易損壞、設(shè)計復(fù)雜、對環(huán)境要求較高、采樣率低、通用性較差[24-26]，難以在現(xiàn)實中使用。皮膚傳感器的靈敏度系統(tǒng)無法獲得碰撞方向信息，并會受到許多因素的影響，例如溫度和濕度等。在利用力傳感器進行碰撞點檢測方面，文獻[27]基于力/力矩傳感器和結(jié)構(gòu)化的探針建立一種物體輪廓探測方法，文獻[28]基于力/力矩傳感器數(shù)據(jù)和機械手指端幾何模型提出求解觸點位置的方法。已有研究主要針對結(jié)構(gòu)化碰撞過程中沒有形變產(chǎn)生的碰撞點進行識別，若碰撞體表面為非規(guī)則形狀或建模較難，則已有的方法應(yīng)用較為受限。

按照碰撞點和碰撞外力的不同，機器人本體碰撞大體可以分為單點單外力碰撞、單點外力/外力矩混合碰撞、多點外力碰撞、多點外力/外力矩混合碰撞等。多點多外力/外力矩的碰撞可以視為單點單外力的特殊組合形式。

針對皮膚力傳感器對機器人碰撞點檢測的不足，本文提出一種利用基座處六維力傳感器采集信息進行自約束，從而對機器人本體碰撞點進行檢測的方法。在對基座處六維力傳感器進行動態(tài)力補償后，運動過程中機器人數(shù)值在零點附近波動，當傳感器數(shù)值超過一定閾值后即可認為機器人與外界發(fā)生碰撞，對經(jīng)過補償后的傳感器每幀數(shù)據(jù)進行自約束，可計算出機器人本體碰撞點坐標。

1 碰撞點檢測模型

與傳統(tǒng)的應(yīng)用六維力傳感器不同，為了實現(xiàn)對機器人本體的碰撞位置檢測將機器人安裝在基座處。傳感器的安裝位置示意圖如圖1所示。

六維力傳感器可以測得力和力矩在3個軸上的分量信息，假設(shè)基座處傳感器采集了F(Fx,Fy,Fz)和M(Mx,My,Mz)，則碰撞點的位置信息表示為

M=PF

(1)

式中P(Px,Py,Pz)為碰撞點相對于傳感器坐標系的位置矢量。式(1)可以改寫為

(2)

由于|F|=0，所以碰撞點的位置解并不唯一，這時需要利用其他約束條件來確定碰撞點的位置。

將傳感器采集到某一幀的數(shù)據(jù)代入式(2)可以得到1條外力作用矢量線Lc(p)，其表達式為

(3)

傳感器每幀采集的不同參數(shù)決定了不同的外力矢量線，在實際中碰撞外力的方向大多數(shù)與機器人的運動方向存在一定夾角，因此利用傳感器不同幀數(shù)計算的外力矢量線在空間中會相交于某點Pc，該點即為碰撞點，具體計算方法為

{P(x,y,z)|(x,y,z)∈Lc1(p)∩Lc2(p)}

(4)

由于傳感器存在的測量誤差會使得計算的空間力矢量線在空間中不會相交在碰撞點，但在某一平面內(nèi)的投影會相交于正式的碰撞點在該平面的投影附近。所以可以通過投影法先求得外力矢量線在平面的交點，再將交點坐標代入原始方程求得碰撞點信息，如圖2所示。

假設(shè)L′XOY是Lc(p)在XOY平面的投影直線，在XOZ平面的投影為L′XOZ，在YOZ平面的投影為L′YOZ，計算式為

(5)

圖2中P′(x,y)是2條外力矢量線的投影L′XOY，1和L′XOY，2的交點，P1、P2為投影交點在外力矢量線上的對應(yīng)點，P1、P2計算式為

(6)

式中η——確定最佳投影平面的選擇因子

投水平面選擇規(guī)則為

(7)

其中

(8)

式中θXOY、θXOZ、θYOZ——外力矢量線在XOY、XOZ、YOZ平面投影L′XOY，1和L′XOY，2的夾角

若XOY平面為最優(yōu)投影平面，則P1(x1,y1,z1)和P2(x2,y2,z2)計算公式為

(9)

(10)

真實的碰撞點坐標在兩點附近，但精確位置無法計算，因此初步判定碰撞點P為兩點的中點，即

(11)

在機器人碰撞瞬間傳感器可以采集到多組數(shù)據(jù)，求解的空間外力矢量線示意圖如圖3所示。為了在多組計算結(jié)果中搜索出和真實碰撞點最近的最優(yōu)解，假設(shè)ζ(ζMx,ζMy,ζMz)為誤差因子，P″(x″,y″,z″)為初步計算的碰撞點坐標?？傻?/p>

(12)

2 動態(tài)力補償算法

將六維力傳感器安裝在機器人基座處時應(yīng)首先消除機器人本體的重力和運動過程中末端負載對傳感器的影響，應(yīng)對傳感器進行動態(tài)力補償。在傳感器上執(zhí)行動態(tài)力補償后，當機器人處于運動狀態(tài)且不受外力作用時，讀數(shù)將恒定為零。若傳感器讀數(shù)超過閾值，即認為機器人與外界意外碰撞。 機器人重力補償如圖5所示。

(13)

由式(13)可得

(14)

將機器人各連桿參數(shù)代入式(13)中，可依次得到相鄰連桿之間的位姿變換矩陣。坐標系{i}相對于坐標系{0}的變換矩陣為

(15)

假設(shè)關(guān)節(jié)i的質(zhì)心ci相對于關(guān)節(jié)坐標系i的位置矢量為

ci=[cixciyciz]T

(16)

各連桿質(zhì)心坐標系{ci}相對于{0}系的齊次變換矩陣為

(17)

(18)

(19)

(20)

(21)

可以對連桿之間建立動力學(xué)方程

(22)

(23)

ri,ci——關(guān)節(jié)i上附著的坐標系原點Oi到質(zhì)心ci的矢徑

ri+1,ci——關(guān)節(jié)i+1上附著的坐標系原點Oi+1到質(zhì)心ci的矢徑

Ii——桿i相對于其質(zhì)心ci的慣性張量

(24)

將基座處的受力信息經(jīng)過式(24)進行坐標變換即可得到基座處六維力傳感器最終所需要的重力補償數(shù)值，即Sf和SM。若機器人處于某一位姿時，基座六維力傳感器的力/力矩傳感器讀數(shù)分別為Df和DM，則經(jīng)過重力補償后的力/力矩信息F和M為

(25)

經(jīng)過動態(tài)力補償后，在運動過程中沒有外力的情況下，機器人可以使六維力傳感器的讀數(shù)恒定為零。一旦基座處的六維力傳感器的讀數(shù)超過設(shè)置的閾值，就可認為機器人與外部發(fā)生碰撞。機器人與外界之間的碰撞是一個連續(xù)的過程，因此，在動態(tài)力補償后，基座上的傳感器讀取的每一幀數(shù)據(jù)都可以用作碰撞點檢測算法的輸入。

在經(jīng)過對數(shù)據(jù)的動態(tài)力補償以后，判斷發(fā)生碰撞與否的閾值確定與機器人的整體結(jié)構(gòu)參數(shù)、末端負載情況、傳感器的精度等情況有關(guān)。因此增加S作為安全閾值的判斷環(huán)節(jié)，S取值0或1。當S=0時機器人與外界不發(fā)生碰撞，機器人正常工作，S=1是繼任為機器人與外界發(fā)生碰撞[29]。表達式為

(26)

通常情況下結(jié)合機器人實際情況進行人為設(shè)定，為了提高碰撞識別的精度，選擇了每個方向最小的力誤差作為識別是否發(fā)生碰撞的閾值，即δf=0.492 3 N。

3 仿真實驗

為了驗證本文提出的動態(tài)力補償算法和碰撞點檢測算法的有效性和精度，構(gòu)建了1個3自由度機器人模型進行仿真實驗，如圖7所示。

為了減少計算量，機械手的連桿和關(guān)節(jié)中心對稱，每個連桿的重心位于其自身的中心軸上。表1給出了本文3自由度機器人每個連桿的結(jié)構(gòu)參數(shù)，材料為合金，ρ=2.7×103kg/m3。圖7中，l1=120 mm,l2=150 mm,l3=200 mm,l4=200 mm,h=30 mm。

表1 連桿參數(shù)

各連桿相對于其質(zhì)心的慣性張量為

(27)

(28)

(29)

基座處傳感器采集的數(shù)據(jù)隨著時間而不斷變化，將傳感器采集到的數(shù)據(jù)經(jīng)過動態(tài)補償處理后作為式(2)的輸入進行碰撞點的計算。將機器人模型導(dǎo)入ADAMS，以驗證所提出的動態(tài)力補償算法和碰撞點檢測算法。實驗中模擬施加的碰撞力的大小、方向和位置已知，并且實驗結(jié)果數(shù)據(jù)以傳感器坐標系為基準。

表2 不同碰撞點的實驗結(jié)果

為了驗證提出算法的精度，對實驗結(jié)果進行了誤差分析。

各方向分力的相對誤差計算式為

(30)

式中 ΔFn——力在3個方向的絕對誤差

|FC|——測量力矢量的模

合力相對誤差計算式為

(31)

式中fCx、fCy、fCz——測量力的3個方向分量

fDx、fDy、fDz——計算力的3個方向分量

位置各方向的相對誤差計算式為

(32)

位置的相對誤差計算式為

(33)

各個方向的分力和位置誤差如圖8所示。在對數(shù)據(jù)進行誤差計算過程中若分母為0，則計算結(jié)果輸出為0。

由圖8可以看出，當碰撞力恒定時，隨著碰撞點距離的變化，各個方向上分力的相對誤差在5%以下范圍內(nèi)波動，合力最大相對誤差為4.892 5%。但隨著碰撞點的距離增加，碰撞位置在各個方向上的絕對誤差和相對誤差不斷增加，在最遠處距離力傳感器坐標系598.61 mm的位置碰撞點坐標相對誤差達到最大值8.711 9%。采用這種方法進行碰撞點檢測滿足機器人最遠點碰撞精度要求。

4 結(jié)束語

提出了一種單點單外力機器人碰撞點檢測算法，并在此基礎(chǔ)上提出了六維力傳感器動態(tài)力補償算法。對該算法進行了仿真實驗，證明了該算法的有效性，同時表明，動態(tài)力補償算法的精度不會隨著碰撞點位置的變化產(chǎn)生明顯變化。碰撞點檢測算法的準確性受碰撞位置影響，在最遠處598.61 mm位置上碰撞點的相對誤差最大值為8.711 9%，滿足機器人碰撞點檢測算法的精度要求。