基于遺傳算法的機器人負載重力補償優化算法研究*

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

（南京航空航天大學機電學院，南京 210016）

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

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

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

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

綜上所述，在現有的關于負載重力補償的研究中，大多默認機器人基坐標系Z軸與重力方向相同，但在實際情況中，往往需要考慮基坐標系與世界坐標系的關系。在進行補償時，應該綜合考慮力負載重力、傳感器零點漂移、機器人安裝傾角、力傳感器安裝偏角。對于航空航天裝配系統而言，由于大多都是大負載，為了使機器人得到更加精準的外力感知能力，應該盡可能使補償效果達到最好。

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

重力補償計算模型建立

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

1 坐標系關系建立

機器人系統由機器人本體、力傳感器、負載組成。設世界坐標系為Ow–XwYwZw，機器人坐標系為Or–XrYrZr，法蘭坐標系為Of–XfYfZf，力傳感器坐標系為Os–XsYsZs，它們之間關系如圖1所示。

假設世界坐標系的Zw方向與重力方向平行且反向。

規定以下轉換關系：

（1）Os–XsYsZs相對于Of–XfYfZf的姿態轉換矩陣為RZ（γ）。

（2）Of–XfYfZf相對于Or–XrYrZr的姿態轉換矩陣為RZ（C）。

（3）Or–XrYrZr相對于Ow–XwYwZw的姿態轉換矩陣為RY（ω）。

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

2 力傳感器與負載關系

首先假設無外力，以力傳感器坐標系為基準分解負載重力，如圖2所示。

從圖2中可以得到：

圖1 坐標系關系Fig.1 Coordinate system relationship

圖2 力傳感器坐標系與負載關系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次不共面的機器人姿態（N≥3），在機器人穩定后，讀取力傳感器數值，得到N組力傳感器測量數據,整理可得：

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

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

3 力傳感器與機器人本體關系

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

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

由式（2）可得：

可簡寫為:

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

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

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

進而可得

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

4 力傳感器與法蘭的關系

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

重力補償優化算法研究

1 重力補償誤差分析

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

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

令

其中，S為測量誤差的平方和，根據所述算法可以發現，它是一個關于α、β、γ的函數，若α、β、γ已知，誤差平方和S便可以求出。

2 優化模型建立

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

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

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

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

給Δα、Δβ、Δγ設置一個范圍，即

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

那么優化問題可以描述為：

本文采用遺傳算法對優化問題進行求解。

由于求解的是目標函數最小值的優化問題，所以應對原始的目標函數進行變換：

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

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

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

使用乘冪尺度變換的方法進行適應度評價，即

其中，F′表示變換后的適應度，k為調整參數，F是步驟(1)中的變換目標函數。

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

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

試驗驗證

1 試驗條件

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

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

2 試驗過程及結果

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

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

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

表3 機器人測量姿態Table 3 Robot measuring attitude

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

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

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

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

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

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

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

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

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

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

從圖4可以看出，在補償傳感器安裝偏角后，各方向的最大重力補償誤差、平均重力補償誤差都明顯減小。圖5中的合力誤差為，合力矩誤差為，從中可以看出，與優化前相比，優化后的合力最大重力補償誤差降低（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%。因此，證明補償的傳感器安裝偏角有效。

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

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

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

結論

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

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

表7 機器人驗證姿態Table 7 Robot verification posture

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

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

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