利用粒子群優(yōu)化方法標定機械臂關節(jié)參數(shù)

朱 衡，楊東超，常 旭，孫可平

（清華大學機械工程系，北京 100084）

1 引言

工業(yè)機器人在裝配、維修和長時間使用后會由于桿件變形、磨損、間隙等原因使關節(jié)參數(shù)發(fā)生變化[1]，這導致控制器中存儲的關節(jié)參數(shù)精度降低，使機械臂無法完成精準的操作任務。為了提升末端執(zhí)行器位姿的控制精度，不得不重新進行標定。機器人關節(jié)參數(shù)的標定過程，分為建模、測量、識別和校正四個階段[2]。

目前，對于機械臂的標定已經(jīng)進行了大量的研究。Nubiola提出了一個29參數(shù)的校準模型，該模型考慮了包含關節(jié)柔順度在內(nèi)的所有可能的幾何誤差，來提高ABB IRB 1600工業(yè)機器人的絕對精度。基于最小二乘法對29個參數(shù)進行識別，并最終對末端執(zhí)行器上的若干個標志點進行了1000組測量，驗證了機器人的精度[3]。文獻[4]提出用三個激光測距傳感器向機械臂發(fā)射激光束，綜合不同傳感器測距，利用機械臂末端位姿的改變所引起的激光測距傳感器的輸出實現(xiàn)末端位姿信息的采集，用以代替?zhèn)鹘y(tǒng)的位姿信息采集方法，提升了后續(xù)關節(jié)參數(shù)標定的精度。文獻[5]研究了對六自由度機械臂的視覺標定，使用簡單的標定板，基于OpenCV中的霍夫變換等圖像算法及線性回歸擬合，獲得較高的標定精度。

從某種意義上來說，機械臂的標定是一個尋找關節(jié)參數(shù)最優(yōu)值的過程，它能使末端執(zhí)行器的標稱位姿和實際位姿間的誤差最小化。因此，優(yōu)化算法可用于機械臂的標定。近年來，針對群體智能（SI）的研究迅速展開，所謂群體智能，是一種基于對高效、高度組織化的自然系統(tǒng)進行觀測而得到的優(yōu)化方法。在整個系統(tǒng)中，個體間通過相對簡單的規(guī)則進行交流與合作，從而使群體表現(xiàn)出特定的復雜行為。因此，群體智能可以為不同的應用程序帶來更有效的算法，可以解決具有非線性、多極值等特點的復雜函數(shù)及組合優(yōu)化問題，尤其是針對不易寫出目標函數(shù)解析式的應用也有較好的優(yōu)化結果[6]。典型的群體智能算法包括蟻群算法、遺傳算法、模擬退火算法、禁忌搜索算法和粒子群算法等。其中，粒子群優(yōu)化算法（Particle Swarm Optimization，PSO）作為一種基于種群的隨機優(yōu)化算法，最早由Kennedy和Eberhart提出，源于對鳥群捕食的行為研究[7]，是一種啟發(fā)式群智能算法。類似遺傳算法，粒子群算法也是執(zhí)行多點搜索，可以在多樣化和集中化之間建立均衡。相較于用其他智能優(yōu)化算法，如遺傳算法進行串聯(lián)機械臂的運動學參數(shù)標定[8]，粒子群算法的原理和編程實現(xiàn)簡單，收斂速度較快，搜索精細、方向明確，且具有并行性[9]。

另外，傳統(tǒng)的機械臂標定方法中，大多采用D-H參數(shù)法進行建模，這一方法在串聯(lián)機械臂相鄰關節(jié)兩軸平行或近似平行時會出現(xiàn)問題：兩軸線相對位置的誤差的建模不符合誤差模型，這導致部分重要的關節(jié)參數(shù)誤差無法辨識[10]。為了解決這一參數(shù)耦合的問題，文獻[11]提出了一種MDH參數(shù)建模方法，額外增加了一個參數(shù)來保證標定得到正確的結果，然而這也增加了算法的復雜性。粒子群算法標定的原理使其能簡單、有效地解決由于兩軸平行導致的參數(shù)耦合問題。因此，選擇粒子群算法研究其在機械臂標定中的應用。

