基于虛擬空間三維體素—語義地圖的機(jī)械臂任務(wù)規(guī)劃

王立鵬，王小晨，劉夢杰，孟 浩

（哈爾濱工程大學(xué)智能科學(xué)與工程學(xué)院，哈爾濱 150001）

0 引言

服務(wù)類機(jī)械臂是當(dāng)前機(jī)器人領(lǐng)域的研究熱點(diǎn)，未來的服務(wù)類機(jī)械臂會進(jìn)入每個家庭并從事重要的自動化和智能化任務(wù)，對于服務(wù)類機(jī)器人的研究存在一些挑戰(zhàn)，包括自主性建模和估計、機(jī)器人環(huán)境建圖、語義驅(qū)動等問題［1-2］。本文圍繞機(jī)械臂多樣化信息的三維地圖構(gòu)建、語義驅(qū)動條件下的機(jī)械臂路徑規(guī)劃和優(yōu)化等開展研究，考慮到虛擬仿真技術(shù)具有低成本和耗時少等優(yōu)點(diǎn)［3-4］，將基于虛擬仿真空間開展以上內(nèi)容的研究工作。

文獻(xiàn)［5］中介紹了視覺同步定位與建圖（Visual Simultaneous Localization and Mapping，VSLAM）經(jīng)典框架，從間接和直接兩種方法綜述了VSLAM 的關(guān)鍵技術(shù)和最新研究進(jìn)展，并對VSLAM 的發(fā)展趨勢進(jìn)行了展望。文獻(xiàn)［6］中提出一種基于三維激光雷達(dá)數(shù)據(jù)的低漂移SLAM算法，針對激光雷達(dá)掃描采樣策略，設(shè)計了一種模型匹配框架，實驗驗證了新方法的有效性。Zhou 等［7］提出一種使用高效通道注意力插入壓縮MobileNet V2（Efficient Channel Attention to insert into Compressed MobileNet V2，ECMobileNet）的策略，減少操作數(shù)同時保持精度，基于ECMobileNet 特征向量的平均分布設(shè)計了相應(yīng)的環(huán)路檢測方法。文獻(xiàn)［8］中提出了3D動態(tài)場景圖，完成可操作空間感知的統(tǒng)一表示，并且提供了算法來獲得室內(nèi)環(huán)境及其關(guān)系的分層表示。Yu等［9］中提出一種自底向上結(jié)構(gòu)化三維場景圖生成框架，以結(jié)構(gòu)化的表示方式高效描述三維室內(nèi)環(huán)境的對象、關(guān)系和屬性，采用視覺感知捕獲語義信息，從場景先驗中進(jìn)行推理，計算出最優(yōu)解析圖。近年來，在提供語義豐富且?guī)缀尉_的空間模型，三維重建已成為一項重要且具有挑戰(zhàn)性的任務(wù)［10］。語義豐富且?guī)缀螠?zhǔn)確的空間模型為工作空間內(nèi)導(dǎo)航服務(wù)提供了關(guān)鍵信息［11］，室內(nèi)環(huán)境建模有不同的表示方法，包括網(wǎng)格模型、計算機(jī)輔助設(shè)計幾何模型以及參數(shù)模型［12］。常用的工作空間建圖方法是規(guī)劃前建圖，為保證機(jī)器人在空間中的任務(wù)規(guī)劃，可對場景中部分或者全部障礙物進(jìn)行操作。Sinha等［13］通過視覺獲取環(huán)境信息，通過移除障礙物使得抓取目標(biāo)不被障礙物遮擋。

