改進(jìn)差分進(jìn)化算法優(yōu)化的機(jī)器人時(shí)間最優(yōu)軌跡規(guī)劃算法

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

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

0 引言

近年來，協(xié)作型工業(yè)機(jī)器人被廣泛應(yīng)用于生產(chǎn)線焊接、裝配等任務(wù)。機(jī)器人快速、穩(wěn)定的控制與軌跡規(guī)劃方法備受學(xué)者的關(guān)注。因此，在保證協(xié)作工業(yè)機(jī)器人運(yùn)行過程“人-機(jī)-環(huán)”安全的前提下，利用智能算法優(yōu)化軌跡規(guī)劃對(duì)于提高作業(yè)效率具有重要意義。

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

不同于以往未考慮協(xié)作型工業(yè)機(jī)器人在作業(yè)過程中的安全性問題，本文在確?！叭?機(jī)-環(huán)”安全的基礎(chǔ)上，采用改進(jìn)DE時(shí)間最優(yōu)3-5-3多項(xiàng)式插值的協(xié)作型機(jī)器人軌跡規(guī)劃算法。該算法在滿足運(yùn)動(dòng)學(xué)約束、動(dòng)力學(xué)約束與負(fù)載約束的前提下，解決了傳統(tǒng)的多項(xiàng)式插值軌跡規(guī)劃方法效率低的問題。最后，通過UR5協(xié)作型機(jī)器人平臺(tái)，驗(yàn)證了該算法的有效性和可行性。

1 UR5機(jī)器人的建模及獲取工作空間

本文以協(xié)作型工業(yè)機(jī)器人UR5為試驗(yàn)對(duì)象。UR5機(jī)器人的位置由前三個(gè)關(guān)節(jié)確定，機(jī)器人的姿態(tài)由后三個(gè)關(guān)節(jié)確定。

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

①計(jì)算UR5機(jī)器人的正運(yùn)動(dòng)學(xué)方程的解，根據(jù)該解求出機(jī)器人末端參考點(diǎn)在機(jī)器人坐標(biāo)系中的位置向量。

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

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

(1)

式中：f(x)為UR5機(jī)器人的正運(yùn)行學(xué)；Γ(x)為機(jī)器人末端的空間位置。

③根據(jù)蒙特卡洛算法，由RAND函數(shù)隨機(jī)產(chǎn)生一個(gè)值作為隨機(jī)變量步長，則關(guān)節(jié)變量公式為：

(2)

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

2 碰撞檢測(cè)算法

協(xié)作型工業(yè)機(jī)器人在作業(yè)過程中，有可能與工作空間的人發(fā)生碰撞。在碰撞過程中，機(jī)器人的動(dòng)量會(huì)發(fā)生較大變化[7-9]，因此，采用動(dòng)量觀測(cè)器方式檢測(cè)是否發(fā)生碰撞。機(jī)器人在碰撞時(shí)緊急停止。

根據(jù)歐拉-拉格朗日方法，建立的UR5機(jī)器人的動(dòng)力學(xué)方程如下：

(3)

基于廣義動(dòng)量的UR5機(jī)器人的系統(tǒng)描述如下：

(4)

(5)

則有：

(6)

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

(7)

對(duì)式(7)兩邊積分：

(8)

因此，定義殘余向量：

(9)

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

如果UR5機(jī)器人在工作空間與人發(fā)生碰撞，即存在一個(gè)未知的擾動(dòng)τf，因此，動(dòng)力學(xué)方程可以表示為：

(10)

則：

(11)

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

(12)

所以，可根據(jù)r觀測(cè)外部力矩τf。

3 基于改進(jìn)DE的軌跡規(guī)劃問題

3.1 優(yōu)化目標(biāo)函數(shù)選取

以3-5-3多項(xiàng)式插值為基礎(chǔ)，通過改進(jìn)DE方法選擇一組最優(yōu)的插值時(shí)間，使協(xié)作型機(jī)器人在執(zhí)行某些焊接、裝配任務(wù)時(shí)，保證運(yùn)行時(shí)間最短，同時(shí)滿足運(yùn)動(dòng)學(xué)約束[10]、動(dòng)力學(xué)約束[11]和負(fù)載約束。依據(jù)第2節(jié)觀測(cè)的外部力矩τf設(shè)計(jì)動(dòng)力學(xué)約束，優(yōu)化目標(biāo)函數(shù)如下：