粒子群算法在機械臂標定中的應用目前已得到了一定的研究。例如，文獻[12]提出用粒子群算法產(chǎn)生誤差估計函數(shù)來預測機械臂的定位誤差。然而，實驗結果表明在對近似函數(shù)使用低次的正規(guī)多項式時，誤差的預測不能得到明顯的改善。文獻[13]提出了一種基于粒子群算法的以單點的重復精度作為適應度函數(shù)的方法，去識別六自由度鉸鏈臂坐標測量機（AACMM）的關節(jié)零偏，但標定方法較為繁瑣，且選用適應度函數(shù)時僅考慮了末端探針位置的重復精度。以上這些方法均沒有考慮粒子群優(yōu)化方法與傳統(tǒng)標定方法的優(yōu)劣比較，且標定過程缺乏普適性。

對此，提出了一種相對傳統(tǒng)方法更簡單有效的、基于粒子群算法的標定方法，用以識別建模中使用Denavit-Hartenberg（DH）方法[14]建立的實際關節(jié)參數(shù)，針對ABB IRB 120工業(yè)機器人進行了實驗，給出了迭代優(yōu)化解，結合實驗結果進行誤差分析。此外，本法易于實現(xiàn)長度參數(shù)和轉(zhuǎn)角參數(shù)的分步標定，且可規(guī)避相鄰關節(jié)扭角為零的發(fā)散問題。

2 問題描述

2.1 機械臂運動模型

研究對象—ABB IRB 120工業(yè)機器人由一個基座和六個旋轉(zhuǎn)關節(jié)組成。該機器人的有效載荷為3kg，工作空間范圍可達580mm。機器人的尺寸參數(shù)和生成運動模型所需的坐標系，如圖1～圖2所示。其中機械臂各關節(jié)坐標系的建立采用了Denavit-Hartenberg（D-H）方法。機械臂的標稱D-H 參數(shù)，如表1 所示。表中的4個D-H 參數(shù)的分別為：桿件長度ɑ，關節(jié)距離d，桿件扭角α，關節(jié)轉(zhuǎn)角θ。

圖1 機械臂工作空間Fig.1 The Size Parameters of the Manipulator

圖2 ABB IRB 120工業(yè)機器人坐標系Fig.2 ABB IRB 120 Industrial Robot Coordinate System

表1 機械臂標稱D-H參數(shù)Tab.1 Actual D-H Parameters

該機械臂的關節(jié)坐標系i與關節(jié)坐標系i-1之間的齊次變換矩陣為：

坐標系i繞Z軸旋轉(zhuǎn)后的齊次變換為：

式中：θ—關節(jié)變量；β—關節(jié)電機驅(qū)動的旋轉(zhuǎn)角度。兩個連續(xù)關節(jié)坐標系i和i-1之間的齊次變換可以由Zi和給出：

由相鄰兩坐標系的齊次變換矩陣，可以累乘得到基坐標系與固定在機械臂末端執(zhí)行器上的坐標系之間的整體變換矩陣：

2.2 標定過程

與傳統(tǒng)標定方法類似，首先利用測量精度較高的測量臂等裝置測量工業(yè)機械臂末端執(zhí)行器上的3個標志點的坐標，同時獲得末端執(zhí)行器的Z軸（法線方向）和Y軸方向矢量，X軸的方向矢量可通過叉乘獲得，由此并可確定機器人的末端位姿。這一過程要重復多次，關節(jié)轉(zhuǎn)角可從示教器中讀出。此后，基于所獲得的測量數(shù)據(jù)，即可利用粒子群優(yōu)化算法對機械臂運動學參數(shù)進行識別，最終獲得實際的機械臂D-H 參數(shù)矩陣，完成對機械臂的標定，并評估標定結果。

3 原理與方法

在粒子群算法中，群表示為由粒子組成的主體，并可以通過群內(nèi)各組成部分之間的通信進行求解。而粒子是群的無質(zhì)量、無體積的組成部分，具有速度和加速度信息。在空間中尋找最優(yōu)解時，每個粒子的位置可以通過適應度函數(shù)來評估。粒子交換彼此的位置信息，通過這種相互作用來更新自己的位置，最終達到所有粒子的最佳位置。在D維空間中搜索時，粒子i的飛行速度和位置通過以下的式子更新：

