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

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

1 引言

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

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

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

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

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

2.1 機器人各連桿的變換矩陣

以某公司的TAWERS-1800機器人為研究對象來推導(dǎo)重力補償算法，該機器人共有六個關(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 機器人的連桿坐標(biāo)系Fig.1 Connecting Rod Coordinate System of the Robot

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

根據(jù)上文推導(dǎo)的重力補償算法式（9），下面結(jié)合具體算例，運用MATLAB軟件進(jìn)行重力補償理論推導(dǎo)的數(shù)值計算與分析。由TAWERS-1800機器人的結(jié)構(gòu)尺寸，可知各連桿變換所需參數(shù) a1，a2，a3，d5，a5，d6的值；設(shè)機器人末端工具重心相對傳感器坐標(biāo)系的位置為SPTO=［115-50 55］T，重力大小為100N，沿基坐標(biāo)系｛0｝的Z0軸負(fù)方向；在機器人各關(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），運用MATLAB編程計算并生成重力在傳感器坐標(biāo)系x，y，z三個方向的力和力矩補償?shù)睦碚撚嬎阒?，如圖3所示。

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

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

3 重力補償仿真分析

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

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

圖4 機器人仿真模型Fig.4 Robot Simulation Model

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

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

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

4 結(jié)論

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