機械臂目標(biāo)區(qū)域跟蹤防撞控制

楊沁林, 張 勁

(1. 四川大學(xué)電氣工程學(xué)院, 成都 610065;2. 四川大學(xué)生物醫(yī)學(xué)工程學(xué)院, 成都 610065)

1 引 言

醫(yī)護(hù)人員短缺已成為全球醫(yī)療行業(yè)共同面臨的嚴(yán)峻問題[1]，在此情況下,因為機械臂具有工作時間長，工作性能穩(wěn)定，對疫情免疫等優(yōu)勢，所以使用機械臂代替醫(yī)護(hù)人員完成常見的，重復(fù)性高的醫(yī)護(hù)工作，可以在很大程度上減少醫(yī)護(hù)人員的工作強度以及感染傳染性疾病的風(fēng)險.為此研究設(shè)計具有醫(yī)護(hù)能力的機器人受到了研究人員的廣泛關(guān)注.Jayaswal等[2]在疫情期間設(shè)計了魯棒柔性機械臂控制方法，并對該方法進(jìn)行了仿真實驗驗證.Ahamed等[3]設(shè)計實現(xiàn)可提醒患者或老年人及時服藥，并在患者需要的時候隨時提供飲用水的移動機器人.

機械臂在醫(yī)護(hù)過程中涉及與患者的交互，例如當(dāng)機械臂在進(jìn)行護(hù)理過程中，機械臂末端執(zhí)行器停留在患者附近時，患者可以根據(jù)個人需要，將機械臂末端執(zhí)行器推離或拉近.在實際設(shè)置實驗裝置之前，需要依據(jù)文獻(xiàn)[4]才能保證機械臂的控制過程涉及人機交互任務(wù).文獻(xiàn)[5-9]將機械臂末端在任務(wù)空間中的目標(biāo)移動路徑設(shè)置為一系列的點，其中所提出的控制方案均能較好實現(xiàn)目標(biāo)軌跡的跟蹤任務(wù)，但是在涉及機械臂操作過程中存在人機交互協(xié)作的情況，將機械臂末端的目標(biāo)設(shè)置為一個區(qū)域更加合理，并且機械臂末端在目標(biāo)區(qū)域內(nèi)表現(xiàn)出柔順性，以確保人機交互過程的安全，順利.

在護(hù)理病房內(nèi)，因為機械臂執(zhí)行護(hù)理任務(wù)的特殊性，需要保證在護(hù)理過程中患者的安全，防止機械臂末端執(zhí)行器誤觸患者重要部位，以及防止誤觸醫(yī)護(hù)用品，避免在護(hù)理過程中發(fā)生醫(yī)療事故，所以機械臂末端在跟隨目標(biāo)區(qū)域移動過程中，需要規(guī)避患者及醫(yī)護(hù)用品等障礙物.文獻(xiàn)[10]提出了雙向時效A*算法，提高了A*算法在規(guī)避障礙物過程中的執(zhí)行效率，但每次迭代的路徑選取未考慮障礙的位置，易陷入障礙密集區(qū)域;文獻(xiàn)[11]使用RRT(Rapidly Exploring Random Tree)算法，提出了一種基子目標(biāo)搜索策略，該算法主要是針對二維平面上的路徑規(guī)劃及避障，對于三維空間路徑規(guī)劃該算法搜索緩慢;文獻(xiàn)[12]運用向量場直方圖方法VFH(Vector Field Histogram)，提出一種波谷寬度計算方法，使得逼近目標(biāo)位置過程中軌跡更加平滑.因為人工勢場法規(guī)避障礙物具有結(jié)構(gòu)清晰，運算量小的優(yōu)點，由此引入人工勢場[13].在人工勢能場中包括兩種勢場：引力勢能場和斥力勢能場.利用人工勢場規(guī)避障礙物也被廣泛使用在無人機[14]、車輛[15]和船舶[15]等領(lǐng)域.在本文中，引力勢能場作用在機械臂末端執(zhí)行器，起到引導(dǎo)機械臂末端執(zhí)行器實現(xiàn)目標(biāo)區(qū)域跟蹤的作用，斥力勢能場同樣作用在機械臂末端執(zhí)行器，起到規(guī)避障礙物的作用.

