機(jī)器人交互作業(yè)中六維力傳感器重力補(bǔ)償研究*

陳浩文，李 堅(jiān)，王 兵，張 濤，管貽生

(廣東工業(yè)大學(xué)機(jī)電工程學(xué)院，廣州 510006)

0 引言

機(jī)器人正在被越來(lái)越廣泛地應(yīng)用到制造工業(yè)中，特別是在存接觸交互的場(chǎng)景，如：工業(yè)機(jī)器人用于拋光、打磨等接觸式作業(yè)[1-4]；協(xié)作機(jī)器人與人的交互過(guò)程[5-6]。這些場(chǎng)景下機(jī)器人需要具備感知外部接觸力的能力，接觸力的感知可以通過(guò)在機(jī)器人腕部與末端執(zhí)行器間安裝六維力傳感器來(lái)實(shí)現(xiàn)，但力傳感器的零點(diǎn)會(huì)因?yàn)闇囟?、安裝預(yù)緊力等原因產(chǎn)生漂移[7]，機(jī)器人末端執(zhí)行器的重力也往往不可忽略，并且其在力傳感器中的反饋值會(huì)隨機(jī)器人姿態(tài)變化而變化，不能直接反映機(jī)器人與環(huán)境的真實(shí)接觸力。因此，為了精確獲得末端執(zhí)行器的真實(shí)受力情況，需要根據(jù)當(dāng)前機(jī)器人姿態(tài)對(duì)六維力傳感器實(shí)時(shí)地進(jìn)行零點(diǎn)校正及重力補(bǔ)償。

在對(duì)力傳感器的零點(diǎn)校正與重力補(bǔ)償?shù)难芯可?，文獻(xiàn)[7-8]采用了在特定機(jī)器人姿態(tài)下事先通過(guò)處理力傳感器的信號(hào)來(lái)獲得傳感器零點(diǎn)，再進(jìn)行重力補(bǔ)償?shù)姆绞?，但均未考慮機(jī)器人的安裝傾角。文獻(xiàn)[2,9-11]在推導(dǎo)重力補(bǔ)償模型時(shí)只考慮了末端執(zhí)行器的重力與重心，沒(méi)有考慮傳感器零點(diǎn)與機(jī)器人的安裝傾角，并且都只進(jìn)行仿真，而未進(jìn)行實(shí)驗(yàn)驗(yàn)證。文獻(xiàn)[12]在設(shè)計(jì)重力補(bǔ)償算法的過(guò)程中，考慮了力傳感器的安裝角度，但未考慮機(jī)器人的安裝傾角。文獻(xiàn)[13-15]綜合考慮了傳感器零點(diǎn)、機(jī)器人安裝傾角、負(fù)載重力、負(fù)載重心，就六維力傳感器的重力補(bǔ)償提出了一套標(biāo)定與計(jì)算方法。文獻(xiàn)[16-17]使用傾角傳感器來(lái)獲得傳感器坐標(biāo)系姿態(tài)，因而無(wú)需考慮機(jī)器人的安裝傾角，但使用傾角傳感器增加了硬件成本，也未涉及傳感器零點(diǎn)問(wèn)題。

基于此，本文提出一種零點(diǎn)校正及重力補(bǔ)償?shù)姆椒?，將機(jī)器人的安裝誤差、力傳感器的零點(diǎn)漂移以及末端執(zhí)行器重力補(bǔ)償?shù)葐?wèn)題綜合考慮。不同于上述文章所提方法的繁雜過(guò)程，本文所提的方法只需要在至少3個(gè)隨機(jī)機(jī)器人姿態(tài)下采集力傳感器數(shù)據(jù)，即可通過(guò)計(jì)算獲得補(bǔ)償所需的參數(shù)，從而實(shí)現(xiàn)對(duì)任意機(jī)器人位姿下的六維力傳感器測(cè)量數(shù)據(jù)進(jìn)行零點(diǎn)校正和重力補(bǔ)償。通過(guò)一系列實(shí)驗(yàn)對(duì)本文方法的測(cè)量和補(bǔ)償精度進(jìn)行驗(yàn)證，將本文方法應(yīng)用于機(jī)器人拋磨中的恒接觸力控制，進(jìn)一步說(shuō)明了本文方法的有效性和可行性。

