利用粒子群優化方法標定機械臂關節參數

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

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

1 引言

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

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

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

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

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

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

2 問題描述

2.1 機械臂運動模型

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

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

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

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

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

坐標系i繞Z軸旋轉后的齊次變換為：

式中：θ—關節變量；β—關節電機驅動的旋轉角度。兩個連續關節坐標系i和i-1之間的齊次變換可以由Zi和給出：

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

2.2 標定過程

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

3 原理與方法

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

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

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

式中：Zi—各關節繞其Z軸的旋轉矩陣—各關節依據D-H參數估計值確定的坐標變換矩陣。末端變換矩陣的形式如下：

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

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

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

相鄰兩關節間由D-H參數得到的轉換矩陣通式為：

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

式中：β—關節驅動器產生的轉角。

在串聯多關節機械臂中，相鄰關節的旋轉矩陣相乘，如果前一關節的扭角為0，即αi=0，則相鄰關節的旋轉變換矩陣變為：

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

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

4 標定實驗

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

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

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

表2 傳統方法標定實驗結果Tab.2 Calibration Results of Traditional Method

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

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

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

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

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

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

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

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

5 結論

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

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

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

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

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

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

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