李倩倩，袁宏祥，馬 昕，宋 銳

(山東大學(xué) 控制科學(xué)與工程學(xué)院，山東 濟(jì)南 250061)

1 問題的描述

力覺感知是智能機(jī)器人與外界環(huán)境交互的一項重要功能，特別是對于抓取[1,2]、輪廓檢測[3]、避障[4]、人機(jī)交互[5-6]以及力反饋控制[7]等操作尤為重要。因此，力傳感器(特別是六維力/力矩傳感器)被越來越多地集成到機(jī)器人系統(tǒng)中[8-9]。例如：在工業(yè)機(jī)器人生產(chǎn)線中，力傳感器被用來做碰撞檢測和反饋控制[10]；在機(jī)械靈巧手系統(tǒng)中，力傳感器被用來實現(xiàn)觸覺感知；在手術(shù)機(jī)器人系統(tǒng)中，力傳感器被用來輔助安全控制和碰撞檢測等[11-12]?？梢?，碰撞檢測和反饋控制是力覺感知的兩個關(guān)鍵功能[13-14]。而碰撞位置檢測則是碰撞檢測的重要內(nèi)容之一。

在傳感器和機(jī)器人技術(shù)的發(fā)展過程中，基于力/力矩傳感器的碰撞位置檢測方法已經(jīng)有很多研究。最早的可追溯到的為文獻(xiàn)[15]中提到的：1989年，Tsujimura等基于力/力矩傳感器和一個結(jié)構(gòu)化的探針建立了一種物體輪廓探測方法;1990年，Bicchig等[1]從力學(xué)的角度對碰撞感知進(jìn)行了分析，在文章中闡述了碰撞感知的基礎(chǔ)理論，并提出了基于幾種常見形狀的結(jié)構(gòu)化探針的位置感知解決方案;1992年，Kazanzides等[16]在文獻(xiàn)中提到了一種在外科手術(shù)機(jī)器人系統(tǒng)中，通過線性回歸方法監(jiān)測碰撞點的方法，但文章中沒有給出計算過程;1996年，周學(xué)才等[17-18]提出一種與文獻(xiàn)[13]類似的方法，用于實現(xiàn)機(jī)械手系統(tǒng)中的指端碰撞位置感知，但與文獻(xiàn)[15]不同，這類方法采用了矩陣?yán)碚摰姆椒?，并且加入了重力校?zhǔn)，為實際應(yīng)用打下了良好基礎(chǔ);2004年，韓壯志等[19-20]也提出一種基于力/力矩傳感器數(shù)據(jù)和機(jī)械手指端幾何模型求解觸點位置的方法;2015年，Leng等[21-22]通過安裝在六自由度機(jī)械臂腕部的力傳感器實現(xiàn)了機(jī)器人末端工具的碰撞位置檢測，并基于機(jī)械臂的運動學(xué)模型建立了重力補(bǔ)償和動態(tài)補(bǔ)償方法。此外，文獻(xiàn)[23]和文獻(xiàn)[24]中也同樣分析了碰撞過程的幾何約束和自然約束，并基于六自由度機(jī)械臂的運動學(xué)模型給出了重力和動態(tài)補(bǔ)償方法。

基于力學(xué)數(shù)據(jù)求解碰撞位置的難點在于多解問題，因此必須引入額外的約束條件來求解。總結(jié)上文中提到的研究方法，無一例外的依靠幾何約束條件來求解，因此要求碰撞物體必須為形狀規(guī)則或者可幾何建模(如長度一定的探針、球形、平面等)，并且要求碰撞表面光滑且不能變形，對實際應(yīng)用條件要求苛刻。然而，現(xiàn)實生活中的碰撞大多數(shù)發(fā)生在非結(jié)構(gòu)化的環(huán)境中，且往往會出現(xiàn)變形。因此，上述基于碰撞表面幾何約束的方法都無法被應(yīng)用。根據(jù)碰撞表面的觸點數(shù)量的不同，可以將實際生活中的碰撞分為3種：單觸點碰撞、平面或表面均勻接觸碰撞、多觸點碰撞，如圖1右側(cè)第一行所示。而單觸點碰撞又可以分為結(jié)構(gòu)化環(huán)境下(可建模預(yù)測)碰撞和非結(jié)構(gòu)化環(huán)境下的碰撞，如圖1右側(cè)第二行所示。除此之外，形變也是碰撞過程中經(jīng)常遇到的一種情況，如圖1右側(cè)f所示。事實上，表面均勻接觸的碰撞和多觸點碰撞都可以看作是單觸點碰撞的特殊組合形式，因此本文重點針對非結(jié)構(gòu)化環(huán)境下的單觸點碰撞及形變問題進(jìn)行了分析。

