李 根，李鵬程，吳 超，沈 燁

（南京航空航天大學(xué)機電學(xué)院，南京 210016）

隨著新型航空裝配對裝配精度以及研制周期的要求越來越高，工業(yè)機器人應(yīng)用在航天裝備自動化裝配的場合越來越多[1–2]。在利用機器人進行裝配任務(wù)時，有很多場合是機器人與人在共享的工作空間中協(xié)作完成復(fù)雜的任務(wù)，即人機協(xié)作，而外力感知是實現(xiàn)人機協(xié)作的關(guān)鍵技術(shù)之一[3]。

在現(xiàn)階段，對于機器人裝配，通常是在負(fù)載與工業(yè)機器人末端法蘭之間安裝六維力傳感器（下文簡稱為力傳感器），以實現(xiàn)機器人的力反饋控制。力傳感器的主要功能是感知作用在其敏感端的力及力矩并使之三維正交化[4]，機器人在靜止?fàn)顟B(tài)下時，力傳感器的測量數(shù)值由3部分組成：負(fù)載重力、力傳感器零點和施加外力。負(fù)載的存在會干擾對外力的感知，對于人機協(xié)作而言，從測量數(shù)據(jù)中得到精準(zhǔn)外力信息格外重要，因此需要對安裝在力傳感器上的負(fù)載進行重力補償。

在機器人末端負(fù)載重力補償方面，Stavros[5]從力傳感器測量值的組成出發(fā)，通過將機器人末端移動到特定位置來標(biāo)定力傳感器的質(zhì)心和重量；Kim等[6]提出了在重力補償之前對六維力傳感器進行回零處理，從而消除零點漂移誤差；Kubela等[7]提出了利用機器人姿態(tài)來計算負(fù)載重力作用在六維力傳感器各方向的分力，并通過KUKA公司的RSI通訊進行了實時負(fù)載重力補償；張光輝等[8]在機器人運動學(xué)的基礎(chǔ)上，將力傳感器信息和負(fù)載信息轉(zhuǎn)換至基坐標(biāo)系求解；繆新等[9]將力傳感器坐標(biāo)系和法蘭坐標(biāo)系分開考慮，通過最優(yōu)解的方式，利用多組機器人姿態(tài)測量數(shù)據(jù)，計算出力傳感器偏角；高強等[10]先在機器人原點測量負(fù)載重力，然后在機器人運動時，利用基坐標(biāo)與力傳感器的旋轉(zhuǎn)矩陣，將負(fù)載重力轉(zhuǎn)換至力傳感器坐標(biāo)系進行求解，從而抵消負(fù)載重力；張立建等[11]通過最小二乘法，利用不少于3個隨機機器人姿態(tài)的力傳感器數(shù)值，分別計算出傳感器零點、負(fù)載重力重心、機器人安裝傾角。

其中，文獻[5–10]都是假設(shè)世界坐標(biāo)系與機器人基坐標(biāo)系重合，即重力方向為機器人的Z軸，但在實際情況下，例如安裝在AGV上的機器人，其基坐標(biāo)系并不一定與世界坐標(biāo)系平行。而且，文獻[7–10]未考慮力傳感器的零點漂移，默認(rèn)力傳感器的初始值為0，這與事實不符。雖然文獻[5–6]考慮了零點漂移問題，但它們未考慮在每次安裝負(fù)載后，力傳感器零點都會改變，如果每次都按照文獻中的方法進行標(biāo)定，標(biāo)定時間過長。文獻[11]默認(rèn)力傳感器坐標(biāo)系與法蘭坐標(biāo)系平行，這對連接件的制造精度有非常高的要求，但如果遇到如文獻[9]中力傳感器與法蘭安裝誤差角度過大的情況，則會出現(xiàn)很大誤差。

綜上所述，在現(xiàn)有的關(guān)于負(fù)載重力補償?shù)难芯恐?，大多默認(rèn)機器人基坐標(biāo)系Z軸與重力方向相同，但在實際情況中，往往需要考慮基坐標(biāo)系與世界坐標(biāo)系的關(guān)系。在進行補償時，應(yīng)該綜合考慮力負(fù)載重力、傳感器零點漂移、機器人安裝傾角、力傳感器安裝偏角。對于航空航天裝配系統(tǒng)而言，由于大多都是大負(fù)載，為了使機器人得到更加精準(zhǔn)的外力感知能力，應(yīng)該盡可能使補償效果達到最好。

