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

（清華大學(xué)機(jī)械工程系，北京 100084）

1 引言

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

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

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

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

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

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

2 問題描述

2.1 機(jī)械臂運(yùn)動模型

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

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

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

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

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

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

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

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

2.2 標(biāo)定過程

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

3 原理與方法

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

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

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

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

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

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

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

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

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

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

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

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

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

4 標(biāo)定實驗

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

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

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

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

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

表3 粒子群算法標(biāo)定實驗結(jié)果Tab.3 Calibration Results of PSO Method

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

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

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

圖4 標(biāo)定位姿誤差分析Fig.4 Posture Error Analysis of Calibration

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

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

5 結(jié)論

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

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

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

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

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

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

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