為了克服基于幾何約束方法的局限性，擺脫觸點位置檢測對碰撞物體形狀信息的依賴，本文基于機(jī)械臂腕力傳感器建立了一種僅依靠力學(xué)數(shù)據(jù)觸點位置檢測方法。該方法將碰撞視為一個連續(xù)的過程，通過碰撞過程中時間軸上的多組力/力矩采樣數(shù)據(jù)互為約束條件來計算觸點的位置，并建立了最小誤差法搜索策略，在多組結(jié)果中搜索出最優(yōu)的解。因此，所提檢測方法既可以應(yīng)用于結(jié)構(gòu)化的環(huán)境，也可以應(yīng)用于非結(jié)構(gòu)化和形變的環(huán)境，有效地推進(jìn)了力覺感知在機(jī)器人系統(tǒng)中的應(yīng)用，同時也為多觸點檢測奠定了基礎(chǔ)。

2 碰撞定位模型

以圖1左側(cè)的機(jī)械臂系統(tǒng)為例，設(shè)最終檢測到的觸點位置為RP，根據(jù)機(jī)械臂的運動學(xué)模型可以得到：

(1)

式中SP為觸點在力傳感器坐標(biāo)系中的位置。因此，對于整個機(jī)器人系統(tǒng)來說，首先應(yīng)該標(biāo)定傳感器在系統(tǒng)中的位置，之后觸點檢測問題可簡化為在傳感器坐標(biāo)系中求解觸點位置。

2.1 基于幾何約束的碰撞定位模型

根據(jù)六維力傳感器的數(shù)據(jù)格式，碰撞力和力矩均可以被分解為3個坐標(biāo)軸上的分量。因此，設(shè)碰撞力和力矩分別為F(Fx,Fy,Fz)，M(Mx,My,Mz)，則兩者與觸點位置的關(guān)系可表示為：

M(Mx,My,Mz)=L(x,y,z)×F(Fx,Fy,Fz)。

(2)

式中L為坐標(biāo)原點到觸點的向量。式(2)用矩陣形式可表達(dá)為：

(3)

{P(x,y,z)|(x,y,z)∈L×F=

M∩S(x,y,z)=0}。

(4)

在這些幾何約束方法中，觸點檢測往往只能采用球形、圓柱或者其他容易描述的結(jié)構(gòu)體作為探針，因此極大的限制了應(yīng)用范圍。此外，由于傳感器數(shù)據(jù)采集過程中的噪聲和碰撞過程中的變形等，導(dǎo)致該類方法的魯棒性極差，使其難以在實際應(yīng)用中發(fā)揮作用。

2.2 動態(tài)力學(xué)數(shù)據(jù)自約束模型

事實上，當(dāng)F(Fx,Fy,Fz)與M(Mx,My,Mz)為常數(shù)時，式(3)的解集{P}分布在一條空間直線上。這條空間曲線通常被稱為外力矢量線，本文將其定義為Lc(P)，參數(shù)形式可表達(dá)為：

(5)

顯然F(Fx,Fy,Fz)為直線的方向向量。因此，不同的碰撞力決定了不同方向的外力矢量線。通常情況下，由于碰撞物體的運動方向與碰撞力方向不同，就會導(dǎo)致不同時刻的兩組外力矢量線不平行(相交)，且交點即為碰撞接觸點。綜上所述，觸點位置可以通過求解兩組外力矢量線的交點來得到：

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

(6)

3 基于自約束模型的碰撞位置檢測方法

3.1 數(shù)據(jù)預(yù)處理

文獻(xiàn)[1]中對碰撞力已經(jīng)進(jìn)行了詳細(xì)分析，并將碰撞過程中觸點所受的外力分解為垂直于碰撞表面的壓力和與碰撞表面相切的摩擦力。而在將碰撞過程看作一個動態(tài)過程來求解觸點位置時，碰撞外力可以不用分解。此外，力傳感器的檢測到的力除了碰撞外力Fcontact，還包括一個內(nèi)力Finter和一個動態(tài)力Fdynamic，即

Fsensor=Fcontact+Finter+Fdynamic。

(7)