本文提出了一種機械臂目標(biāo)區(qū)域跟蹤防撞控制器；實現(xiàn)機械臂末端執(zhí)行器在跟蹤目標(biāo)區(qū)域過程中規(guī)避障礙物，并在目標(biāo)區(qū)域內(nèi)具有柔順的人機交互性能；借鑒船舶領(lǐng)域的最近會遇距離和最小會遇時間，提出了一種機械臂碰撞預(yù)測模型；通過機械臂與障礙物的實時狀態(tài)，得到防撞安全指數(shù)；將障礙物作用半徑與防撞安全指數(shù)相聯(lián)系，以此改變障礙物作用半徑，實現(xiàn)機械臂在跟蹤目標(biāo)區(qū)域的同時，規(guī)避障礙物，保護(hù)患者，提高了醫(yī)護(hù)機械臂的安全性.

2 問題描述

2.1 機械臂動力學(xué)模型

根據(jù)拉格朗日定理，機械臂在關(guān)節(jié)空間的動力學(xué)模型可以表示為

(1)

其中q∈n表示n自由度機械臂的廣義關(guān)節(jié)角坐標(biāo)向量；M(q)∈n×n為對稱正定慣性矩陣；n×n為哥氏力與離心力矩陣；g(q)∈n為重力項；τ∈n表示控制輸出力矩.

設(shè)x∈n表示機械臂末端執(zhí)行器在任務(wù)空間中的位置向量

x=h(q)

(2)

其中h(·)表示從關(guān)節(jié)空間的關(guān)節(jié)角度q映射到任務(wù)空間末端執(zhí)行器位置x的正向運動學(xué)變化矩陣.對式(2)兩邊同時求導(dǎo)，可得

(3)

(4)

結(jié)合關(guān)節(jié)空間動力學(xué)模型式(1)和文獻(xiàn)[16]，可以推導(dǎo)出任務(wù)空間動力學(xué)模型為

(5)

其中

Mx(x)=J-T(q)M(q)J-1(q)

J-T(q)M(q)J-1(q))J-1(q)

g(x)=J-T(q)g(q)

F=J-T(q)τ

(6)

在本文中，因為實現(xiàn)控制任務(wù)都指定在任務(wù)空間中進(jìn)行，所以將關(guān)節(jié)空間轉(zhuǎn)換為任務(wù)空間，在任務(wù)空間中設(shè)計控制器更加方便，合理.

2.2 控制目標(biāo)

本文的控制目標(biāo)為：設(shè)計一種基于機械臂目標(biāo)區(qū)域跟蹤防撞控制器，將機械臂末端執(zhí)行器的位置收斂到目標(biāo)區(qū)域并對移動的目標(biāo)區(qū)域進(jìn)行跟蹤，在目標(biāo)區(qū)域跟蹤過程中防止碰撞障礙物.障礙物中心定義為點xob∈3，障礙物作用半徑為λ0，選擇運動目標(biāo)區(qū)域Τ為球形，其中心定義為點x0∈3，半徑定義為r0.并且假設(shè)目標(biāo)區(qū)域中心點x0的軌跡為二階可導(dǎo).

3 控制器設(shè)計

3.1 人工勢能場

人工勢能場是由引力勢能場和斥力勢能場共同組成.機械臂末端會受到目標(biāo)區(qū)域的引力作用，同時也會受到障礙物的斥力作用.機械臂會在引力和斥力的共同作用下，規(guī)避障礙物向目標(biāo)區(qū)域移動.

在三維空間中，目標(biāo)區(qū)域?qū)⑹褂靡粋€集合表示

(7)

其中Δx=x-x0,x0是目標(biāo)區(qū)域的中心；f(Δx):3→為目標(biāo)函數(shù).目標(biāo)函數(shù)關(guān)于Δx連續(xù)，且可微.f(Δx)的一階偏導(dǎo)數(shù)?f(Δx)/?Δx和二階偏導(dǎo)數(shù)?2f(Δx)/?2Δx有界.定義引力勢能函數(shù)，斥力勢能函數(shù)分別表示為[17]

(8)

(9)

