機(jī)器人運(yùn)動中六維力傳感器的重力補(bǔ)償研究

王志軍，韓靜如，李占賢，劉立偉

（華北理工大學(xué) 機(jī)械工程學(xué)院，河北 唐山 063009）

1 引言

六維力傳感器能夠檢測大小和方向不斷變化的三維力和三維力矩信息，一般安裝在工業(yè)機(jī)器人末端，協(xié)助機(jī)器人完成力/位置控制、輪廓跟蹤、軸孔配合等一些精細(xì)復(fù)雜的操作，在機(jī)器人中具有廣泛應(yīng)用[1-3]。但是，六維力傳感器在隨機(jī)器人運(yùn)動的過程中，由于其位姿不斷發(fā)生變化，同時受安裝在傳感器上的操作工具重力的影響，傳感器的零位值也會隨之不斷變化，這就導(dǎo)致傳感器測量上產(chǎn)生一定偏差，降低了其測量精度，進(jìn)而影響機(jī)器人的操作。因此，有必要對機(jī)器人運(yùn)動中的六維力傳感器進(jìn)行重力補(bǔ)償。

關(guān)于不同情況下的重力補(bǔ)償，國內(nèi)一些學(xué)者也分別進(jìn)行過理論算法推導(dǎo)或相關(guān)實(shí)驗研究等，并取得了一定的研究成果。文獻(xiàn)[4]提出了基于最小二乘法的參數(shù)識別方法，可用于對機(jī)器人臂的重力補(bǔ)償；文獻(xiàn)[5]采用矢量分解的方法推導(dǎo)出一種機(jī)器人的重力補(bǔ)償算法；文獻(xiàn)[6]利用拉格朗日方程的方法對機(jī)構(gòu)進(jìn)行重力補(bǔ)償?shù)睦碚撚嬎?，?shí)現(xiàn)了對力覺交互設(shè)備的重力補(bǔ)償；文獻(xiàn)[7]通過對機(jī)器人建立動力學(xué)方程，分析得出重力補(bǔ)償可使控制器克服重力，從而使機(jī)器人到達(dá)期望位置。以上這些研究從不同出發(fā)點(diǎn)和落腳點(diǎn)，根據(jù)各自不同情況分析或推導(dǎo)了機(jī)械臂重力補(bǔ)償?shù)乃惴?、驗證及控制等問題，證明了重力補(bǔ)償在機(jī)器人操作中的重要和必要性，對重力補(bǔ)償研究具有一定的學(xué)習(xí)和借鑒意義。

針對機(jī)器人姿態(tài)變化導(dǎo)致末端工具重力影響六維力傳感器零位值的問題，在結(jié)合前人研究的基礎(chǔ)上，提出了一種簡單有效的重力補(bǔ)償算法，利用機(jī)器人各連桿變換矩陣和不同坐標(biāo)系的力坐標(biāo)變換，消除六維力傳感器測量中工具重力的影響，能夠?qū)崟r補(bǔ)償傳感器的零位值，使傳感器在機(jī)器人運(yùn)動過程中也能檢測出末端實(shí)際所受外力的大小，從而使機(jī)器人能夠完成更加精密的操作任務(wù)。

2 重力補(bǔ)償算法推導(dǎo)與分析

工業(yè)機(jī)器人在一些主動柔順作業(yè)中，機(jī)械臂的腕關(guān)節(jié)上會通過一個六維力傳感器來連接末端執(zhí)行工具，完成各項操作任務(wù)[8]。傳感器能夠測得末端工具所受的外力/力矩，進(jìn)而反饋給控制系統(tǒng)控制機(jī)器人實(shí)現(xiàn)柔順運(yùn)動。在這種情況下，為了消除工具自身重力對傳感器示數(shù)造成的影響，得到準(zhǔn)確的外力反饋信息，我們需要以機(jī)器人本體、六維力傳感器、末端工具為研究平臺，推導(dǎo)和研究相應(yīng)的補(bǔ)償算法對傳感器測量結(jié)果進(jìn)行重力補(bǔ)償。

2.1 機(jī)器人各連桿的變換矩陣

以某公司的TAWERS-1800機(jī)器人為研究對象來推導(dǎo)重力補(bǔ)償算法，該機(jī)器人共有六個關(guān)節(jié)，每個關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)。

由D-H法建立基坐標(biāo)系｛0｝及各關(guān)節(jié)坐標(biāo)系｛i｝，如圖1所示，同時定義各連桿的參數(shù)與變量，并列出連桿參數(shù)表，如表1所示。

圖1 機(jī)器人的連桿坐標(biāo)系Fig.1 Connecting Rod Coordinate System of the Robot

表1 機(jī)器人的連桿參數(shù)Tab.1 Connecting Rob Parameters of the Robot