式中的內(nèi)力主要來自于工具和力傳感器裝配過程中產(chǎn)生的擠壓力，通常在裝配完成后就可視為定值。動態(tài)力則來自于末端工具的重力，會隨著機(jī)械臂姿態(tài)的變化而變化。如此一來，由碰撞產(chǎn)生的外力可以通過以下公式來計算：

Fcontact=Fsensor-Finter-FgravityRg→s。

(8)

式中：Fgravity為末端工具在大地坐標(biāo)系中的重力;Rg→s為大地坐標(biāo)系到力傳感器坐標(biāo)系的轉(zhuǎn)換矩陣。

此外，基于幾何約束的方法一般要通過重力補(bǔ)償和動態(tài)補(bǔ)償來將碰撞外力從傳感器采集到的數(shù)據(jù)中分離出來。但從碰撞過程(從接觸到發(fā)生位移)的角度，隨時間變化的只有碰撞力。因此，本文只將碰撞產(chǎn)生的外力視為外力，其他力均視為內(nèi)力。同時，將碰撞從一個即刻的狀態(tài)擴(kuò)展到一個時間軸上的過程，也意味著可以獲取到多組傳感器數(shù)據(jù)，建立時間函數(shù)：

Fcontact(t)=Fsensor(t)-Finter-FgravityRg→s。

(9)

對式(9)求導(dǎo)可得：

ΔFcontact=ΔFsensor。

(10)

顯然ΔFcontact也同樣適用于式(2)。因此，采用碰撞力的導(dǎo)數(shù)，即傳感器兩幀之間的差值作為式(2)的輸入量。

3.2 基于投影法的外力矢量線交點求解

(11)

圖2b給出了兩條外力矢量線在某個坐標(biāo)平面上的投影。如圖2所示，兩條外力矢量線雖然在三維空間中的可探測范圍內(nèi)不相交，但是其在坐標(biāo)平面上投影線的交點卻會落在真實觸點投影的附近。因此，可以先通過投影交點求得觸點在3個坐標(biāo)平面的投影點坐標(biāo)，然后結(jié)合原始的外力矢量線方程求得觸點的三維坐標(biāo)值，如圖3所示。

(12)

其中σ為一個判定因子，用于從三個坐標(biāo)平面中選擇一個最優(yōu)的作為投影平面，其具體規(guī)則為：

(13)

(14)

假設(shè)最終選定的投影平面為XOY，則P1(x,y,z1)和P2(x,y,z2)的解算公式為：

(15)

和

(16)

3.3 最小誤差搜索策略

為進(jìn)一步縮小誤差，提高觸點位置檢測的魯棒性，本文采用在碰撞過程的多組數(shù)據(jù)中搜索最優(yōu)解的方法。假設(shè)碰撞過程的多組力傳感器數(shù)據(jù)產(chǎn)生的外力矢量線如圖4a所示，則利用兩組相鄰數(shù)據(jù)通過投影法求得的觸點集如圖4b所示，其中表示真實的觸點位置，為通過最小誤差法搜索出的最優(yōu)解。最小誤差法的原理為：

設(shè)最終的觸點位置檢測結(jié)果為P″(x″,y″,z″)，則通過式(3)可得：

(17)

其中δ(δMx,δMy,δMz)表示誤差，其定義為：

(18)

最終，改進(jìn)的觸點位置檢測方法可以歸納為如圖5所示的流程。

4 實驗結(jié)果及分析

本文分別通過一組隨機(jī)點檢測實驗和一組形變實驗驗證了改進(jìn)的觸點位置檢測方法的精度和有效性。其中，隨機(jī)點實驗將改進(jìn)方法的精度與傳統(tǒng)的基于幾何約束的方法進(jìn)行了比較，驗證了改進(jìn)方法精度的可靠性；形變實驗則驗證了改進(jìn)方法在碰撞物體發(fā)生形變的情況下依然有效。根據(jù)前期實驗研究[25]，系統(tǒng)所涉及的雙目立體定位的視覺測量方法精度誤差在0.2 mm以內(nèi)，因此，兩組實驗都在一套由機(jī)械臂和紅外雙目相機(jī)組成的手眼系統(tǒng)中完成，并將視覺定位的結(jié)果作為金標(biāo)準(zhǔn)。

