基于力傳感器重力補償?shù)臋C器人柔順控制研究

黃玲濤 王 彬 倪 水 楊勁松 倪 濤

(1.吉林大學機械與航空航天工程學院，長春 130022；2.中國石油大學國家大學科技園管理委員會，東營 257091)

0 引言

隨著機器人技術的快速發(fā)展[1-3]，其應用領域也越來越廣泛。傳統(tǒng)的工業(yè)機器人大多是基于位置的運動控制，為了更好地滿足工業(yè)需求，比如完成拋光、裝配、打磨、拖動示教等[4-7]任務，需要給機器人加入力覺感知系統(tǒng)。

作為機器人的力覺感知系統(tǒng)，力傳感器是實現(xiàn)柔順控制的基礎。關于力傳感器的應用，相關學者進行了大量的理論算法推導和實踐研究。文獻[8]提出了基于最小二乘法的機器人動力學參數(shù)識別方法。文獻[9]通過機器人各連桿的變換矩陣和不同坐標系下的力分量變換，解決了工具重力對力傳感器讀數(shù)的影響，但未考慮機器人底座安裝角及力傳感器零點數(shù)據(jù)對機器人受力感知的影響。文獻[10]通過對力傳感器的零點及機器人安裝角等參數(shù)的求解，實現(xiàn)了機器人末端負載的受力感知。文獻[11]提出，采用極大似然估計法降低了力傳感器標定誤差。但文獻[10-11]只進行了機器人末端負載受力感知實驗和力傳感器的標定實驗，并未對理論進行具體的實際應用。

針對機器人運動過程中的柔順控制，國內(nèi)外學者提出了阻抗控制[12-16]理論，其具體實現(xiàn)方式主要分為兩類：阻抗控制和導納控制。阻抗控制由基于位置控制的外環(huán)與力控制的內(nèi)環(huán)構成，將位置偏差作為控制系統(tǒng)的輸入，調(diào)整機器人各關節(jié)的扭矩，從而使機器人保持適當?shù)慕佑|力。導納控制[17-18]則是基于位置控制的內(nèi)環(huán)與力控制的外環(huán)，將力偏差作為控制系統(tǒng)的輸入，調(diào)整機器人的末端位置，從而達到對力的控制效果。LEE等[19]提出，在不了解環(huán)境的情況下，通過在線改變目標剛度來調(diào)節(jié)所需的接觸力。李正義等[20]針對環(huán)境的阻尼、剛度參數(shù)未知或變化的情況,提出一種機器人自適應阻抗控制方法，但并未考慮力傳感器的測量誤差對柔順控制的影響。

本文采用對力傳感器重力補償?shù)姆椒ń档土鞲衅鲗α土氐臏y量誤差，為機器人柔順控制提供準確的受力感知。采集機器人在不同位姿下力傳感器數(shù)據(jù)，計算機器人底座安裝傾角、傳感器零點數(shù)據(jù)和末端工具重力及重心在力傳感器坐標系上的坐標，利用姿態(tài)變換矩陣，實現(xiàn)對力傳感器的重力補償。通過導納控制方法，實現(xiàn)機器人對抓取物體在重力方向上的柔順放置，以期使機器人能夠更加準確、可靠地實現(xiàn)力和位置的雙重控制。

1 理論模型建立

1.1 重力補償算法

六維力傳感器作為機器人的力覺感知系統(tǒng)，可以實時檢測大小和方向不斷變化的三維正交力(fx、fy、fz)和三維正交力矩(mx、my、mz)。六維力傳感器測得的力與力矩由3部分構成，即力傳感器的零點數(shù)據(jù)(系統(tǒng)本身誤差)、末端工具重力產(chǎn)生的力與力矩、外界環(huán)境施加的力與力矩。為了更加精準地檢測機器人末端工具所受的力與力矩，需考慮力傳感器零點及末端工具重力產(chǎn)生的誤差干擾。因為本文機器人工作在低加速工況下，所以只考慮標定工件與傳感器敏感端的重力，忽略了其造成的慣性力，在需更高精度標定等慣性力不能忽略場合，需基于Kalman Filter的動態(tài)標定[21]。本文模型計算參考機器人靜力學理論[22]。

1.1.1機器人安裝傾角與末端工具重力計算

力傳感器的零點數(shù)據(jù)記為：F0=[fx0fy0fz0]T,M0=[mx0my0mz0]T，實測的力和力矩記為：F=[fxfyfz]T,M=[mxmymz]T，末端工具重力記為G，G在力傳感器坐標系上產(chǎn)生的力和力矩記為：Gs=[gxgygz]T，Ms=[mgxmgymgz]T。本文坐標系的定義如圖1所示，定義世界坐標系為O0X0Y0Z0，令其Z0軸方向豎直向上，即重力的反方向，機器人基坐標系為O1X1Y1Z1，力傳感器坐標系為OsXsYsZs。世界坐標系O0X0Y0Z0通過先繞X0軸旋轉(zhuǎn)U，再繞當前Y軸旋轉(zhuǎn)V，即可得到機器人基坐標系O1X1Y1Z1，姿態(tài)變換矩陣可表示為

