陳新度，徐 學(xué)，高 萌，孔德良

（1.廣東工業(yè)大學(xué)機電工程學(xué)院，廣東 廣州 510006；2.佛山智能裝備技術(shù)研究院，廣東 佛山 528234 3.廣東工業(yè)大學(xué)省部共建精密電子制造技術(shù)與裝備國家重點實驗室，廣東 廣州 510006）

1 前言

工業(yè)機械臂作為仿人手的自動裝置，具有通用性強、運動靈活等優(yōu)點［1］。隨著機器人應(yīng)用場景的復(fù)雜、非結(jié)構(gòu)化，對其提出了實時感知和避障的要求。常規(guī)可預(yù)先建立障礙物空間模型或通過示教方法，以形成無碰撞軌跡。但此類方法只能針對靜態(tài)環(huán)境，對動態(tài)環(huán)境不具有泛化性。因此，在未知環(huán)境中的三維重建和避障規(guī)劃成為亟需解決的實際問題。

在目標(biāo)識別方面，主要分為基于模板匹配、三維局部特征、學(xué)習(xí)的方法。文獻［2］利用快速形狀匹配算法將三維CAD模型在多視角的位姿映射作為模板，與位姿完成匹配。文獻［3］通過提取二維圖像和點云圖像的多模態(tài)信息，實現(xiàn)物體的識別定位。文獻［4］以各視點下的RGB-D圖像融合CNN網(wǎng)絡(luò)，完成待測物體的目標(biāo)識別。

在三維重建方面，主要以基于傳統(tǒng)多視角幾何、深度學(xué)習(xí)算法為主。如文獻［5］提出的Kinect Fusion，開啟了RGB實時三維重建的序幕，此后有Elastic Fusion［6］、Bundle Fusion［7］等。文獻［8］提出Pixel2Mesh，用端到端的神經(jīng)網(wǎng)絡(luò)實現(xiàn)了從單張彩色圖直接生成用網(wǎng)格表示的物體三維信息。

在機器人避障規(guī)劃方面，文獻［9］將障礙物映射到構(gòu)形空間，利用A?算法搜索避障路徑，但將空間障礙物投影到二維平面中，無法充分發(fā)揮高自由度工業(yè)機械臂在避障方面的優(yōu)勢。文獻［10］提出RRT-connect，通過雙樹搜索大幅提升規(guī)劃速度，但由于隨機采樣，不具備漸進最優(yōu)。

綜上分析可看出，雖然識別定位、三維重建和避障規(guī)劃方面研究成果較豐富，但三者的結(jié)合，即目標(biāo)物識別后對于未知環(huán)境空間的重建及效果、避障算法驅(qū)動機械臂避開重建的障礙的效果等研究尚存在不足。據(jù)此，本研究提出一種基于視覺的三維重建與避障規(guī)劃方法。

通過搭建基于機器人操作系統(tǒng)（ROS）的實驗平臺，使用深度相機獲取ArUco標(biāo)記上的特征點坐標(biāo)，求解目標(biāo)物的姿態(tài)；通過改進重建方法，融合機械臂末端位姿和點云數(shù)據(jù)，對未知環(huán)境空間進行三維重建，作為后續(xù)規(guī)劃的障礙空間；應(yīng)用RRT的改進算法進行機械臂的避障運動規(guī)劃，并進行了真機驗證。

2 原理與方法

2.1 相機模型與標(biāo)定

物體的三維空間坐標(biāo)與圖像坐標(biāo)存在線性關(guān)系。世界坐標(biāo)系、相機坐標(biāo)系以及圖像坐標(biāo)系之間的關(guān)系，如圖1所示。而坐標(biāo)點間的轉(zhuǎn)換關(guān)系，如式（1）所示。

圖1 各坐標(biāo)系轉(zhuǎn)換模型Fig.1 Transformation Model of Each Coordinate System

式中：Puv—標(biāo)記在像素坐標(biāo)系下的像素坐標(biāo)；Pw—標(biāo)記在世界坐標(biāo)系中的坐標(biāo)；K—相機內(nèi)參；T—相機外參。這里采用ArUco 自帶的標(biāo)定算法［11］對相機進行標(biāo)定，確定攝像頭的內(nèi)參矩陣K以及畸變參數(shù)D，標(biāo)定結(jié)果為：