(13)

①運(yùn)動(dòng)學(xué)約束。

位置約束：

(14)

速度約束：

(15)

②動(dòng)力學(xué)約束。

關(guān)節(jié)力矩約束：

(16)

如果機(jī)器人與人未發(fā)生碰撞，則τf=0。

負(fù)載約束：

Fgmin≤Fk≤Fgmaxk=1,2

(17)

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

3.2 多項(xiàng)式插值函數(shù)軌跡規(guī)劃

3-5-3次多項(xiàng)式插值法是結(jié)合三次多項(xiàng)式插值和五次多項(xiàng)式插值提出的一種新的軌跡規(guī)劃方法。該方法是在起始點(diǎn)和終止點(diǎn)中間選取兩個(gè)中間點(diǎn)，將運(yùn)行過程分成三段。第一段和第三段軌跡采用三次多項(xiàng)式插值，第二段軌跡采用五次多項(xiàng)式插值。第i個(gè)關(guān)節(jié)的3-5-3多項(xiàng)式插值的各段軌跡方程如下。

第一段三次曲線：

(18)

第二段五次曲線:

(19)

第三段三次曲線：

(20)

式中：θi1、θi2和θi3分別為第i個(gè)關(guān)節(jié)的第一段三次多項(xiàng)式的運(yùn)行軌跡、第二段五次多項(xiàng)式的運(yùn)行軌跡和第三段三次多項(xiàng)式的運(yùn)行軌跡；ai1j、ai2j和ai3j分別為多項(xiàng)式系數(shù)；ti1、ti2和ti3分別為第i個(gè)關(guān)節(jié)的運(yùn)行軌跡的第一段、第二段和第三段的運(yùn)行時(shí)間。

為求出各多項(xiàng)式系數(shù)，已知條件為第i個(gè)關(guān)節(jié)各段的初始點(diǎn)xi0,中間點(diǎn)xi1、xi2和末端點(diǎn)xi3，初始點(diǎn)和終點(diǎn)的加速度和速度為0，路徑點(diǎn)之間的速度和加速度連續(xù)。

根據(jù)以上推導(dǎo)條件，可以得出：

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

(21)

a=A-1b

(22)

式中：A為關(guān)于時(shí)間的矩陣;A-1為A的逆矩陣。

a=[ai13ai12ai11ai10ai25ai24ai23ai22ai21ai20ai33ai32ai31ai30]T

3.3 基于改進(jìn)DE的軌跡規(guī)劃

本文在3-5-3多項(xiàng)式插值軌跡規(guī)劃的基礎(chǔ)上，利用給定的UR5機(jī)器人各個(gè)關(guān)節(jié)角度狀態(tài)的起始點(diǎn)、終點(diǎn)、中間路徑點(diǎn)，各關(guān)節(jié)的初始速度、加速度、最大運(yùn)行速度，采用改進(jìn)DE算法，以優(yōu)化函數(shù)式(13)最小為目的，滿足約束。

考慮到種群的多樣性，在UR5機(jī)器人的工作空間，采用統(tǒng)一的概率分布。通過初始化種群，染色體采用實(shí)數(shù)編碼[12]，隨機(jī)產(chǎn)生每個(gè)個(gè)體的染色體為：

Xji=lowji+rand×(highji-lowji)

(23)

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

當(dāng)選擇的個(gè)體使式(13)值為最小時(shí)，選擇的個(gè)體達(dá)到最優(yōu)。

進(jìn)化操作時(shí)，首先計(jì)算每個(gè)個(gè)體對(duì)應(yīng)的交叉因子CR和變異因子F，然后按照DE/rand計(jì)算每個(gè)新個(gè)體，可以得到：

Vkj=Wr1G+F×(Vr2G-Vr3G)

(24)