(1)

圖1 坐標系示意圖

坐標系OsXsYsZs可由坐標系O1X1Y1Z1先繞Z1軸旋轉(zhuǎn)α，再繞當前Y軸旋轉(zhuǎn)β，最后繞當前X軸旋轉(zhuǎn)γ得到，姿態(tài)變換矩陣可表示為

(2)

末端工具重力G在坐標系O0X0Y0Z0的分力表示為G0=[0 0 -G]T，通過位姿變換矩陣，可得到G在OsXsYsZs上的分力，計算公式為

(3)

力傳感器的實際讀數(shù)由兩部分組成，即工具重力在力傳感器上產(chǎn)生的力和力矩以及力傳感器的零點數(shù)據(jù)，表示為

(4)

令

(5)

式(4)可整理為

(6)

式中I——3×3的單位矩陣

選取N(N≥3)個不同姿態(tài)下的力傳感器數(shù)據(jù)，代入式(6)，可得

(7)

其可表示為F=Ra

(8)

其中

a=[axayazfx0fy0fz0]T

式(8)進一步整理得

a=(RTR)-1RTF

(9)

由式(9)可以計算出力傳感器零點的分力數(shù)據(jù)fx0、fy0、fz0及常數(shù)ax、ay、az。

由式(5)可計算出末端工具的重力為

(10)

同時可得機器人底座的安裝角U、V為

(11)

1.1.2力傳感器零點與末端工具重心坐標計算

機器人末端安裝工具后，記工具重心在坐標系OsXsYsZs上的坐標為P=[pxpypz]T，末端工具重力G在坐標系OsXsYsZs上產(chǎn)生的力與力矩的示意圖如圖2所示，其力矩計算公式為

(12)

圖2 工具重力在力傳感器坐標系上的示意圖

由式(4)可知

式(12)整理后得

(13)

令

(14)

整理式(13)得

(15)

選取N(N≥3)個不同姿態(tài)下的力傳感器數(shù)據(jù)，代入式(15)，可得

(16)

式(16)可表示為

M=Fb

(17)

其中

整理式(17)可得

b=(FTF)-1FTM

(18)

由式(18)可得末端工具重心在坐標系OsXsYsZs上的坐標px、py、pz和常數(shù)bx、by、bz。

再由式(9)和式(14)得

(19)

至此，機器人底座安裝傾角U和V、力傳感器的零點數(shù)據(jù)F0=[fx0fy0fz0]T、M0=[mx0my0mz0]T、末端工具重力G及重心坐標P=[pxpypz]T均已求出。

1.1.3重力補償值與負載重力計算

由式(5)可得工具重力在坐標系OsXsYsZs的力分量為

(20)

再由式(12)可得工具重力在OsXsYsZs上的力矩分量mgx、mgy、mgz。此時坐標系OsXsYsZs上需補償?shù)牧土乜捎洖?/p>

(21)

經(jīng)過重力補償后，機器人末端工具受到的真實接觸力和力矩為

(22)

由式(22)可計算出機器人末端工具抓取負載的真實重力，用G′表示，即

(23)

1.2 導納控制

導納控制將力作為輸入，機器人的位移或者速度作為輸出。通過對外界接觸力的檢測，控制機器人末端工具的位置和速度，其結構圖如圖3所示。圖3中，F(xiàn)0為力的期望值，F(xiàn)′為經(jīng)過重力補償后得到的接觸力，ΔF為力的偏差值，即糾正誤差。導納控制包括電納控制和電導控制。

圖3 導納控制的結構圖

電納控制輸出的是機器人位置信息，可表示為

ΔX=K1ΔF

(24)

其中

ΔX=[ΔxΔyΔzΔuΔvΔw]T

K1=diag[k11k12k13k14k15k16]

ΔF=[ΔfxΔfyΔfzΔmxΔmyΔmz]T

式中K1——ΔF對ΔX的反饋系數(shù)，為6×6對角矩陣

ΔX——機器人末端位姿的偏移量

電導控制輸出的是機器人速度信息，可表示為

ΔV=K2ΔF

(25)

其中K2=diag[k21k22k23k24k25k26]

ΔV=[ΔvxΔvyΔvzΔvuΔvvΔvw]T

式中K2——ΔF對ΔV的反饋系數(shù)，為6×6對角矩陣