研究構(gòu)建了眼在手上的手眼系統(tǒng)，如圖2所示。A為末端與機械臂基坐標(biāo)系的變換關(guān)系，可通過機械臂正解獲得；C為相機與標(biāo)定板的變換，即已求的相機外參；B為相機與機械臂末端的變換關(guān)系，該關(guān)系是最終要求解的手眼矩陣。

圖2 手眼標(biāo)定示意圖Fig.2 Schematic Diagram of Hand-eye Calibration

為求解B，需要多次變換機械臂姿態(tài)拍攝標(biāo)定板，獲得不同角度下的標(biāo)定圖像，從而推導(dǎo)出相機相對于末端的變換矩陣。

建立不同空間轉(zhuǎn)換回路，如式（2）所示：

式（4）、式（5）代入式（3）得：

式（6）就轉(zhuǎn)換成了經(jīng)典方程AX=XB的求解問題［12］。通過兩步法求解手眼矩陣為：

2.2 目標(biāo)識別與位姿估計

位姿估計即找到現(xiàn)實世界和圖像投影之間的對應(yīng)點，求得相機與目標(biāo)之間的相對位姿。本研究選用基于二進制標(biāo)記的方法，優(yōu)點在于單個標(biāo)記即可提供足夠多的對應(yīng)點，且具有錯誤檢測與校正機制［13］。選用的ArUco標(biāo)記由一個四邊全黑的邊框以及內(nèi)部的二進制矩陣組成，黑色代表0，白色代表1，每一個標(biāo)記都有一個獨有的ID，標(biāo)記大小為（5×5），標(biāo)記尺寸為(40×40)mm，ID為1，標(biāo)記的中心點為原點，如圖3所示。

表1 集成的插件及參數(shù)設(shè)置Tab.1 Integrated Plug-in and Parameter Settings

圖3 ArUco標(biāo)記圖Fig.3 Marked Diagram of Aruco

ArUco標(biāo)記的識別步驟，如圖4所示。將獲取的RGB圖像經(jīng)過灰度化、自適應(yīng)閾值分割、輪廓濾波、透視變換得到正視圖；并利用Otsu 分離出白黑色位，分析標(biāo)記ID；將角點亞像素級細(xì)化后，根據(jù)小孔成像模型，通過ArUco標(biāo)記的4個角點像素坐標(biāo)，使用solvePnP函數(shù)［14］即可求得標(biāo)記與相機之間的變換矩陣。

圖4 ArUco位姿估計流程圖Fig.4 Flow Chart of Aruco Pose Estimation

ArUco標(biāo)記、相機、末端和基座關(guān)系，如圖5所示。

圖5 各坐標(biāo)系變換關(guān)系圖Fig.5 Transformation Relation Diagram of Each Coordinate System

轉(zhuǎn)換關(guān)系，如式（7）所示。通過求得的標(biāo)記與相機之間的變換矩陣、手眼矩陣和末端位姿，可得標(biāo)記與機械臂基坐標(biāo)系之間的變換矩陣，即目標(biāo)物空間三維姿態(tài)。

2.3 融合末端位姿的三維重建

ICP算法是用來解決點云之間的配準(zhǔn)問題，計算出最優(yōu)匹配變換矩陣，使得誤差最小［15］。但在兩點云相差較大的情況下，ICP容易陷入局部最優(yōu)解，所以需要在精配準(zhǔn)之前進行粗配準(zhǔn)［16］，并以粗配準(zhǔn)得到的轉(zhuǎn)換矩陣作為其初始轉(zhuǎn)換矩陣。由于采用的是手眼系統(tǒng)，而末端相對于機械臂基座的位姿關(guān)系容易獲得，可通過在配準(zhǔn)之前，獲取機器人末端位姿，將對應(yīng)時刻的點云數(shù)據(jù)統(tǒng)一到同一坐標(biāo)系下，再進行后續(xù)的配準(zhǔn)融合工作。

假定相機在位置1、2 處分別獲取點云，且獲得對應(yīng)處的位姿，有：

由式（8）、式（9）可得：

化簡式（10）得：

式中：n∈N+，n≥2。

得到各點云剛體變換矩陣后，再將其融合FPFH_ICP配準(zhǔn)算法，得到基于基坐標(biāo)系的全局點云模型。