本文研究一種工業(yè)機器人末端負(fù)載重力補償優(yōu)化方法，通過分析機器人系統(tǒng)各部分之間的位置關(guān)系，建立重力補償模型，考慮力傳感器安裝偏角對補償結(jié)果的影響，以誤差平方和最小為目標(biāo)建立最優(yōu)解模型，利用遺傳算法進行求解，同時在測量時設(shè)置了特定姿態(tài)來減少系統(tǒng)誤差，最終可以在不使用測量儀器的情況下，估計力傳感器安裝偏角，從而提高重力補償精度。

重力補償計算模型建立

本文的重力補償模型一部分參考了文獻[11]。

1 坐標(biāo)系關(guān)系建立

機器人系統(tǒng)由機器人本體、力傳感器、負(fù)載組成。設(shè)世界坐標(biāo)系為Ow–XwYwZw，機器人坐標(biāo)系為Or–XrYrZr，法蘭坐標(biāo)系為Of–XfYfZf，力傳感器坐標(biāo)系為Os–XsYsZs，它們之間關(guān)系如圖1所示。

假設(shè)世界坐標(biāo)系的Zw方向與重力方向平行且反向。

規(guī)定以下轉(zhuǎn)換關(guān)系：

（1）Os–XsYsZs相對于Of–XfYfZf的姿態(tài)轉(zhuǎn)換矩陣為RZ（γ）。

（2）Of–XfYfZf相對于Or–XrYrZr的姿態(tài)轉(zhuǎn)換矩陣為RZ（C）。

（3）Or–XrYrZr相對于Ow–XwYwZw的姿態(tài)轉(zhuǎn)換矩陣為RY（ω）。

其中，R為3×3的旋轉(zhuǎn)矩陣。的設(shè)置是為了表示機器人安裝平面與地面的安裝傾角，一般只存在于Ow–XwYwZw的XwZw平面和YwZw平面，因此不考慮Os–XsYsZs相對于Ow–XwYwZw的Zw軸旋轉(zhuǎn)。

2 力傳感器與負(fù)載關(guān)系

首先假設(shè)無外力，以力傳感器坐標(biāo)系為基準(zhǔn)分解負(fù)載重力，如圖2所示。

從圖2中可以得到：

圖1 坐標(biāo)系關(guān)系Fig.1 Coordinate system relationship

圖2 力傳感器坐標(biāo)系與負(fù)載關(guān)系Fig.2 Relationship between force sensor coordinate system and load

將式（2）代入式（1）并整理成矩陣形式可得式（3）。

其中，d1=Tx0+Fy0×z–Fz0×y，d2=Ty0+Fz0×x–Fx0×z，d3=Tz0+Fx0×y–Fy0×x。

可簡寫為：

通過改變N次不共面的機器人姿態(tài)（N≥3），在機器人穩(wěn)定后，讀取力傳感器數(shù)值，得到N組力傳感器測量數(shù)據(jù),整理可得：

式（5）兩邊同時左乘ΦT可得：

由此可以算出負(fù)載在力傳感器坐標(biāo)系下的坐標(biāo)（x,y,z）以及d1,d2,d3。

3 力傳感器與機器人本體關(guān)系

重力在世界坐標(biāo)系Ow–XwYwZw中的方向向量為gw=[0 0–1]T,則重力在力傳感器坐標(biāo)系Os–XsYsZs中可以表示為：

其中，k1=Gcosθsinω,k2=–Gsinθ,k3=–Gcosθcosω。

由式（2）可得：

可簡寫為:

其中，K=[k1k2k3Fx0Fy0Fz0]T。根據(jù)前文得到的N組測量數(shù)據(jù)，整合可得：

式（10）兩邊同時左乘RT得：

由此可以算出k1、k2、k3、Fx0、Fy0、Fz0。

進而可得

至此，若力傳感器安裝偏角α、β、γ已知，可將負(fù)載在力傳感器坐標(biāo)系下的坐標(biāo)（x，y，z），力傳感器零點[Fx0Fy0Fz0Tx0Ty0Tz0]T，負(fù)載重量，機器人底座傾角θ=arcsin（–ky/G），ω=arctan（–kx/kz）全部得出。

4 力傳感器與法蘭的關(guān)系

