基于粒子群算法尋優的機器人自適應阻抗力控制

趙丕洋 洪榮晶,2 方成剛,2

1(南京工業大學機械與動力工程學院 江蘇 南京 210009) 2(南京工大數控科技有限公司 江蘇 南京 210009)

0 引 言

機器人在打磨、噴涂和裝配上具有很大優勢，已成為制造業的發展趨勢。在六自由度機器人末端安裝六維力傳感器，可與外界的接觸力進行交互，以適應在飛機發動機、汽輪機葉片和模具等外形輪廓復雜零部件的高精度要求[1]。

Hogan提出的阻抗控制需要很少離線任務規劃，對擾動和不確定性有很好的魯棒性[2]，但其采用基于理想化的物理系統模型方法，無法準確實現機械臂的位置控制和力控制。Seul在Hogan的阻抗控制算法的基礎上提出了一種自適應阻抗控制算法，不需要確定的機器人模型就能實現力反饋信息的處理，因此在環境剛度多變的情況下，也能夠把機器人與環境之間的接觸力穩定在給定的期望值[3]。粒子群算法(Particle swarm optimization，PSO)是最早由Kennedy和Eberhart在1995年提出的一種群體智能的優化算法，該算法來源于對鳥群捕食行為的研究，模擬鳥集群飛行覓食的行為。PSO是從這種生物種群行為特征中得到啟發并用于求解優化問題，具有簡單易行、收斂速度快、設置參數少等優點[4]。在分析力控基本原理后，本文提出一種采用PSO對機器人力控過程中接觸空間的自適應阻抗控制算法的慣性、阻尼和剛度矩陣參數進行尋優的方法，以提高系統的控制性能。

1 阻抗控制模型

Hogan提出了機器人同時具有阻抗和導納的性質，因此能實現基于傳感器力反饋的機器人末端位置控制。外界的環境接觸力看作一種“干擾”信號，同時在受外界力的作用而偏離原定軌跡時向機器人傳遞具有阻抗形式的擾動信號。通過調整相應的阻抗參數，從而調節機器人與外界環境之間的動態響應。阻抗控制原理如圖1所示。

圖1 阻抗系統原理

機器人模型可以看作一個二階的阻尼-彈簧-質量阻抗系統，當機器人末端實際位置X和期望位置Xd有偏差E時，此時會使得機器人末端產生阻抗力F，即：

(1)

式中：M、B、K分別對應慣性系數、阻尼系數和剛度系數，統稱為阻抗參數。

2 自適應阻抗控制算法原理及平臺搭建

2.1 自適應阻抗控制原理

在研磨復雜面時，機械臂與環境的接觸力一直變化，自適應阻抗控制算法可以適應力變化。在很短的時間內快速準確地跟蹤位置信號并將力穩定在輸入的期望值附近，且具有很好的魯棒性[5]。N自由度機械臂在關節空間的動力學仿真如下：

(2)

(3)

閉環機器人系統滿足目標阻抗關系式：

(4)

式中：Fe表示機械臂與外部環境的接觸力；Fd是期望力；E=Xe-X,Xe是環境位置。為簡化討論，只考慮一個方向上受力情況。令fd、fe、m、b、k分別為向量Fd、Fe、M、B、K的x方向上的對應元素。

則式(4)改寫為：

(5)

控制過程分為自由空間和接觸空間兩個階段。自由空間狀態下，是機械臂與外界環境之間不存在作用力，接觸空間狀態下，是機械臂與外界環境之間存在作用力。在自由空間，機器人與外界環境之間的作用力fe=0，式(5)則改寫為：

(6)

在接觸空間內，求解式(5)的微分方程，要求對任意的ke，在fe=fd點都滿足理想的穩定條件。解得在受力狀態下的剛度系數k=0，故式(5)可改寫為：

(7)

在力控方向上的剛度增益k保持不變。可以用彈簧的受力情況表示機器人與環境之間的作用力，設fe=ke(x-xe)=-kee，式(7)可以改寫為：