利用八叉樹數(shù)據(jù)結(jié)構(gòu)描述作業(yè)空間（全局點云），將各象限定義為八叉樹的葉子節(jié)點，并不斷進行迭代，直到滿足終止條件［17］。假設(shè)t=1，…，T時刻，觀測的數(shù)據(jù)為z1，…，zT，則第n個葉子節(jié)點的狀態(tài)信息為：

式中：P(n|z1：T)—空間節(jié)點n在1到t時刻被占有的概率；

P(n|z1：T-1)—空間節(jié)點n在1到t-1時刻被占有的概率；

P(n|zT)—空間節(jié)點n在t時刻被占有的概率；

P(n)—先驗概率。

2.4 RRT＊改進算法

RRT-Connect算法通過雙向搜索和貪婪策略大大降低了計算量，提高了搜索速度，但由于隨機采樣，路徑較長，無法達到漸進最優(yōu)。

RRT*通過引入路徑代價的概念，不斷迭代找到漸進最優(yōu)路徑，但降低了計算效率。

針對上述缺陷，提出了改進算法CAN-RRT*（RRT* with Caucy Sample、Attractive Force and NodeReject），通過引入柯西采樣函數(shù)取代隨機采樣，使采樣點更大概率落在障礙物附近，降低采樣的盲目性；通過引入目標(biāo)引力和動態(tài)步長法，提高局部搜索速度；利用節(jié)點拒絕策略來剔除不必要的采樣節(jié)點，使算法能夠以高效率生成最終路徑。

柯西分布概率密度函數(shù)為f(x，x0，g)，其中，x0—峰值的位置參數(shù)；γ—最大值一半處的一半寬度的尺度參數(shù)。

令l=|x-x0|，表示采樣點與障礙物的距離，l越小，函數(shù)值越大，l越大，函數(shù)值越小。

引入目標(biāo)引力，引力勢能為：

式中：k—引力系數(shù)。可得qgoal對qnear的目標(biāo)引力為：

擴展后新節(jié)點位置，如式（18）所示。

式中：ρ1—隨機方向步長；ρ2—目標(biāo)方向步長。

在采樣過程選擇一個節(jié)點之后，在節(jié)點擴展前使用節(jié)點剔除策略，如式（19）所示，即如果當(dāng)前的最低成本低于直接從初始節(jié)點到該節(jié)點以及從該節(jié)點到目標(biāo)節(jié)點的總成本，則剔除該節(jié)點。

式中：qstart—起點位姿；qgoal—目標(biāo)位姿；q—候選節(jié)點；σbest—當(dāng)前最低路徑成本。

偽代碼，如圖6所示。

圖6 CAN-RRT*偽代碼Fig.6 Pseudocode of CAN-RRT*

其中，（1）CaucySample（）：柯西采樣函數(shù)，在C-space得到隨機節(jié)點xrand；（2）Neares（t）：最近鄰點函數(shù)，得到最接近qrand的節(jié)點qnearest；（3）AttractiveForce（（）：目標(biāo)引力與動態(tài)步長函數(shù)，產(chǎn)生新節(jié)點qnew；（4）NodeRejec（t（）：節(jié)點拒絕函數(shù)；（5）GetSortedLis（t（）：集合排序函數(shù)，返回按成本函數(shù)升序排列的列表；（6）ChooseBest-Paren（t（）：最優(yōu)父節(jié)點函數(shù)；（7）RewireVertices（（）：重布線函數(shù)，找到隨機樹Tb中距離xnew最近的節(jié)點xconn；（8）connec（t）：返回連接xstart和xgoal的無碰撞路徑；（9）SwapTrees（）：交換雙樹。

在三維、隨機障礙的環(huán)境下，對CAN-RRT*算法進行避障規(guī)劃，如圖7所示。

圖7 CAN-RRT*運動規(guī)劃Fig.7 Motion Planning of CAN-RRT*

3 實驗驗證與分析

3.1 實驗平臺搭建

3.1.1 真實平臺搭建

所搭建的實驗平臺，如圖8所示。該平臺包含機器人、深度相機和末端執(zhí)行器。采用的機器人由Universal Robots 制造，型號為UR5，負(fù)載5kg，工作范圍為850mm，理論重復(fù)定位精度為±0.03mm；深度相機為INTEL 公司的RealSense D435i，其具有彩色、紅外及IMU模塊，測量距離為（0.105～10）m；末端執(zhí)行器為因時機器人的EG2-4B2電動夾爪，重復(fù)定位精度±0.5mm，最大速度97mm/s。