其中c為引力增益常量；η為斥力增益常量；λ0為障礙物作用半徑；λ為機械臂末端執(zhí)行器位置x到障礙物中心x0的距離λ=|xobx|；m為大于0的常數(shù).機械臂勢能函數(shù)為

(10)

勢能函數(shù)P(Δx)關(guān)于Δx求偏導(dǎo)數(shù)表示為

(11)

其中：

其中?Uat(Δx)/?Δx為引力勢能對Δx求偏導(dǎo)，方向為從機械臂末端指向目標(biāo)區(qū)域中心，將機械臂末端引導(dǎo)到目標(biāo)區(qū)域.斥力勢能對Δx求偏導(dǎo)由兩部分組成，其中?Ure1(Δx)/?Δx的方向為障礙物指向機械臂末端，作用為將機械臂末端引導(dǎo)到障礙物作用范圍以外，?Ure2(Δx)/?Δx的方向為從機械臂末端指向目標(biāo)點，引導(dǎo)機械臂末端收斂到目標(biāo)區(qū)域，其表達(dá)式分別為

(12)

當(dāng)機械臂末端位于目標(biāo)區(qū)域外，或在障礙物附近時，?P(Δx)/?Δx將會驅(qū)使機械臂末端離開障礙物作用范圍，向目標(biāo)區(qū)域f(Δx)≤0的區(qū)域移動，當(dāng)機械臂末端位置在目標(biāo)區(qū)域內(nèi)，且不在障礙物附近時，f(Δx)≤0，?P(Δx)/?Δx=0.

3.2 碰撞預(yù)測模型

在使用斥力場規(guī)避障礙物的過程中，障礙物的作用范圍λ0為主要參數(shù).通常λ0為固定值，λ0設(shè)置過大造成機械臂避障效率降低，λ0設(shè)置過小造成規(guī)避不及時，存在碰撞危險.由此，本文提出一種機械臂的碰撞預(yù)測模型.碰撞預(yù)測模型已在船舶導(dǎo)航[18]，自動駕駛汽車[19,20]等領(lǐng)域得到廣泛的應(yīng)用，本文借鑒船舶領(lǐng)域中最近會遇距離DCPA(Distance of Closest Point of Approach)與最小會遇時間TCPA(Time of Closest Point of Approach)概念[21]，通過對發(fā)生碰撞在空間和時間上的危險度分別進(jìn)行分析，得到空間防撞安全度與時間防撞安全度，通過對兩個安全度的聯(lián)立計算得到防撞安全指數(shù)，并由防撞安全指數(shù)確定障礙物作用范圍.

圖1 機械臂與障礙物會遇示意圖

DCPA=λsin (αT)

(13)

(14)

其中λ=|xobx|為機械臂末端點到障礙物中心的距離，αT∈(0，π)為機械臂末端點速度方向與機械臂末端點到障礙物中心連線夾角.

首先設(shè)空間防撞安全度udT定義為

(15)

其中k為正常數(shù).設(shè)d1為最小避讓距離，d2為機械臂安全避讓距離取d2=2d1.最小避讓距離d1的確定，當(dāng)機械臂末端安裝執(zhí)行器，末端執(zhí)行器側(cè)邊面對障礙物中心時，存在碰撞危險，設(shè)機械臂末端執(zhí)行器長度為Lend=0.1 m，將d1設(shè)置為d1=1.5Lend，以此保證末端執(zhí)行器在規(guī)避障礙物過程中不會觸碰障礙物.

在機械臂末端與障礙物有碰撞可能的情況下，時間防撞安全度utT定義為

(16)

其中t1和t2分別表示為

式中，t1表示機械臂末端速度方向上距離障礙物為d1的位置，達(dá)到最近會與點的時間；t2表示機械臂末端速度方向上距離障礙物為d2的位置，達(dá)到最近會與點的時間.

由式(15)和式(16)分別得到空間防撞安全度udT與時間防撞安全度utT后，防撞安全指數(shù)uT，由utT和udT量化為

(17)

將機械臂末端與障礙物的狀態(tài)信息代入碰撞預(yù)測模型中，得到防撞安全指數(shù)，根據(jù)以下公式確定障礙物作用范圍λ0.

