機(jī)械臂環(huán)境三維重建與避障算法研究

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

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

1 前言

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

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

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

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

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

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

2 原理與方法

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

物體的三維空間坐標(biāo)與圖像坐標(biāo)存在線性關(guān)系。世界坐標(biāo)系、相機(jī)坐標(biāo)系以及圖像坐標(biāo)系之間的關(guān)系，如圖1所示。而坐標(biāo)點(diǎn)間的轉(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—相機(jī)內(nèi)參；T—相機(jī)外參。這里采用ArUco 自帶的標(biāo)定算法［11］對(duì)相機(jī)進(jìn)行標(biāo)定，確定攝像頭的內(nèi)參矩陣K以及畸變參數(shù)D，標(biāo)定結(jié)果為：

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

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

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

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

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

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

2.2 目標(biāo)識(shí)別與位姿估計(jì)

位姿估計(jì)即找到現(xiàn)實(shí)世界和圖像投影之間的對(duì)應(yīng)點(diǎn)，求得相機(jī)與目標(biāo)之間的相對(duì)位姿。本研究選用基于二進(jìn)制標(biāo)記的方法，優(yōu)點(diǎn)在于單個(gè)標(biāo)記即可提供足夠多的對(duì)應(yīng)點(diǎn)，且具有錯(cuò)誤檢測(cè)與校正機(jī)制［13］。選用的ArUco標(biāo)記由一個(gè)四邊全黑的邊框以及內(nèi)部的二進(jìn)制矩陣組成，黑色代表0，白色代表1，每一個(gè)標(biāo)記都有一個(gè)獨(dú)有的ID，標(biāo)記大小為（5×5），標(biāo)記尺寸為(40×40)mm，ID為1，標(biāo)記的中心點(diǎn)為原點(diǎn)，如圖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)記的識(shí)別步驟，如圖4所示。將獲取的RGB圖像經(jīng)過(guò)灰度化、自適應(yīng)閾值分割、輪廓濾波、透視變換得到正視圖；并利用Otsu 分離出白黑色位，分析標(biāo)記ID；將角點(diǎn)亞像素級(jí)細(xì)化后，根據(jù)小孔成像模型，通過(guò)ArUco標(biāo)記的4個(gè)角點(diǎn)像素坐標(biāo)，使用solvePnP函數(shù)［14］即可求得標(biāo)記與相機(jī)之間的變換矩陣。

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

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

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

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

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

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

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

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

化簡(jiǎn)式（10）得：

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

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

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

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

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

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

P(n)—先驗(yàn)概率。

2.4 RRT＊改進(jìn)算法

RRT-Connect算法通過(guò)雙向搜索和貪婪策略大大降低了計(jì)算量，提高了搜索速度，但由于隨機(jī)采樣，路徑較長(zhǎng)，無(wú)法達(dá)到漸進(jìn)最優(yōu)。

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

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

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

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

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

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

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

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

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

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

偽代碼，如圖6所示。

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

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

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

圖7 CAN-RRT*運(yùn)動(dòng)規(guī)劃Fig.7 Motion Planning of CAN-RRT*

3 實(shí)驗(yàn)驗(yàn)證與分析

3.1 實(shí)驗(yàn)平臺(tái)搭建

3.1.1 真實(shí)平臺(tái)搭建

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

圖8 UR5機(jī)器人平臺(tái)Fig.8 UR5 Robot Platform

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

3.1.2 仿真及控制搭建

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

圖9 機(jī)器人仿真與控制平臺(tái)Fig.9 Robot Simulation and Control Platform

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

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

3.2 識(shí)別定位分析

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

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

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

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

3.3 三維建模分析

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

圖12 全局點(diǎn)云效果圖Fig.12 Rendering of Global Point Cloud

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

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

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

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

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

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

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

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

3.4 避障運(yùn)動(dòng)規(guī)劃分析

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

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

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

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

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

3.5 避障運(yùn)動(dòng)失敗原因分析

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

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

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

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

4 結(jié)束語(yǔ)

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

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