根據(jù)采集場景數(shù)據(jù)的完整性，把路徑規(guī)劃分為：基于先驗完全信息的全局路徑規(guī)劃和基于傳感器信息的局部路徑規(guī)劃［14］。現(xiàn)階段，機(jī)器人軌跡規(guī)劃算法較為成熟，對未知非結(jié)構(gòu)化環(huán)境內(nèi)的任務(wù)規(guī)劃和機(jī)器人協(xié)調(diào)規(guī)劃是研究的熱點(diǎn)。基于全局3D 傳感器（深度相機(jī)，雷達(dá)等）或機(jī)器人與傳感器結(jié)合等完成非結(jié)構(gòu)環(huán)境建圖，Deeb 等［15］提出分段確定性準(zhǔn)靜態(tài)同步定位與建圖（Piecewise-deterministic Quasi-static Simultaneous Localization and Mapping，PDQS-SLAM），解決了地標(biāo)長期微位移對機(jī)器人定位與建圖的影響。Wan等［16］提出一種基于被動立體視覺的高精度位姿測量系統(tǒng)，以實現(xiàn)非結(jié)構(gòu)化環(huán)境下的精確目標(biāo)定位和位姿估計；為解決在障礙物環(huán)境中軌跡規(guī)劃問題，分別為避障規(guī)劃和與障礙物的交互［17］。在任務(wù)與運(yùn)動規(guī)劃融合方面，強(qiáng)化學(xué)習(xí)等方法開展端到端的任務(wù)規(guī)劃，采用傳統(tǒng)基于任務(wù)的運(yùn)動規(guī)劃或兩者根據(jù)實際情況考慮優(yōu)先級，與此同時，軌跡優(yōu)化也是有著許多優(yōu)秀的算法，根據(jù)目標(biāo)不同可分為最小能量、最優(yōu)時間、速度連續(xù)和凸優(yōu)化等［18-19］。

目前機(jī)械臂任務(wù)規(guī)劃存在如下問題：機(jī)器人地圖更多關(guān)注地圖精度與效率，很少將建圖與機(jī)器人任務(wù)相結(jié)合，地圖模型不能完全跟隨環(huán)境信息的變化而更新，需要消耗資源，擴(kuò)展性受到限制；其次，規(guī)劃算法很多，但需根據(jù)實際使用情況進(jìn)行調(diào)整匹配，不具有速度信息的路徑需經(jīng)過后續(xù)軌跡優(yōu)化得到安全、可行的路徑，優(yōu)化過程也會因機(jī)器人、優(yōu)化目標(biāo)不同而不同，同時，直接在速度、加速度空間上開展運(yùn)動規(guī)劃則會存在規(guī)劃時間長或者規(guī)劃后依然需要優(yōu)化的情況。

為解決以上問題，提出基于虛擬空間的機(jī)械臂建圖與規(guī)劃方法，技術(shù)路線如圖1 所示。

圖1 技術(shù)路線

由圖1 可見，利用虛擬空間，構(gòu)建一種三維體素-語義混合地圖，訓(xùn)練Mask RCNN 神經(jīng)網(wǎng)絡(luò)模型，得到物體圖像掩膜結(jié)合深度圖像分割、拼接單個較完整的物體點(diǎn)云，利用迭代最近點(diǎn)（Iterative Closest Point，ICP）算法估計物體位姿，通過優(yōu)化A*算法和位姿插值方法開展機(jī)械臂末端執(zhí)行器路徑規(guī)劃，通過貝塞爾曲線插值進(jìn)行軌跡優(yōu)化，使得關(guān)節(jié)軌跡光滑連續(xù)。設(shè)計語義驅(qū)動下的典型幾何體抓取位姿和任務(wù)分解策略，通過虛擬場景和實際物理場景開展實驗仿真。本文研究成果可提供其他算法的驗證性環(huán)境，也可接入實際物理環(huán)境提供的采集數(shù)據(jù)，在虛擬場景中建圖，形成體素-語義地圖，虛擬場景完成規(guī)劃和優(yōu)化、分析和分解任務(wù)，驅(qū)動實際機(jī)械臂執(zhí)行。

1 三維體素-語義融合地圖構(gòu)建

機(jī)械臂任務(wù)規(guī)劃需要描述場景中的障礙物以及與任務(wù)相關(guān)的物體信息，本文構(gòu)建三維體素-語義融合地圖，采用Gazebo構(gòu)建三維虛擬場景，場景中含有利用SoildWorks軟件設(shè)計的平臺、機(jī)械臂和待抓取物體。

