基于環(huán)境吸引域的冗余機(jī)器人運(yùn)動(dòng)控制與實(shí)時(shí)位型優(yōu)化

呂曉靜，徐智浩，徐恩華

(1.廣州民航職業(yè)技術(shù)學(xué)院飛機(jī)維修工程學(xué)院，廣東廣州 510403；2.廣東省科學(xué)院智能制造研究所，廣東省現(xiàn)代控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，廣東廣州 510070；3.廣州民航職業(yè)技術(shù)學(xué)院民航經(jīng)營(yíng)管理學(xué)院，廣東廣州 510403)

0 前言

隨著智能制造國(guó)家戰(zhàn)略的縱深發(fā)展，以多品種、多型號(hào)為導(dǎo)向的智能柔性制造成為機(jī)器人化制造的一種重要趨勢(shì)。人機(jī)協(xié)作融合了人類的操作智慧和機(jī)器人的操作穩(wěn)定性，是實(shí)現(xiàn)智能柔性生產(chǎn)的重要途徑。

在人機(jī)協(xié)作模式下，機(jī)器人與人類的作業(yè)空間更加緊湊，導(dǎo)致機(jī)器人與人類作業(yè)空間高度重疊，如圖1所示。由于空間的限制，機(jī)器人的實(shí)際工作空間被限制在一個(gè)狹窄的范圍內(nèi)。這些狹窄的空間是靈活的，盡管機(jī)器人不必嚴(yán)格遵守，但人類希望機(jī)器人主要在其中活動(dòng)。上述愿景給機(jī)器人的運(yùn)動(dòng)控制帶來(lái)了巨大挑戰(zhàn)，主要體現(xiàn)在以下幾個(gè)方面：(1)機(jī)器人任務(wù)的動(dòng)態(tài)性。在有人參與的生產(chǎn)線上，由于人為操作和任務(wù)變化引入的不確定性，機(jī)器人要完成的任務(wù)往往不是固定、重復(fù)的，這將使傳統(tǒng)的離線計(jì)劃控制方法難以適應(yīng)任務(wù)要求；(2)在部署機(jī)器人的過(guò)程中，要求快速定義和修改首選領(lǐng)域；(3)在機(jī)器人的實(shí)時(shí)控制過(guò)程中，必須同時(shí)滿足系統(tǒng)的物理約束。

圖1 機(jī)器人機(jī)協(xié)作工作場(chǎng)景示例

在已有研究中，基于RRT的軌跡規(guī)劃方法應(yīng)用較多。文獻(xiàn)[1]在RRT邊界探索算法的基礎(chǔ)上，引入GFE算法進(jìn)行細(xì)化搜索，并提取連續(xù)邊界域的形心作為探索目標(biāo)點(diǎn)，該方法在多機(jī)器人協(xié)同空間探索任務(wù)中效率更高。文獻(xiàn)[2]針對(duì)傳統(tǒng)RRT算法在機(jī)械臂的運(yùn)動(dòng)規(guī)劃上缺乏導(dǎo)向性導(dǎo)致收斂速度慢的問(wèn)題，提出一種擴(kuò)展點(diǎn)選擇策略和自適應(yīng)步長(zhǎng)策略，有效避免了算法陷入極小值。文獻(xiàn)[3]提出一種快速探索隨機(jī)樹固定節(jié)點(diǎn)算法，該算法在搜索過(guò)程中限制節(jié)點(diǎn)數(shù)，同時(shí)對(duì)采樣節(jié)點(diǎn)擴(kuò)展和父節(jié)點(diǎn)選擇方法進(jìn)行了類似的設(shè)計(jì)。文獻(xiàn)[4]提出在生成新點(diǎn)之前先確定父節(jié)點(diǎn)可以提高搜索性能，但該方法存在局部最小值問(wèn)題。

針對(duì)狹小的工作空間，主流的規(guī)劃方法是將空間的限制描述為避障問(wèn)題。文獻(xiàn)[5]提出一種基于水平集的機(jī)器人避障方法，其主要思想是用一組幾何基元逼近狹窄空間的邊界，然后通過(guò)組合一組水平集函數(shù)得到的不等式約束重新表述避障問(wèn)題。文獻(xiàn)[6]在規(guī)劃過(guò)程中考慮了機(jī)器人的運(yùn)動(dòng)學(xué)模型，構(gòu)造了運(yùn)動(dòng)態(tài)勢(shì)感知圖(MSAM)，使機(jī)器人能夠更好地感知周圍環(huán)境信息以做出最佳決策。文獻(xiàn)[7]提出一種基于距離計(jì)算和離散檢測(cè)的路徑規(guī)劃方法。文獻(xiàn)[8]針對(duì)人工勢(shì)場(chǎng)法容易出現(xiàn)的局部極小值問(wèn)題和目標(biāo)不可達(dá)問(wèn)題進(jìn)行分析，引入了虛擬障礙物模型，較好地解決了人工勢(shì)場(chǎng)法的局部極小值問(wèn)題；文獻(xiàn)[9]提出一種基于采樣的規(guī)劃方法，其主要思想是使用基于學(xué)習(xí)的運(yùn)動(dòng)建模方法來(lái)預(yù)測(cè)障礙物，該方法在移動(dòng)障礙物等較為雜亂的環(huán)境中顯示出優(yōu)越性。