1 理論模型構(gòu)建

1.1 接觸模型構(gòu)建

接觸力作為拋磨工藝的核心參數(shù)，在機(jī)器人拋磨中，實(shí)現(xiàn)穩(wěn)定的接觸力控制是目前機(jī)器人拋磨研究中的重點(diǎn)[2, 17]。本文采用在機(jī)器人腕部與末端執(zhí)行器之間安裝力傳感器，當(dāng)拋磨工具與工件表面接觸時(shí)，其接觸狀態(tài)及受力分析如圖1所示。在機(jī)器人拋磨作業(yè)中，進(jìn)給速度較為穩(wěn)定，慣性力的影響可忽略不計(jì)[2, 9, 18]。因此，可以認(rèn)為力傳感器的測(cè)量值只由力傳感器零點(diǎn)、末端執(zhí)行器的重力以及外部接觸力組成。因此，為了準(zhǔn)確獲得接觸力，需要進(jìn)行零點(diǎn)校正及重力補(bǔ)償，以從力傳感器原始信號(hào)中消除零點(diǎn)漂移和末端執(zhí)行器重力的影響。

圖1 機(jī)器人接觸受力示意圖

1.2 零點(diǎn)校正及重力補(bǔ)償

為了對(duì)安裝于機(jī)器人腕部的六維力傳感器進(jìn)行重力補(bǔ)償，對(duì)末端執(zhí)行器所受重力及外部作用力進(jìn)行分析。首先，如圖2所示，在機(jī)器人基座上建立機(jī)器人基坐標(biāo)系{B}，在機(jī)器人腕部建立腕部坐標(biāo)系{W}，在力傳感器上建立傳感器坐標(biāo)系{S}。末端執(zhí)行器受自身重力G，記其在坐標(biāo)系{B}和{S}下的坐標(biāo)分別為BG和SG，記末端執(zhí)行器重心在坐標(biāo)系{S}下的坐標(biāo)為l，末端執(zhí)行器受外部接觸力和力矩為Fr和Tr。力傳感器的測(cè)量值由傳感器零點(diǎn)、外部接觸力和末端執(zhí)行器的重力組成，將力傳感器的測(cè)量值記為F和T，力傳感器零點(diǎn)記為F0和T0。

圖2 坐標(biāo)系與受力分析示意圖

據(jù)此，可以列出如下方程：

Fr=F-F0-SG

(1)

Tr=T-T0-l×SG

(2)

(3)

(4)

由于機(jī)器人基座是固定的，因此在機(jī)器人運(yùn)動(dòng)過(guò)程中BG是常量，注意到，如圖2所示，由于存在機(jī)器人安裝傾角，BG與坐標(biāo)系{B}負(fù)z軸存在夾角，該夾角體現(xiàn)了機(jī)器人安裝傾角?？梢园l(fā)現(xiàn)，只需求得F0、T0、l和BG(本文將稱之為補(bǔ)償參數(shù))，即可由式(1)、式(2)和式(4)進(jìn)行零點(diǎn)校正及重力補(bǔ)償，從而求得末端執(zhí)行器的真實(shí)受力。

為求解補(bǔ)償參數(shù)，現(xiàn)使末端執(zhí)行器僅受自身重力，而不受其他外力的作用，則式(1)、式(2)可簡(jiǎn)化為：

F-F0-SG=0

(5)

T-T0-l×SG=0

(6)

將式(4)代入式(5)并移項(xiàng)，得：

(7)

(8)

至少需要3個(gè)不同的姿態(tài)，才能保證式(8)有解，將式(8)記為：

f=Ag

(9)

式中，f是3n維列向量，A是形狀為3n×6的矩陣，g是6維列向量。式(9)是一個(gè)超定方程組，可以使用最小二乘法來(lái)求解。為此，將式(9)兩邊左乘AT，再左乘(ATA)-1，得：

