柴 銅 莊春剛 張 波

上海交通大學(xué) 機械與動力工程學(xué)院 上海 200240

1 研究背景

隨著工業(yè)生產(chǎn)過程中自動化與智能化的普及,人機協(xié)作場景的應(yīng)用變得越來越普遍,人機協(xié)作的功能要求也越來越高,如安全性能保障、人機協(xié)作裝配、智能人機交互[1]等。對于協(xié)作機器人,如何保證安全性、生產(chǎn)效率、運動精度[2],是運動規(guī)劃中的重要問題。安全性要求機器人能夠在人體進入工作空間時減速或暫停[3],或者能夠?qū)M入工作空間的障礙物,如人的手臂、生產(chǎn)工具等進行避障。生產(chǎn)效率要求在保證安全的同時,盡快完成指定的任務(wù),實現(xiàn)時間最優(yōu)、能量最優(yōu)等。

對于運動規(guī)劃優(yōu)化問題,首先需要進行幾何路徑規(guī)劃,然后針對不同需求進行優(yōu)化,如時間最優(yōu)、能量最優(yōu)、避障規(guī)劃、消除抖動等。Oberherber等[4]在優(yōu)化問題中加入扭矩導(dǎo)數(shù)項和對扭矩突變的約束,在后續(xù)研究中使用分段動態(tài)規(guī)劃得到初始路徑,再對初始路徑進行B樣條曲線逼近優(yōu)化[5]。比較流行的運動規(guī)劃方法有協(xié)變哈密頓量優(yōu)化運動規(guī)劃[6]、隨機軌跡優(yōu)化運動規(guī)劃[7]、增量軌跡優(yōu)化運動規(guī)劃[8]等,一般都使用無約束優(yōu)化,將碰撞和軌跡抖動作為懲罰項。其中,增量軌跡優(yōu)化運動規(guī)劃使用了規(guī)劃和執(zhí)行同步進行的策略,提高了實時性,缺點在于每次的規(guī)劃結(jié)果不一定是最優(yōu)解,因此仍有產(chǎn)生碰撞的可能性。協(xié)變哈密頓量優(yōu)化運動規(guī)劃、隨機軌跡優(yōu)化運動規(guī)劃、增量軌跡優(yōu)化運動規(guī)劃三種方法的共同點是優(yōu)化問題的決策變量均為整條軌跡,屬于離線規(guī)劃算法,能夠方便找到全局最優(yōu)解。

Tsai[9]提出一種較為通用的在線實時規(guī)劃算法,將基于時序的軌跡更新融合至優(yōu)化問題中,并將復(fù)雜的非線性非凸規(guī)劃近似為二次凸規(guī)劃,簡化了計算。但是這一方法的路徑更新是在離散空間中完成的,會引入系統(tǒng)誤差。Ragaglia等[10]使用了這一方法,在每個時刻下根據(jù)當前環(huán)境狀態(tài)和對一段時間后環(huán)境的預(yù)測,計算當前時刻的輸出量,滿足避障要求和動力學(xué)約束,并將該輸出量傳至下層控制器完成控制,實現(xiàn)在線實時規(guī)劃。文獻[10]將所研究的問題定義為線性約束下的二次優(yōu)化問題,收斂速度快,且不同于增量軌跡優(yōu)化運動規(guī)劃,將避障定義為約束,保證了避障的安全性。但是這一方法仍然存在一些缺陷,如未解決任務(wù)空間下規(guī)劃的軌跡抖動問題、任務(wù)空間下路徑更新的誤差問題、局部最優(yōu)解問題,筆者在研究中針對這些問題提出了解決措施。

筆者借鑒Ragaglia、Tsai等的方法,預(yù)先定義一條參考路徑,將所研究的問題定義為避障約束下的二次規(guī)劃問題,可以得到較快的收斂速度,基于此提出一種使用自適應(yīng)權(quán)重參數(shù)的關(guān)節(jié)空間在線規(guī)劃算法,提高了軌跡的抗干擾能力,避免了軌跡抖動問題,實現(xiàn)了軌跡的平滑。針對真實場景下的規(guī)劃問題,使用基于卡爾曼濾波的路徑更新方法,提高規(guī)劃過程中路徑狀態(tài)預(yù)測的準確性,抑制了系統(tǒng)誤差。筆者基于MATLAB軟件UR10機械臂平臺,進行了算法仿真對比。

2 安全約束建模

為保證人機協(xié)作工作空間中人體的安全性,在線規(guī)劃算法需要實現(xiàn)避障功能。增量軌跡優(yōu)化運動規(guī)劃將避障約束轉(zhuǎn)換為目標函數(shù)中的懲罰項,進行無約束優(yōu)化問題求解,但如果發(fā)生無解或不收斂的情況,就無法避免碰撞。筆者采用文獻[10]的方法,將所研究的問題轉(zhuǎn)換為帶避障約束的優(yōu)化問題,限定優(yōu)化問題的搜索范圍,可以保證安全性。