式中：r1≠r2≠r3≠j，分別為[1，NP]內(nèi)的隨機(jī)整數(shù)；新個(gè)體V由W生成。

基于以上分析，本文提出的改進(jìn)DE交互式工業(yè)機(jī)器人的時(shí)間最優(yōu)軌跡規(guī)劃算法流程圖如圖1所示。

圖1 軌跡規(guī)劃算法流程圖

算法具體步驟如下。

①給出軌跡規(guī)劃任務(wù)中的初始點(diǎn)、兩個(gè)中間點(diǎn)，以及終止點(diǎn)的位置、速度和加速度，設(shè)置每個(gè)關(guān)節(jié)的最大允許位置、速度和加速度，對(duì)這三段軌跡的時(shí)間分別優(yōu)化，設(shè)置優(yōu)化目標(biāo)函數(shù)的最大速度Vmax和最大關(guān)節(jié)力矩τmax。為了便于驗(yàn)證本文提出的軌跡規(guī)劃方法，令τf=0。

②初始化差分進(jìn)化算法的參數(shù)，設(shè)置N、M、F和CR。

③分別設(shè)置ti1、ti2、ti3的可行性范圍，通過3-5-3多項(xiàng)式軌跡方程,求得每個(gè)關(guān)節(jié)的三段軌跡規(guī)劃的時(shí)間，構(gòu)成初始種群。

④對(duì)初始種群進(jìn)行交叉、變異，得到試驗(yàn)種群。

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

⑥判斷是否達(dá)到終止條件。如沒有達(dá)到條件，則轉(zhuǎn)到步驟④。

⑦重復(fù)步驟②到步驟⑥的操作，分別得到三段軌跡的最優(yōu)時(shí)間，把它們相加得到整個(gè)軌跡規(guī)劃最優(yōu)時(shí)間。

4 試驗(yàn)結(jié)果與分析

本文以六自由度協(xié)作型工業(yè)機(jī)器人UR5為研究對(duì)象，采用D-H法建立UR5機(jī)器人模型，在MATLAB平臺(tái)下，驗(yàn)證本文所提出的基于改進(jìn)DE的時(shí)間最優(yōu)的軌跡規(guī)劃算法。為方便驗(yàn)證，本文以UR5機(jī)器人的前三個(gè)關(guān)節(jié)為研究對(duì)象，后三個(gè)關(guān)節(jié)的驗(yàn)證和前三個(gè)關(guān)節(jié)一致。在前文中已經(jīng)建立了UR5機(jī)器人的模型，并求取了工作空間。在工作空間下，獲得UR5機(jī)器人的初始位置和末端任務(wù)位置，在確保無碰撞的前提下，選取初始位置和末端任務(wù)位置的中間點(diǎn)。前三個(gè)關(guān)節(jié)對(duì)應(yīng)的角度狀態(tài)如表1所示。

表1 角度狀態(tài)

使用3-5-3多項(xiàng)式插值的軌跡規(guī)劃進(jìn)行點(diǎn)到點(diǎn)的運(yùn)動(dòng)，多項(xiàng)式插值軌跡規(guī)劃圖如圖2所示。單獨(dú)使用3-5-3多項(xiàng)式插值進(jìn)行軌跡規(guī)劃，設(shè)置第一段軌跡運(yùn)行時(shí)間為2 s，第二段軌跡運(yùn)行時(shí)間為4 s，第三段軌跡運(yùn)行時(shí)間為2 s，起始點(diǎn)終止點(diǎn)的速度、加速度均為0。由三個(gè)關(guān)節(jié)的位置曲線、速度曲線和加速度曲線可以看出，單獨(dú)使用3-5-3多項(xiàng)式插值法規(guī)劃機(jī)器人的運(yùn)行軌跡的位置、速度和加速度都連續(xù)，但運(yùn)行速度并未接近最大速度Vmax，加速度的變化幅度也較大。加速度的抖動(dòng)也會(huì)導(dǎo)致UR5機(jī)器人在作業(yè)過程中對(duì)設(shè)備本身產(chǎn)生不良影響。

圖2 多項(xiàng)式插值軌跡規(guī)劃圖