而根據(jù)挫折理論，面對(duì)經(jīng)營(yíng)失敗，尤其是一而再的經(jīng)營(yíng)失敗，教師要引導(dǎo)學(xué)生以積極的態(tài)度對(duì)待挫折，將消極性行為轉(zhuǎn)化為積極性、建設(shè)性行為。

(10)

即可求得g，也即補(bǔ)償參數(shù)BG及F0。

對(duì)式(6)移項(xiàng)變換為：

T=l×SG+T0

(11)

(12)

式(11)可寫(xiě)成：

(13)

(14)

同樣需要至少3個(gè)不同的機(jī)器人姿態(tài)才能使式(14)有解，將式(14)記為：

t=Bh

(15)

式中，t是3n維列向量，B是形狀為3n×6的矩陣，h是6維列向量。將式(15)兩邊左乘BT，再左乘(BTB)-1，得：

(16)

1.3 顯式恒力控制

為解決通過(guò)調(diào)整機(jī)器人位置實(shí)現(xiàn)接觸力控制所帶來(lái)的低帶寬導(dǎo)致出現(xiàn)欠拋和過(guò)拋的問(wèn)題，開(kāi)發(fā)了一款主動(dòng)力控制拋磨末端執(zhí)行器，具有高帶寬、低慣量的特性[19]，其結(jié)構(gòu)如圖3所示。

圖3 主動(dòng)力控制拋磨末端執(zhí)行器

末端執(zhí)行器使用音圈電機(jī)作為驅(qū)動(dòng)單元，音圈電機(jī)的輸出力與輸入電流成正比。假設(shè)經(jīng)重力補(bǔ)償后獲得末端執(zhí)行器主軸方向的實(shí)際接觸力為Fc、期望接觸力為Fd，則力跟蹤誤差為：

Fe=Fd-Fc

(17)

為了消除誤差Fe，可以通過(guò)PID算法構(gòu)造如下的反饋控制：

(18)

式中，u為控制信號(hào)，也即控制器輸出到音圈電機(jī)驅(qū)動(dòng)器的控制電流，Km為音圈電機(jī)電流常數(shù)，Kp、Ki、Kd分別為PID控制器的比例、積分、微分增益系數(shù)，相對(duì)應(yīng)的控制框圖如圖4所示。

圖4 顯式恒接觸力控制框圖

2 實(shí)驗(yàn)與分析

2.1 實(shí)驗(yàn)平臺(tái)與設(shè)備

本文實(shí)驗(yàn)使用Nachi公司生產(chǎn)的MC20-01型工業(yè)機(jī)器人，額定負(fù)載為20 kg，重復(fù)定位精度為±0.06 mm。力傳感器采用了ATI公司型號(hào)為Mini45 IP65的六維力傳感器，該傳感器關(guān)鍵參數(shù)見(jiàn)表1。

表1 ATI Mini45 IP65六維力傳感器技術(shù)參數(shù)

同時(shí)，使用本課題組開(kāi)發(fā)的用于機(jī)器人拋磨的末端執(zhí)行器進(jìn)行重力補(bǔ)償實(shí)驗(yàn)，實(shí)驗(yàn)場(chǎng)景如圖5所示。

圖5 實(shí)驗(yàn)場(chǎng)景

2.2 重力補(bǔ)償實(shí)驗(yàn)與分析

(19)

表2 實(shí)驗(yàn)中隨機(jī)選取的機(jī)器人姿態(tài)

通過(guò)以太網(wǎng)從六維力傳感器采集設(shè)備中讀取每個(gè)機(jī)器人姿態(tài)下的六維力信息。采集設(shè)備的采樣頻率被為1000 Hz，在每個(gè)姿態(tài)下，均采集1000個(gè)數(shù)據(jù)求平均值作為當(dāng)前姿態(tài)下的讀數(shù)，如表3所示。

表3 各個(gè)姿態(tài)下的六維力傳感器測(cè)量值

使用上述測(cè)得的機(jī)器人姿態(tài)及六維力傳感器數(shù)據(jù)，根據(jù)1.2節(jié)的方法進(jìn)行計(jì)算，得到各項(xiàng)補(bǔ)償參數(shù)，如表4所示。

