王志軍 劉 璐 李占賢

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

0 引言

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

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

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

1 碰撞點(diǎn)檢測模型

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

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

M=PF

(1)

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

(2)

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

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

(3)

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

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

(4)

由于傳感器存在的測量誤差會使得計算的空間力矢量線在空間中不會相交在碰撞點(diǎn)，但在某一平面內(nèi)的投影會相交于正式的碰撞點(diǎn)在該平面的投影附近。所以可以通過投影法先求得外力矢量線在平面的交點(diǎn)，再將交點(diǎn)坐標(biāo)代入原始方程求得碰撞點(diǎn)信息，如圖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的交點(diǎn)，P1、P2為投影交點(diǎn)在外力矢量線上的對應(yīng)點(diǎn)，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)

真實(shí)的碰撞點(diǎn)坐標(biāo)在兩點(diǎn)附近，但精確位置無法計算，因此初步判定碰撞點(diǎn)P為兩點(diǎn)的中點(diǎn)，即

(11)

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

(12)

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

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

(13)

由式(13)可得

(14)

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

(15)

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

ci=[cixciyciz]T

(16)

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

(17)

(18)

(19)

(20)

(21)

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

(22)

(23)

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

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

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

(24)

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

(25)

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

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

(26)

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

3 仿真實(shí)驗

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

為了減少計算量，機(jī)械手的連桿和關(guān)節(jié)中心對稱，每個連桿的重心位于其自身的中心軸上。表1給出了本文3自由度機(jī)器人每個連桿的結(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)補(bǔ)償處理后作為式(2)的輸入進(jìn)行碰撞點(diǎn)的計算。將機(jī)器人模型導(dǎo)入ADAMS，以驗證所提出的動態(tài)力補(bǔ)償算法和碰撞點(diǎn)檢測算法。實(shí)驗中模擬施加的碰撞力的大小、方向和位置已知，并且實(shí)驗結(jié)果數(shù)據(jù)以傳感器坐標(biāo)系為基準(zhǔn)。

表2 不同碰撞點(diǎn)的實(shí)驗結(jié)果

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

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

(30)

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

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

合力相對誤差計算式為

(31)

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

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

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

(32)

位置的相對誤差計算式為

(33)

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

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

4 結(jié)束語

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