改進差分進化算法優化的機器人時間最優軌跡規劃算法

郭明明，劉滿祿,2，張 華，王 姮，霍建文，朱曉明

(1.西南科技大學信息工程學院,四川 綿陽 621010；2.中國科學技術大學信息科學技術學院，安徽 合肥 230026)

0 引言

近年來，協作型工業機器人被廣泛應用于生產線焊接、裝配等任務。機器人快速、穩定的控制與軌跡規劃方法備受學者的關注。因此，在保證協作工業機器人運行過程“人-機-環”安全的前提下，利用智能算法優化軌跡規劃對于提高作業效率具有重要意義。

目前，國內外學者采用智能算法進行軌跡規劃優化。Frederik Debrouwere等人[1]針對機器人的優化控制問題，引入非凸的加加約束，解決由凸框架產生的加速度跳躍問題。居鶴華等學者[2]提出了基于遺傳算法的3-5-3多項式插值軌跡規劃算法，通過與粒子群優化(particle swarm optimization,PSO)算法的3-5-3多項式機械臂軌跡規劃對比，證明了所提算法在優化時間上的優越性。李小為等研究人員[3]在速度約束的前提下，采用PSO的時間最優3-5-3多項式插值的軌跡規劃方法，證明了該方法能夠準確地實現任意速度約束的時間最優軌跡規劃。王學琨等學者[4]提出一種差分進化(differential evolution，DE)算法優化的時間最優3-5-3多項式插值機械臂軌跡規劃方法，解決了傳統軌跡規劃方法效率不高的問題。但該方法未考慮人和機器人協作過程中的安全性問題，而且優化時間約束條件考慮不全面，優化時間未達到最優。

不同于以往未考慮協作型工業機器人在作業過程中的安全性問題，本文在確保“人-機-環”安全的基礎上，采用改進DE時間最優3-5-3多項式插值的協作型機器人軌跡規劃算法。該算法在滿足運動學約束、動力學約束與負載約束的前提下，解決了傳統的多項式插值軌跡規劃方法效率低的問題。最后，通過UR5協作型機器人平臺，驗證了該算法的有效性和可行性。

1 UR5機器人的建模及獲取工作空間

本文以協作型工業機器人UR5為試驗對象。UR5機器人的位置由前三個關節確定，機器人的姿態由后三個關節確定。

本文利用隨機概率方法獲取協作型工業機器人的工作空間。蒙特卡洛方法[5-6]是一種使用隨機數來解決計算問題的數值方法。采用蒙特卡洛方法求解UR5機器人的工作空間主要包括以下步驟。

①計算UR5機器人的正運動學方程的解，根據該解求出機器人末端參考點在機器人坐標系中的位置向量。

②求解其末端執行器的空間位置集合WΩ為：

WΩ(xi,yi,zi)=Γ[f(qi)]i=1,2,…,n

(1)

式中：f(x)為UR5機器人的正運行學；Γ(x)為機器人末端的空間位置。

③根據蒙特卡洛算法，由RAND函數隨機產生一個值作為隨機變量步長，則關節變量公式為：

(2)

根據上述步驟，可以獲得UR5機器人的工作空間WΩ。

2 碰撞檢測算法

協作型工業機器人在作業過程中，有可能與工作空間的人發生碰撞。在碰撞過程中，機器人的動量會發生較大變化[7-9]，因此，采用動量觀測器方式檢測是否發生碰撞。機器人在碰撞時緊急停止。

根據歐拉-拉格朗日方法，建立的UR5機器人的動力學方程如下：

(3)

基于廣義動量的UR5機器人的系統描述如下：

(4)

(5)

則有：

(6)

結合式(3)、式(5)和式(6)可得：

(7)

對式(7)兩邊積分：

(8)

因此，定義殘余向量：

(9)

式中:r為觀測的外力矩;k為增益矩陣。

如果UR5機器人在工作空間與人發生碰撞，即存在一個未知的擾動τf，因此，動力學方程可以表示為：

(10)

則：

(11)

因此，對式(11)進行拉普拉斯變換可得：

(12)

所以，可根據r觀測外部力矩τf。

3 基于改進DE的軌跡規劃問題

3.1 優化目標函數選取

以3-5-3多項式插值為基礎，通過改進DE方法選擇一組最優的插值時間，使協作型機器人在執行某些焊接、裝配任務時，保證運行時間最短，同時滿足運動學約束[10]、動力學約束[11]和負載約束。依據第2節觀測的外部力矩τf設計動力學約束，優化目標函數如下：

(13)

①運動學約束。

位置約束：

(14)

速度約束：

(15)

②動力學約束。

關節力矩約束：

(16)

如果機器人與人未發生碰撞，則τf=0。

負載約束：

Fgmin≤Fk≤Fgmaxk=1,2

(17)