實驗系統(tǒng)包括：六自由度機(jī)械臂1臺(UR5，Universal Robots有限公司，丹麥)；紅外立體相機(jī)1套(北極星系統(tǒng)，Northern Digital 有限公司，加拿大)；六自由度力/力矩傳感器(M3813C，日出有限公司，中國)；帶有標(biāo)記點的三維打印的樹脂立方體模型1個。在力覺定位實驗前，首先通過手—眼標(biāo)定、傳感器和工具標(biāo)定以及工件(立方體)標(biāo)定完成機(jī)械臂、立體相機(jī)、力傳感器、末端工具、立方體模型的坐標(biāo)系的統(tǒng)一，建立系統(tǒng)坐標(biāo)系，然后通過立體相機(jī)和其配套的視覺探針定位立方體上的隨機(jī)標(biāo)記點在系統(tǒng)坐標(biāo)系下的坐標(biāo)，如圖6所示。

4.1 隨機(jī)點實驗

隨機(jī)點實驗的步驟如下：

步驟1將六維力/力矩傳感器作為腕力傳感器安裝在機(jī)械臂末端關(guān)節(jié)上，然后在工具端安裝一個探針作為末端工具.

步驟2在立方體模型上隨機(jī)選取10個標(biāo)記點作為測試點。

步驟3如圖7所示，在視覺引導(dǎo)下使機(jī)械臂末端探針抵達(dá)測試點，采集碰撞過程中力傳感器的數(shù)據(jù)。

步驟4分別采用傳統(tǒng)的基于幾何約束的方法和新方法計算力覺感知定位結(jié)果。設(shè)視覺定位結(jié)果為P(x,y,z)，則傳統(tǒng)方法檢測結(jié)果為P′(x′,y′,z′)，改進(jìn)方法檢測結(jié)果為P″(x″,y″,z″)。Δ′和Δ″分別為傳統(tǒng)方法和改進(jìn)方法與視覺檢測結(jié)果相比的誤差。圖8給出了10個測試點的實驗結(jié)果。

步驟5從立方體模型3個不同方向的面上各選取2個測試點，然后用改進(jìn)的方法針對這6個測試點進(jìn)行10次重復(fù)定位實驗，用以驗證改進(jìn)方法的收斂性和魯棒性，實驗結(jié)果如圖9所示。

對比實驗結(jié)果表明，改進(jìn)方法的精度和收斂性均優(yōu)于基于幾何約束的方法。重復(fù)實驗結(jié)果的統(tǒng)計數(shù)據(jù)表明，改進(jìn)方法的精度于機(jī)械臂的位姿無關(guān)，且在3個坐標(biāo)軸上的誤差分布沒有明顯差異。

4.2 形變實驗

由于改進(jìn)的方法不用依賴碰撞物體的表面信息，即使碰撞面發(fā)生形變也不會影響檢測結(jié)果。為了驗證改進(jìn)算法在形變環(huán)境下的有效性，本文設(shè)計了一組形變實驗，具體步驟如下：

步驟1與隨機(jī)點實驗相似，將力傳感器和探針安裝于機(jī)械臂末端。

步驟2在立方體模型上選取10個測試點，運行機(jī)械臂，使探針到達(dá)測試點，繼續(xù)施力直至探針發(fā)生一定程度的形變，如圖10所示。

步驟3收集發(fā)生形變碰撞過程中的力/力矩傳感器數(shù)據(jù)，并用改進(jìn)的方法計算碰撞位置。

步驟4在每個測試點上各重復(fù)試驗10次，試驗結(jié)果如圖11所示。

5 結(jié)束語

正如人類同時具備視覺、力覺、聽覺等感知能力一樣，力覺感知也是機(jī)器人與外界環(huán)境交互所必不可少的功能。而基于力覺的碰撞位置感知，是力覺感知系統(tǒng)的重要組成部分。針對現(xiàn)有的基于腕力傳感器的觸點位置檢測方法必須依賴碰撞物體幾何信息的不足，本文系統(tǒng)建立了力學(xué)數(shù)據(jù)自約束模型，擺脫了對幾何信息的依賴，并基于該模型建立了改進(jìn)的碰撞檢測位置檢測方法，提出了一種基于最小誤差的搜索策略。實驗表明，改進(jìn)方法的檢測精度優(yōu)于基于幾何約束的方法，且精度不受碰撞力大小、機(jī)械臂位姿以及碰撞物體的形變程度等因素的影響，具有更好的魯棒性，能夠應(yīng)用于形變環(huán)境。本文所提方法為機(jī)械臂作業(yè)過程中的單觸點位置檢測提供了完整的解決方案，未來將進(jìn)一步研究多點接觸時的位置檢測問題。