基于MRD-DDPG的機械臂避障路徑規(guī)劃方法*

付子強,鄭威強,張立萍,何 麗,袁 亮,邵明明

(1.新疆大學機械工程學院,烏魯木齊 830047;2.北京化工大學信息科學與技術(shù)學院,北京 100029)

0 引言

隨著機械臂的應用越來越廣泛,機械臂的路徑規(guī)劃成為機器人控制的重要組成部分。但傳統(tǒng)的機械臂路徑規(guī)劃方法[1-3]在有障礙物的場景下面臨著動態(tài)規(guī)劃較差、智能性較低的問題。而深度強化學習的發(fā)展,讓智能體具備了自主學習和自主規(guī)劃的能力[4-6]。

目前,學者們對深度強化學習進行了以下研究。EVERETT等[7]使用混合CPU/GPU的A3C方法,提升了運動規(guī)劃速度,實現(xiàn)了避障任務的快速運動規(guī)劃;趙寅甫等[8]提出了機械臂控制模型先2D后3D的訓練方法,大幅縮短了訓練時間;POPOV等[9]基于DPG算法,融合A3C分布式思想,將訓練16臺機器人所用的時間成功縮短到10 h,提高了數(shù)據(jù)收集率;李躍等[10]提出了一種新型方位獎勵函數(shù)的機械臂軌跡規(guī)劃方法A-DPPO解決機械臂軌跡規(guī)劃方法在未知環(huán)境中學習效率偏低的問題;JIANG等[11]設(shè)計了基于DDPG的機器人操作算法,構(gòu)造了非對稱的AC結(jié)構(gòu),并在演員網(wǎng)絡結(jié)構(gòu)中加入了輔助任務分支,可以有效提高學習效率。除此之外,李龍等[12]提出了一種基于接近視覺傳感器的避障路徑規(guī)劃方法,解決了人工勢場法的局部最優(yōu)問題,并在UR10機械臂的小臂連桿驗證此方法的有效性;李廣創(chuàng)等[13]通過DQN算法離線訓練控制機械臂規(guī)劃出一條無碰撞的路徑,具有較強的避障能力;FUJIMOTO等[14]在雙Q的基礎(chǔ)上提出TD3算法,解決了DDPG算法Q值高估問題,并在OpenAI Gym的平臺上驗證了TD3算法的性能;ANDRYCHOWICZ等[15]提出HER(hindsight experience replay)算法,并在OpenAI Gym平臺中極大的提高了機械臂任務的成功率。

雖然深度強化學習算法發(fā)展較為成熟,但在機械臂路徑規(guī)劃應用方面,仍存在學習效率低、樣本利用效率低的問題,特別是存在障礙物的工作環(huán)境中尤其明顯。由此提出了多經(jīng)驗池延遲采樣的方法,實現(xiàn)了對樣本的有效利用,解決了學習效率低的問題;設(shè)計了基于位置的避障獎勵函數(shù),解決了機械臂交互探索過程中的獎勵稀疏問題;并成功實現(xiàn)了MRD-DDPG算法在障礙物工作環(huán)境中的避障路徑規(guī)劃任務。

1 基于多經(jīng)驗池延遲采樣的深度確定性策略梯度算法

本文在DDPG算法的基礎(chǔ)上通過改進經(jīng)驗池機制,從而提出MRD-DDPG算法。MRD-DDPG算法具有和DDPG算法相同的4個神經(jīng)網(wǎng)絡,MRD-DDPG神經(jīng)網(wǎng)絡模型如圖1所示。Actor網(wǎng)絡負責策略網(wǎng)絡參數(shù)θμ的迭代更新,根據(jù)當前狀態(tài)st選擇當前動作at,并生成st+1和rt;Actor目標網(wǎng)絡負責根據(jù)經(jīng)驗回放池中采樣的下一狀態(tài)st+1,選擇最優(yōu)下一動作at+1;Critic網(wǎng)絡負責價值網(wǎng)絡參數(shù)θQ的迭代更新,計算當前Q值;Critic目標網(wǎng)絡計算目標Q值。

圖1 MRD-DDPG神經(jīng)網(wǎng)絡模型

為了減少單一經(jīng)驗池的復雜度增加樣本的利用率,根據(jù)機械臂與環(huán)境交互時是否完成規(guī)劃任務、是否發(fā)生碰撞,將獲取的樣本分為正樣本、普通樣本和負樣本。其中正樣本為完成規(guī)劃任務的樣本、普通樣本為既沒有完成規(guī)劃任務又沒有發(fā)生碰撞的樣本、負樣本為發(fā)生碰撞的樣本。