連接件與法蘭、力傳感器連接處一般通過定位銷定位，用螺栓連接，只要連接件的機械加工精度達到要求，一般認(rèn)為法蘭坐標(biāo)系Of–XfYfZf和力傳感器坐標(biāo)系Os–XsYsZs平行，即α=β=γ=0。但有時由于加工誤差、安裝誤差等因素，往往會出現(xiàn)或大或小的傳感器安裝偏角，而這些角度很難測得。因此，為了獲取更加準(zhǔn)確的傳感器安裝偏角并使重力補償效果提升，對重力補償算法進行了優(yōu)化。

重力補償優(yōu)化算法研究

1 重力補償誤差分析

本文的重力補償算法是建立在最小二乘法的基礎(chǔ)之上的，而最小二乘法的目的是使所求數(shù)據(jù)與實際數(shù)據(jù)之間的誤差平方和最小[12]，因此誤差平方和是判定所求數(shù)據(jù)是否更貼近于實際的指標(biāo)。當(dāng)無外力時，對于第i個（1≤i≤N）測量姿態(tài)有

其中，δxi、δyi、δzi、δai、δbi、δci、為第i個測量姿態(tài)下的重力補償誤差，F(xiàn)xi、Fyi、Fzi、Txi、Tyi、Tzi為第i個測量姿態(tài)下的實際測量值（即力傳感器數(shù)值），Gxi、Gyi、Gzi、和Tgxi、Tgyi、Tgzi可分別由式（7）和式（1）得到。從前文的重力補償模型可以看出，重力補償誤差與負(fù)載重量G有密切的關(guān)系，而負(fù)載重量G與六維力傳感器的測量力值相關(guān)。因此本文的優(yōu)化模型只考慮測量力值。

令

其中，S為測量誤差的平方和，根據(jù)所述算法可以發(fā)現(xiàn)，它是一個關(guān)于α、β、γ的函數(shù)，若α、β、γ已知，誤差平方和S便可以求出。

2 優(yōu)化模型建立

通過仿真發(fā)現(xiàn)，α、β、γ越接近實際轉(zhuǎn)角，誤差的平方和會越小，且重力補償效果會越好。因此，可以利用尋找最優(yōu)解的方法，找出誤差的平方和最小時的α、β、γ值，將其設(shè)為預(yù)測值，步驟如下。

α、β、γ的值由兩部分組成，可以表示為：

其中，α0、β0、γ0為傳感器偏角的初始預(yù)估值，Δα、Δβ、Δγ為傳感器偏角的優(yōu)化值。

如果連接件的加工誤差不大，Of–XfYfZf和Os–XsYsZs的XY平面幾乎重合，因此β0、γ0一般設(shè)置為0°。α0可以通過安裝傳感器時法蘭X正方向與傳感器X正方向的關(guān)系設(shè)置，如果同向，α0可以設(shè)置為0°；如果反向，α0可以設(shè)置為±180°，以此類推。

給Δα、Δβ、Δγ設(shè)置一個范圍，即

其中，lα、lβ、lγ分別為Δα、Δβ、Δγ的下界，uα、uβ、uγ分別為Δα、Δβ、Δγ的上界。范圍的選取與初始預(yù)估值的準(zhǔn)確度有關(guān)，如果初始預(yù)估值較為準(zhǔn)確（例如法蘭X正方向與傳感器X正方向幾乎重合），可以選取在一個較小的范圍；相反，如果初始預(yù)估值無法準(zhǔn)確預(yù)估（例如傳感器與法蘭之間隨機安裝），可以選取一個較大的范圍。Δα、Δβ、Δγ的范圍影響遺傳算法的計算效率及準(zhǔn)確度。

那么優(yōu)化問題可以描述為：

本文采用遺傳算法對優(yōu)化問題進行求解。

由于求解的是目標(biāo)函數(shù)最小值的優(yōu)化問題，所以應(yīng)對原始的目標(biāo)函數(shù)進行變換：

其中，Cmax為參考系數(shù)，根據(jù)試驗經(jīng)驗，一般取10左右；0為遺傳算法中的個體。

個體編碼采用浮點數(shù)編碼方法。假設(shè)lα=lβ=lγ= –1，uα=uβ=uγ=1，由于染色體X的初始值是隨機的，則

其中，xα，xβ，xγ∈[–1.000，1.000]，精確到小數(shù)點后3位，比如，X的表現(xiàn)型可以是[–0.061，0.123，–0.950]T。

使用乘冪尺度變換的方法進行適應(yīng)度評價，即