近年來(lái)，隨著深度學(xué)習(xí)和神經(jīng)網(wǎng)絡(luò)的快速發(fā)展，智能無(wú)碰撞路徑規(guī)劃方法得到了廣泛的研究。文獻(xiàn)[10]提出一種改進(jìn)的基于梯度投影的偽逆方法來(lái)獲得任務(wù)空間中的無(wú)碰撞軌跡，針對(duì)可調(diào)增益選擇引起的軌跡不連續(xù)問(wèn)題，建立了一種反向傳播神經(jīng)網(wǎng)絡(luò)來(lái)產(chǎn)生更平滑的路徑。模型驅(qū)動(dòng)神經(jīng)網(wǎng)絡(luò)結(jié)合了神經(jīng)網(wǎng)絡(luò)的學(xué)習(xí)性能和自適應(yīng)性能，以及機(jī)器人模型所帶來(lái)的客觀物理規(guī)律和穩(wěn)定性，在機(jī)器人控制領(lǐng)域得到了廣泛的應(yīng)用。文獻(xiàn)[11]提出一種基于不等式的避障方法，首先討論了4種不同的QP在線算法，然后在關(guān)節(jié)速度層面建立了基于LVI的原始對(duì)偶神經(jīng)網(wǎng)絡(luò)。

在上述研究的基礎(chǔ)上，本文作者提出一種基于環(huán)境吸引域的新型運(yùn)動(dòng)控制與位型優(yōu)化方法。該方法采用一組關(guān)鍵點(diǎn)定義人類對(duì)機(jī)器人的預(yù)設(shè)工作區(qū)域，并構(gòu)建了機(jī)器人本體與該關(guān)鍵點(diǎn)集的距離指標(biāo)；設(shè)計(jì)了兼顧末端執(zhí)行器跟蹤、環(huán)境吸引域指標(biāo)優(yōu)化與物理約束不等式的控制問(wèn)題模型，并建立了遞歸神經(jīng)網(wǎng)絡(luò)進(jìn)行實(shí)時(shí)求解。該方法能夠在保證機(jī)器人末端完成高精度運(yùn)動(dòng)控制的同時(shí)，使機(jī)器人的工作區(qū)域盡可能收縮到人類預(yù)設(shè)的區(qū)域，從而有效保證人機(jī)協(xié)作安全性。

1 問(wèn)題描述

1.1 機(jī)器人模型

不失一般性，文中所考慮的機(jī)器人具有串聯(lián)結(jié)構(gòu)，所有關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)，沒有結(jié)構(gòu)柔性。對(duì)于自由度為n的冗余機(jī)器人，其正向運(yùn)動(dòng)學(xué)模型可以描述為

x=f(θ)

(1)

其中：θ(t)∈Rn是關(guān)節(jié)角向量；x(t)∈Rm是描述末端執(zhí)行器在笛卡爾空間中位置的向量；f(?)：Rn→Rm是一個(gè)非線性函數(shù)，描述了從關(guān)節(jié)空間到笛卡爾空間的映射。對(duì)式(1)求導(dǎo)可得：

(2)

1.2 本體關(guān)鍵點(diǎn)

為了便于對(duì)機(jī)器人本體位形進(jìn)行描述，利用機(jī)器人本體上的關(guān)鍵點(diǎn)(Body Key Points，BKP)進(jìn)行表征。如圖2所示，通過(guò)選取機(jī)器人的BKP，可以對(duì)機(jī)器人本體在笛卡爾空間中的分布情況進(jìn)行刻畫。BKP通常是每個(gè)關(guān)節(jié)中心以及連桿上均勻分布的離散點(diǎn)。

圖2 機(jī)器人的關(guān)鍵點(diǎn)和環(huán)境原理

與BKPs類似，定義關(guān)鍵點(diǎn)集P={P1，P2，…，Pn1}近似描述偏好工作區(qū)域，則偏好工作區(qū)域與機(jī)器人本體之間的廣義距離可描述為點(diǎn)集P與K之間距離綜合：

