王 東，黃瑞元，李偉政，黃之峰

（廣東工業(yè)大學 自動化學院，廣東 廣州 510006）

當移動機械臂在室內(nèi)環(huán)境中執(zhí)行抓取任務時，主要挑戰(zhàn)之一是如何高效地避開障礙并且成功抓取目標物。在室內(nèi)環(huán)境較為復雜、障礙物較多的情況下，移動機械臂的抓取規(guī)劃成功率會受到障礙物的影響，并且抓取規(guī)劃成功率低也會導致RRT的規(guī)劃時間增加。因此，移動機械臂首先要預處理環(huán)境信息，然后生成并到達優(yōu)化后的?？课恢?，再進行抓取。

隨著機器人技術(shù)的不斷發(fā)展，固定位置操作的機械臂不能滿足多方面需求，而且機械臂需要安全高效地在動態(tài)環(huán)境中工作。移動機械臂所面臨的環(huán)境更加復雜，若移動機械臂搜尋路徑效率不高，則會嚴重影響其工作質(zhì)量。因此如何快速地規(guī)劃出一條有效的安全路徑極具研究意義與實際應用價值[1-3]。在較狹窄或較擁擠場所中，移動機械臂需要根據(jù)環(huán)境中的信息快速獲取目標的位置，然后結(jié)合機械臂抓取空間的評價標準，得到優(yōu)化后的底盤?？课恢?，最后結(jié)合RRT算法避開障礙物，完成目標點抓取任務。

對于移動機械臂避障抓取問題，早期大多數(shù)研究都是對移動機械臂進行分散控制，即先控制移動底盤到達目標位置，然后再進行機械臂的運動規(guī)劃避障抓取。例如，先進行移動底盤導航定位及地圖創(chuàng)建[4-5]，然后進行機械臂運動規(guī)劃及軌跡生成[6]；現(xiàn)在有部分研究涉及移動底盤與機械臂的全身控制，通過協(xié)調(diào)控制移動機械臂縮短控制時間，并且能夠?qū)崿F(xiàn)在狹小空間調(diào)整位姿抓取[7-8]。其中都未提及到該如何結(jié)合環(huán)境信息、機械臂位置信息和各約束條件，生成物體抓取的最優(yōu)?？课恢玫认嚓P(guān)問題。

對于機械臂抓取過程中的避障問題，文獻[9]首次提出應用于機器人路徑規(guī)劃的虛擬力場方法——人工勢場法，文獻[10-11]提出一種基于A*算法的移動機械臂無碰抓取方法，文獻[12]在傳統(tǒng)蟻群算法的基礎(chǔ)上，引入帶方向信息的全局啟發(fā)因子來提高最優(yōu)路徑的搜索效率，能夠在較短時間內(nèi)規(guī)劃出滿足條件的最優(yōu)路徑。文獻[13]提出一種改進的快速擴展隨機樹算法(Rapidly-exploring Random trees, RRT)。文獻[14]提出低振蕩人工勢場?自適應快速擴展隨機樹(Adaptive Rapidly-exploring Random Trees,ARRT)混合算法進行路徑規(guī)劃。文獻[15]設(shè)計基于雙向RRT算法的運動規(guī)劃方法，有效地解決仿人機器人的抓取操作問題。上述算法都未考慮將機械臂連桿與障礙物碰撞條件加入避障。

本文對環(huán)境中目標物及障礙物的識別、定位[16-17]采用的是aruco碼[18]標記以及在動作捕捉系統(tǒng)下粘貼感應點的方法。在不同的約束條件下，如何找到移動機械臂優(yōu)化后的?？课恢?，提高移動機械臂抓取物體的規(guī)劃成功率是主要挑戰(zhàn)。本文重點研究如何在更短時間內(nèi)提高移動機械臂在多障礙物環(huán)境下的避障抓取規(guī)劃成功率，主要貢獻如下：(1) 分析了機械臂結(jié)合夾爪碰撞約束條件且無障礙物的抓取空間；(2) 分析了移動底盤、機械臂末端以及機械臂連桿與障礙物之間碰撞的多約束條件；(3) 提出了移動機器人在有障礙物的環(huán)境下優(yōu)化?？课恢玫挠嬎惴椒?，有效地提高了移動機器人進行抓取任務規(guī)劃的效率。