其中，F(xiàn)′表示變換后的適應(yīng)度，k為調(diào)整參數(shù)，F(xiàn)是步驟(1)中的變換目標(biāo)函數(shù)。

選擇算子采用比例選擇方法，變異算子采用均勻變異的方法，交叉算子選擇每個基因的相連處。群體大小M、交叉概率pc（0.4～0.99)、變異概率pm（0.0001～0.1）、終止代數(shù)T（100～1000）根據(jù)Δα、Δβ、Δγ的取值范圍具體調(diào)整，其中群體大小M可以選取較大的數(shù)。由于篇幅原因，在此不進行具體描述。

通過遺傳算法算出最優(yōu)解，設(shè)選取的值為Δαc、Δβc、Δγc（用下標(biāo)c表示其已知），使α=α0+Δαc，β=β0+Δβc，γ=γ0+Δγc，得到預(yù)估的傳感器安裝偏角。

試驗驗證

1 試驗條件

試驗采用KUKA公司的KR210–2700機器人，其主要參數(shù)見表1。六維力傳感器采用ATI公司的Gamma IP60型傳感器，其測量范圍及精度參數(shù)見表2。

試驗用的力傳感器負(fù)載重約12kg，試驗實物如圖3所示。

2 試驗過程及結(jié)果

圖3 重力補償試驗平臺Fig.3 Gravity compensation experiment platform

表1 KR210–2700機器人技術(shù)參數(shù)Table 1 KR210–2700 robot technical parameters

表2 Gamma IP60 F/T力傳感器技術(shù)參數(shù)Table 2 Gamma IP60 F/T force sensor technical parameters

表3 機器人測量姿態(tài)Table 3 Robot measuring attitude

由于最小二乘法對于個體數(shù)據(jù)的誤差比較敏感，理論上是測量數(shù)據(jù)越多，優(yōu)化結(jié)果越好，而獲取過多的測量數(shù)據(jù)會花費較多的時間。因此為了提高優(yōu)化效果，并避免測量時間冗長，選取對稱機器人姿態(tài)作為測量姿態(tài)，如表3所示。這樣設(shè)置的好處在于，不僅可以充分利用到每個軸的數(shù)據(jù)，還可以避免一些不必要的誤差。例如，由于傳感器的分辨率，當(dāng)在測量姿態(tài)1狀態(tài)下時，可能Z軸測得的負(fù)載重量會多一個或少一個分辨率單位的誤差，這就會導(dǎo)致計算結(jié)果偏大或者是偏??；而在測量姿態(tài)14狀態(tài)下，Z軸測得的數(shù)據(jù)會反方向多一個或少一個分辨率單位的誤差，根據(jù)最小二乘法原理，可使計算結(jié)果更加準(zhǔn)確。

因為本文的重點在于驗證預(yù)估力傳感器安裝偏角的有效性，對于不同負(fù)載的重力補償效果在此不進行贅述。考慮兩種情況：

（1）力傳感器安裝失誤（力傳感器X正方向與法蘭X正方向反向）；

（2）力傳感器安裝正確（力傳感器X正方向與法蘭X正方向幾乎重合）。

為了更好說明本文方法的有效性，兩種情況均不使用定位銷，從而使傳感器安裝偏角存在隨機性。下文簡稱這兩種情況為情況（1）和情況（2）。

分別進行試驗，每次試驗依次調(diào)整機器人到表3所列的14個姿態(tài)，每個姿態(tài)下，連續(xù)采集1000組傳感器數(shù)據(jù)，求其平均值作為計算參考數(shù)據(jù)，表4和表5為試驗數(shù)據(jù)。

在情況（1）時，設(shè)置傳感器安裝偏角初始預(yù)估值為[180°，0°，0°]；在情況（2）時，設(shè)置傳感器安裝偏角初始預(yù)估值為[0°，°0，°0]。首先分別通過優(yōu)化前的重力補償算法得到安裝后的負(fù)載重力、負(fù)載重心、傳感器零點、機器人安裝偏角。之后利用遺傳算法，求解最優(yōu)解，并將最優(yōu)解代入算法算出補償后的重力補償數(shù)據(jù)，得到優(yōu)化前后的數(shù)據(jù)，如表6所示。

表4 安裝失誤時力傳感器測量數(shù)據(jù)Table 4 Force sensor measurement data when installed incorrectly