圖8 UR5機器人平臺Fig.8 UR5 Robot Platform

視覺系統(tǒng)獲取工件及作業(yè)環(huán)境的信息，機械臂經(jīng)規(guī)劃，運動到工件位置，末端執(zhí)行器對工件進行夾持，并將工件放到貨架指定位置處。

3.1.2 仿真及控制搭建

基于ROS開發(fā)了仿真及控制平臺，集成碰撞檢測、正逆運動學(xué)求解、軌跡平滑等功能，部分插件選擇及參數(shù)設(shè)置，如表1 所示。RVIZ界面的部署，如圖9所示。系統(tǒng)的避障規(guī)劃流程，如圖10所示。

圖9 機器人仿真與控制平臺Fig.9 Robot Simulation and Control Platform

圖10 機器人仿真及控制系統(tǒng)Fig.10 Robot Simulation and Control System

視覺定位系統(tǒng)獲取工件姿態(tài)，三維重建系統(tǒng)生成障礙模型，信息匯總到規(guī)劃場景中，提供給CAN-RRT*規(guī)劃器生成無碰撞路徑，并通過Movelt驅(qū)動機械臂，運動到目標(biāo)位置成夾持任務(wù)。

3.2 識別定位分析

為了驗證視覺定位精度，開展了精度評估實驗，如圖13 所示。使用4點法將機器人末端TCP移到頂針末端，使用Halcon手眼標(biāo)定獲得相機與機器人坐標(biāo)系之間的變換矩陣，ArUco標(biāo)記張貼在平面上。將頂針末端移動至與ArUco原點重合，從RVIZ中監(jiān)聽該點在機器人基座坐標(biāo)系下的位姿，作為真實值，即1.2節(jié)式（7）中的，通過ArUco 的姿態(tài)估計矩陣、機器人手眼標(biāo)定矩陣和機械臂末端位姿矩陣，得到ArUco標(biāo)記原點的計算值。通過比較兩者之間的偏差可以評估視覺定位精度。使用ID字典為5的ArUco標(biāo)記，相機距離平面350mm，垂直向下拍攝，統(tǒng)計實驗數(shù)據(jù)，如表2、圖11所示。

表2 同一位置目標(biāo)點識別位姿Tab.2 Recognized Pose of the Same Target Point

圖11 同一位置定位數(shù)據(jù)偏差圖Fig.11 Location Data Deviation Diagram of the Same Location

重復(fù)測量十次，實驗結(jié)果表明，X、Y、Z方向基本穩(wěn)定在8mm以內(nèi)，姿態(tài)角可以穩(wěn)定在2°以內(nèi)，驗證了目標(biāo)識別精度的可行性。

3.3 三維建模分析

對機械臂作業(yè)的環(huán)境進行重建，重建效果，如圖12所示。可以看出，融合末端位姿的重建方法能夠有效的將環(huán)境中的障礙物重建到規(guī)劃環(huán)境中，但某些地方還存在空洞現(xiàn)象。

圖12 全局點云效果圖Fig.12 Rendering of Global Point Cloud

這里采用定位精度和尺寸精度來衡量重建的效果。如圖13中的長方體物體，實際尺寸為（277mm，200mm，126mm），進行多次重建，取其中8次結(jié)果對比。

圖13 作業(yè)空間障礙物及標(biāo)記點Fig.13 Obstacles and Markers in Working Space

分析表3，可知在物體長寬高方向上誤差均值分別為1.37mm、1.78mm、1.34mm，誤差控制在5mm以內(nèi)，基本上達到重建的要求，表3中單位為mm。

表3 三維重建尺寸數(shù)據(jù)Tab.3 Dimensional Data of 3D Reconstruction

為了驗證三維建模的定位精度，在障礙物表面貼上黑白標(biāo)定板，如圖13所示。選取10個不同位置處的角點，采用類似上文的方案，通過尖端觸點得到各角點真實位姿，將模型轉(zhuǎn)換到機械臂基坐標(biāo)系下，比較對應(yīng)角點的偏差來評估定位精度。實驗結(jié)果，如表4、圖14所示。