表4 零點(diǎn)校正及重力補(bǔ)償參數(shù)

2.2.1 靜態(tài)下的重力補(bǔ)償

在表2之外另選的5個(gè)姿態(tài)下分別使用0.5 kg、1 kg、1.5 kg、2 kg、2.5 kg、3 kg質(zhì)量的標(biāo)準(zhǔn)砝碼懸掛于末端執(zhí)行器受力點(diǎn)以及不懸掛砝碼，共設(shè)計(jì)35組實(shí)驗(yàn)。在每一組實(shí)驗(yàn)中讀取六維力信息，并使用上述求得的補(bǔ)償參數(shù)對(duì)測(cè)量值進(jìn)行補(bǔ)償。同時(shí)取廣州地區(qū)重力加速度g=9.783 3 m/s2計(jì)算砝碼產(chǎn)生的重力和重力矩作為真實(shí)值。為了分析本文方法的精確程度，計(jì)算實(shí)驗(yàn)結(jié)果的相對(duì)誤差，由于六維力傳感器的負(fù)載包含了末端執(zhí)行器與砝碼的質(zhì)量，本文將測(cè)量誤差除以傳感器的總負(fù)載來(lái)計(jì)算相對(duì)誤差：

(20)

(21)

式中，F(xiàn)m、Tm分別是經(jīng)補(bǔ)償后的測(cè)量值，F(xiàn)r、Tr分別是力、力矩真實(shí)值，G是末端執(zhí)行器重力，l是末端執(zhí)行器重心坐標(biāo)，lt是砝碼懸掛點(diǎn)坐標(biāo)，以上變量均在坐標(biāo)系{S}下表示。實(shí)驗(yàn)誤差分布如圖6所示，力和力矩平均誤差分別為2.07%和1.35%。對(duì)比圖6左右結(jié)果來(lái)看，力補(bǔ)償誤差基本在平均值2.07%附近波動(dòng)；而力矩補(bǔ)償誤差會(huì)隨著懸掛砝碼質(zhì)量增大而增大，這可能是由于工具坐標(biāo)系的標(biāo)定誤差，放大了砝碼重力的力矩誤差所致。

圖6 零點(diǎn)校正及重力補(bǔ)償實(shí)驗(yàn)誤差

2.2.2 動(dòng)態(tài)下的重力補(bǔ)償

為了進(jìn)一步驗(yàn)證在機(jī)器人運(yùn)動(dòng)速度較低的情況下慣性力對(duì)補(bǔ)償效果的影響，在末端執(zhí)行器懸空不接觸工件的情況下，控制機(jī)器人在在三個(gè)姿態(tài)間連續(xù)運(yùn)動(dòng)，其中姿態(tài)1為(r=75.70°、p=-42.79°、y=126.23°)、姿態(tài)2為(r=144.62°、p=-67.87°、y=77.70°)、姿態(tài)3為(r=109.40°、p=-89.92°、y=70.66°)，記錄此過(guò)程的重力補(bǔ)償前后的力和力矩值，結(jié)果如圖7所示，可以看到補(bǔ)償后的力和力矩均在0附近，整個(gè)過(guò)程的力和力矩相對(duì)補(bǔ)償誤差分別為1.50%和0.75%。實(shí)驗(yàn)結(jié)果表明本文重力補(bǔ)償方法在機(jī)器人運(yùn)動(dòng)過(guò)程中也能獲得靜態(tài)測(cè)試下同樣的效果，進(jìn)一步驗(yàn)證了本文在分析過(guò)程中忽略機(jī)器人在平穩(wěn)運(yùn)動(dòng)中的慣性力是合理的。

圖7 運(yùn)動(dòng)過(guò)程中的重力補(bǔ)償效果

2.2.3 恒力控制實(shí)驗(yàn)與分析