可以看出，無論力傳感器安裝是否正確，本文的優(yōu)化算法都可以進行有效預(yù)估傳感器安裝偏角。其中，在情況（1）時，如果不考慮傳感器安裝偏角，重力補償結(jié)果異常，誤差平方和也非常大，而經(jīng)過優(yōu)化后，重力補償結(jié)果正常（主要從負(fù)載重量項看出），誤差平方和大幅度減小。

隨后在情況（2）下，任意調(diào)整機器人姿態(tài)并采集傳感器數(shù)據(jù)，驗證姿態(tài)如表7所示的，比較補償傳感器安裝偏角前后的重力補償誤差，如圖4和圖5所示。

從圖4可以看出，在補償傳感器安裝偏角后，各方向的最大重力補償誤差、平均重力補償誤差都明顯減小。圖5中的合力誤差為，合力矩誤差為，從中可以看出，與優(yōu)化前相比，優(yōu)化后的合力最大重力補償誤差降低（1–0.9095/1.5298）=40.55%，合力平均重力補償誤差降低（1–0.4027/0.7063）=42.98%，合力矩最大重力補償誤差降低（1–0.2602/0.2751）=5.42%，合力矩平均重力補償誤差降低（1–0.2353/0.2507）=6.14%。因此，證明補償?shù)膫鞲衅靼惭b偏角有效。

表5 安裝正確時力傳感器測量數(shù)據(jù)Table 5 Force sensor measurement data when installed correctly

之后，對負(fù)載重力進行測量。根據(jù)表5的結(jié)果，優(yōu)化前的負(fù)載重力測量值為120.5394N。優(yōu)化后和負(fù)載重力測量值為120.5392N。將負(fù)載拆下，和連接用的螺栓一起利用標(biāo)準(zhǔn)儀器稱重，測得12.057kg，重量為12.057×9.805=118.218885N。由于力傳感器敏感端也有重量，它也被計算在負(fù)載重力之內(nèi)，所以在沒有負(fù)載的時候，再次移動到表5所示的姿態(tài)并測量，對得到的力傳感器數(shù)值進行求解，得敏感端重量為1.6515N，則測量負(fù)載共119.870385N，測量誤差在0.558%之內(nèi)，證明重力補償結(jié)果有效。

如文獻[11]所述，姿態(tài)誤差和采樣數(shù)據(jù)隨機誤差會影響重力補償結(jié)果，這不僅會影響負(fù)載重量的計算，也會進一步導(dǎo)致有些位姿下的優(yōu)化效果不佳，這就解釋了圖4和圖5中為何會出現(xiàn)優(yōu)化后誤差不降反增的情況，但從總體看來，優(yōu)化效果良好。

結(jié)論

本文面向工業(yè)機器人在柔順控制、人機協(xié)作等應(yīng)用中的外力感知需求，在已有的重力補償算法上進行了優(yōu)化。

此方法在最小二乘法的基礎(chǔ)上，通過分析力傳感器偏角對補償結(jié)果的影響，以誤差平方和最小為目標(biāo)建立最優(yōu)解模型，利用遺傳算法，補償力傳感器安裝偏角。試驗中通過采集指定姿態(tài)下的力傳感器數(shù)值來減少系統(tǒng)誤差，利用遺傳算法優(yōu)化后，任意調(diào)整機器人姿態(tài)，驗證優(yōu)化效果。試驗結(jié)果表明，優(yōu)化后的各方向最大重力補償誤差、平均重力補償誤差均有所減少。

表7 機器人驗證姿態(tài)Table 7 Robot verification posture

與現(xiàn)有方法相比，本文方法的優(yōu)勢在于可以在不使用外部測量儀器的情況下，估計出力傳感器安裝偏角。此方法可以補償由于連接件加工精度不足或者安裝失誤導(dǎo)致的誤差。而已有方法是通過精密的機械結(jié)構(gòu)保證力傳感器坐標(biāo)系與法蘭坐標(biāo)系平行，或者是通過單獨調(diào)姿過程對力傳感器安裝偏角、力傳感器零點進行標(biāo)定，這樣會增加成本或者增加標(biāo)定時間。利用本文方法不僅會使效率高、成本降低而且魯棒性得到提升。

圖4 優(yōu)化前后各方向重力補償誤差Fig.4 Gravity compensation error of all direction before and after optimization

圖5 優(yōu)化前后合力 （矩）重力補償誤差Fig.5 Gravity compensation error of resultant force (torque) before and after optimization