動態環境下六自由度機械臂在線運動規劃算法

柴 銅 莊春剛 張 波

上海交通大學 機械與動力工程學院 上海 200240

1 研究背景

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

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

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

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

2 安全約束建模

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

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

▲圖1 安全約束建模

根據圖1建立數學模型:

(1)

(2)

3 優化問題建模

3.1 目標函數

筆者提出的規劃算法需要一條參考軌跡作為優化目標,參考軌跡由傳統軌跡規劃方法得到。參考軌跡在關節空間中可以表示為:

(3)

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

(4)

(5)

(6)

xk+1為:

(7)

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

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

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

qlow≤qk+1≤qhigh

(8)

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

3.2 自適應權重參數方法

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

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

(9)

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

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

▲圖2 權重與距離擬合曲線

4 算法實現

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

▲圖3 在線運動規劃算法流程

5 算法仿真

筆者進行了兩方面仿真。第一,使用筆者提出的關節空間在線運動規劃算法與文獻[10]任務空間規劃算法進行比較。第二,對自適應權重參數方法和固定參數法的仿真結果進行比較。

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

▲圖4 計算機仿真環境

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

為了評估不同規劃算法產生的軌跡的平滑程度差異,除給出直觀的軌跡圖像外,還使用滑動窗標準差來衡量軌跡的平滑程度。滑動窗標準差原理如圖5所示。使用一個固定大小的窗口在軌跡圖像上滑動,計算窗口內數據的標準差。這樣,對于兩條整體軌跡相似但局部振動幅度相差較大的軌跡,可以通過這一方法來量化平滑程度的差異。如果直接計算整條軌跡的標準差,那么無法衡量局部軌跡的平滑程度。同樣,窗口寬度太大也無法達到要求,因此必須選擇適當的窗口長度和滑動步幅。筆者仿真的滑動標準差窗口長度和滑動步幅分別取1 s、0.5 s。

▲圖5 滑動窗標準差原理

6 關節空間規劃與任務空間規劃比較

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

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

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

表1 規劃算法仿真參數

以UR10機器人關節3為例,關節3軌跡變化曲線如圖6所示。兩種規劃算法均使用了固定參數法。

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

▲圖6 關節空間規劃和任務空間規劃關節3軌跡

7 自適應權重方法和固定參數法比較

筆者在仿真中,關節空間規劃算法使用固定參數法,相比文獻[10]提出的任務空間規劃算法在軌跡穩定性方面已經有很大提升,但避障過程中仍存在輕微抖動。使用自適應權重方法,能夠對避障過程中的軌跡抖動問題進行進一步優化,相比固定參數法,軌跡的平滑性可以得到進一步提高。

自適應權重方法所使用的權重擬合曲線參數見表2。

表2 自適應權重方法權重擬合曲線參數

同樣以UR10機器人關節3為例,關節3在固定參數法和自適應權重方法下關節角度、角速度、角加速度的軌跡變化曲線如圖7所示。

▲圖7 固定參數法和自適應權重方法關節3軌跡

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

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

表3 三種算法仿真結果比較

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

8 結束語

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