以上兩式中：Xi=(xi1，xi2，xi3，...，xiD)和Vi=(vi1，vi2，vi3，...，viD)是粒子i的當前位置和當前飛行速度，Pi=(pi1，pi2，pi3，...，piD)表示粒子i到當前迭代次數(shù)時的最佳位置，即個體極值；G=(g1，g2，g3，...，gD)表示在所有粒子中的適應度函數(shù)最小的粒子到當前迭代次數(shù)時的最佳位置，即全局極值。粒子飛行速度的迭代式由三個部分組成，第一部分代表了前一速度對當前速度的影響，由慣性權重w來量化；第二部分可理解為粒子當前位置與自身歷史最好位置之間的距離，這一差值對當前速度的影響可由加速度系數(shù)c1來控制；第三部分可理解為粒子當前位置與群體的歷史最好位置之間的距離，這一差值對當前速度的影響可由加速度系數(shù)c2來控制。rand為隨機函數(shù)，取值范圍為[0，1]，用以增加搜索的隨機性。慣性權重w可以控制粒子的搜索能力。一般來說，權值很大時粒子搜索空間的范圍會大大提高，而權值很小時粒子的局部搜索能力會提高，但搜索空間會被限制。

具體到標定機械臂的過程中，每個粒子代表著ABB IRB 120工業(yè)機器人在校準過程中的一組D-H參數(shù)估計值。首先根據(jù)DH參數(shù)的初始值在一定范圍內(nèi)進行隨機化，作為粒子的初始位置。基于每一粒子，可獲得執(zhí)行器的末端齊次變換矩陣估計值T～：

式中：Zi—各關節(jié)繞其Z軸的旋轉(zhuǎn)矩陣—各關節(jié)依據(jù)D-H參數(shù)估計值確定的坐標變換矩陣。末端變換矩陣的形式如下：

在標定的計算過程中，需要確定一個適應度函數(shù)來衡量末端變換矩陣實際值T和理論值T～的差異。合適的適應度函數(shù)是標定成功的關鍵：適應度函數(shù)代表著迭代前進的方向，控制著每次迭代中粒子群個體極值和全局極值的更新；適應度函數(shù)應該包含各關節(jié)的信息，通過對所有D-H參數(shù)的約束來控制迭代尋優(yōu)的方向，并且保證D-H參數(shù)的微小變化不會引起函數(shù)的躍變從而避免奇異。在傳統(tǒng)標定方法中，可利用系數(shù)矩陣的廣義逆左乘誤差向量的積作為適應度函數(shù)，但引入了所測機械臂的模型；而粒子群算法由于“黑箱”的優(yōu)勢，不必引入實際模型，但必須選擇一個有普適性的適應度函數(shù)用于迭代。考慮到實際物理意義，選擇末端位置理論值與實際值間的空間歐氏距離，以及末端姿態(tài)的橫滾-俯仰-偏航（RPY）歐拉角與實際值的差值作為適應度函數(shù)f：

此外，基于粒子群的優(yōu)化標定方法非常容易實現(xiàn)分步式標定，無需重新推導模型。這個特點對相機姿態(tài)的校準和標定等應用非常有意義。由于串聯(lián)機械臂末端執(zhí)行器的姿態(tài)僅取決于12個角度參數(shù)，可以先確定12個與角度相關的參數(shù)，再確定剩下的12個參數(shù)，即每一步粒子的維度都降為12。利用粒子群算法標定時，第一步先將12個長度參數(shù)固定為初始的D-H參數(shù)（長度參數(shù)的取值對姿態(tài)誤差沒有影響），將式（12）中的forientation作為適應度函數(shù)。迭代至收斂后，再將12個角度參數(shù)固定，以式（11）中的fposition作為適應度函數(shù)，迭代確定剩余的12個長度參數(shù)。

根據(jù)機械臂D-H參數(shù)的意義可知，在標定角度參數(shù)時，如果機械臂前一關節(jié)扭角為零，這一關節(jié)的轉(zhuǎn)角會與相鄰的后一關節(jié)的轉(zhuǎn)角耦合，從而造成發(fā)散。對這一現(xiàn)象的原因和解決方法分析如下：