MRD方法則將獲取的正樣本、普通樣本分別存儲在2個經(jīng)驗池中,在訓練過程中先從普通樣本經(jīng)驗池中采樣,在延遲固定Episode后再按比例從2個經(jīng)驗池中隨機采樣。MRD-DDPG算法的流程圖如圖2所示。

圖2 MRD-DDPG算法流程圖

2 機械臂環(huán)境交互系統(tǒng)

MRD-DDPG算法的每個step與環(huán)境進行交互的過程如圖3所示。Agent在時間步t從仿真環(huán)境觀察到的狀態(tài)st,根據(jù)Actor的現(xiàn)有策略π來選擇動作at作為機械臂的執(zhí)行動作,執(zhí)行動作at到達下一個時間步t+1后,Agent從仿真環(huán)境中獲取狀態(tài)st+1、根據(jù)獎勵函數(shù)獲取獎勵值rt+1,隨后按照策略與環(huán)境交互直到周期結(jié)束。機械臂在訓練過程中,并沒有通過ROS對真機進行控制,而是在測試過程中,實現(xiàn)仿真與真機的聯(lián)控。在深度強化學習算法中狀態(tài)空間、動作空間以及獎勵函數(shù)起著關(guān)鍵性作用,分別用狀態(tài)向量描述狀態(tài)空間的環(huán)境特征,動作向量描述動作空間的動作輸出,獎勵函數(shù)來描述交互的獎懲程度。

圖3 每個step的環(huán)境交互系統(tǒng)

2.1 狀態(tài)向量

一個好的狀態(tài)向量可以用來加速Agent學習速度,使算法可以更快的收斂。在實驗中與環(huán)境相關(guān)的變量如圖3所示,有機械臂抓取點位置E(機械臂末端下方15 cm處)、機械臂抓取點在XY平面的投影位置E′、障礙物位置O、目標位置G;根據(jù)以上變量定義狀態(tài)向量為:

State=[E′O1,EO1i,E′O2,EO2i,EG,EGi]

(1)

式中:i=x、y、z,E′O1、E′O2為E′點與O點的水平面距離,EG為E點與G點的空間直線距離,EO1i、EO2i、EGi為EO1、EO2、EG空間直線在X、Y、Z三個坐標軸的距離。

2.2 動作向量

深度強化學習算法應用在機械臂上,一般是Actor網(wǎng)絡直接輸出機械臂關(guān)節(jié)值q或關(guān)節(jié)增量Δq來控制機械臂進行正向運動,但是正向運動控制在環(huán)境復雜的情況下需要將關(guān)節(jié)空間映射到笛卡爾空間,Agent難以獲取到有效的數(shù)據(jù)樣本。采用Actor網(wǎng)絡輸出機械臂抓取點三維坐標的逆向運動方法,這個方法根據(jù)輸出的抓取點坐標進行機械臂逆解得到一組無碰撞的機械臂各關(guān)節(jié)值,然后通過得到的機械臂關(guān)節(jié)值控制機械臂。因此定義動作向量為:

Action=[Ex,Ey,Ez]

(2)

式中:Ex、Ey、Ez為抓取點的三維坐標。

2.3 獎勵函數(shù)的設(shè)計

為提高機械臂在有障礙物環(huán)境中的路徑規(guī)劃學習效率,利用位置E、位置E′、位置O和位置G設(shè)計了一種新型位置獎勵函數(shù),如圖4所示。在存在障礙物的環(huán)境下,機械臂既要躲避障礙物,又要能夠到達目標位置。因此針對障礙物設(shè)計了對應的避障懲罰函數(shù)和碰撞懲罰函數(shù),針對目標物設(shè)計了目標獎勵函數(shù)和終止獎勵函數(shù)。

圖4 位置獎勵函數(shù)示意圖

(1)避障懲罰函數(shù):避障懲罰函數(shù)根據(jù)位置E′與位置O水平面之間的距離E′O和正態(tài)分布建立,如式(3)所示:

(3)

式中:E′O為機械臂抓取點與障礙物水平面之間的距離,c1為懲罰值閾值,主要來調(diào)節(jié)懲罰程度;c2、c3為調(diào)節(jié)系數(shù),主要調(diào)節(jié)各個懲罰區(qū)域懲罰值的收斂速度。

以障礙物為中心,將懲罰區(qū)域分為碰撞區(qū)、警示區(qū)和安全區(qū),如圖5所示為懲罰區(qū)域和懲罰函數(shù)對應圖,橫坐標為E′O,縱坐標為對應的懲罰值。碰撞區(qū)為深灰色區(qū)域,區(qū)域半徑d1為障礙物的最大半徑和機械臂末端的半徑之和;警示區(qū)為淺灰色區(qū)域,區(qū)域半徑d2大小為障礙物的最大半徑和機械臂末端的直徑之和;安全區(qū)為綠色區(qū)域外,其大小為機械臂所能達到的最大范圍。