ΔV——機器人末端速度的偏移量

若同時轉(zhuǎn)換為速度與位置控制則屬于導納控制，即

(26)

在完成實際任務的過程中，針對操作工件的幾何特征和具體的任務要求，在導納控制的基礎上應增加一個選擇矩陣S，即

(27)

其中，S為6×6對角矩陣，對角元素為0或1，0表示位置控制不受力偏差的影響，1表示位置控制受力偏差的影響，通過對S的合理設置，進行運動過程中力和位置的混合控制。

2 實驗及數(shù)據(jù)分析

2.1 實驗平臺

采用的力傳感器為Leptrino六維力傳感器，fx、fy、fz的測量范圍為:±250 N，mx、my、mz的測量范圍為:±6 N·m，f過載范圍為:±400%，m過載范圍為:±200%。測量誤差為：±1.5%。分辨率為：±1/2 000(DC)，輸出頻率為：1.2 kHz。實驗采用EPSON C4-A901S型六自由度機器人，機器人末端工具為一個機械手爪，手爪固定在力傳感器上，力傳感器與機器人通過法蘭聯(lián)結，采用Kinect相機作為視覺采集器，與機器人分別固定在工作環(huán)境中，實驗平臺如圖4所示。

圖4 機器人實驗平臺

2.2 實驗及統(tǒng)計分析

2.2.1力傳感器重力補償實驗及分析

為準確得到機器人底座安裝傾角和力傳感器零點數(shù)據(jù)，本文采集機器人末端在任意9種不同位姿(表1)下的力傳感器數(shù)據(jù)，每種位姿連續(xù)采集500組力傳感器數(shù)據(jù)，并對500組數(shù)據(jù)求取平均值，降低力傳感器產(chǎn)生的讀數(shù)誤差，力傳感器數(shù)據(jù)見表2，由表2知，不同位姿下力傳感器數(shù)據(jù)有明顯區(qū)別。

根據(jù)1.1.1節(jié)，對9組數(shù)據(jù)進行計算，得到機器人底座安裝傾角為U=-0.204°,V=-0.129°。末端工具重力G=3.153 N，工具重力包含了安裝在力傳感器上的手爪重為以及力傳感器敏感端的自身重力，因為力傳感器本身無法拆卸，所以不能通過稱重獲得其敏感端重力。本文拆除安裝在力傳感器上的手爪，在完全空載情況下，再次采集9種位姿下力傳感器數(shù)據(jù)，計算得力傳感器敏感端重力為0.835 N。

表1 機器人末端位姿

表2 9種位姿下力傳感器的讀數(shù)

根據(jù)1.1.2節(jié)，可得力傳感器的零點數(shù)據(jù)為

圖5 位姿連續(xù)變化時的重力補償效果

表3為力傳感器在9種不同位姿下，力與力矩的相對補償誤差，相對補償誤差是誤差與測量值絕對值的百分比。δ為9種位姿下相對補償誤差的平均值。根據(jù)表3可知，力的相對補償誤差平均值在2.98%以內(nèi)，力矩的相對補償誤差平均值在3.89%以內(nèi)，其中fy在位姿4和mx在位姿5的相對補償誤差較大，主要原因是該位姿下力傳感器測得的力與力矩較小，導致在較小的誤差下也會產(chǎn)生較大的相對誤差。

表3 重力補償誤差分析結果

為了進一步驗證重力補償算法的準確度，分別選取6種不同質(zhì)量的負載，再次進行實驗驗證。仍采用表1的9種位姿，根據(jù)1.1.3節(jié)，得到6種負載重力的計算值，然后將6種負載重力的計算值與真實值作對比。負載的真實重力通過電子秤測得的質(zhì)量乘以g(9.8 m/s2)得到，統(tǒng)計結果如表4所示，其中δG表示重力的相對誤差，δG在3.86%以內(nèi)，與之前重力補償誤差結果相近。

由于本實驗所用的力傳感器測量誤差為±1.5%，所以力傳感器自身的誤差為3%左右，這部分誤差無法消除。對比δG與δ的實驗結果可知，本文所提出的重力補償算法能夠?qū)崿F(xiàn)較好的重力補償效果，可以在機器人柔順控制時，提供更加精準的力覺感知。

表4 負載重力測量誤差

2.2.2機器人柔順放置實驗及分析

機器人柔順放置實驗，是在實現(xiàn)了力傳感器重力補償?shù)幕A上，通過力傳感器準確感知手爪抓取物體的重力，從而完成對抓取物體的柔順放置。這種控制方式可以對物體輕拿輕放，針對搬運一些易碎、怕磕碰的物體具有良好的保護措施。