相鄰兩關節(jié)間由D-H參數(shù)得到的轉(zhuǎn)換矩陣通式為：

其中，左上角的3階矩陣為旋轉(zhuǎn)矩陣。根據(jù)式（7）可知，其與關節(jié)繞Z軸的旋轉(zhuǎn)矩陣共同決定了相鄰關節(jié)的旋轉(zhuǎn)變換矩陣，如式（14）所示。

式中：β—關節(jié)驅(qū)動器產(chǎn)生的轉(zhuǎn)角。

在串聯(lián)多關節(jié)機械臂中，相鄰關節(jié)的旋轉(zhuǎn)矩陣相乘，如果前一關節(jié)的扭角為0，即αi=0，則相鄰關節(jié)的旋轉(zhuǎn)變換矩陣變?yōu)椋?/p>

這說明如果前一關節(jié)的扭角為0，即兩軸平行時，則其與相鄰后一關節(jié)的旋轉(zhuǎn)矩陣相乘會造成兩關節(jié)轉(zhuǎn)角參數(shù)的耦合，即通過與末端齊次變換矩陣相關的適應度函數(shù)進行迭代尋優(yōu)時，只能確定式（15）中θi與θj的和，而無法區(qū)分每一個轉(zhuǎn)角的值。傳統(tǒng)標定方法利用誤差函數(shù)模型優(yōu)化D-H參數(shù)值，這一模型要求末端執(zhí)行器位姿的微小誤差必須能由模型參數(shù)的微小誤差來表示（否則系數(shù)矩陣的廣義逆矩陣發(fā)生奇異），顯然當兩關節(jié)轉(zhuǎn)角參數(shù)發(fā)生耦合時，這一要求不能被滿足，即不滿足誤差模型，因此必須增加第五個參數(shù)。而基于粒子群算法的標定并不分析機械臂的誤差模型。

在使用粒子群算法迭代前對關節(jié)參數(shù)的初始值進行了一定范圍內(nèi)的隨機化處理，并且如式（5）所示。在每一步迭代過程中對關節(jié)參數(shù)也進行了隨機化處理，因此不會出現(xiàn)扭角為零導致發(fā)散的結果；而且事實上，對于機器人的零位而言，幾乎不可能出現(xiàn)扭角精確為0的情況，所以這并不會影響實際標定操作中程序的使用，不需要額外增加參數(shù)就能標定出精確結果。

4 標定實驗

為了驗證基于粒子群優(yōu)化算法的標定方法在實際應用中的效果，采用傳統(tǒng)標定方法和粒子群算法整體式標定分別對一臺實際工作環(huán)境中的ABB IRB 120六自由度工業(yè)機器人進行了標定實驗。

首先，遵循D-H 規(guī)則建立該工業(yè)機器人的坐標系，如圖2所示。以廠家給出的初始D-H 參數(shù)作為實驗初始值，如表1所示。

之后利用測量臂測量機械臂基坐標系，多次測量后擬合，獲得基坐標系相對于測量坐標系的齊次變換矩陣。然后利用測量系統(tǒng)獲得機械臂的50 組轉(zhuǎn)角數(shù)據(jù)以及對應的末端位姿測量值。利用傳統(tǒng)標定方法迭代計算機械臂的實際D-H參數(shù)，以D-H 參數(shù)估計值的增量矩陣的二范數(shù)小于閾值10-10作為停止準則，獲得標定結果，如表2所示。

表2 傳統(tǒng)方法標定實驗結果Tab.2 Calibration Results of Traditional Method

利用粒子群優(yōu)化算法整體法標定，參數(shù)設置如下：粒子數(shù)N=300，慣性權重w=0.85，加速度常數(shù)c1=c2=2，最大迭代次數(shù)k=2000；標定結果，如表3所示。

表3 粒子群算法標定實驗結果Tab.3 Calibration Results of PSO Method

以標定過程中的粒子群適應度函數(shù)值的常用對數(shù)作為指標，迭代過程，如圖3所示。已迭代至收斂：

圖3 粒子群算法迭代過程Fig.3 Iterative Process of PSO Method