圖5 避障懲罰函數(shù)區(qū)域?qū)獔D

(2)目標獎勵函數(shù):目標獎勵函數(shù)根據(jù)機械臂抓取位置E與目標物位置G之間的空間距離,采用分層獎勵的形式引導機械臂到達目標位置。如式(4)所示為分層獎勵函數(shù),以機械臂抓取點與目標物之間的距離進行分層。

(4)

式中:EG為機械臂抓取點與目標物之間的空間距離,ak(k=1,2,3)為獎賞程度,|ak|值越大表征獎賞程度越高;bn(n=1,2,3)用來連接各層,防止層與層之間出現(xiàn)獎勵值堆疊的問題。

(3)碰撞懲罰函數(shù):碰撞懲罰函數(shù)主要用來約束機械臂與障礙物、工作臺之間不要發(fā)生碰撞。碰撞懲罰函數(shù)如式(5)所示,其中a取-200:

fcollision=a

(5)

(4)終止獎勵函數(shù):終止獎勵函數(shù)主要是用來獎勵機械臂到達目標位置,當EG<2 cm,則認為機械臂完成避障路徑規(guī)劃任務。終止獎勵函數(shù)如式(6)所示,其中b取400:

fdone=b

(6)

故設(shè)計出的最終獎勵函數(shù)如式(7)所示:

R=fEO(EO)+fEG(EG)+fcollision+fdone

(7)

3 機械臂模型及仿真環(huán)境

通過將所提出的MRD-DDPG算法應用在Gazebo仿真平臺中建立的機械臂模型進行實驗驗證。其中,機械臂模型以大族機械臂E05型的六自由度協(xié)作機械臂為原型。

3.1 機械臂建模

根據(jù)大族機械臂的參數(shù)建立其D-H坐標系,如圖6所示為機械臂的D-H坐標系。

圖6 機械臂D-H坐標系

根據(jù)機械臂D-H坐標系,列出用于描述各連桿以及相鄰連桿之間的4個參數(shù),其中,θi為關(guān)節(jié)角;αi為機械臂的連桿轉(zhuǎn)角;ai為相鄰關(guān)節(jié)間連桿的長度;di為相鄰關(guān)節(jié)間連桿的偏移量,如表1所示為機械臂D-H參數(shù)表。

表1 D-H參數(shù)表

3.2 仿真環(huán)境搭建

仿真環(huán)境主要有大族六自由度協(xié)作機械臂的模型、1個試驗桌、目標物和多個障礙物。如圖7所示為仿真環(huán)境1,障礙物有2個大小相同的長方體,其長寬高分別為7 cm、7 cm、30 cm。如圖8所示為仿真環(huán)境2,障礙物為1個長寬高為5 cm、40 cm、30 cm的長方體;1個長寬高為40 cm、4 cm、20 cm的長方體;1個長寬高為4 cm、50 cm、40 cm的長方體。如圖9所示為仿真環(huán)境3,障礙物為2個長寬高為7 cm、7 cm、30 cm的長方體;1個長寬高為5 cm、40 cm、30 cm的長方體;1個長寬高為4 cm、50 cm、40 cm的長方體。

圖7 機械臂仿真環(huán)境1 圖8 機械臂仿真環(huán)境2 圖9 機械臂仿真環(huán)境3

4 實驗與分析

本文以機械臂完成路徑規(guī)劃的平均成功率作為算法性能的評判指標。如式(8)為單次實驗的成功率的定義。

(8)

式(9)則為平均成功率的定義,即多次實驗成功率的平均值,實驗的次數(shù)N取值為3次。

(9)

4.1 超參數(shù)設(shè)置

MRD-DDPG算法在實驗過程中所使用的超參數(shù)如表2所示。

表2 MRD-DDPG算法的超參數(shù)

4.2 MRD參數(shù)的影響

對提出的MRD方法進行分析,主要探究MRD方法的參數(shù)對算法的影響效果。因此針對MRD方法的2個參數(shù),即不同延遲Episode和采樣比例,在仿真環(huán)境1中進行參數(shù)探究的實驗。

針對不同延遲Episode的對比實驗,設(shè)定延遲Episode為400、600、800、1000、1200來進行參數(shù)對比。經(jīng)實驗發(fā)現(xiàn),如圖10所示,在延遲Episode為600時MRD-DDPG算法訓練的效果最好;當延遲400Episode到800Episode時,平均成功率在95%以上,訓練效果較好。