實驗過程: 物體抓取系統(tǒng)[23]包括物體檢測系統(tǒng)、物體定位系統(tǒng)和機器人運動系統(tǒng)。首先，通過Kinect相機進行物體檢測，識別實驗臺上物體；然后，利用標定技術對物體定位；最后，驅(qū)動機器人完成物體的抓取與放置實驗。采用無力控和力控兩種方式進行實驗。無力控實驗是僅基于位置控制的放置實驗，力控實驗是以導納控制為基礎的柔順放置實驗。

圖6為機器人在無力控條件下位移和力隨時間的變化情況，放置平臺為海綿墊和書本。由圖6a知，物體在放置過程中，位移均勻下降；由圖6b知，物體在接近放置平臺時，力傳感器感知重力迅速減小，放置平臺為海綿墊時力傳感器最終測得力為-1.467 N；當放置平臺為書本時，力減小的速度更快，力傳感器最終測得力為-2.534 N，因為非力控實驗時，速度恒定，故不做分析。

圖6 無力控實驗位移、力隨時間的變化情況

僅基于位置控制的機器人運動系統(tǒng)，在放置過程中會產(chǎn)生較大的力沖擊信號，放置平臺質(zhì)地越硬，力變化越快，產(chǎn)生的力沖擊信號越大，無法實現(xiàn)對抓取物體的柔順放置。

在對機器人進行力控實驗時，主要研究被抓取物體的柔順放置過程，因為物體在放置過程中只在重力方向上與接觸平臺有力信息的交流，除重力方向，其他方向并沒有接觸力，故導納控制時，選擇矩陣S的設置是僅對Z軸方向進行力控制即可，即研究的對象為重力方向上的力、位移和速度的變化情況，所以本文選擇對Z軸方向的力控反饋系數(shù)做對比實驗。根據(jù)式(24)和式(25)可得，選取系數(shù)k13和k23進行研究，對比實驗采用控制變量法。

實驗過程分為無重力補償和有重力補償兩種實驗方式，根據(jù)式(23)，可準確得到抓取物體的重力。將力傳感器重力方向的期望力設置為零，考慮有測量誤差，將區(qū)間設置為±0.05 N作為判斷物體是否已經(jīng)平穩(wěn)放置的條件，放置平臺為海綿墊。圖7為力傳感器感知重力、物體位移和速度隨時間的變化情況。圖7a、7b的力控反饋系數(shù)設置：固定k23為0.1 mm/(s·N)，k13作為變量。圖7c的力控反饋系數(shù)設置：固定k13為0.1 mm/N,k23作為變量。

由圖7有重力補償時曲線可知，在物體放置過程中，力傳感器感知重力逐漸減到零時，位移和速度也逐漸減為零。力控反饋系數(shù)影響力、位移和速度的變化，力控反饋系數(shù)越大，放置時間越短，但力和速度的波動情況也越明顯，原因為接近放置平臺時，反饋系數(shù)越大的調(diào)節(jié)程度也會越大。由圖7a無重力補償時力曲線可知，力傳感器測得的力明顯偏大，主要原因為該數(shù)據(jù)包含了未補償?shù)牧鞲衅髁泓c數(shù)據(jù)和末端工具重力在力傳感器上的分力，故導致測得數(shù)據(jù)較大。由圖7b無重力補償時位移曲線可知，物體下降位移明顯偏大，主要原因為力傳感器測得的數(shù)據(jù)偏大，導致機器人對物體是否已平穩(wěn)放置判斷有誤差。實驗表明基于重力補償?shù)膶Ъ{控制能夠更加準確地實現(xiàn)機器人對物體的柔順放置。

圖7 導納控制時力、位移、速度隨時間的變化曲線

3 結束語

提出了一種基于力傳感器重力補償?shù)臋C器人柔順控制方法?？紤]機器人底座安裝傾角、力傳感器零點數(shù)據(jù)和末端工具重力在力傳感器上產(chǎn)生的力與力矩對力傳感器讀數(shù)的影響，分別完成了力傳感器重力補償實驗和機器人柔順放置實驗，并進行了數(shù)據(jù)分析及驗證。力傳感器重力補償實驗結果表明，力的相對補償誤差平均值在2.98%以內(nèi)，力矩的相對補償誤差平均值在3.89%以內(nèi)，手爪對所夾持物體重力感知的相對誤差在3.86%以內(nèi)，相對誤差較小，且可以做到對不同負載質(zhì)量進行實時性補償。由機器人柔順放置實驗中力、位移和速度的曲線可知，當力傳感器感知重力逐漸減為零時，機器人的位移和速度也逐漸減為零。實驗結果表明，將力傳感器的重力補償應用于機器人導納控制的方法能夠提高機器人對外界環(huán)境感知的精準度，實現(xiàn)更精準的機器人柔順控制。