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

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

1 引言

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

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

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

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

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

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

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

由D-H法建立基坐標(biāo)系｛0｝及各關(guān)節(jié)坐標(biāo)系｛i｝，如圖1所示，同時(shí)定義各連桿的參數(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)系相對(duì)于基坐標(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｝相對(duì)機(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ī)器人末端工具相對(duì)空間基坐標(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)動(dòng)過(guò)程中，隨著末端工具的位置和姿態(tài)不斷改變，其重力在工具坐標(biāo)系｛T｝下的三維分量會(huì)發(fā)生變化，相應(yīng)地，傳感器的零位值也會(huì)隨之變化。因此，我們需要推導(dǎo)出末端工具重力在傳感器坐標(biāo)系｛S｝下的三維力和力矩值，即在測(cè)量機(jī)器人所受外力之前，對(duì)傳感器零位位置的重力補(bǔ)償值。

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

因?yàn)橹匦脑诠ぞ咦鴺?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ǔ)償值為：

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

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

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

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

根據(jù)上文推導(dǎo)的重力補(bǔ)償算法式（9），下面結(jié)合具體算例，運(yùn)用MATLAB軟件進(jìn)行重力補(bǔ)償理論推導(dǎo)的數(shù)值計(jì)算與分析。由TAWERS-1800機(jī)器人的結(jié)構(gòu)尺寸，可知各連桿變換所需參數(shù) a1，a2，a3，d5，a5，d6的值；設(shè)機(jī)器人末端工具重心相對(duì)傳感器坐標(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)動(dòng)角速度為3rad/s。將以上參數(shù)帶入式（9），運(yùn)用MATLAB編程計(jì)算并生成重力在傳感器坐標(biāo)系x，y，z三個(gè)方向的力和力矩補(bǔ)償?shù)睦碚撚?jì)算值，如圖3所示。

圖3 重力補(bǔ)償?shù)挠?jì)算值Fig.3 The Calculated Value of Gravity Compensation

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

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

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

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

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

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

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

通過(guò)與圖3對(duì)比可知，六維力傳感器的力和力矩仿真測(cè)量曲線與重力補(bǔ)償?shù)睦碚撚?jì)算值基本一致，從而驗(yàn)證了推導(dǎo)重力補(bǔ)償算法的正確性。

4 結(jié)論

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