連桿變換矩陣i-1iT的通式為：

根據(jù)表1的各連桿參數(shù)，帶入上述通式（1），可得到各連桿的位姿變換矩陣將以上各連桿變換矩陣相乘，可得到機(jī)器人腕部坐標(biāo)系相對于基坐標(biāo)的位姿變換矩陣：

2.2 重力補(bǔ)償算法的推導(dǎo)

定義傳感器坐標(biāo)系｛S｝及工具坐標(biāo)系｛T｝與空間基坐標(biāo)系｛0｝的位姿關(guān)系，如圖2所示。

圖2 機(jī)器人傳感器及末端工具坐標(biāo)系Fig.2 Sensor and Tool Coordinate System of the Robot

傳感器坐標(biāo)系｛S｝相對機(jī)器人腕部坐標(biāo)系｛6｝沿z6正方向平移距離h，工具坐標(biāo)系｛T｝與傳感器坐標(biāo)系｛S｝姿態(tài)相同，坐標(biāo)原點(diǎn) OT到坐標(biāo)系｛S｝的 x 軸，y 軸，z軸投影分別為 px，py，pz，則傳感器及末端工具與相鄰坐標(biāo)系的變換矩陣分別為：

由以上各位姿變換矩陣，最終可以得到機(jī)器人末端工具相對空間基坐標(biāo)系｛0｝的變換矩陣為：

設(shè)機(jī)器人末端工具重力的大小為G，方向沿空間基坐標(biāo)系｛0｝的z0軸負(fù)方向，重心在工具坐標(biāo)系｛T｝的坐標(biāo)原點(diǎn)，則在基坐標(biāo)系｛0｝下，重力的三維力分量矩陣恒為：0f=［0 0 G］T。

機(jī)器人運(yùn)動過程中，隨著末端工具的位置和姿態(tài)不斷改變，其重力在工具坐標(biāo)系｛T｝下的三維分量會發(fā)生變化，相應(yīng)地，傳感器的零位值也會隨之變化。因此，我們需要推導(dǎo)出末端工具重力在傳感器坐標(biāo)系｛S｝下的三維力和力矩值，即在測量機(jī)器人所受外力之前，對傳感器零位位置的重力補(bǔ)償值。

由式（4）可知工具坐標(biāo)系相對于基坐標(biāo)系的旋轉(zhuǎn)矩陣，則重力在工具坐標(biāo)系｛T｝下的三維力分量為：

因為重心在工具坐標(biāo)系｛T｝的坐標(biāo)原點(diǎn)，所以重力在｛T｝坐標(biāo)系三軸方向的力矩為0。因此，重力在｛T｝坐標(biāo)系下的六維力/力矩為：

根據(jù)文獻(xiàn)[9]中力和力矩矢量在不同坐標(biāo)系中的變換關(guān)系，可得重力在傳感器｛S｝與工具｛T｝坐標(biāo)系中的關(guān)系式為：

由此可得重力在傳感器坐標(biāo)系｛S｝下的六維力/力矩，即最終所需要的重力補(bǔ)償值為：

六維力傳感器測得的力和力矩值，是工具自身重力與其所受外力共同作用的結(jié)果。通過式（9）中求得的操作工具重力隨機(jī)器人位姿變化在六維力傳感器上的力/力矩值，即需要進(jìn)行的重力補(bǔ)償值，對傳感器測得示數(shù)進(jìn)行補(bǔ)償，能夠消除由機(jī)器人姿態(tài)變化引起的末端工具重力影響，提高六維力傳感器的測量精度。

當(dāng)機(jī)器人處于某一位姿下，末端工具受到外力時，如果六維力傳感器上力與力矩分量的讀數(shù)分別用Sfc、Smc表示，那么經(jīng)過重力補(bǔ)償，最終實(shí)際機(jī)器人末端所受外力/力矩信息f、m為：

由式（10）可以看到，通過重力補(bǔ)償，我們可以準(zhǔn)確得到機(jī)器人末端執(zhí)行工具所受的外力/力矩，將末端工具實(shí)際所受外力反饋到控制系統(tǒng)，就能控制機(jī)器人按照操作人員的意圖實(shí)現(xiàn)主動柔順操作。

2.3 重力補(bǔ)償算法的數(shù)值算例