在延遲600Episode的MRD-DDPG算法的基礎(chǔ)上探究經(jīng)驗池的采樣比例對算法性能的影響,實驗結(jié)果如圖11所示。經(jīng)實驗發(fā)現(xiàn)正樣本與普通樣本的采樣比例為0.5時效果最好,當采樣比例小于0.4或者大于0.7時,訓練效果會變差。

圖10 不同延遲Episode下的平均成功率 圖11 不同采樣比例下的平均成功率

4.3 算法對比與分析

在仿真環(huán)境1中進行兩組對比實驗,一組是MRD與其他種類的經(jīng)驗池進行對比對比分析;另一組是MRD-DDPG與其他同類算法的對比分析。

在第1組實驗中,將DDPG算法分別應用單一經(jīng)驗池、多經(jīng)驗池(multi-replay buffer,MR)、非負樣本經(jīng)驗池(non-negative sample,NNS)、非負樣本多經(jīng)驗池(MR-NNS)以及延遲采樣多經(jīng)驗池(MRD),通過對比完成規(guī)劃任務的平均成功率驗證MRD的有效性。如圖12所示為MRD和不同經(jīng)驗池的性能對比。通過實驗數(shù)據(jù)可以發(fā)現(xiàn),MR-DDPG和MR-NNS-DDPG的成功率幾乎接近于2%,NNS-DDPG和原始的DDPG算法最終的平均成功率在10%左右,MRD-DDPG算法的平均成功率在97%左右。經(jīng)此可以說明MRD在保證樣本一定的比例后,再進行采樣能夠大幅提升訓練效果。

第2組實驗將MRD-DDPG算法與DDPG和TD3算法進行對比,同時也測試了MRD在其他算法上是否有效。如圖13所示為DDPG、TD3、MRD-DDPG、MRD-TD3的訓練結(jié)果。由圖13可知,MRD-DDPG算法其成功率比DDPG算法提高了88%、比TD3算法提高了91%。并且MRD方法不僅可以提升DDPG算法的性能,同樣可以大幅度提高TD3算法的性能,MRD-TD3比TD3算法提高了76%。

圖12 不同經(jīng)驗池下的平均成功率 圖13 不同算法下的平均成功率

4.4 MRD-DDPG實驗驗證與分析

仿真實驗分別在仿真環(huán)境1、仿真環(huán)境2、仿真環(huán)境3各測試了40組,測試的采樣比例為0.5、延遲Episode為600。

如圖14～圖16所示為各個仿真環(huán)境中測試40組中的一組路徑軌跡。經(jīng)實驗發(fā)現(xiàn),MRD-DDPG算法能夠很好的規(guī)劃出一條無碰撞的路徑,使抓取點與目標點的誤差保持在2 cm以內(nèi)。

圖14 仿真環(huán)境1規(guī)劃路徑 圖15 仿真環(huán)境2規(guī)劃路徑 圖16 仿真環(huán)境3規(guī)劃路徑

如表3所示為機械臂規(guī)劃的測試結(jié)果,在仿真環(huán)境1中規(guī)劃了40次,成功率為100%,最大規(guī)劃時間為2.206 s,最小時間為0.255 s,平均時間為0.638 s;在仿真環(huán)境2中規(guī)劃了40次,成功率為100%,最大規(guī)劃時間為1.923 s,最小時間為0.153 s,平均規(guī)劃時間為0.706 s;在仿真環(huán)境3中規(guī)劃了40次,成功率為97.5%,最大規(guī)劃時間為1.761 s,最小時間為0.353 s,平均規(guī)劃時間為0.712s。表4為其它算法[16-19]完成規(guī)劃所需時間,由此可知提出的算法可以滿足機械臂規(guī)劃的實時性要求。

表3 MRD-DDPG算法的驗證結(jié)果

表4 其它算法的規(guī)劃時間 (s)

5 結(jié)論

本文將改進后的MRD-DDPG算法用在大族機械臂上,讓機械臂根據(jù)當前的狀態(tài)選擇最優(yōu)動作,使機械臂在躲避障礙物的前提下從抓取點位置順利到達目標點位置。在ROS下的Gazebo仿真環(huán)境中對機械臂進行訓練和測試來驗證方法的可行性和算法的有效性。結(jié)果表明,在MRD-DDPG算法下避障路徑規(guī)劃的成功率達97%左右,能夠很好的實現(xiàn)避障路徑規(guī)劃任務;相比于傳統(tǒng)的避障路徑規(guī)劃方法,本文提出的方法具有更好的實時規(guī)劃能力。