對粒子群算法獲得的D-H參數(shù)標定結果，與標定前的初始D-H 參數(shù)、利用傳統(tǒng)方法標定得到的D-H 參數(shù)進行誤差分析。利用測量臂測量得到的機械臂末端位姿作為參考值。定義利用標定結果計算得到的末端執(zhí)行器位置與參考位置的平均空間歐氏距離作為位置誤差；利用末端執(zhí)行器坐標系相對于基坐標系的RPY歐拉角來描述末端姿態(tài)，定義標定結果計算得到的末端姿態(tài)與參考姿態(tài)的平均差值作為姿態(tài)誤差，結果如下，如圖4所示。

圖4 標定位姿誤差分析Fig.4 Posture Error Analysis of Calibration

在位置誤差方面，根據(jù)傳統(tǒng)方法的標定結果計算得到的機械臂末端位置與測量得到的參考位置（實際位置）的誤差約為標定前的2.6%，而根據(jù)粒子群算法整體式標定得到的末端位置與實際位置的誤差約為標定前的2.4%，如表4所示。在姿態(tài)誤差方面，以歐拉角—橫滾、俯仰、偏航角誤差平均值為參考，根據(jù)傳統(tǒng)方法標定結果計算得到的末端姿態(tài)與測量得到的參考姿態(tài)（實際姿態(tài)）的誤差約為標定前的56.7%，而根據(jù)粒子群算法整體式標定得到的末端姿態(tài)與實際姿態(tài)的誤差約為標定前的54.0%。根據(jù)以上分析可知，利用粒子群算法整體式標定得到的D-H參數(shù)，已經(jīng)在位置誤差和姿態(tài)誤差上均略優(yōu)于傳統(tǒng)方法標定結果，且遠遠優(yōu)于未標定前的D-H參數(shù)。這證明了粒子群優(yōu)化算法完成多自由度機械臂運動學參數(shù)標定的可行性和優(yōu)越性。另外，對相同的測量數(shù)據(jù)進行多次優(yōu)化標定，在充分迭代至收斂后，都能得到相同的標定結果，這證明了粒子群算法標定的穩(wěn)定性。

表4 兩種標定方法的位姿誤差分析Tab.4 Posture Error Analysis of Two Calibration Methods

5 結論

（1）提出了基于粒子群優(yōu)化算法的機械臂標定方法，并利用ABB IRB 120六自由度工業(yè)機器人驗證了算法的準確性、穩(wěn)定性。

（2）根據(jù)標定實驗的結果，比較了傳統(tǒng)標定方法與粒子群算法整體式標定的結果，后者在標定的位置精度與姿態(tài)精度（以橫滾、俯仰、偏航角描述姿態(tài)）上更優(yōu)于前者。

（3）與傳統(tǒng)的機械臂運動學參數(shù)標定方法相比，利用粒子群算法原理簡單，收斂性好，不需要機械臂運動學參數(shù)的計算存在解析解，且跳過了繁瑣的機械臂建模的過程，只需在編寫好的程序中自動運算即可。

綜上所述，所提出的標定方法具有簡單易行、標定精度高、適用范圍廣等優(yōu)點，便于在工業(yè)上的應用。

粒子群優(yōu)化算法的參數(shù)選擇對算法的性能有一定影響。機械臂D-H 參數(shù)標定的程序中，一個動態(tài)的慣性權重w可以更好地達到全局搜索與局部搜索之間的平衡。比如，設置慣性權重w隨著迭代次數(shù)增加而下降，使粒子群在開始階段較快定位到最優(yōu)解的附近；隨著w的減小，粒子速度減慢，進行精細的局部搜索。

粒子群優(yōu)化算法可以有效避免關節(jié)扭角為零所產(chǎn)生的發(fā)散，而導致發(fā)散的原因是矩陣相乘時，相連關節(jié)的轉(zhuǎn)角耦合在了一起，只能對轉(zhuǎn)角之和進行標定，而無法計算出每一個轉(zhuǎn)角。

此外，有些實際問題僅關心末端執(zhí)行器的姿態(tài)，即只需標定機械臂各關節(jié)的角度參數(shù)，如照相機作為末端執(zhí)行器（自動調(diào)焦功能使得相機的拍攝精度對位置誤差不敏感），則也可以利用提出的分步式方法來標定。