(18)

式中，μ>1，由此可得到隨著空間防撞安全度與時間防撞安全度變化的障礙物作用范圍.

3.3 區(qū)域跟蹤防撞控制器設(shè)計

本文中在任務(wù)空間的區(qū)域跟蹤防撞控制器設(shè)計為

(19)

(20)

假設(shè)Lyapunov-like泛函數(shù)被設(shè)置為

(21)

其中P(Δx)在式(10)中被定義，V為正定，對式(21)兩邊求導(dǎo)得

(22)

將(20)代入(22)中，得到

(23)

證明 為了證明以上定理，先證明

(24)

由于dV/dt為負(fù)，V對t是遞減的，可設(shè)

(25)

如果ε≠0,則對于任意t≥t0有，又由V連續(xù)，正定，并且在平衡點處t=t0時，V=0，z2=0，P(Δx)=0，故存在λ1>0或λ2>0， 使得‖z2‖>λ1或P(Δx)>λ2，所以如果?。?/p>

(26)

由dV/dt為負(fù)定，故m<0，于是

(27)

4 實驗結(jié)果及分析

本文通過數(shù)值仿真以及實驗，驗證了所設(shè)計的目標(biāo)區(qū)域跟蹤防撞控制器的有效性.本文實驗過程使用UR10機械臂.UR10有6個轉(zhuǎn)動關(guān)節(jié)，負(fù)載能力為10 kg，操作范圍1300 mm.因為在醫(yī)療護(hù)理機械臂工作過程中，完成任務(wù)空間中的目標(biāo)區(qū)域跟蹤，使用前3個關(guān)節(jié)即可實現(xiàn)，后3個關(guān)節(jié)被視作一個整體，醫(yī)護(hù)人員在機械臂末端進(jìn)入目標(biāo)區(qū)域后單獨控制，完成更加具體的醫(yī)護(hù)工作.在本文中，仿真及實驗通過控制前3個關(guān)節(jié)角，實現(xiàn)機械臂末端的在任務(wù)空間的位置控制，后3個關(guān)節(jié)作為末端執(zhí)行器看作一個整體，并將關(guān)節(jié)角均設(shè)置為固定值.基于UR10機械臂建立動力學(xué)模型，UR10各關(guān)節(jié)坐標(biāo)系如圖2所示，機械臂DH參數(shù)如表1所示，前3個連桿的質(zhì)量如表2所示.

圖2 UR10各關(guān)節(jié)坐標(biāo)Fig.2 Coordinates for each joint of the UR10

表1 UR10DH參數(shù)

表2 UR10物理參數(shù)

4.1 仿真跟蹤移動目標(biāo)區(qū)域

首先使用Matlab2020a數(shù)值仿真對目標(biāo)區(qū)域跟蹤防撞控制器的有效性進(jìn)行驗證.采用時間步長0.005 s的固定步長ODE4 (RungeKutta)求解器.仿真過程中，各參數(shù)設(shè)置為μ=1.1，k=1，kα=1，kν=3，c=5，m=1.05，η=0.1.機械臂前3個關(guān)節(jié)初始角度分別設(shè)置為：[2.37,-π/3,π/3]Trad，后3個關(guān)節(jié)的角度保持為固定值[0,-π/3,0]Trad.控制目標(biāo)為UR10機器臂的末端執(zhí)行器跟蹤運動的目標(biāo)區(qū)域，該區(qū)域定義為一個半徑為r0=0.1 m的球體.目標(biāo)區(qū)域中心x0沿著期望軌跡運動.

x0=[0.3cos(0.05t)，-0.8+0.3sin(0.05)t，0.2]

機械臂末端執(zhí)行器軌跡三維及二維視圖如圖3和圖4所示，機械臂末端執(zhí)行器收斂到目標(biāo)區(qū)域后，跟隨目標(biāo)區(qū)域移動.

圖4 移動區(qū)域軌跡跟蹤: UR10:末端執(zhí)行器二維軌跡;REG:目標(biāo)區(qū)域;START:開始點;END:停止點