表4 各角點真實值與掃描值數(shù)據(jù)Tab.4 Real Value and Scanning Value of Each Corner

圖14 角點定位數(shù)據(jù)偏差圖Fig.14 Data Deviation Diagram of the Corner Location

實驗結(jié)果表明，角點在X、Y方向誤差穩(wěn)定在8mm以內(nèi)，深度方向誤差保持在5mm左右，姿態(tài)角誤差穩(wěn)定在3°以內(nèi)，驗證了三維建模的定位精度。

3.4 避障運動規(guī)劃分析

將1.3節(jié)中的尺度參數(shù)γ設(shè)為1，最小距離ε設(shè)為0.5，步長ρ1設(shè)為1.6°，步長ρ2設(shè)為1.8°，引力系數(shù)設(shè)為1.1，搜索半徑設(shè)為3，最大迭代次數(shù)設(shè)為1000，在三維空間、確定障礙物的環(huán)境場景下進行1000次重復(fù)實驗，相關(guān)數(shù)據(jù)經(jīng)均值處理，統(tǒng)計，如表5所示。

表5 各規(guī)劃算法規(guī)劃對比Tab.5 Comparison of Planning Algorithms

由表5 可看出，相比RRT，改進后的算法路徑長度縮短了22.2%，搜索時間降低了55.9%，采樣節(jié)點數(shù)下降了54.3%，有效性得到驗證。

將CAN-RRT*算法添加到開源運動規(guī)劃庫OMPL 中，通過Movelt控制機械臂。分別設(shè)置規(guī)劃時間上限為0.1s，0.2s，0.5s，初始狀態(tài)位置不變，目標(biāo)狀態(tài)為通過ArUco 碼識別的姿態(tài)（0.152，0.536，0.437，-0.287，-1.48645，0.0902），障礙空間為三維重建獲得的柵格地圖，每種情況進行1000次重復(fù)實驗，統(tǒng)計結(jié)果，如表6所示?？煽闯鲈撍惴ㄔ跁r間上限為0.1s時，成功率達到96.5%，表明該算法有效且可靠。

表6 CAN-RRT＊規(guī)劃成功率Tab.6 Planning Success Rate of CAN-RRT＊

3.5 避障運動失敗原因分析

為了給后期的研究工作提供改進思路，對規(guī)劃失敗原因進行分析統(tǒng)計，如表7所示。主要由以下原因造成：（1）在當(dāng)前目標(biāo)狀態(tài)及障礙物的情況下，逆解求解錯誤；（2）規(guī)劃超時；（3）碰撞檢測失??；（4）識別誤差及機械臂運動誤差。

表7 失敗原因統(tǒng)計Tab.7 Statistics on the Cause of Failure

由表7可知，識別誤差與運動誤差占總體5.7%，說明對于目標(biāo)物的姿態(tài)定位比較準(zhǔn)確，而規(guī)劃超時僅有1 次，說明CANRRT*算法在規(guī)劃速度方面滿足需求。

逆解求解失敗和路徑段碰撞檢測錯誤占總體88%，前者主要是因為在當(dāng)前目標(biāo)狀態(tài)及障礙物位置下，機械臂本身不存在到達該點的構(gòu)型；而后者失敗原因在于碰撞檢測分辨率設(shè)置略大，導(dǎo)致部分情況下碰撞檢測跳過了障礙物?？煽紤]設(shè)置更小的碰撞檢測分辨率來解決這個問題，但同時會導(dǎo)致規(guī)劃時間增長。

4 結(jié)束語

鑒于當(dāng)前工業(yè)機器人在復(fù)雜、未知環(huán)境中的避障規(guī)劃方面的不足，本研究提出了基于視覺的三維重建與避障規(guī)劃的方法和流程。首先使用ArUco碼對工件識別定位，然后利用機械臂末端位姿和FPFH_ICP算法對環(huán)境點云進行融合，重建了復(fù)雜環(huán)境下的工作空間，最后應(yīng)用改進的CAN-RRT*算法，實現(xiàn)了未知環(huán)境下的避障運動。

研究通過實驗驗證了方法的可行性和穩(wěn)定性，不僅適用于關(guān)節(jié)臂機器人，而且也適用于其他構(gòu)型的機器人。