機械臂環境三維重建與避障算法研究

陳新度，徐 學，高 萌，孔德良

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

1 前言

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

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

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

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

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

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

2 原理與方法

2.1 相機模型與標定

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

圖1 各坐標系轉換模型Fig.1 Transformation Model of Each Coordinate System

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

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

圖2 手眼標定示意圖Fig.2 Schematic Diagram of Hand-eye Calibration

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

建立不同空間轉換回路，如式（2）所示：

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

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

2.2 目標識別與位姿估計

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

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

圖3 ArUco標記圖Fig.3 Marked Diagram of Aruco

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

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

ArUco標記、相機、末端和基座關系，如圖5所示。

圖5 各坐標系變換關系圖Fig.5 Transformation Relation Diagram of Each Coordinate System

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

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

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

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

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

化簡式（10）得：

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

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

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

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

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

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

P(n)—先驗概率。

2.4 RRT＊改進算法

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

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

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

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

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

引入目標引力，引力勢能為：

式中：k—引力系數。可得qgoal對qnear的目標引力為：

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

式中：ρ1—隨機方向步長；ρ2—目標方向步長。

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

式中：qstart—起點位姿；qgoal—目標位姿；q—候選節點；σbest—當前最低路徑成本。

偽代碼，如圖6所示。

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

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

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

圖7 CAN-RRT*運動規劃Fig.7 Motion Planning of CAN-RRT*

3 實驗驗證與分析

3.1 實驗平臺搭建

3.1.1 真實平臺搭建

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

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

視覺系統獲取工件及作業環境的信息，機械臂經規劃，運動到工件位置，末端執行器對工件進行夾持，并將工件放到貨架指定位置處。

3.1.2 仿真及控制搭建

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

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

圖10 機器人仿真及控制系統Fig.10 Robot Simulation and Control System

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

3.2 識別定位分析

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

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

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

重復測量十次，實驗結果表明，X、Y、Z方向基本穩定在8mm以內，姿態角可以穩定在2°以內，驗證了目標識別精度的可行性。

3.3 三維建模分析

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

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

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

圖13 作業空間障礙物及標記點Fig.13 Obstacles and Markers in Working Space

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

表3 三維重建尺寸數據Tab.3 Dimensional Data of 3D Reconstruction

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

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

圖14 角點定位數據偏差圖Fig.14 Data Deviation Diagram of the Corner Location

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

3.4 避障運動規劃分析

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

表5 各規劃算法規劃對比Tab.5 Comparison of Planning Algorithms

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

將CAN-RRT*算法添加到開源運動規劃庫OMPL 中，通過Movelt控制機械臂。分別設置規劃時間上限為0.1s，0.2s，0.5s，初始狀態位置不變，目標狀態為通過ArUco 碼識別的姿態（0.152，0.536，0.437，-0.287，-1.48645，0.0902），障礙空間為三維重建獲得的柵格地圖，每種情況進行1000次重復實驗，統計結果，如表6所示。可看出該算法在時間上限為0.1s時，成功率達到96.5%，表明該算法有效且可靠。

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

3.5 避障運動失敗原因分析

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

表7 失敗原因統計Tab.7 Statistics on the Cause of Failure

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

逆解求解失敗和路徑段碰撞檢測錯誤占總體88%，前者主要是因為在當前目標狀態及障礙物位置下，機械臂本身不存在到達該點的構型；而后者失敗原因在于碰撞檢測分辨率設置略大，導致部分情況下碰撞檢測跳過了障礙物。可考慮設置更小的碰撞檢測分辨率來解決這個問題，但同時會導致規劃時間增長。

4 結束語

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

研究通過實驗驗證了方法的可行性和穩定性，不僅適用于關節臂機器人，而且也適用于其他構型的機器人。