1 硬件規(guī)格

圖1為研究中使用的移動機械臂，包括加拿大Clearpath公司生產(chǎn)的ridgeback移動底盤、優(yōu)傲機器人公司生產(chǎn)的UR5協(xié)作式機械臂和Robotiq公司生產(chǎn)的三指夾爪。其中ridgeback移動底盤裝有4個麥克納姆輪，為全方位移動小車；UR5為六關(guān)節(jié)機械臂，末端負載為5 kg；Robotiq夾爪為自適應三指夾爪，可抓取多種不同形狀物體。Realsense相機安裝在機械臂末端，其還使用了動作捕捉系統(tǒng)，目的是建立環(huán)境中各物體的位置信息。電腦通過網(wǎng)絡(luò)端口連接控制移動機械臂，通過局域網(wǎng)連接動作捕捉系統(tǒng)傳輸數(shù)據(jù)。

圖1 移動機械臂硬件Fig.1 Mobile manipulator hardware

2 方法

2.1 碰撞約束條件

根據(jù)任務要求，碰撞約束條件主要包括在對工作空間進行分析時的夾爪與機械臂自身連桿的碰撞和在進行RRT避障規(guī)劃時的機械臂末端、連桿與障礙物的碰撞。此處將夾爪和障礙物簡化為球體，則碰撞約束問題簡化為求空間中一點到一條線段的最短距離是否滿足條件。

如圖2所示，問題描述為P是夾爪中心點或障礙物中心點，XH，YH分別為連桿的2個端點，r為球體半徑，RH為連桿半徑。當參數(shù)t∈(0,1)時，Oproject為XHYH線段上的點；當t≤0時，其為YHXH延長線上的點；當t≥1時，其為XHYH延長線上的點。L為Oproject與P的距離，比較L與r+RH的大小判斷是否滿足碰撞約束條件。

圖2 碰撞約束條件分析Fig.2 Analysis of collision constraints

根據(jù)式(2)計算t的取值，便可得到空間中點到直線的最短距離L，根據(jù)式(4)可知是否滿足碰撞約束條件。

2.2 機械臂抓取工作空間分析

為移動底盤提供?？坎呗灾?，需要在無障礙物環(huán)境下，先對結(jié)合夾爪碰撞約束的機械臂進行工作空間的分析。

根據(jù)上述碰撞約束的判斷和機械臂抓取情況，物體在機械臂基座上方和前方，機械臂工作空間區(qū)域為(x：0～900 mm，y：?900～900 mm，z：0～900 mm)，依次按比例劃分為3×6×3個區(qū)域，隨機給定6組關(guān)節(jié)角，檢測并記錄滿足碰撞約束的末端點，繪制空間網(wǎng)格圖，根據(jù)每個區(qū)域中包含末端點數(shù)目的不同繪制不同顏色。平抓方式與下抓方式滿足碰撞約束的末端點數(shù)目不同，具體劃分規(guī)則見表1，m和n分別代表平抓與下抓區(qū)域中滿足點的數(shù)目。

表1 2種抓取方式劃分規(guī)則Table 1 Division Rules of two grab modes

實驗得到了平抓、下抓抓取方式滿足碰撞約束條件的工作空間網(wǎng)格分布，見圖3。

圖3 機械臂抓取空間分析Fig.3 Analysis of grasping space of manipulator

將機械臂工作空間可達到的高度0～1 000 mm依次劃分為0～20個區(qū)域，平抓與下抓抓取方式在機械臂空間高度上滿足點數(shù)目的對比見圖4。