為了進(jìn)一步檢驗(yàn)本文方法的實(shí)用性，將本文的零點(diǎn)校正及重力補(bǔ)償方法應(yīng)用于機(jī)器人拋磨中的恒力控制。使末端執(zhí)行器以不同姿態(tài)接觸工件，使用PID控制器對(duì)末端執(zhí)行器主軸方向上的接觸力進(jìn)行控制，使其維持在期望值附近。實(shí)驗(yàn)分別在兩個(gè)不同的機(jī)器人姿態(tài)下進(jìn)行，其中姿態(tài)1為(r=-9.81°、p=-44.16°、y=-125.20°)，姿態(tài)2為(r=-56.01°、p=-64.70°、y=-71.17°)。在每個(gè)姿態(tài)下，分別進(jìn)行兩組實(shí)驗(yàn)，其中一組使用的力信號(hào)經(jīng)過(guò)本文零點(diǎn)校正及重力補(bǔ)償方法進(jìn)行處理(經(jīng)重力補(bǔ)償)，而另一組實(shí)驗(yàn)只在初始姿態(tài)下將力傳感器信號(hào)偏置為0(未經(jīng)重力補(bǔ)償)，每組實(shí)驗(yàn)各進(jìn)行以5 N到20 N、30 N、40 N的階躍期望力的力控制。

對(duì)比如圖8所示的未經(jīng)/經(jīng)過(guò)重力補(bǔ)償?shù)牧刂平Y(jié)果可知，本文所提方法能在不同姿態(tài)和不同目標(biāo)接觸力下有效補(bǔ)償重力，使得機(jī)器人拋磨系統(tǒng)可以有效實(shí)現(xiàn)恒力控制。而未經(jīng)重力補(bǔ)償?shù)膶?shí)驗(yàn)中，力傳感器無(wú)法給機(jī)器人系統(tǒng)提供有效的接觸力反饋，恒力控制效果較差。特別是在姿態(tài)1下機(jī)器人系統(tǒng)難以跟蹤較小的目標(biāo)接觸力且在階躍響應(yīng)后出現(xiàn)了比經(jīng)過(guò)重力補(bǔ)償后的力控制較大的超調(diào)量，這嚴(yán)重影響了機(jī)器人系統(tǒng)進(jìn)行恒力控制的效果。其原因即是未經(jīng)重力補(bǔ)償?shù)牧鞲衅魉鶞y(cè)得的法向接觸力包含了末端執(zhí)行器重力沿接觸法向的分量，不能反映拋磨工具的實(shí)際受力情況。

圖8 力控制效果對(duì)比

3 結(jié)論

針對(duì)機(jī)器人接觸作業(yè)等需要精確感知外部接觸力的應(yīng)用場(chǎng)景，對(duì)安裝于機(jī)器人腕部與末端執(zhí)行器之間的六維力傳感器受力進(jìn)行分析，建立了六維力傳感器的零點(diǎn)校正及重力補(bǔ)償計(jì)算模型，在使用最少3個(gè)機(jī)器人姿態(tài)信息及其對(duì)應(yīng)的力傳感器數(shù)據(jù)即可計(jì)算出零點(diǎn)校正及重力補(bǔ)償所需的參數(shù)。本文綜合考慮了傳感器的零點(diǎn)漂移、機(jī)器人安裝誤差、末端執(zhí)行器重力等影響力傳感器測(cè)量數(shù)據(jù)精度的因素，有效消除了力傳感器零點(diǎn)漂移、機(jī)器人安裝誤差以及末端執(zhí)行器重力對(duì)力傳感器測(cè)量結(jié)果的影響。實(shí)驗(yàn)結(jié)果表明，使用本方法進(jìn)行補(bǔ)償后，感知接觸力和力矩的平均相對(duì)誤差分別可達(dá)到2.07%和1.35%。同時(shí)，實(shí)驗(yàn)結(jié)果也顯示機(jī)器人在不同位姿連續(xù)運(yùn)動(dòng)的過(guò)程中，本文方法也能有良好的補(bǔ)償效果，在實(shí)驗(yàn)中力和力矩的相對(duì)補(bǔ)償誤差分別為1.50%和0.75%。此外，本文方法應(yīng)用于機(jī)器人拋磨中的恒力控制時(shí)，有效地提高力控精度的同時(shí)，驗(yàn)證了本文方法的正確性、實(shí)用性。