筆者使用基于Gilbert-Johnson-Keerthi算法[11]的距離估計對約束進行建模。為了簡化計算,假設(shè)碰撞模型為凸包模型。安全約束建模如圖1所示,包括障礙物碰撞模型和末端執(zhí)行器碰撞模型。

▲圖1 安全約束建模

根據(jù)圖1建立數(shù)學(xué)模型:

(1)

(2)

3 優(yōu)化問題建模

3.1 目標函數(shù)

筆者提出的規(guī)劃算法需要一條參考軌跡作為優(yōu)化目標,參考軌跡由傳統(tǒng)軌跡規(guī)劃方法得到。參考軌跡在關(guān)節(jié)空間中可以表示為:

(3)

算法的優(yōu)化目標是使每一時刻的運動軌跡點和參考軌跡點的誤差最小,使機器人關(guān)節(jié)空間的角度、角速度、角加速度對參考軌跡進行跟隨,同時對障礙物的距離增加一個懲罰項。將uk作為第k時刻優(yōu)化問題的決策變量,基于上述參考軌跡定義目標函數(shù)為:

(4)

(5)

(6)

xk+1為:

(7)

式中:xk為末端執(zhí)行器第k時刻的空間位置。

式(4)中等號右邊前三項依次表示對參考軌跡位置、速度、加速度的跟蹤,最后一項表示對末端執(zhí)行器和障礙物之間的距離的懲罰。

綜合式(1)～式(7),定義優(yōu)化問題為:

qlow≤qk+1≤qhigh

(8)

可以看出,該優(yōu)化問題是有約束二次規(guī)劃問題,可以直接使用二次規(guī)劃求解器求解。筆者在仿真中使用MATLAB軟件的優(yōu)化工具箱。

3.2 自適應(yīng)權(quán)重參數(shù)方法

對于目標函數(shù)中的四個權(quán)重p、v、a、o,很多學(xué)者通過試驗給出了最優(yōu)值。然而使用已確定的最優(yōu)值得到的軌跡往往存在振動過大和偏離問題。例如,在沒有障礙物的時候,如果參考加速度的跟蹤權(quán)重過大,那么會使末端執(zhí)行器的軌跡偏離參考軌跡;在避障過程中,如果僅僅對位置項和速度項進行跟蹤,那么會降低軌跡的抗干擾能力,由于傳感器引入的誤差,軌跡往往會產(chǎn)生較大的振動。對此,筆者提出自適應(yīng)權(quán)重參數(shù)方法,使權(quán)重與障礙物距離成函數(shù)映射關(guān)系,用于擬合不同障礙物距離下的權(quán)重參數(shù)。

考慮到良好的函數(shù)性質(zhì),筆者使用Sigmoid函數(shù)變體來擬合權(quán)重參數(shù)變化。以加速度誤差權(quán)重為例,有:

(9)

式中:μ、β、γ、λ為控制曲線形狀的參數(shù),μ、β分別控制縱軸方向上的拉伸比例和位置,γ、λ分別控制橫軸方向上的拉伸比例和位置。

筆者使用的角度誤差權(quán)重、角速度誤差權(quán)重、角加速度誤差權(quán)重及避障懲罰項權(quán)重與距離的擬合曲線如圖2所示。

▲圖2 權(quán)重與距離擬合曲線

4 算法實現(xiàn)

應(yīng)用筆者提出的在線運動規(guī)劃算法,在每一個時刻使用二次規(guī)劃求解器求解優(yōu)化問題,得到當前時刻的輸出,并將控制參數(shù)傳至底層控制器。在線運動規(guī)劃算法流程如圖3所示。這一算法相比離線規(guī)劃算法,優(yōu)勢是計算量小,收斂速度快,實時性強。

▲圖3 在線運動規(guī)劃算法流程

5 算法仿真

筆者進行了兩方面仿真。第一,使用筆者提出的關(guān)節(jié)空間在線運動規(guī)劃算法與文獻[10]任務(wù)空間規(guī)劃算法進行比較。第二,對自適應(yīng)權(quán)重參數(shù)方法和固定參數(shù)法的仿真結(jié)果進行比較。

計算機仿真環(huán)境如圖4所示,障礙物碰撞模型和末端執(zhí)行器碰撞模型均為凸包模型,參考軌跡為預(yù)先定義的B樣條曲線,實時規(guī)劃軌跡為使用在線規(guī)劃算法得到的實際運動軌跡。

▲圖4 計算機仿真環(huán)境

為了模擬實際的傳感器噪聲,筆者在仿真過程中對障礙物模型的位置變量加入高斯噪聲,使Gilbert-Johnson-Keerthi算法測量得到的距離符合高斯分布,從而更加接近真實環(huán)境。并且,直接對Gilbert-Johnson-Keerthi算法的輸出距離加以均值為0、方差為0.003的高斯噪聲。

