基于BAS-PSO算法的機器人定位精度提升

喬貴方，呂仲艷，張 穎，宋光明，宋愛國

（1.南京工程學院 自動化學院，江蘇 南京211167；2.東南大學 儀器科學與工程學院，江蘇 南京210096）

1 引言

近年來，工業機器人逐漸被應用于航天發動機的葉片智能磨拋、航天工業中鉆鉚/裝配以及制造現場在線測量系統等高端制造領域［1-3］。“智能機器人”重點專項指出工業機器人應用于高端制造領域時，其絕對定位精度應優于±0.1 mm，姿態角精度應優于±0.1°。目前，工業機器人的重復定位精度最高能夠達到0.01 mm，而絕對定位精度仍為毫米級，無法滿足高端制造領域的精度要求。

機器人標定技術常被用于提高工業機器人的絕對定位精度［4-5］。機器人定位誤差的誘因較多，其中運動學參數誤差是影響機器人定位精度的主要誤差源之一［6-7］。目前，機器人標定方法主要包含誤差模型法和軸線測量法，相比于軸線測量法，誤差模型法的標定精度更高［8］?；谡`差模型法的機器人標定過程主要有誤差建模、誤差測量、參數辨識及誤差補償4個基本步驟。運動學模型對誤差模型的建模影響較大，當前機器人控制器中廣泛使用的運動學模型有DH模型，MDH模型，POE模型和ZRM模型等。誤差模型的完整性決定了運動學模型參數的辨識精度。根據測量數據的不同，誤差模型主要分為位姿誤差模型、位置誤差模型和距離誤差模型，其中位姿誤差模型完整描述了機器人的位置誤差和姿態誤差特性［9］；從建模方法主要分為微分變換［10］和誤差傳遞兩種［11］，兩者在建模過程中忽略的高階誤差項不同，目前尚未有針對兩者精度性能對比的相關研究。

誤差測量主要采用激光跟蹤儀、測量臂、拉線傳感器和多目視覺測量系統等。其中，激光跟蹤儀因其高精度、測量范圍大等特點，是最為常用的測量設備［12-13］。參數辨識是非線性函數求解問題，主要采用Levenberg-Marquardt法、極大似然法［14］等傳統優化算法，但算法的收斂速度和精度依賴于目標函數的雅克比矩陣的奇異值。而智能優化算法則不存在此類問題，LIU等人利用遺傳算法實現六自由度串聯機器人的運動學參數標定［15］。房立金等人基于量子粒子群算法分別實現了串聯/并聯機器人的運動學參數標定［16］。溫秀蘭等人利用改進烏鴉算法辨識機器人的運動學參數，將史陶比爾TX 60機器人的位姿精度提升了70%［17］。

本文針對六自由度串聯機器人定位精度的提升方法展開研究，提出了一種基于天牛須搜索和粒子群優化（Beetle Antennea Search-Particle Swarm Optimization，BAS-PSO）算法的運動學參數辨識方法，并通過實驗驗證了不同誤差模型構建方法的辨識精度和穩定性。

2 機器人運動學誤差模型建立

2.1 建立M-DH運動學模型

如圖1（a）所示，傳統的DH模型利用連桿長度a、連桿距離d、連桿扭角α和關節零位θ4個參數描述了機器人連桿間的位姿關系。而M-DH解決了傳統DH模型在機器人相鄰兩軸平行或接近平行時的奇異性問題，該模型在傳統DH模型基礎上添加了一個角度參數β來描述相鄰平行軸的位置關系［18］，如圖1（b）所示。依據M-DH模型的建立規則，機器人連桿相鄰坐標系之間的變換關系為：

圖1 非平行關節（a）與平行關節（b）的坐標系轉換Fig.1 Coordinate transformation between non-parallel joints（a）and parallel joints（b）

式中：ai，d i，αi，θi，βi分別表示機器人第i個關節的連桿長度、連桿距離、連桿扭角、關節零位及關節扭角，i=1，2，…，6。n表示該轉換矩陣帶入名義運動學模型參數。

因此，機器人的名義末端位姿可由矩陣Tn求取：

式中：Rn和Pn分別為名義旋轉矩陣和名義位移向量。

本文待標定的工業機器人Staubli TX60的M-DH模型參數的名義值如表1所示。由于關節2與關節3理論上是平行關系，則運動學模型參數d2=0，β2=0。其余關節的βi參數均為0，則式（1）變化為傳統DH模型。

表1 Staubli TX60機器人M-DH運動學模型的名義參數Tab.1 Nominal parameters of M-DH kinematics model of Staubli TX60 robot

而實際情況中機器人的運動學模型參數ai，d i，αi，θi，βi存在一定誤差Δai，Δd i，Δαi，Δθi，Δβi，因此，機器人實際末端位姿可由矩陣Tr計算：

式中：Rr和Pr分別為實際的旋轉矩陣和位移向量。r表示該轉換矩陣帶入的實際運動學模型參數。

建立機器人運動學誤差模型是機器人標定技術的關鍵步驟之一，誤差模型決定了運動學參數的辨識精度。誤差模型類型主要有位姿誤差模型、位置誤差模型和距離誤差模型，其中位姿誤差模型能夠較為完整地描述機器人的誤差特性。建模方法主要分為微分變換和誤差傳遞兩種，本文通過實驗驗證了兩種不同運動學誤差模型用于標定的實際效果。

2.2 基于位姿微分變換的運動學誤差模型

機器人末端的位姿誤差矩陣dT表示為：

對Tn每一列的運動學參數進行微分，并忽略高階項（即2階及以上），可以得到機器人的位姿誤差模型：

2.3 基于坐標誤差傳遞的運動學誤差模型

由于各運動學參數存在誤差，相鄰連桿坐標系間的轉換矩陣存在誤差，即：

式中：d i=（d xi，d yi，d zi）T，代表連桿i的一階微分平移向量；δi=（δxi，δyi，δzi）T，代表連桿i的實際姿態與名義姿態間的誤差。

式（7）中給出了連桿i的誤差在連桿i-1坐標系下的位姿誤差和微分變化，因此，相對于機器人基坐標系的末端位姿可以表示為：

當僅考慮一階模型，忽略高階項后，可得：

將各連桿相對于前一連桿的微分運動均轉換為相對機器人基坐標系的微分運動，可得：

式中：dΔ=（d xΔ，d yΔ，d zΔ）T，代表機器人末端的一階微分平移向量；δΔ=（δxΔ，δyΔ，δzΔ）T，代表機器人末端實際姿態與名義姿態的誤差。

3 基于BAS-PSO的運動學參數辨識

3.1 BAS-PSO混合優化算法

天牛須搜索算法（Beetle Antennae Search，BAS）是2017年提出的一種新型智能優化算法，具有求解速度快、精度高、運算量少等特點［19］。BAS是模擬天牛覓食原理，天牛的兩只觸角感知食物氣味的強弱實現覓食，若左邊觸角感知的氣味強度比右邊大，天牛就往左飛；否則就往右飛，具體示意圖如圖2所示。BAS算法與其他智能優化算法類似，無需梯度信息就可以實現高效尋優。并且BAS算法中僅需一個個體，運算量大為降低，但容易陷入局部最優。

圖2 天牛須搜索算法的仿生原理示意圖Fig.2 Schematic diagram of bionic principle for beetle antennae search algorithm

群體智能算法具有并行運算特性，通過多個個體的相互協作實現尋優，不易陷入局部最優。粒子群優化算法（Particle Swarm Optimization，PSO）是一種經典的群體智能算法，通過群體中粒子個體之間的協作和信息共享來尋找最優解，粒子個體通過跟蹤個體極值和全局極值來更新位置。為提高粒子個體的多樣性，同時提高BAS算法的并行性，本文提出一種BAS-PSO混合優化算法來實現機器人運動學參數的誤差辨識。

BAS-PSO混合優化算法求解最優問題時，粒子個體的位置更新有兩種途徑：一是根據天牛須搜索算法更新當前位置；二是根據個體的最優值和個體群的全局最優值決定其更新位置。因此，第i個個體在k+1時刻的位置更新如下：

從抽樣調查的統計數據可以看出，城陽區鄉村旅游者大都有2次以上的鄉村旅游經歷，由此得知本次調查的鄉村旅游相關數據比較客觀。

天牛個體的速度主要由左/右須位置的函數值決定，為保證該項在精度和全局收斂速度之間的平衡，通過變步長實現粗略搜索后再精細化搜索。因此，天牛個體的速度更新依據如下：

式中：r3是［0，1］之間的隨機數，為天牛個體的須寬，為天牛個體的運動步長，d0與δ0分別確定了最小須寬和最小步長。

通常個體的初始位置是通過偽隨機數隨機產生的，但其隨機性過強而均勻性不足。根據文獻［20］可知，擬隨機序列相比于偽隨機序列能夠更加均勻地充滿采樣空間，可以提升優化算法的收斂速度，使計算結果更加穩定可靠。鑒于擬隨機Halton序列概念清晰，算法易實現，用于產生BAS-PSO的初始位置。

3.2 誤差模型的適應度函數

在采用BAS-PSO優化計算機器人的幾何參數誤差時，其適應度函數為：

式中：N為標定點數目，k為調節因子。由于位置誤差和姿態誤差的量綱不同，并且誤差值相差較大，利用調節因子k平衡位置誤差向量和姿態誤差向量在目標函數中的權重。

由式（14）可知：f是幾何參數誤差集（Δai，Δd i，Δαi，Δθi，Δβi）的函數，經優化算法搜索機器人的幾何參數誤差集（Δai，Δd i，Δαi，Δθi，Δβi），使適應度函數f最小。本文中待標定的Staubli TX60六自由度串聯機器人2，3關節的軸線理論上是平行的。因此，參數d2，β1，β3，β4，β5，β6不需要辨識，因此待辨識參數的個數為24，屬于復雜的多參數非線性優化問題，非常適合采用智能優化算法求解。

4 運動學參數辨識實驗

圖3為本文所搭建的機器人標定實驗系統，該系統使用Leica AT 960激光跟蹤儀，其測量不確定度為±（15μm+6μm/m）。配套使用的測量分析軟件為Spatial Analyzer，該軟件提供了擬合幾何體、建立坐標系等功能。該系統待標定的工業機器人為Staubli TX60，該機器人的重復定位精度為±0.02 mm，額定負載為3 kg，最大負載為5 kg。激光跟蹤儀的T-MAC工具安裝在工業機器人的末端法蘭盤上。本文所涉及的測量過程均符合GB/T-12642-2013及ISO-9283工業機器人性能規范及其試驗方法標準。

圖3 工業機器人標定系統Fig.3 Photo of industrial robot calibration system

實驗中以Staubli TX 60機器人的基坐標系為參考坐標系，以坐標值（550，0，550）為中心點，在邊長為1000 mm的正方體空間內隨機選擇50個測量點用于運動學模型參數標定，并使這50個測量點要盡可能地分布在整個正方體空間內，同時在相同空間內選擇30個驗證點來驗證運動學模型精度。

4.1 基于BAS-PSO的運動學參數辨識

本文首先基于位姿微分變換構建的運動學誤差模型，利用BAS-PSO算法辨識機器人運動學模型參數，并與PSO算法和BAS算法進行對比。算法優化過程如圖4所示，從圖中可以看出，BASPSO算法的收斂速度比PSO算法和BAS算法快，并且適應度函數的值也比PSO算法的小。

圖4 不同優化算法的計算過程對比Fig.4 Calculation process comparison of different opti?mization algorithms

機器人運動學模型參數的辨識精度如圖5和圖6所示。平均綜合位置誤差（Average Comprehensive Position Error，ACPE）和平均綜合姿態誤 差（Average Comprehensive Attitude Error，ACAE）的計算如下：

圖5 標定前后機器人的平均綜合位置誤差Fig.5 Average comprehensive position errors（ACPE）of robot before and after calibration

圖6 標定前后機器人的平均綜合姿態誤差Fig.6 Average comprehensive attitude error（ACAE）of robot before and after calibration

利用BAS-PSO算法辨識機器人的運動學參數誤差，TX60機器人的ACPE，ACAE分別從（0.312 mm，0.221°）降低為（0.0938 mm，0.0442°）。而經過PSO算法和BAS算法辨識后的TX60機器 人 的ACPE，ACAE分 別 為（0.0942 mm，0.0459°）和（0.0977 mm，0.0632°）。從以上結果可以看出，BAS-PSO算法辨識得到的運動學參數誤差具有更好的精度特性。為進一步驗證辨識模型精度，本文分別計算30個驗證點的ACPE和ACAE。利用BAS-PSO算法辨識后的機器人的ACPE，ACAE分別從（0.301 mm，0.233°）降 低 為（0.111 mm，0.047°）。而 經 過PSO算法和BAS算法辨識后的TX60機器人的ACPE，ACAE分 別 為（0.113 mm，0.051°）和（0.113 mm，0.062°）。由此說明，BAS-PSO算法辨識得到的機器人運動學模型的泛化能力相比其他兩種算法較好。

4.2 運動學誤差模型精度分析

前文已介紹機器人運動學誤差模型的兩種建模方法，理論上在建立基于位姿微分變換的運動學誤差模型過程中，僅作一次高階項忽略；而建立基于坐標誤差傳遞的運動學誤差模型時，做兩次高階項忽略。因此，基于位姿微分變換的運動學誤差模型的精度理論上要優于基于坐標誤差傳遞的運動學誤差模型。同時，由于智能優化算法不用必須建立誤差模型，本文可直接利用式（4）構建的正運動學模型辨識M-DH運動學模型參數，此種方式未做任何誤差忽略，其理論精度會比其他兩種誤差模型高。

由于BAS-PSO是一種隨機優化算法，其辨識結果具有一定的隨機性，主要體現在初始種群選取及個體位置更新。為評價誤差模型的標定效果，本文利用相同的數據集對3種模型分別標定10次，標定結果如圖7所示。為評價模型的性能，本文利用平均位置誤差（Average Posi?tion Error，APE）和平均姿態誤差（Average Atti?tude Error，AAE）作為評價指標，其計算公式如下：

圖7 不同誤差模型辨識后平均綜合位置/姿態誤差的對比Fig.7 Comparison of average comprehensive position and attitude errors（ACPE/ACAE）after identifi?cation with different error models

從圖7可以看出，基于運動學模型辨識的實際運動學模型的精度更為穩定，而基于位姿微分變換的運動學誤差模型和基于坐標誤差傳遞的運動學誤差模型辨識得到的實際運動學模型的精度穩定性較差。其中，基于坐標誤差傳遞的運動學誤差模型（誤差模型1）辨識后的機器人APE和AAE分別為0.1035 mm和0.1642°，基于位姿微分變換的運動學誤差模型（誤差模型2）辨識后的機器人APE和AAE分別為0.1009 mm和0.1565°，而基于正運動學模型（誤差模型3）直接辨識后的機器人APE和AAE分別為0.0975 mm和0.0986°。因此，基于正運動學模型直接辨識的機器人精度更高。

5 結論

本文首先闡述了工業機器人基于位姿微分變換的運動學誤差模型和基于坐標誤差傳遞的運動學誤差模型的構建方法；其次，提出了一種基于BAS-PSO算法的運動學參數辨識方法，相比于PSO和BAS算法，該算法具有更好的辨識精度和收斂速度。實驗結果表明，經該算法辨識后TX60機器人的ACPE，ACAE分別從（0.312 mm，0.221°）降低為（0.0938 mm，0.0442°）。最后，分析了基于位姿微分變換的運動學誤差模型、基于坐標誤差傳遞的運動學誤差模型以及基于正運動學模型在機器人運動學參數辨識中的精度特性。從實驗結果可以看出，基于正運動學模型辨識的機器人運動學參數具有更好的穩定性和精度，而基于坐標誤差傳遞的運動學誤差模型辨識的機器人運動學參數精度相對較差。