1.1 基于Mask RCNN目標(biāo)識別、分割及位姿估計

利用基于掩碼區(qū)域的卷積神經(jīng)網(wǎng)絡(luò)（Mask Region-based Convolutional Neural Network，Mask RCNN）完成彩色圖像中的目標(biāo)物體識別與分割，數(shù)據(jù)集來源于虛擬場景中相機(jī)的拍攝，數(shù)據(jù)集類別有：碗、可樂罐、啤酒罐、圓柱、立方體、球體以及機(jī)械臂本身。為豐富數(shù)據(jù)集，避免過擬合，在仿真中選擇了不同機(jī)器人的工作背景。考慮到目標(biāo)物體種類較少，本文采集標(biāo)定400 張圖片作為數(shù)據(jù)集，部分樣本如圖2 所示。

圖2 部分?jǐn)?shù)據(jù)集

在Mask RCNN訓(xùn)練時選用ResNet101 模塊，先訓(xùn)練heads模塊40 epochs，學(xué)習(xí)率0.001，訓(xùn)練總體40 epochs，學(xué)習(xí)率是heads模塊學(xué)習(xí)率的1/10，ROIS設(shè)置為100，每個epoch設(shè)置100 步，validation_steps是50，訓(xùn)練結(jié)束后的loss值為83%，訓(xùn)練結(jié)果較好。部分樣本圖片的識別結(jié)果如圖3 所示。

圖3 Mask RCNN訓(xùn)練模型檢測結(jié)果

每個RGB 圖像經(jīng)Mask RCNN 可輸出物體種類，為同一種物體分配不同編號和對應(yīng)的掩膜，將掩膜圖像保存為二值灰白圖像，將三維點(diǎn)云數(shù)據(jù)對應(yīng)掩膜圖像，如掩膜圖像中值為255，則將該點(diǎn)加入點(diǎn)云，按照該方式多幅圖像經(jīng)掩膜篩選后的點(diǎn)云拼接在一起，即可組合成一個相對完整的物體目標(biāo)點(diǎn)云，分割物體、掩膜圖像以及拼接后的目標(biāo)點(diǎn)云如圖4 所示。

圖4 分割后物體點(diǎn)云圖像

利用ICP算法可得目標(biāo)點(diǎn)云到源點(diǎn)云的平移和旋轉(zhuǎn)矩陣，將目標(biāo)點(diǎn)云的點(diǎn)在世界坐標(biāo)系下表示，且源點(diǎn)云坐標(biāo)系與世界坐標(biāo)系重合，即源點(diǎn)云中心點(diǎn)與世界坐標(biāo)系原點(diǎn)重合，則得轉(zhuǎn)換矩陣是目標(biāo)點(diǎn)云在世界坐標(biāo)下位姿的逆矩陣，即可估計出目標(biāo)物體的位姿。

1.2 體素-語義地圖融合

在虛擬場景，采集獲得的RGBD 數(shù)據(jù)轉(zhuǎn)化為點(diǎn)云數(shù)據(jù)，可將三維點(diǎn)云按照膨脹半徑擴(kuò)展為體素地圖，經(jīng)過Mask RCNN識別、分割后的目標(biāo)物體并通過位姿估計可得物體語義信息，包括物體名稱、編號和位姿信息等。將該語義信息中包含的物體與上述體素地圖分離標(biāo)注，再融合處理，得到體素-語義地圖，如圖5 所示。

圖5 三維體素-語義地圖

通過Mask RCNN 和ICP 算法可獲得目標(biāo)點(diǎn)云位姿，在執(zhí)行機(jī)械臂抓取任務(wù)后，可在體素-語義地圖中直接更新目標(biāo)點(diǎn)云，不需要重新掃描建圖、識別和分割，這是本文虛擬空間的優(yōu)勢。

2 基于體素-語義混合地圖的機(jī)械臂運(yùn)動規(guī)劃