(3)

1.3 基于動(dòng)態(tài)規(guī)劃的初始描述

(4a)

(4b)

θ-≤θ≤θ+

(4c)

(4d)

在實(shí)時(shí)解決原始動(dòng)態(tài)規(guī)劃問(wèn)題時(shí)，優(yōu)化指標(biāo)與實(shí)際需要解決方案的決策變量角速度隱相關(guān)。此外，機(jī)器人的角速度與優(yōu)化指標(biāo)的變化率直接相關(guān)。將式(4a)在時(shí)間維度上展開，并假設(shè)吸引域?yàn)殪o態(tài)或準(zhǔn)靜態(tài)，可以得到虛擬吸引力與機(jī)器人運(yùn)動(dòng)之間的關(guān)系：

(5)

在時(shí)間維度上對(duì)優(yōu)化指標(biāo)進(jìn)行重構(gòu)，可得：

(6a)

(6b)

θ-≤θ≤θ+

(6c)

(6d)

(7a)

(7b)

(7c)

(8a)

(8b)

(8c)

其中：c1和c2是兩個(gè)可調(diào)的正參數(shù)。對(duì)于式(8)，其高度非線性的性質(zhì)使機(jī)器人的實(shí)時(shí)控制量獲取變得困難。

1.4 控制器設(shè)計(jì)

提出一種模型驅(qū)動(dòng)的神經(jīng)網(wǎng)絡(luò)來(lái)實(shí)時(shí)解決非線性約束優(yōu)化問(wèn)題。引入一個(gè)用于處理等式約束(8b)的對(duì)偶變量λ，拉格朗日函數(shù)L可以定義為

(9)

根據(jù)Karush-Kuhn-Tucker條件，式(9)的最優(yōu)解滿足

(10)

其中，

(11)

由此可得：

(12)

(13)

其中：∈為正參數(shù)，主要用于調(diào)節(jié)控制器的收斂速度。

式(13)為所構(gòu)造控制器的一般形式。在實(shí)際控制過(guò)程中，為了盡可能簡(jiǎn)化控制器，可以選擇適當(dāng)?shù)膮?shù)和變量來(lái)減少控制器的結(jié)構(gòu)。例如取c2=1，式(13)可以簡(jiǎn)化為

此處為對(duì)提出控制器的進(jìn)一步討論。在后面的證明中，為了驗(yàn)證控制器的通用性，仍然選擇公式(13)作為對(duì)象分析系統(tǒng)穩(wěn)定性。

1.5 穩(wěn)定性分析

(14)

(15)

根據(jù)F(ξ)的定義，將式(13)進(jìn)一步改寫為

(16)

(17)

對(duì)應(yīng)于式(13)的最優(yōu)解。

2 數(shù)值仿真

分別以平面四自由度機(jī)器人與七自由度協(xié)作機(jī)器人FRANKA PANDA為仿真對(duì)象，通過(guò)與傳統(tǒng)方法的對(duì)比，說(shuō)明文中所提算法的有效性。

2.1 仿真1

以平面四自由度機(jī)器人為對(duì)象，其各連桿長(zhǎng)度為l1=0.296 m、l2=0.296 m、l3=0.296 m、l4=0.212 m；選擇機(jī)器人每個(gè)連桿的中點(diǎn)和節(jié)點(diǎn)作為BKP；選取機(jī)器人初始關(guān)節(jié)為θ(0)=[π/2，-π/3，-π/4，0]T；調(diào)整控制參數(shù)為c1=1、c2=1、∈=0.001、k=5、κ=10，定義機(jī)器人的期望軌跡為xd=[0.4+0.1cos(0.5t)，0.35+0.1sin(0.5t)]T，并與JMPI方法進(jìn)行對(duì)比。設(shè)J-1為雅克比矩陣J的偽逆，并在機(jī)器人接近奇異位形時(shí)引入阻尼，同時(shí)對(duì)機(jī)器人角速度進(jìn)行限幅處理。為保證對(duì)比試驗(yàn)的一致性，k同樣設(shè)置為5。對(duì)比結(jié)果如圖3、4所示。對(duì)比圖3(a)和圖3(b)可以看出：兩種控制方法都可以跟蹤期望的軌跡，收斂時(shí)間約為1.5 s，穩(wěn)態(tài)誤差分別為2×10-6、2×10-4m，表明所提方法在軌跡跟蹤方面與主流方法相差不大。圖3(c)和圖3(d)為兩種算法下機(jī)器人關(guān)節(jié)角度的對(duì)比?；贘MPI方法，機(jī)器人的關(guān)節(jié)角1在t=0.8～15 s時(shí)間段內(nèi)超過(guò)了物理極限；相比之下，基于文中提出的方法，機(jī)器人的關(guān)節(jié)角度可以保持在最大(小)值，從而保證了系統(tǒng)的安全。在角速度方面，JMPI方法與文中提出的模型驅(qū)動(dòng)方法(圖3(f))不同，無(wú)法保證仿真初始階段(圖3(e))機(jī)器人角速度的有界性(t=0～0.5 s)。