圖4 平抓與下抓對比Fig.4 Comparison of flat grip and down grip

根據(jù)圖3～4可知在機械臂的工作區(qū)域(x：0～300 mm，y：?300～300 mm，z：0～300 mm)中，選取合適抓取姿態(tài)會有較高的抓取規(guī)劃成功率。

2.3 機械臂抓取難易評價標準分析

根據(jù)上面對機械臂工作空間的分析，可知機械臂的可抓取性會隨物體在抓取空間內(nèi)的位置變化而變化，為綜合評價機械臂抓取質(zhì)量，下文使用上述實驗數(shù)據(jù)來綜合評價抓取質(zhì)量Qf，其中a為被抓取物體寬度，b為夾爪最大張開寬度，d為被抓取物體與移動機械臂底盤的距離，h為被抓取物體相對移動機械臂底盤的高度，抓取方式此處定為平抓與下抓方式。

定義a

最終抓取質(zhì)量評分為Qf=Q1Q2Q3Q4，27分為最高分，0分為最低分。由式(5)可知當被抓取物體寬度大于夾爪最大張開寬度時，直接導致抓取得分為0。對于評分過低的抓取操作需要以此為初始值進行搜索，以得到可靠性更高的抓取。

2.4 障礙物環(huán)境下移動機械臂停靠策略

針對移動機械臂工作環(huán)境中存在障礙物等情況，結(jié)合2.2和2.3節(jié)，本文提出了移動底盤?？课恢脙?yōu)化算法，并且加入碰撞約束的RRT算法，使機械臂末端在空間中進行避障抓取。

成功抓取目標物體的關(guān)鍵是移動底盤的優(yōu)化位置搜索，針對在桌面上抓取物體，移動底盤?？课恢脙?yōu)化算法流程圖如圖5所示。

圖5 移動底盤?？课恢脙?yōu)化算法Fig.5 Parking position optimization algorithm of mobile chassis

其中機械臂初始六關(guān)節(jié)角為θ0(1×6)，移動基座起始位置為p_start，目標物體位置為p_goal，障礙物位置為p_obs；圓心為p_goal，半徑為r，定義圓心與基座起始位置連線為分割線l，圓上與分割線相交的解通過極坐標方程求解，定義為s。

C1：障礙物簡化為點后與分割線的最短距離判斷依據(jù)為2.1中碰撞約束條件的求解方式。

C2：交點s與目標p_goal是否滿足在同一側(cè)的判斷依據(jù)為p_goal(1),s(1)≤p_cen(1)或p_goal(1),s(1)>p_cen(1)，其中p_cen為桌面的中心位置。

C3：機械臂抓取難易評價標準Q2。

3 仿真和實驗

為了評估移動底盤?？课恢脙?yōu)化方法的有效性，結(jié)合上述對機械臂抓取工作空間的分析，限定物體的高度及抓取方式之后，抓取過程只涉及到物體與移動機械臂底盤的距離，使用提出的算法進行仿真，并在實體機器人上進行實驗。該算法在Intel Core i5-8300HQ的PC上運行(2.3 GHz) MATLAB版本2016b實驗。對比移動底盤不移動與加入移動底盤停靠位置優(yōu)化算法在抓取物體上的區(qū)別，結(jié)果表明加入移動底盤?？课恢脙?yōu)化方法后，計算時間更短，抓取成功率更高。

3.1 仿真

未加入移動底盤?？课恢脙?yōu)化算法的移動機械臂保持位置不變，直接運行RRT避障規(guī)劃算法，生成機械臂末端軌跡點。加入移動底盤停靠位置優(yōu)化算法，移動機械臂會移動到優(yōu)化位置，然后機械臂旋轉(zhuǎn)朝向目標物體，再采用RRT避障規(guī)劃算法規(guī)劃末端點軌跡。