將機(jī)械臂運(yùn)動規(guī)劃分為無碰撞軌跡規(guī)劃以及基于速度約束的軌跡優(yōu)化，通過改進(jìn)A*算法完成機(jī)械臂末端軌跡規(guī)劃，利用貝塞爾曲線優(yōu)化關(guān)節(jié)軌跡獲得平滑的軌跡。

2.1 基于A*算法機(jī)械臂軌跡規(guī)劃

優(yōu)化A*算法的實現(xiàn)流程，為每個節(jié)點(diǎn)添加一個狀態(tài)標(biāo)簽state，state=0 表示節(jié)點(diǎn)未遍歷，state=1 表示節(jié)點(diǎn)已擴(kuò)展，state=-1 表示節(jié)點(diǎn)已移除同時代價值計算并未修改。代價數(shù)值均用歐拉距離計算，各節(jié)點(diǎn)代價函數(shù)的總代價值

式中：g（n）為節(jié)點(diǎn)n到起點(diǎn)的代價值；h（n）為節(jié)點(diǎn)n到終點(diǎn)的預(yù)計代價值。

選擇set紅黑樹容器和無序set 容器，其中set 紅黑樹容器保存state=1，用以存儲已經(jīng)擴(kuò)展但未遍歷的節(jié)點(diǎn)，記為open_list_set；無序set 容器，用以存儲所有擴(kuò)展遍歷的節(jié)點(diǎn)，記為had_unordered_set。在體素-語義地圖中，已知起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)，計算兩個節(jié)點(diǎn)的g（·）、h（·）和f（·）值，把起始和目標(biāo)節(jié)點(diǎn)放入容器中。從open_list_set中取出f（·）值最小的節(jié)點(diǎn)X，并從open_list_set 中移除，令其狀態(tài)值為-1，遍歷節(jié)點(diǎn)X所有非障礙鄰居節(jié)點(diǎn)Y，如果Y在open_list_set中且Y的state值為1，是則判斷euler（X，Y）+g（X）＜g（Y）是否成立，成立則更新節(jié)點(diǎn)Y的g（·）和f（·）值，Y的父節(jié)點(diǎn)為X，更新open_list_set中Y的值；否則計算節(jié)點(diǎn)Y的g和f值，Y的父節(jié)點(diǎn)記為X，放入open_list_set和had_unordered_set。

2.2 機(jī)械臂關(guān)節(jié)軌跡優(yōu)化

在末端執(zhí)行路徑初步規(guī)劃結(jié)果的基礎(chǔ)上，在起點(diǎn)和終點(diǎn)之間將位姿插值處理，機(jī)械臂末端姿態(tài)角用四元數(shù)表示，采用四元數(shù)球面線性插值方法，設(shè)A*算法規(guī)劃路徑長度為l，則需要進(jìn)行位姿插值個數(shù)為l-2，起點(diǎn)姿態(tài)為qst，終點(diǎn)姿態(tài)為qend，則相鄰兩個點(diǎn)的姿態(tài)差

第i個點(diǎn)的機(jī)械臂模塊位姿

與起點(diǎn)和終點(diǎn)相關(guān)的末端執(zhí)行器坐標(biāo)系到世界坐標(biāo)系的齊次轉(zhuǎn)換矩陣分別為，末端執(zhí)行器坐標(biāo)系和世界坐標(biāo)系的Z軸單位向量表示為Pzst和Pzend，則在世界坐標(biāo)系中起點(diǎn)位姿：

若兩個向量均在與坐標(biāo)面平行的平面上時，計算兩向量夾角

通過式（2）、（3）計算第2 點(diǎn)的姿態(tài)角，轉(zhuǎn)換到世界坐標(biāo)系得到向量Pw2，計算Pw2和Pwend兩個向量的夾角θ2end，若θ2end＞θstend，則認(rèn)為Δq取值錯誤并將qend修改為其共軛四元數(shù)并進(jìn)行插值，反之則直接進(jìn)行插值。兩個向量不在與坐標(biāo)面平行的平面上，則投影在任意兩坐標(biāo)軸形成的平面上，方法同上。