為了評估不同規(guī)劃算法產(chǎn)生的軌跡的平滑程度差異,除給出直觀的軌跡圖像外,還使用滑動窗標準差來衡量軌跡的平滑程度?；瑒哟皹藴什钤砣鐖D5所示。使用一個固定大小的窗口在軌跡圖像上滑動,計算窗口內(nèi)數(shù)據(jù)的標準差。這樣,對于兩條整體軌跡相似但局部振動幅度相差較大的軌跡,可以通過這一方法來量化平滑程度的差異。如果直接計算整條軌跡的標準差,那么無法衡量局部軌跡的平滑程度。同樣,窗口寬度太大也無法達到要求,因此必須選擇適當?shù)拇翱陂L度和滑動步幅。筆者仿真的滑動標準差窗口長度和滑動步幅分別取1 s、0.5 s。

▲圖5 滑動窗標準差原理

6 關(guān)節(jié)空間規(guī)劃與任務(wù)空間規(guī)劃比較

文獻[10]提出的任務(wù)空間實時規(guī)劃算法,在任務(wù)空間中對參考軌跡點的位置、速度進行跟蹤,軌跡的抗噪聲能力較差。

筆者在關(guān)節(jié)空間中對參考軌跡點的位置、速度、加速度進行跟蹤,在一定程度上提升了抗噪聲能力,可以有效抑制避障過程中軌跡的抖動。

對比兩種不同規(guī)劃方法在有障礙物的情況下得到的軌跡規(guī)劃結(jié)果,并使用滑動窗標準差來評估兩條軌跡的平滑程度。兩種規(guī)劃算法的仿真參數(shù)見表1。

表1 規(guī)劃算法仿真參數(shù)

以UR10機器人關(guān)節(jié)3為例,關(guān)節(jié)3軌跡變化曲線如圖6所示。兩種規(guī)劃算法均使用了固定參數(shù)法。

由圖6可以看出,對于筆者提出的規(guī)劃算法得到的軌跡,關(guān)節(jié)角度、角速度、角加速度的滑動窗標準差明顯減小,相比任務(wù)空間規(guī)劃算法,可以顯著減小軌跡的抖動。

▲圖6 關(guān)節(jié)空間規(guī)劃和任務(wù)空間規(guī)劃關(guān)節(jié)3軌跡

7 自適應(yīng)權(quán)重方法和固定參數(shù)法比較

筆者在仿真中,關(guān)節(jié)空間規(guī)劃算法使用固定參數(shù)法,相比文獻[10]提出的任務(wù)空間規(guī)劃算法在軌跡穩(wěn)定性方面已經(jīng)有很大提升,但避障過程中仍存在輕微抖動。使用自適應(yīng)權(quán)重方法,能夠?qū)Ρ苷线^程中的軌跡抖動問題進行進一步優(yōu)化,相比固定參數(shù)法,軌跡的平滑性可以得到進一步提高。

自適應(yīng)權(quán)重方法所使用的權(quán)重擬合曲線參數(shù)見表2。

表2 自適應(yīng)權(quán)重方法權(quán)重擬合曲線參數(shù)

同樣以UR10機器人關(guān)節(jié)3為例,關(guān)節(jié)3在固定參數(shù)法和自適應(yīng)權(quán)重方法下關(guān)節(jié)角度、角速度、角加速度的軌跡變化曲線如圖7所示。

▲圖7 固定參數(shù)法和自適應(yīng)權(quán)重方法關(guān)節(jié)3軌跡

由圖7可以看出,自適應(yīng)權(quán)重方法相比固定參數(shù)法,滑動窗標準差有明顯減小,進一步抑制了軌跡的抖動。

滑動窗標準差均值可以在很大程度上反映軌跡的整體穩(wěn)定情況,均值越小,說明軌跡穩(wěn)定性越好,均值越大,說明軌跡穩(wěn)定性越差。綜合仿真結(jié)果,將文獻[10]提出的任務(wù)空間規(guī)劃算法稱為算法1,將使用固定參數(shù)法的關(guān)節(jié)空間規(guī)劃算法稱為算法2,將使用自適應(yīng)權(quán)重方法的關(guān)節(jié)空間規(guī)劃算法稱為算法3,對三種算法的軌跡平滑效果進行比較,計算所有關(guān)節(jié)的滑動窗標準差均值,結(jié)果見表3。

表3 三種算法仿真結(jié)果比較

由表3可以看出,對于關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4,角度的滑動窗標準差均值在三個算法中無明顯變化,而角速度和角加速度的滑動窗標準差均值從算法1到算法3逐漸減小,說明在軌跡穩(wěn)定性方面,算法3優(yōu)于算法2和算法1;對于關(guān)節(jié)5,由于滑動窗標準差數(shù)量級過小,因此即使有明顯的提升或降低,也無法對最終軌跡產(chǎn)生影響,效果可以忽略;對于關(guān)節(jié)6,算法2相比算法1性能有明顯提升,而算法3和算法2的效果完全相同。綜合而言,算法3優(yōu)于算法2,算法2優(yōu)于算法1。

8 結(jié)束語

筆者針對人機協(xié)作場景的安全性、實時性需求,提出了一種基于自適應(yīng)權(quán)重方法的六自由度機械臂關(guān)節(jié)空間規(guī)劃算法,具有實時性強、求解快等優(yōu)點,能夠在避免碰撞的同時兼顧軌跡的穩(wěn)定性。經(jīng)過計算機仿真和試驗,驗證這一算法可行有效。