圖3 所提算法與JMPI方法在平面四自由度機(jī)器人的仿真對(duì)比

在此基礎(chǔ)上，進(jìn)一步驗(yàn)證引入環(huán)境吸引域優(yōu)化之后的控制效果。采用[0.3；0.55]作為單一環(huán)境吸引域描述點(diǎn)，并選擇機(jī)器人的關(guān)鍵點(diǎn)1作為吸引對(duì)象，仿真結(jié)果如圖4所示。

圖4 引入環(huán)境吸引域前后的平面四自由度機(jī)器人仿真對(duì)比

由圖4(a)可以看出：無(wú)論是否考慮虛擬環(huán)境優(yōu)化，機(jī)器人的軌跡跟蹤誤差均收斂于0，收斂時(shí)間約為1 s，兩種情況幾乎沒有差異；引入環(huán)境吸引域后，機(jī)器人關(guān)鍵點(diǎn)到傾斜工作空間的距離對(duì)比如圖4(d)所示，平均距離從3.6 m減小到3.4 m。引入環(huán)境吸引機(jī)制后，可以直觀地看到機(jī)器人的構(gòu)型變化，機(jī)器人的第二個(gè)關(guān)節(jié)進(jìn)入偏好區(qū)域。在這種情況下，機(jī)器人的關(guān)節(jié)角1和關(guān)節(jié)角2保證了機(jī)器人的關(guān)鍵點(diǎn)靠近期望區(qū)域，關(guān)節(jié)角3和關(guān)節(jié)角4的變化更承擔(dān)了軌跡跟蹤的任務(wù)。

2.2 仿真2

為進(jìn)一步驗(yàn)證所提方法的有效性，選擇FRANKA PANDA機(jī)器人作為仿真對(duì)象。分別選取[0.346 2；-0.675；0.210][-0.253 8；-0.675；0.210]為機(jī)器人運(yùn)動(dòng)的始末點(diǎn)，以模仿機(jī)器人的抓放任務(wù)。機(jī)器人關(guān)節(jié)角的初始值為[-0.5；0.33；-1；-1.7；0；2.3；0]rad，空間吸引域的點(diǎn)選擇為[0.4；-0.1；0.7]m，實(shí)驗(yàn)結(jié)果如圖5所示。t=0時(shí)，機(jī)器人末端執(zhí)行器的位置[0.08，-0.674，0.40]m，相應(yīng)的跟蹤誤差約為[-0.25；0；0.2]m。在該控制器的作用下，機(jī)器人末端執(zhí)行器可以快速跟蹤期望的軌跡。跟蹤誤差在t=3.14、6.28、9.42、12.57 s等處存在一定的抖動(dòng)，這主要是由于機(jī)器人在這些點(diǎn)的期望軌跡導(dǎo)數(shù)不連續(xù)(主要指機(jī)器人抓取和放置點(diǎn)時(shí)的反向速度，即速度幅值相同但方向相反)。這也證明了所提控制器的適應(yīng)性，即使在期望軌跡速度不平穩(wěn)的情況下，系統(tǒng)也能實(shí)現(xiàn)高精度的軌跡跟蹤。

圖5 FRANKA PANDA機(jī)器人的仿真結(jié)果對(duì)比

3 總結(jié)

針對(duì)冗余機(jī)械手同步運(yùn)動(dòng)控制過(guò)程，提出一種基于環(huán)境吸引域的構(gòu)型優(yōu)化方法。將人類定義的機(jī)器人偏好工作空間簡(jiǎn)單地建模為一組關(guān)鍵點(diǎn)，并建立一個(gè)稱為環(huán)境吸引域的勢(shì)函數(shù)。在APF方法的啟發(fā)下，從優(yōu)化的角度建立一個(gè)有吸引力的方案。將物理約束下的運(yùn)動(dòng)控制與構(gòu)型優(yōu)化問(wèn)題表述為QP問(wèn)題，建立模型驅(qū)動(dòng)神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)閉環(huán)系統(tǒng)的全局收斂，以軟限制的方式在物理約束下同時(shí)進(jìn)行運(yùn)動(dòng)控制和構(gòu)型優(yōu)化。通過(guò)仿真驗(yàn)證了所提方法的有效性。