(8)

(9)

(10)

(11)

式中：η為更新率；λ是采樣周期。將式(11)代入式(10)，求Laplace變換，建立位置誤差和接觸力誤差的傳遞函數。根據二階時滯系統穩定條件Taylor級數展開可得：

2.2 自適應阻抗控制器設計

設計基于位置的自適應阻抗器，為保證系統的魯棒性，需要利用已知的信息來消除高度復雜的不確定性[6]。將式(2)的機器人的動力學方程改寫為：

(12)

選擇控制律為：

(13)

整理可得：

(14)

(15)

此時控制律變換為：

(16)

(17)

自適應阻抗控制律可寫作：

(18)

通過控制規律式(18)可得到該算法的原理如圖2所示[7-9]。

圖2 自適應阻抗控制算法原理

打磨機器人為六自由度，為便于仿真得到結果，以平面二自由度連桿(如圖3所示)為例進行分析計算[10-11]。

圖3 打磨機器人模型及簡化連桿

連桿2末端位置坐標為：

(19)

雅可比矩陣為：

(20)

由式(2)可得此機械手的慣性矩陣為：

D(q)=

(21)

根據自適應阻抗控制律在Simulink中搭建實驗平臺，如圖4所示。

圖4的自適應阻抗控制器模型中f(x)是適應度函數，給定期望位置為0.876，環境位置為0.866，期望力為10 N，system函數模塊均用S-Function進行編寫。

3 粒子群算法流程及程序設計

自適應阻抗控制器的優化問題就是確定一組合適的參數M、B和K，使得系統的性能指標達到最優。在尋優過程中，將粒子群優化算法的目標函數(即適應度函數)作為評價控制系統性能的指標，并根據適應度函數計算出每個粒子對應的適應值，通過適應度值來判斷粒子優劣，得出自適應阻抗參數M、B和K對應的最優粒子。

圖5所示為PSO尋優流程，PSO與Smulink的自適應阻抗控制算法之間連接的橋梁是粒子(即自適應控制器參數)和該粒子對應的適應值(即控制系統的性能指標)。

圖5 PSO優化阻抗參數的過程示意圖

具體的算法運行過程如下[12]：

(1) 初始化粒子群。在設定的參數范圍內，隨機生成一群初試粒子。設置好粒子群規模和最大迭代次數等參數，并確定粒子迄今為止搜索到的最優位置Pt和整個粒子群迄今為止搜索到的最優位置Gt。

(2) 尋找個體極值和群體極值。對每個粒子，將其適應度值與該粒子所經過的最優位置Pt適應度值進行比較，若較好，則將其作為當前的Pt。然后將其適應度值與整個粒子群所經歷的最優位置Gt的適應度值進行比較，若較好，則將其作為當前的Gt。

(3) 粒子適應度值計算。將當前最優的粒子依次賦值給自適應阻抗控制器的參數M、B、K，然后通過sim函數運行控制系統的Simulink模型，由適應度函數計算出適應度值，以作為判斷是否更新粒子的速度和位置的條件，讓粒子向全局最優點進行尋優。在本文系統中，適應度函數采用二次型，選取線性二次型最優控制性能指標函數作為適應度函數，使粒子向誤差和輸出能量綜合起來最小的方向進行尋優[13]，即：

(22)

式中：e為控制誤差；u表示控制器輸出；分別對應控制器中的ΔF和ΔX，ΔX是x方向上的位置跟蹤誤差，ΔF為期望力和實際接觸力的誤差；ρ為常數[14]。

(4) 更新粒子的速度和位置。粒子在搜索空間中的速度和位置根據式(23)-式(24)確定。

vt+1=wvt+c1r1(Pt-xt)+c2r2(Gt-xt)

(23)

xt+1=xt+vt+1

(24)