這里用m表示關(guān)節(jié)編號，用i表示第i個軌跡點(diǎn)。設(shè)機(jī)械臂的關(guān)節(jié)速度最大限制為ωT，在插值過程中對6 個關(guān)節(jié)角的變化值求累加和，用Δθ 表示，當(dāng)累加和Δθ 加到某關(guān)節(jié)角度相對于前一關(guān)節(jié)角變化值Δθm，i時，如Δθm，i∈［0.95ωT，ωT］，則將該點(diǎn)作為最后一個控制點(diǎn)來進(jìn)行貝塞爾曲線插值，如Δθm，i＞ωT，則額外添加一組控制點(diǎn)，保證該關(guān)節(jié)變量添加值θ=θm，i-1+0.95wT，其余關(guān)節(jié)角選擇與最后的關(guān)節(jié)角一致。

在插值完成后，根據(jù)關(guān)節(jié)軌跡按順序向機(jī)械臂發(fā)送期望關(guān)節(jié)角和期望關(guān)節(jié)速度，時間步長T=0.02 s，期望關(guān)節(jié)速度為

終點(diǎn)速度為0。

3 基于語義驅(qū)動的機(jī)器人空間任務(wù)規(guī)劃

體素-語義混合地圖已附帶語義信息，利用語義為機(jī)械臂指定運(yùn)動規(guī)劃目標(biāo)，驅(qū)動機(jī)械臂完成抓取任務(wù)，使機(jī)械臂任務(wù)規(guī)劃靈活性更高，交互性更強(qiáng)且具有智能性。

3.1 機(jī)械臂末端抓取位姿

根據(jù)不同物體的幾何形狀，給出一個合適的抓取位姿，位姿的限定需滿足3 個基本條件：抓取位置以穩(wěn)定為主，避免物體脫落；抓取位姿選擇靠近機(jī)械臂基底的位姿；抓取位置沒有障礙物。在本文虛擬空間建立的模型主要有以下幾類：

（1）圓柱體。機(jī)械臂抓取圓柱體高度一半的位置，末端執(zhí)行器姿態(tài)角要求Z軸平行于圓柱底面且垂直于圓柱中心線。

（2）長方體。機(jī)械臂抓取兩個對立面的中心點(diǎn)，姿態(tài)角選擇要求Z軸平行于長方體底面且垂直于長方體中心線。

（3）碗狀物體。以碗中心與機(jī)械臂基底連線，來判斷最靠近機(jī)械臂的中部位置，抓取位置選擇碗沿，姿態(tài)角選擇需要判斷碗的傾斜程度，保持Z軸與斜邊平行朝向碗底面；斜邊傾斜角需要根據(jù)物體的CAD模型進(jìn)行測量計算。

以上模型的抓取效果如圖6 所示。

圖6 虛擬場景中目標(biāo)抓取

3.2 抓取任務(wù)規(guī)劃

機(jī)械臂任務(wù)分解主要包括：移動、抓取和放置。本文利用任務(wù)分解的方法，把復(fù)雜任務(wù)分解成子任務(wù)，具體如下：

（1）移動任務(wù)。以機(jī)械臂的運(yùn)動規(guī)劃為基礎(chǔ)，是機(jī)械臂的空間任務(wù)規(guī)劃的基礎(chǔ)任務(wù)。

（2）抓取任務(wù)。分為預(yù)抓取、可抓取、抓取和后退4 個步驟。預(yù)抓取，選擇靠近物體的一個位姿，距離能抓取物體有一段微調(diào)整；可抓取，機(jī)械臂貼近目標(biāo)物體，物體已經(jīng)在機(jī)械爪中間；抓取，機(jī)械爪閉合抓取，更新機(jī)械爪和物體在地圖中的狀態(tài)；后退，機(jī)械臂末端與物體為一個整體，體積增大，調(diào)整地圖的膨脹半徑，向后移動一定距離。