根據(jù)上文推導(dǎo)的重力補(bǔ)償算法式（9），下面結(jié)合具體算例，運(yùn)用MATLAB軟件進(jìn)行重力補(bǔ)償理論推導(dǎo)的數(shù)值計算與分析。由TAWERS-1800機(jī)器人的結(jié)構(gòu)尺寸，可知各連桿變換所需參數(shù) a1，a2，a3，d5，a5，d6的值；設(shè)機(jī)器人末端工具重心相對傳感器坐標(biāo)系的位置為SPTO=［115-50 55］T，重力大小為100N，沿基坐標(biāo)系｛0｝的Z0軸負(fù)方向；在機(jī)器人各關(guān)節(jié)工作角度范圍內(nèi)，取θ1∈［0，-π/6］rad，θ2∈［-π/2，-2π/3］rad，θ3∈［0，-π/6］rad，θ4∈［0，-π/6］rad，θ5∈［π/2，π/3］rad，θ6∈［0，-π/6］rad 各關(guān)節(jié)轉(zhuǎn)動角速度為3rad/s。將以上參數(shù)帶入式（9），運(yùn)用MATLAB編程計算并生成重力在傳感器坐標(biāo)系x，y，z三個方向的力和力矩補(bǔ)償?shù)睦碚撚嬎阒担鐖D3所示。

圖3 重力補(bǔ)償?shù)挠嬎阒礔ig.3 The Calculated Value of Gravity Compensation

從圖中可以看出，隨著機(jī)器人的運(yùn)動，工具重力在六維力傳感器上的力和力矩隨機(jī)器人的姿態(tài)不斷變化，通過重力補(bǔ)償結(jié)果能夠?qū)αS力傳感器的零位值進(jìn)行實(shí)時補(bǔ)償，從而消除了由機(jī)器人姿態(tài)變化引起的末端工具重力影響，提高了六維力傳感器的測量精度。

3 重力補(bǔ)償仿真分析

Adams具有強(qiáng)大的運(yùn)動學(xué)和動力學(xué)仿真功能，使用該軟件可以產(chǎn)生復(fù)雜機(jī)械系統(tǒng)的虛擬樣機(jī)，真實(shí)地仿真其運(yùn)動過程，進(jìn)行相關(guān)分析[10]。運(yùn)用Adams軟件進(jìn)行重力補(bǔ)償相關(guān)仿真，目的是希望通過對機(jī)器人的運(yùn)動仿真，測得在機(jī)器人運(yùn)動中傳感器所在坐標(biāo)系的六維力/力矩值，與我們推導(dǎo)出的重力補(bǔ)償算法的理論計算值進(jìn)行比較，驗證推導(dǎo)的重力補(bǔ)償算法是否正確。

首先將簡化的TAWERS-1800機(jī)器人三維模型導(dǎo)入Adams中，如圖4所示，然后定義各零部件的材料、質(zhì)量等相關(guān)屬性，使仿真模型盡可能與TAWERS-1800機(jī)器人實(shí)際物理屬性相近，最后定義模型的各種約束及驅(qū)動，進(jìn)行運(yùn)動仿真。

圖4 機(jī)器人仿真模型Fig.4 Robot Simulation Model

按照2.3節(jié)所描述的運(yùn)動，定義機(jī)器人各關(guān)節(jié)旋轉(zhuǎn)驅(qū)動函數(shù)為：Funcition（time）=3.0d*time，設(shè)置仿真時間為 t=10s，仿真步數(shù)step=500進(jìn)行仿真，并測量傳感器坐標(biāo)系三個方向的力和力矩，生成六維力傳感器的力與力矩測量曲線，如圖5所示。

圖5 重力補(bǔ)償?shù)臏y量值Fig.5 The Measured Value of the Gravity Compensation

通過與圖3對比可知，六維力傳感器的力和力矩仿真測量曲線與重力補(bǔ)償?shù)睦碚撚嬎阒祷疽恢?，從而驗證了推導(dǎo)重力補(bǔ)償算法的正確性。

4 結(jié)論

通過對機(jī)器人、六維力傳感器及其末端執(zhí)行工具的位姿變換進(jìn)行分析，推導(dǎo)得到一種簡單有效的六維力傳感器重力補(bǔ)償算法，并運(yùn)用MATLAB、Adams軟件對機(jī)器人運(yùn)動中六維力傳感器的重力補(bǔ)償分別進(jìn)行理論計算與仿真分析，得到了末端執(zhí)行工具重力隨機(jī)器人位姿變化，實(shí)時作用在六維力傳感器上的力/力矩值曲線，通過比較可以看出重力補(bǔ)償算法的理論計算與仿真測量的結(jié)果曲線基本一致，最終驗證了該重力補(bǔ)償算法的正確性和可行性。該重力補(bǔ)償算法能夠消除由機(jī)器人姿態(tài)變化引起的末端工具重力影響，提高六維力傳感器的測量精度，實(shí)時獲得機(jī)器人運(yùn)動中實(shí)際作用在機(jī)末端執(zhí)行機(jī)構(gòu)的外力/力矩信息，進(jìn)而反饋給控制系統(tǒng)以實(shí)現(xiàn)機(jī)器人的主動柔順運(yùn)動。