以上公式中：i=1,2,…,n，由于UR5機器人有6個自由度，因此n=6；tij為第i個關節的運行軌跡的第1段、第2段和第3段的運行時間；f(t)為第i個關節完成三段總的運行時間；j為1,2,3；Fk為末端執行器的夾持力；Fgmin為協作型工業機器人和環境交互過程中的最小夾持力；Fgmax為最大夾持力。

3.2 多項式插值函數軌跡規劃

3-5-3次多項式插值法是結合三次多項式插值和五次多項式插值提出的一種新的軌跡規劃方法。該方法是在起始點和終止點中間選取兩個中間點，將運行過程分成三段。第一段和第三段軌跡采用三次多項式插值，第二段軌跡采用五次多項式插值。第i個關節的3-5-3多項式插值的各段軌跡方程如下。

第一段三次曲線：

(18)

第二段五次曲線:

(19)

第三段三次曲線：

(20)

式中：θi1、θi2和θi3分別為第i個關節的第一段三次多項式的運行軌跡、第二段五次多項式的運行軌跡和第三段三次多項式的運行軌跡；ai1j、ai2j和ai3j分別為多項式系數；ti1、ti2和ti3分別為第i個關節的運行軌跡的第一段、第二段和第三段的運行時間。

為求出各多項式系數，已知條件為第i個關節各段的初始點xi0,中間點xi1、xi2和末端點xi3，初始點和終點的加速度和速度為0，路徑點之間的速度和加速度連續。

根據以上推導條件，可以得出：

b=[0 0 0 0 0 0θi30 0θi00 0θi2θi1]T

(21)

a=A-1b

(22)

式中：A為關于時間的矩陣;A-1為A的逆矩陣。

a=[ai13ai12ai11ai10ai25ai24ai23ai22ai21ai20ai33ai32ai31ai30]T

3.3 基于改進DE的軌跡規劃

本文在3-5-3多項式插值軌跡規劃的基礎上，利用給定的UR5機器人各個關節角度狀態的起始點、終點、中間路徑點，各關節的初始速度、加速度、最大運行速度，采用改進DE算法，以優化函數式(13)最小為目的，滿足約束。

考慮到種群的多樣性，在UR5機器人的工作空間，采用統一的概率分布。通過初始化種群，染色體采用實數編碼[12]，隨機產生每個個體的染色體為：

Xji=lowji+rand×(highji-lowji)

(23)

式中：highji為遺傳基因染色體的上限；lowji為遺傳基因染色體的下限；rand為[0,1]內的隨機數；j∈[1,NP],NP為種群個體個數;i∈[1,M],M為遺傳代數。

當選擇的個體使式(13)值為最小時，選擇的個體達到最優。

進化操作時，首先計算每個個體對應的交叉因子CR和變異因子F，然后按照DE/rand計算每個新個體，可以得到：

Vkj=Wr1G+F×(Vr2G-Vr3G)

(24)

式中：r1≠r2≠r3≠j，分別為[1，NP]內的隨機整數；新個體V由W生成。

基于以上分析，本文提出的改進DE交互式工業機器人的時間最優軌跡規劃算法流程圖如圖1所示。

圖1 軌跡規劃算法流程圖

算法具體步驟如下。

①給出軌跡規劃任務中的初始點、兩個中間點，以及終止點的位置、速度和加速度，設置每個關節的最大允許位置、速度和加速度，對這三段軌跡的時間分別優化，設置優化目標函數的最大速度Vmax和最大關節力矩τmax。為了便于驗證本文提出的軌跡規劃方法，令τf=0。

②初始化差分進化算法的參數，設置N、M、F和CR。

③分別設置ti1、ti2、ti3的可行性范圍，通過3-5-3多項式軌跡方程,求得每個關節的三段軌跡規劃的時間，構成初始種群。

④對初始種群進行交叉、變異，得到試驗種群。

⑤對得到的初始種群和試驗種群進行適應度計算，查看是否超過設定的最大值。如果初始種群和試驗種群的粒子都滿足速度要求，則選擇較小的t；如果有一個粒子沒有滿足，則選擇滿足的粒子；如果都不滿足，則選擇超出約束量小的粒子。

⑥判斷是否達到終止條件。如沒有達到條件，則轉到步驟④。

⑦重復步驟②到步驟⑥的操作，分別得到三段軌跡的最優時間，把它們相加得到整個軌跡規劃最優時間。

4 試驗結果與分析

本文以六自由度協作型工業機器人UR5為研究對象，采用D-H法建立UR5機器人模型，在MATLAB平臺下，驗證本文所提出的基于改進DE的時間最優的軌跡規劃算法。為方便驗證，本文以UR5機器人的前三個關節為研究對象，后三個關節的驗證和前三個關節一致。在前文中已經建立了UR5機器人的模型，并求取了工作空間。在工作空間下，獲得UR5機器人的初始位置和末端任務位置，在確保無碰撞的前提下，選取初始位置和末端任務位置的中間點。前三個關節對應的角度狀態如表1所示。

