基于遺傳算法的機(jī)器人負(fù)載重力補(bǔ)償優(yōu)化算法研究*

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

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

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

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

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

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

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

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

重力補(bǔ)償計(jì)算模型建立

本文的重力補(bǔ)償模型一部分參考了文獻(xiàn)[11]。

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

機(jī)器人系統(tǒng)由機(jī)器人本體、力傳感器、負(fù)載組成。設(shè)世界坐標(biāo)系為Ow–XwYwZw，機(jī)器人坐標(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è)置是為了表示機(jī)器人安裝平面與地面的安裝傾角，一般只存在于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次不共面的機(jī)器人姿態(tài)（N≥3），在機(jī)器人穩(wěn)定后，讀取力傳感器數(shù)值，得到N組力傳感器測量數(shù)據(jù),整理可得：

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

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

3 力傳感器與機(jī)器人本體關(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。

進(jìn)而可得

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

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

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

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

1 重力補(bǔ)償誤差分析

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

其中，δxi、δyi、δzi、δai、δbi、δci、為第i個測量姿態(tài)下的重力補(bǔ)償誤差，F(xiàn)xi、Fyi、Fzi、Txi、Tyi、Tzi為第i個測量姿態(tài)下的實(shí)際測量值（即力傳感器數(shù)值），Gxi、Gyi、Gzi、和Tgxi、Tgyi、Tgzi可分別由式（7）和式（1）得到。從前文的重力補(bǔ)償模型可以看出，重力補(bǔ)償誤差與負(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)，α、β、γ越接近實(shí)際轉(zhuǎn)角，誤差的平方和會越小，且重力補(bǔ)償效果會越好。因此，可以利用尋找最優(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ù)估（例如傳感器與法蘭之間隨機(jī)安裝），可以選取一個較大的范圍。Δα、Δβ、Δγ的范圍影響遺傳算法的計(jì)算效率及準(zhǔn)確度。

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

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

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

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

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

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

使用乘冪尺度變換的方法進(jìn)行適應(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ù)。由于篇幅原因，在此不進(jìn)行具體描述。

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

試驗(yàn)驗(yàn)證

1 試驗(yàn)條件

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

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

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

圖3 重力補(bǔ)償試驗(yàn)平臺Fig.3 Gravity compensation experiment platform

表1 KR210–2700機(jī)器人技術(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 機(jī)器人測量姿態(tài)Table 3 Robot measuring attitude

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

因?yàn)楸疚牡闹攸c(diǎn)在于驗(yàn)證預(yù)估力傳感器安裝偏角的有效性，對于不同負(fù)載的重力補(bǔ)償效果在此不進(jìn)行贅述。考慮兩種情況：

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

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

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

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

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

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

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

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

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

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

之后，對負(fù)載重力進(jìn)行測量。根據(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。由于力傳感器敏感端也有重量，它也被計(jì)算在負(fù)載重力之內(nèi)，所以在沒有負(fù)載的時候，再次移動到表5所示的姿態(tài)并測量，對得到的力傳感器數(shù)值進(jìn)行求解，得敏感端重量為1.6515N，則測量負(fù)載共119.870385N，測量誤差在0.558%之內(nèi)，證明重力補(bǔ)償結(jié)果有效。

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

結(jié)論

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

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

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

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

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

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