（3）放置任務(wù)。分為機(jī)械爪占用狀態(tài)判斷、可放置、放置和遠(yuǎn)離物體等步驟。機(jī)械爪占用狀態(tài)判斷，避免不合理的任務(wù)指令進(jìn)行無效放置任務(wù)；可放置，機(jī)械爪移動到目標(biāo)位置；放置，張開機(jī)械爪，與物體分離；遠(yuǎn)離物體，機(jī)械臂末端向遠(yuǎn)離物體方向移動，仍朝向目標(biāo)物體，更新地圖數(shù)據(jù)。

4 機(jī)械臂任務(wù)規(guī)劃實驗仿真

虛擬空間中基于語義驅(qū)動的任務(wù)規(guī)劃

針對語義驅(qū)動的機(jī)械臂任務(wù)規(guī)劃方法，本文設(shè)計如下仿真：

（1）機(jī)械臂把小球放入2 號盤子中（執(zhí)行流程：抓取小球并放入2 號盤子）。

（2）機(jī)械臂移動到長方體旁（執(zhí)行流程：從不同起始位置開始，方便于下一步抓取長方體，多次仿真統(tǒng)計平均時間和路程）。

任務(wù)1把小球放在2 號盤子。

語義內(nèi)容：拿起小球放置到2 號盤子。

仿真初始條件：機(jī)械臂起始關(guān)節(jié)位置為隨機(jī)初始位置。

仿真預(yù)期效果：機(jī)械臂末端抓取小球，并放置到2號盤子中。

在虛擬場景下機(jī)械臂執(zhí)行抓取和放置過程如圖7所示。

圖7 任務(wù)1仿真過程

本次任務(wù)仿真路徑長度為215 cm，規(guī)劃所用時間為4.5 s，執(zhí)行時間為36 s。

任務(wù)2機(jī)械臂移動到長方體旁。

語義內(nèi)容：移動到長方體旁。

仿真初始條件：機(jī)械臂起始關(guān)節(jié)位置為隨機(jī)初始位置。

仿真預(yù)期效果：機(jī)械臂多次從不同的初始位置出發(fā)，移動到長方體旁邊。

在虛擬場景下機(jī)械臂移動過程如圖8 所示。

圖8 任務(wù)2仿真過程

本次任務(wù)仿真路徑長度為78 cm，規(guī)劃所用時間為2 s，執(zhí)行時間為10.2 s。

綜上所述，本文實現(xiàn)語義驅(qū)動機(jī)器人空間任務(wù)，規(guī)劃速度快，占用時間較短，運(yùn)行時間由于末端軌跡的限制，但符合實際情況。

5 結(jié)語

本文提出機(jī)器人在未知環(huán)境下三維體素-語義建圖以及機(jī)械臂運(yùn)動規(guī)劃方法，構(gòu)建一種三維體素-語義混合地圖，由點(diǎn)云、語義信息和物體三維模型組成，該地圖支持避障運(yùn)動規(guī)劃，附帶語義信息支持語義驅(qū)動任務(wù)，支持實時更新物體在地圖中的位姿，具有廣泛適用性；利用優(yōu)化A*算法實現(xiàn)機(jī)械臂末端笛卡爾空間軌跡規(guī)劃，達(dá)到末端執(zhí)行器路徑最短，用貝塞爾曲線完成關(guān)節(jié)軌跡平滑，使運(yùn)動過程速度連續(xù)，減小能量損耗，實現(xiàn)完整機(jī)械臂運(yùn)動規(guī)劃，并且以語義驅(qū)動機(jī)械臂任務(wù)規(guī)劃，智能完成多種機(jī)械臂的空間任務(wù)，利用機(jī)械臂抓取位姿來分解復(fù)雜任務(wù)。本文尚未對機(jī)械臂的關(guān)節(jié)軌跡做碰撞檢測，后續(xù)重點(diǎn)研究碰撞檢測和重規(guī)劃。