表1 角度狀態

使用3-5-3多項式插值的軌跡規劃進行點到點的運動，多項式插值軌跡規劃圖如圖2所示。單獨使用3-5-3多項式插值進行軌跡規劃，設置第一段軌跡運行時間為2 s，第二段軌跡運行時間為4 s，第三段軌跡運行時間為2 s，起始點終止點的速度、加速度均為0。由三個關節的位置曲線、速度曲線和加速度曲線可以看出，單獨使用3-5-3多項式插值法規劃機器人的運行軌跡的位置、速度和加速度都連續，但運行速度并未接近最大速度Vmax，加速度的變化幅度也較大。加速度的抖動也會導致UR5機器人在作業過程中對設備本身產生不良影響。

圖2 多項式插值軌跡規劃圖

使用基于改進DE優化的3-5-3多項式插值法，根據3.3節中的步驟優化。設置N為50，M為150，F為0.5，CR為0.7，時間取值范圍為0.5～5 s，三個關節對應的角度狀態同表1。設置UR5機器人運行最大角速度設置為3.14 rad/s，UR5機器人的最大關節力矩為100 N，協作型工業機器人和環境交互的夾持力的最小值為0 N，最大值為50 N。總體進化代數最優時間曲線如圖3所示。

圖3 總體進化代數最優時間曲線

經過優化得到第一段軌跡最優時間為1.151 s，第二段軌跡最優時間為2.351 s，第三段最優時間為0.800 s，所以總的運行時間為4.302 s，比未進行優化的3-5-3多項式軌跡規劃節省了3.698 s。經過改進DE優化后的UR5機器人的位置曲線、速度曲線、加速度曲線均連續，能夠在滿足運動學約束、動力學約束和負載約束的前提下，使UR5機器人在不損壞設備的情況下，執行任務時間更短。改進機器人關節性能曲線如圖4所示。

圖4 改進機器人關節性能曲線

5 結束語

在執行某些焊接、裝配任務時，協作型工業機器人在運行過程的關節狀態可能因沒有達到最大約束而影響效率。在確保“人-機-環”安全的前提下，提出了碰撞檢測方法和基于改進DE的機器人時間優化方法。在3-5-3多項式插值的協作型工業機器人的軌跡規劃的基礎上，基于改進DE的協作型工業機器人的作業時間可以減少3.699 s，優于傳統方法。通過試驗驗證了該方法的可行性和有效性。

與現有的基于差分進化算法等智能算法的機器人軌跡規劃不同，本文方法能夠根據DE的優化能力對時間進行優化，該方法的不足之處在于DE的收斂速度相對較慢。

[1] DEBROUWERE F,LOOCK W V,PIPELEERS G,et al.Time-optimal path following for robots with trajectory jerk constraints using sequential convex programming[C]//IEEE International Conference on Robotics and Automation.2013:1916-1921.

[2] 居鶴華,付榮.基于GA的時間最優機械臂軌跡規劃算法[J].計算機應用研究,2011,19(9):3275-3278.

[3] 李小為,胡立坤,王琥.速度約束下PSO的六自由度機械臂時間最優軌跡規劃[J].智能系統學報,2015(3):393-398.

[4] 王學琨,李剛,周東凱,等.基于DE的時間最優6-DOF機械臂軌跡規劃算法[J].計算機仿真,2015,32(8):332-337.

[5] 印峰,王耀南,余洪山.基于蒙特卡羅方法的除冰機器人作業空間邊界提取[J].控制理論與應用,2010(7):891-896.

[6] 王婷.RoboCup人形組足球機器人的蒙特卡羅自主定位算法研究[D].長春：吉林大學,2012.

[7] ZHIJU M A,HAIBIN W U.Sensorless collision detection for robot manipulator[J].Journal of Test & Measurement Technology,2013,27(3):254-259.

[8] HE S,YE J,LI Z,et al.A momentum-based collision detection algorithm for industrial robots[C]// IEEE International Conference on Robotics and Biomimetics,IEEE,2015:1253-1259.

[9] 吳海彬,李實懿,吳國魁.基于動量偏差觀測器的機器人碰撞檢測算法[J].電機與控制學報,2015,19(5):97-104.

[10]FARES J,FRANCISCO V,VICENTE M.Evolutionary path planning algorithm for industrial Robots[J].Advanced Robotics,2012,26(11-12):1369-1392.

[11]LUO L P,YUAN C,YAN R J,et al.Trajectory planning for energy minimization of industry robotic manipulators using the Lagrange interpolation method[J].International Journal of Precision Engineering & Manufacturing,2015,16(5):911-917.

[12]李飛,劉建昌,石懷濤,等.基于分解和差分進化的多目標粒子群優化算法[J].控制與決策,2017(3):403-410.