式中：x表示粒子的位置；v表示粒子的速度，為防止粒子的盲目搜索，將位置和速度限制在區間[Lb,Ub]和[Vmin,Vmax]內；w表示慣性因子；c1、c2為加速常數；r1、r2為[0,1]區間的隨機數。本文中取w=6、c1=c2=2、Vmin=-1、Vmax=1,且確定阻抗參數M、B、K的搜索范圍。

(5) 迭代尋優并進行個體極值和群體極值更新。每次迭代后判斷是否達到最大迭代次數或小于最小適應值的條件。若符合條件則終止算法，得到最優解；若不符合條件，則返回步驟(2)。

4 實驗仿真

在simulink中，設置機械臂關節質量m1=m2=1 kg；長度l1=l2=1 m。代入式(21)可得：

(25)

在粒子群算法中，設置迭代次數為30，粒子個數為30，為了使結果盡可能精確，最小適應度值設置為0，適應度函數參數ρ=0.5。M、B、K三個參數尋優范圍的上限為Ub=[10,200,800]，下限為Lb=[1,50,300]。為了便于分析，只對x方向上的受力空間進行尋優，因此設置x方向環境剛度Ke=4 000。仿真采用固定步長、ode4算法，起始時間為0 s，仿真步長為0.01 s，仿真時間為3 s。運行結果如圖6和圖7所示。

圖7 適應度值迭代曲線

由于適應度值變化幅度較小，令fit=1 000×(適應度值-0.007 499 999)，作fit隨迭代次數變化圖如圖8所示。

分別得到尋優參數M、B、K隨迭代次數的變化曲線如圖9-圖11所示。

圖9 剛度參數K優化曲線

圖11 慣性參數M優化曲線

可以看出，當迭代次數在22次時，三個參數尋優趨于穩定，達到最優值。由MATLAB計算結果可得，PSO尋得的最優參數為M=6.513 9、B=191.755 9、K=496.728 5(結果保留四位小數)，將尋得的最優值代入自適應阻抗控制的Simulink模型中并運行，得到x方向上的位置跟蹤和力跟蹤圖如圖12和圖13所示。

可以看出，控制器在0.7 s后穩定在0.868 5的位置上，控制力在0.7 s后達到穩定，并且穩定在10 N的期望力。在力跟隨過程中的最大力約為17.5 N，最小力約為9 N。

為更加明顯地看出優化后的控制器性能，根據文獻中搭建遺傳算法平臺尋優得到參數[15]為M=3.562 6、B=105.590 2、K=377.523 9，再根據比較整定法[16]得出系統參數為M=1、B=50、K=625。

將三種優化算法尋優結果代入自適應阻抗控制器中，得到x方向上位置跟蹤和力跟蹤曲線如圖14和圖15所示。

圖14 位置跟蹤仿真結果比較

由圖14和圖15可得，遺傳算法在0.85 s后達到穩定狀態，比較整定法在2.2 s后達到穩定，且兩種方法的振蕩次數均較多。

5 結 語

本文對阻抗控制原理進行了基本介紹，并將其與自適應控制算法相結合，推導出自適應阻抗控制律。為便于研究，以二自由度連桿為例在MATLAB/Simulink軟件中搭建自適應阻抗控制器。為得到較好的系統控制性能，編寫基于M文件的粒子群尋優算法對阻抗參數進行優化。根據優化結果得到的跟蹤曲線，與遺傳算法和整定比較法相比，粒子群尋優結果在x方向上的位置和力跟蹤曲線具有更短的上升時間、下降時間，更少的振蕩次數和有更小的最大超調量。實驗結果驗證了粒子群優化算法對阻抗參數尋優的有效性和優越性，對目前打磨、裝配等機器人的過程力控制的研究具有重要意義。

本文的不足之處在于需要多次離線的仿真計算確定，耗時較長且不能實現參數的在線整定，在應用中有所不便。且只對x方向阻抗參數做了整定和優化，后續將考慮實際加工中的六維力情況并開展相關實驗研究。