勢能函數(shù)如圖5所示，機器人末端執(zhí)行器能夠快速收斂到目標(biāo)區(qū)域.關(guān)節(jié)角度和控制輸入力矩隨時間變化如圖6所示.仿真結(jié)果證明了目標(biāo)區(qū)域跟蹤防撞控制器對完成運動軌跡跟蹤任務(wù)的有效性.

圖5 移動區(qū)域軌跡跟蹤:勢能函數(shù)

圖6 移動區(qū)域軌跡跟蹤：關(guān)節(jié)角和控制輸入(a) 關(guān)節(jié)角; (b) 控制輸入Fig.6 Moving region trajectory tracking: joint angles and control inputs (a) Joint angles ; (b) control inputs

4.2 仿真安全防撞

醫(yī)療護(hù)理機械臂在完成不同醫(yī)護(hù)任務(wù)移動過程中，存在機械臂末端執(zhí)行器碰撞患者或者醫(yī)護(hù)用品的風(fēng)險，為此在已知患者與醫(yī)護(hù)用品位置的前提下，對控制器的防撞安全性進(jìn)行驗證.各參數(shù)不變，所考慮的軌跡與上一個仿真實驗相同目標(biāo)區(qū)域球體半徑r0=0.2 m，分別在xob1=[0.25,-0.55,0.43]T,xob2=[-0.12,-0.64,0.52]T,xob3=[0.25,-0.90,0.33]T3個位置放置障礙物代表護(hù)理病房空間中的三個醫(yī)護(hù)用品，障礙物最小作用距離λ0=0.1 m.機械臂末端執(zhí)行器軌跡的三維視圖如圖7所示，機械臂在規(guī)避障礙物后，繼續(xù)保持穩(wěn)定，并跟蹤運動的目標(biāo)區(qū)域.勢能函數(shù)如圖8所示，關(guān)節(jié)角度和控制力矩輸入如圖9所示.仿真結(jié)果驗證了目標(biāo)區(qū)域跟蹤防撞控制器在機械臂運行過程中出現(xiàn)障礙物時，防止機械臂末端執(zhí)行器碰撞障礙物的有效性.

圖7 規(guī)避碰撞:UR10:3D:末端執(zhí)行器三維軌跡;UR10:2D:末端執(zhí)行器二維軌跡;REG:目標(biāo)區(qū)域二維視圖;OBS-REG:障礙物最小影響范圍;OSB:障礙物；START:開始點;END:停止點

圖8 規(guī)避碰撞：勢能函數(shù)

圖9 規(guī)避碰撞:關(guān)節(jié)角和控制輸入

4.3 人機交互實驗

人機交互實驗，在UR10機械臂平臺上完成，該平臺由UR10機械臂本體、控制柜CB3、上位機控制系統(tǒng)三部分組成.上位機控制系統(tǒng)使用URScript編寫控制算法，負(fù)責(zé)在線運行控制算法，向控制柜發(fā)送各關(guān)節(jié)角速度控制指令，控制柜負(fù)責(zé)接收控制指令，控制移動機械臂，并獲取機械臂信息，將機械臂狀態(tài)反饋到上位機控制系統(tǒng)，實現(xiàn)對機械臂的控制，實驗平臺如圖10所示.

圖10 實驗平臺Fig.10 Experimental setup

人機交互實驗如圖11，機械臂末端執(zhí)行器收斂到目標(biāo)區(qū)域后，可在目標(biāo)區(qū)域內(nèi)通過手持移動機械臂末端執(zhí)行器位置，在目標(biāo)區(qū)域內(nèi)實現(xiàn)柔順人機交互.在實驗過程中，各參數(shù)設(shè)置為μ=1.1，k=1,kα=1，kν=1，c=2，m=1.05，η=0.1.目標(biāo)區(qū)域中心點坐標(biāo)設(shè)置為x0=[0,-0.3918,0.274]T，目標(biāo)區(qū)域球體半徑r0=0.2 m,以及機械臂前三個關(guān)節(jié)初始角度分別設(shè)置為：[1.5,-1.5,2.41]Trad，后三個關(guān)節(jié)的角度保持固定值[0,-π/3,0]Trad.在完成人機交互實驗過程中，機械臂關(guān)節(jié)角度和勢能變化如圖12所示，機械臂位置變化如圖13所示，實驗結(jié)果顯示機械臂末端執(zhí)行器在目標(biāo)區(qū)域內(nèi)，勢能為0，關(guān)節(jié)角度不改變，t=2 s通過移動機械臂末端執(zhí)行器位置到目標(biāo)區(qū)域外，勢能大于0，控制器控制機械臂改變關(guān)節(jié)角度，將機械臂末端執(zhí)行器收斂至目標(biāo)區(qū)域，保持穩(wěn)定.t=7.9 s，在目標(biāo)區(qū)域內(nèi)移動末端執(zhí)行器位置，勢能為0，機械臂保持穩(wěn)定，不改變各關(guān)節(jié)角度.根據(jù)實驗結(jié)果，驗證了目標(biāo)區(qū)域跟蹤防撞控制器在機械臂操作過程中人機交互有效性.