使用基于改進(jìn)DE優(yōu)化的3-5-3多項(xiàng)式插值法，根據(jù)3.3節(jié)中的步驟優(yōu)化。設(shè)置N為50，M為150，F(xiàn)為0.5，CR為0.7，時(shí)間取值范圍為0.5～5 s，三個(gè)關(guān)節(jié)對(duì)應(yīng)的角度狀態(tài)同表1。設(shè)置UR5機(jī)器人運(yùn)行最大角速度設(shè)置為3.14 rad/s，UR5機(jī)器人的最大關(guān)節(jié)力矩為100 N，協(xié)作型工業(yè)機(jī)器人和環(huán)境交互的夾持力的最小值為0 N，最大值為50 N?？傮w進(jìn)化代數(shù)最優(yōu)時(shí)間曲線如圖3所示。

圖3 總體進(jìn)化代數(shù)最優(yōu)時(shí)間曲線

經(jīng)過優(yōu)化得到第一段軌跡最優(yōu)時(shí)間為1.151 s，第二段軌跡最優(yōu)時(shí)間為2.351 s，第三段最優(yōu)時(shí)間為0.800 s，所以總的運(yùn)行時(shí)間為4.302 s，比未進(jìn)行優(yōu)化的3-5-3多項(xiàng)式軌跡規(guī)劃節(jié)省了3.698 s。經(jīng)過改進(jìn)DE優(yōu)化后的UR5機(jī)器人的位置曲線、速度曲線、加速度曲線均連續(xù)，能夠在滿足運(yùn)動(dòng)學(xué)約束、動(dòng)力學(xué)約束和負(fù)載約束的前提下，使UR5機(jī)器人在不損壞設(shè)備的情況下，執(zhí)行任務(wù)時(shí)間更短。改進(jìn)機(jī)器人關(guān)節(jié)性能曲線如圖4所示。

圖4 改進(jìn)機(jī)器人關(guān)節(jié)性能曲線

5 結(jié)束語

在執(zhí)行某些焊接、裝配任務(wù)時(shí)，協(xié)作型工業(yè)機(jī)器人在運(yùn)行過程的關(guān)節(jié)狀態(tài)可能因沒有達(dá)到最大約束而影響效率。在確?！叭?機(jī)-環(huán)”安全的前提下，提出了碰撞檢測(cè)方法和基于改進(jìn)DE的機(jī)器人時(shí)間優(yōu)化方法。在3-5-3多項(xiàng)式插值的協(xié)作型工業(yè)機(jī)器人的軌跡規(guī)劃的基礎(chǔ)上，基于改進(jìn)DE的協(xié)作型工業(yè)機(jī)器人的作業(yè)時(shí)間可以減少3.699 s，優(yōu)于傳統(tǒng)方法。通過試驗(yàn)驗(yàn)證了該方法的可行性和有效性。

與現(xiàn)有的基于差分進(jìn)化算法等智能算法的機(jī)器人軌跡規(guī)劃不同，本文方法能夠根據(jù)DE的優(yōu)化能力對(duì)時(shí)間進(jìn)行優(yōu)化，該方法的不足之處在于DE的收斂速度相對(duì)較慢。

[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的時(shí)間最優(yōu)機(jī)械臂軌跡規(guī)劃算法[J].計(jì)算機(jī)應(yīng)用研究,2011,19(9):3275-3278.

[3] 李小為,胡立坤,王琥.速度約束下PSO的六自由度機(jī)械臂時(shí)間最優(yōu)軌跡規(guī)劃[J].智能系統(tǒng)學(xué)報(bào),2015(3):393-398.

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

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

[6] 王婷.RoboCup人形組足球機(jī)器人的蒙特卡羅自主定位算法研究[D].長春：吉林大學(xué),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] 吳海彬,李實(shí)懿,吳國魁.基于動(dòng)量偏差觀測(cè)器的機(jī)器人碰撞檢測(cè)算法[J].電機(jī)與控制學(xué)報(bào),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ìn)化的多目標(biāo)粒子群優(yōu)化算法[J].控制與決策,2017(3):403-410.