下文在幾組不同條件下驗證加入移動底盤?？课恢脙?yōu)化算法的可行性?？紤]機械臂平抓方式比下抓方式抓取成功率更高，并且實際環(huán)境中物體擺放位置有一定高度，驗證中采用平抓方式。其中障礙物及夾爪簡化為球體，半徑r設(shè)置為40 mm；連桿近似為圓柱體，半徑RH設(shè)置為40 mm，移動機械臂底盤起點均為坐標系中的(0, 0, 0)位置。選取可執(zhí)行率均值、路徑點完全執(zhí)行次數(shù)、總規(guī)劃路徑點數(shù)和RRT規(guī)劃時間均值作為評價指標，可執(zhí)行率為一組測試中機械臂可執(zhí)行路徑點數(shù)目與總規(guī)劃路徑點數(shù)目的比值。

3.1.1 空間單障礙物靠近目標物體避障仿真實驗

此實驗中，障礙物位置為(400 mm, 400 mm, 400 mm)，目標點位置為(500 mm, 500 mm, 400 mm)，機械臂起始六關(guān)節(jié)角為(30°, ?75°, ?104°, ?29°, 91°,231°)。加入移動底盤位置控制前后的對比見圖6。

圖6 單障礙物靠近目標物體避障實驗加入移動底盤位置控制前后對比Fig.6 Comparison before and after adding mobile chassis position control in obstacle avoidance experiment when single obstacle approaching target object

圖7為20次測試下移動底盤是否移動在可執(zhí)行率均值及規(guī)劃時間均值上的對比。表2為移動底盤是否移動的評價指標數(shù)據(jù)對比。

圖7 單障礙物靠近目標物體避障20次實驗數(shù)據(jù)對比Fig.7 Comparison of 20 experimental data in obstacle avoidance experiment when single obstacle approaching target object

表2 單障礙物靠近目標物體避障實驗底盤是否移動的數(shù)據(jù)對比Table 2 Data comparison of whether the chassis moves in obstacle avoidance experiment when single obstacle approaching target object

3.1.2 空間三障礙物避障仿真實驗

此實驗中，障礙物位置分別為(200 mm, 200 mm,200 mm)，(300 mm, 0 mm, 500 mm)，(200 mm, 400 mm, 300 mm)，目標點位置為(550 mm, 500 mm, 500 mm)，機械臂起始六關(guān)節(jié)角為(?179°, ?52°, 152°,?290°, 98°, ?180°)。

加入移動底盤?？课恢脙?yōu)化算法仿真步驟如圖8所示，移動機械臂的底盤先移動到計算得到的優(yōu)化?？课恢?，然后調(diào)整機械臂初始朝向，最后進行RRT算法避障。

圖8 三障礙物避障實驗移動機械臂仿真抓取Fig.8 Simulation and grasping of mobile manipulator in three obstacles avoidance experiment

圖9為20次測試下移動底盤是否移動在可執(zhí)行率均值和規(guī)劃時間均值上的對比。表3為移動底盤是否移動的評價指標數(shù)據(jù)對比。

圖9 三障礙物避障20次實驗數(shù)據(jù)對比Fig.9 Comparison of 20 experimental data in three obstacles avoidance experiment

表3 三障礙物避障實驗底盤是否移動的數(shù)據(jù)對比Table 3 Data comparison of whether the chassis moves in three obstacles avoidance experiment

3.1.3 空間三障礙物靠近目標物體避障仿真實驗

此實驗中，障礙物位置分別為(400 mm, 700 mm,400 mm)，(500 mm, 300 mm, 500 mm)，(300 mm, 550 mm, 400 mm)，目標點位置為(500 mm, 500 mm, 400 mm)，機械臂起始六關(guān)節(jié)角(?179°, ?52°, 152°, ?290°,98°, ?180°)。圖10為加入移動底盤位置控制前后的對比。圖11為20次測試下移動底盤是否移動在可執(zhí)行率均值和規(guī)劃時間均值上的對比。表4為移動底盤是否移動的評價指標數(shù)據(jù)對比。