圖11 人機交互實驗

圖12 人機交互:關(guān)節(jié)角和勢能函數(shù)

圖13 人機交互:REG:目標(biāo)區(qū)域;UR10:簡化UR10

4.4 安全防撞實驗

安全防撞實驗，機械臂末端執(zhí)行器在跟蹤移動區(qū)域過程中，規(guī)避障礙物，實現(xiàn)機械臂末端執(zhí)行器對障礙物的安全防撞.實驗在UR10機械臂平臺上完成，各參數(shù)不變，各關(guān)節(jié)初始角度不變，與前一個實驗相同，目標(biāo)區(qū)域中心x0期望軌跡由以下公式表示，目標(biāo)區(qū)域球體半徑r0=0.2 m,x0=[0,-0.3918-0.02t,0.274]T分別在xob1=[-0.08,-0.55,0.24]T,xob2=[0.15,-0.83,0.24]T兩個位置放置障礙物代表護(hù)理病房空間中的兩個個醫(yī)護(hù)用品，設(shè)置實驗如圖14所示.

本文使用防撞預(yù)測模型，通過計算防撞安全指數(shù)實時改變障礙物作用半徑，與不改變障礙物作用半徑的無差別避障進(jìn)行試驗對比，因在護(hù)理病房內(nèi)如水杯，體溫槍，藥瓶，最大邊長約0.1 m，為保證機械臂末端安全避障，預(yù)留安全距離，所以將無差別避障中障礙物作用半徑設(shè)置為λ0=0.17 m，通過實驗數(shù)據(jù)繪制機械臂末端執(zhí)行器運動軌跡三維視圖及俯視圖對比如圖15所示.

圖14 規(guī)避碰撞實驗

(a)

(b)

(c)

各關(guān)節(jié)角度變化對比如圖16所示，根據(jù)實驗結(jié)果可得到以下結(jié)論，在規(guī)避障礙物過程中應(yīng)用防撞預(yù)測模型改變障礙物作用范圍，減少了各關(guān)節(jié)角的變化幅度，使得障礙物規(guī)避過程中機械臂末端執(zhí)行器運動軌跡更加平滑，并與障礙物保持安全距離，驗證了目標(biāo)區(qū)域跟蹤防撞控制器在防止機械臂末端執(zhí)行器碰撞障礙物過程中的有效性.

圖16 規(guī)避碰撞:CAP:防撞預(yù)測; IOA:無差別避障

5 結(jié) 論

本文針對醫(yī)療護(hù)理機械臂在控制過程中存在的人機交互、軌跡跟蹤，以及在護(hù)理過程防止碰撞的實際問題，提出目標(biāo)區(qū)域跟蹤防撞控制器.該方法結(jié)合人工勢能函數(shù)使用了區(qū)域到達(dá)控制，實現(xiàn)醫(yī)療護(hù)理機械臂有效跟蹤目標(biāo)區(qū)域，并在目標(biāo)區(qū)域?qū)崿F(xiàn)柔順人機交互.本文提出的防撞預(yù)測模型通過改變障礙物斥力場作用半徑實現(xiàn)機械臂對障礙物的規(guī)避，既保留了人工勢場法簡明的數(shù)學(xué)結(jié)構(gòu)，又提高了機械臂的護(hù)理安全性，以及避障軌跡的平滑穩(wěn)定.