圖10 三障礙物靠近目標物體避障實驗加入移動底盤位置控制前后對比Fig.10 Comparison before and after adding mobile chassis position control in obstacle avoidance experiment when three obstacles approaching target object

圖11 三障礙物靠近目標物體避障20次實驗數(shù)據(jù)對比Fig.11 Comparison of 20 experimental data in obstacle avoidance experiment when three obstacles approaching target object

表4 三障礙物靠近目標物體避障實驗底盤是否移動的數(shù)據(jù)對比Table 4 Data comparison of whether the chassis moves in obstacle avoidance experiment when three obstacles approaching target object

3.1.4 空間六障礙物避障仿真實驗

此仿真實驗中，障礙物位置分別為(300 mm, 200 mm, 400 mm)，(300 mm, 0 mm, 500 mm)，(200 mm,400 mm, 300 mm)，(600 mm, 0 mm, 400 mm)，(500 mm, 700 mm, 500 mm)，(600 mm, 700 mm, 300 mm)，目標點位置為(500 mm, 500 mm, 400 mm)，機械臂起始六關(guān)節(jié)角設(shè)置為(?179°, ?52°, 152°, ?290°, 98°,?180°)；圖12為加入移動底盤位置控制前后的對比。圖13為20次測試下移動底盤是否移動在可執(zhí)行率均值和規(guī)劃時間均值上的對比。表5為移動底盤是否移動的評價指標數(shù)據(jù)對比。

圖12 六障礙物避障實驗加入移動底盤位置控制前后對比圖Fig.12 Comparison before and after adding mobile chassis position control in six obstacles avoidance experiment

圖13 六障礙物避障20次實驗數(shù)據(jù)對比Fig.13 Comparison of 20 experimental data in six obstacles avoidance experiment

表5 六障礙物避障實驗底盤是否移動的數(shù)據(jù)對比Table 5 Data comparison of whether the chassis moves in six obstacles avoidance experiment

3.1.5 空間桌面避障仿真實驗

在仿真環(huán)境中添加桌面條件，使其更符合實際機械臂抓取環(huán)境。3個障礙物的位置采用隨機生成的方式，障礙物在桌面上方。目標位置為(500 mm, 500 mm, 400 mm)，桌面以(850 mm, 800 mm, 210 mm)為角點，長、寬、高分別為600 mm、400 mm、80 mm，機械臂起始六關(guān)節(jié)角為(44, ?42°, ?104°, ?29°, 91°,231°)。圖14為根據(jù)隨機障礙物的位置生成移動底盤?？績?yōu)化位置后抓取的效果圖。

圖14 根據(jù)隨機障礙物位置生成底盤?？績?yōu)化位置后抓取的效果圖Fig.14 Sketch of grabbing after the chassis stops at the optimized position is generated by the random obstacle position

圖15為20次測試下移動底盤是否移動在可執(zhí)行率均值和規(guī)劃時間均值上的對比。表6為在有桌面和障礙物的情況下，考慮機械臂連桿與其之間的碰撞約束條件后，移動底盤是否移動的評價指標對比。

圖15 桌面避障20次實驗數(shù)據(jù)對比Fig.15 Comparison of 20 experimental data in desktop obstacle avoidance experiment

表6 桌面避障實驗底盤是否移動的數(shù)據(jù)對比Table 6 Data comparison of whether the chassis moves in desktop obstacle avoidance experiment

3.1.6 樣機仿真實驗

在仿真環(huán)境中添加桌面條件，使其更加符合實際環(huán)境，分別針對正方體、長方體桌面進行了仿真。

正方體桌面仿真實驗條件：長寬為500 mm×500 mm，桌面位置為(x：1 250～1 750 mm，y：1 250～1 750 mm，z：210～290 mm），移動機械臂初始位置為(0 mm,0 mm,0 mm)，障礙物位置分別為(1 550 mm,1 500 mm, 350 mm)，(1 400 mm, 1 350 mm, 350 mm)，(1 450 mm, 1 650 mm, 350 mm)，目標位置為(1 300 mm,1 500 mm, 400 mm)；移動機械臂移動抓取過程如圖16所示。

圖16 正方體桌面下移動機械臂抓取過程Fig.16 Grasping process of mobile manipulator under cube table

長方體桌面實驗條件：長寬為1 800 mm×600 mm，桌面的位置為（x：1 250～1 850 mm,y：400～2 200 mm,z：210～290 mm），移動機械臂初始位置為(0 mm, 0 mm, 0 mm)，障礙物位置分別為(1 500 mm,500 mm, 350 mm)，(1 400 mm, 1 200 mm, 350 mm)，(1 700 mm, 700 mm, 350 mm)，目標物位置為(1 700 mm,500 mm, 400 mm)；仿真實驗結(jié)果如圖17所示。

圖17 長方體桌面下移動機械臂抓取過程Fig.17 Grasping process of mobile manipulator under cuboid table

3.2 樣機實驗

在正方體和長方體桌面上對目標物體及障礙物分別進行了實驗，實際環(huán)境均在動作捕捉系統(tǒng)下搭建，桌面長寬根據(jù)仿真距離搭建，障礙物用方盒代替，目標物體用水瓶代替。

圖18顯示的是移動機械臂抓取正方體桌面上的目標物體，正方體桌面長寬為500 mm×500 mm，在動作捕捉系統(tǒng)下，正方體桌面的位置為(x：1 250～1 750 mm,y：1 250～1 750 mm,z：600 mm)，移動機械臂初始位置為(0 mm, 0 mm, 300 mm)，障礙物及目標物位置根據(jù)仿真條件設(shè)定，移動機械臂最終?？课恢脼?945 mm, 1 080 mm, 300 mm)。

圖18 移動機械臂抓取正方體桌面上的目標物Fig.18 The mobile manipulator grabs the object on the cube table top

圖19顯示的是移動機械臂抓取長方體桌面上的目標物體。長方體桌面長寬為1 800 mm×600 mm，在動作捕捉系統(tǒng)下，長方體桌面的位置為(x：1 250～1 850 mm,y：400～2 200 mm,z：600 mm)，移動機械臂初始位置為(0 mm, 0 mm, 300 mm)，障礙物及目標物位置根據(jù)仿真條件設(shè)定，移動機械臂最終?？课恢脼?2 276 mm, 330 mm, 300 mm)。

圖19 移動機械臂抓取長方體桌面上的目標物Fig.19 The mobile manipulator grabs the object on the cuboid table top

圖18和圖19中，(a)和(b)為移動機械臂底盤移動至優(yōu)化后的停靠位置；(c)和(d)為機械臂末端移動至目標物體；(e)和(f)為夾爪閉合夾取目標物體并舉起。

4 討論

上述仿真實驗數(shù)據(jù)顯示，不同條件下加入移動底盤?？课恢脙?yōu)化算法后可提高可執(zhí)行率及增加路徑點完全執(zhí)行次數(shù)。

仿真實驗3.1.1顯示當單個障礙物較大且靠近目標物體時，移動機械臂不移動的情況下不存在能完全執(zhí)行路徑點到達目標點的情況；而底盤的移動能有效地提高目標物體的抓取規(guī)劃成功率，并縮短規(guī)劃時間。

樣機實驗實現(xiàn)了目標物體的成功抓取，證明了仿真計算的移動底盤?？课恢脙?yōu)化算法及RRT避障算法在實際應用中的可行性。與之前的方法相比，本文所提出的加入連桿碰撞約束的RRT避障方法增強了機械臂的可操作性與避障能力；移動底盤?？课恢脙?yōu)化算法不僅提高了規(guī)劃速度，而且提高了物體的抓取規(guī)劃成功率。未來的研究將集中在動態(tài)方面，根據(jù)實時環(huán)境監(jiān)測提出移動底盤實時搜尋移動位置的控制策略。