簡(jiǎn)介計(jì)算機(jī)視覺與機(jī)器人控制的交叉應(yīng)用正在重塑智能分揀領(lǐng)域。深度學(xué)習(xí)目標(biāo)檢測(cè)模型能夠?qū)ξ矬w進(jìn)行實(shí)時(shí)定位與分類而運(yùn)動(dòng)規(guī)劃框架則為機(jī)械臂提供安全可靠的軌跡生成能力。YOLOv5作為輕量級(jí)檢測(cè)模型在推理速度與識(shí)別精度之間取得了良好平衡MoveIt作為ROS生態(tài)下的機(jī)器人運(yùn)動(dòng)規(guī)劃框架可高效完成運(yùn)動(dòng)學(xué)求解、碰撞檢測(cè)與路徑規(guī)劃。將二者結(jié)合可構(gòu)建“感知-決策-執(zhí)行”的完整閉環(huán)在垃圾自動(dòng)分類、工業(yè)抓取、服務(wù)機(jī)器人等場(chǎng)景中具有廣泛的應(yīng)用價(jià)值。以一套基于YOLOv5和MoveIt的桌面垃圾分類機(jī)械臂項(xiàng)目為例系統(tǒng)介紹了從數(shù)據(jù)集構(gòu)建、模型訓(xùn)練、手眼標(biāo)定、URDF建模到狀態(tài)機(jī)集成的完整技術(shù)鏈路并針對(duì)實(shí)機(jī)調(diào)試中的識(shí)別混淆、坐標(biāo)轉(zhuǎn)換誤差、規(guī)劃失敗等典型問題給出了具體解決方案為從事視覺機(jī)械臂開發(fā)的工程師和學(xué)生提供了一份可落地的工程實(shí)踐參考。 做這個(gè)項(xiàng)目的起因特別樸素實(shí)驗(yàn)室樓下的垃圾分類督導(dǎo)員每天要拆開幾百個(gè)垃圾袋把塑料瓶、易拉罐、紙張、剩飯分門別類工作量巨大。當(dāng)時(shí)正好在啃YOLOv5和MoveIt就想著能不能做一個(gè)長(zhǎng)了眼睛的機(jī)械臂自動(dòng)識(shí)別垃圾類別然后抓起來扔進(jìn)對(duì)應(yīng)垃圾桶。斷斷續(xù)續(xù)折騰了三個(gè)多月整套系統(tǒng)跑通之后我把源碼、設(shè)計(jì)文檔、訓(xùn)練好的權(quán)重和調(diào)試記錄整理成了一個(gè)壓縮包也就是你在標(biāo)題里看到的那個(gè)分類機(jī)器人源碼設(shè)計(jì)文檔.zip。這篇博文就把整個(gè)項(xiàng)目從選型到落地、從原理到踩坑的過程梳理一遍給打算做類似視覺機(jī)械臂項(xiàng)目的朋友一個(gè)比較完整的參考。這個(gè)項(xiàng)目適合兩類人看一類是正在做機(jī)器人類畢業(yè)設(shè)計(jì)或競(jìng)賽項(xiàng)目的學(xué)生YOLOv5MoveIt的組合覆蓋面廣、工程量適中、演示效果好另一類是剛接觸ROS和機(jī)械臂控制、想找一個(gè)完整案例把視覺和運(yùn)動(dòng)控制串起來的開發(fā)者。我會(huì)先講為什么選這套方案再拆解視覺模塊和機(jī)械臂模塊各自怎么實(shí)現(xiàn)然后是聯(lián)動(dòng)和調(diào)試最后說說源碼和文檔怎么用以及后續(xù)可以怎么擴(kuò)展。1. 為什么選YOLOv5MoveIt選型背后的真實(shí)考量很多人一上來就問為什么用YOLOv5不用YOLOv8為什么用MoveIt不用自己寫逆解這類問題其實(shí)沒有標(biāo)準(zhǔn)答案關(guān)鍵看項(xiàng)目約束條件。我這個(gè)項(xiàng)目的約束非常明確ROS Melodic環(huán)境下開發(fā)機(jī)械臂是自組裝的6自由度桌面臂算力平臺(tái)是帶GTX 1660Ti的筆記本目標(biāo)物體是塑料瓶、易拉罐、廢紙團(tuán)、果皮等常見垃圾識(shí)別類別一共四類。在這個(gè)前提下YOLOv5和MoveIt幾乎是當(dāng)時(shí)最穩(wěn)的組合。1.1 視覺方案對(duì)比YOLOv5比傳統(tǒng)圖像處理和更重模型更合適傳統(tǒng)圖像處理方案我也試過比如用顏色閾值提取塑料瓶的藍(lán)色、用形狀匹配識(shí)別易拉罐的圓柱輪廓。效果在單一光照、單一背景的實(shí)驗(yàn)室里還可以但只要把垃圾桶放到窗邊陽(yáng)光一照顏色全飄誤檢率直接崩到?jīng)]法看。深度學(xué)習(xí)檢測(cè)模型對(duì)光照、角度、遮擋的魯棒性明顯更好而YOLOv5在速度和精度的平衡上剛剛好。為什么不選更重的模型當(dāng)時(shí)也考慮過Faster R-CNN和YOLOv5x。Faster R-CNN在COCO上的mAP確實(shí)高一些但在筆記本CPU上跑一次推理要兩三百毫秒加上機(jī)械臂運(yùn)動(dòng)時(shí)間整個(gè)流程會(huì)拖得很長(zhǎng)YOLOv5x同理權(quán)重文件150多MB前向推理速度快不到哪去。垃圾識(shí)別場(chǎng)景的物體類別不算精細(xì)不需要檢測(cè)出農(nóng)夫山泉還是怡寶只要能區(qū)分塑料瓶易拉罐這種大類YOLOv5s或者YOLOv5m的精度完全夠用而且推理時(shí)間能壓到20ms以內(nèi)給后面的機(jī)械臂規(guī)劃留出了充足的系統(tǒng)余量。還有一個(gè)很實(shí)際的原因YOLOv5的生態(tài)太成熟了。從數(shù)據(jù)標(biāo)注到訓(xùn)練、導(dǎo)出onnx、推理腳本網(wǎng)上有海量現(xiàn)成資料遇到問題搜一下就有解決方案。對(duì)于一個(gè)需要控制整體進(jìn)度的項(xiàng)目來說選一個(gè)社區(qū)活躍度高的模型框架比單純追求指標(biāo)上的一點(diǎn)提升重要得多。1.2 機(jī)械臂控制框架MoveIt在ROS生態(tài)中的地位與優(yōu)勢(shì)機(jī)械臂控制的方案選擇上我當(dāng)時(shí)糾結(jié)過三條路一是用廠商自帶的SDK直接控制二是用ROS Industrial的插件三是用MoveIt。自組臂用的是舵機(jī)步進(jìn)電機(jī)沒有廠商SDK所以第一條路直接pass。ROS Industrial的驅(qū)動(dòng)鏈雖然成熟但主要面向工業(yè)機(jī)器人配置起來相當(dāng)繁瑣而且對(duì)自組臂不友好。剩下MoveIt它其實(shí)不是一個(gè)控制庫(kù)而是一個(gè)運(yùn)動(dòng)規(guī)劃框架把運(yùn)動(dòng)學(xué)求解、碰撞檢測(cè)、路徑規(guī)劃、軌跡執(zhí)行這些模塊整合到了一起還提供了圖形化配置工具。也就是說我只需要提供機(jī)械臂的URDF模型MoveIt就能自動(dòng)生成運(yùn)動(dòng)學(xué)求解器、規(guī)劃場(chǎng)景、執(zhí)行器接口然后用幾行代碼就能讓機(jī)械臂從A點(diǎn)運(yùn)動(dòng)到B點(diǎn)。MoveIt還內(nèi)置了多種規(guī)劃器比如OMPL里的RRT、RRTConnect、BKPIECE等。我實(shí)際用得最多的是RRTConnect原因后面細(xì)講。對(duì)ZERO基礎(chǔ)的人來說可能一下子理解不了MoveIt是框架這句話可以類比成Photoshop你不用自己寫圖像處理算法只需要通過它提供的界面和接口把素材拖進(jìn)去選擇合適的工具就能處理圖像。MoveIt就是機(jī)械臂領(lǐng)域的Photoshop你負(fù)責(zé)提供模型和期望的目標(biāo)位姿它負(fù)責(zé)算出一條無碰撞的軌跡。1.3 整體系統(tǒng)框架與數(shù)據(jù)流整個(gè)系統(tǒng)的物理組成其實(shí)很簡(jiǎn)單一臺(tái)帶GPU的電腦一個(gè)RGB相機(jī)一臺(tái)6自由度桌面機(jī)械臂幾個(gè)對(duì)應(yīng)類別的垃圾桶。軟件層面分成三個(gè)模塊視覺識(shí)別節(jié)點(diǎn)YOLOv5推理、決策調(diào)度節(jié)點(diǎn)狀態(tài)機(jī)、機(jī)械臂控制節(jié)點(diǎn)MoveIt控制器。數(shù)據(jù)流是這樣的相機(jī)實(shí)時(shí)采集桌面畫面YOLOv5推理出畫面中垃圾的類別和像素坐標(biāo)決策節(jié)點(diǎn)拿到檢測(cè)結(jié)果后根據(jù)預(yù)設(shè)的類別-垃圾桶映射表計(jì)算出機(jī)械臂的目標(biāo)抓取位姿和投放位姿MoveIt規(guī)劃出從當(dāng)前位姿到目標(biāo)位姿的無碰撞軌跡發(fā)布給底層控制器執(zhí)行機(jī)械臂抓取垃圾后移動(dòng)到對(duì)應(yīng)垃圾桶上方松開夾爪完成一次分類投放。這套流程聽起來直接但實(shí)際調(diào)試時(shí)每一步都有坑尤其是坐標(biāo)轉(zhuǎn)換和狀態(tài)同步這兩個(gè)環(huán)節(jié)后面我會(huì)單獨(dú)拿出兩個(gè)章節(jié)重點(diǎn)講。2. 視覺識(shí)別模塊從數(shù)據(jù)集構(gòu)建到Y(jié)OLOv5模型部署的完整鏈路視覺模塊是整個(gè)系統(tǒng)里工程量最大、也最容易出問題的地方。網(wǎng)上很多人分享YOLO訓(xùn)練教程大多是拿公開數(shù)據(jù)集跑一遍demo但到了自己采集數(shù)據(jù)、標(biāo)注、訓(xùn)練、部署就會(huì)發(fā)現(xiàn)各種意想不到的問題。我把這一整條鏈路拆成三個(gè)關(guān)鍵階段每個(gè)階段里面都標(biāo)注了我認(rèn)為最重要的細(xì)節(jié)。2.1 垃圾數(shù)據(jù)集的采集與標(biāo)注細(xì)節(jié)垃圾數(shù)據(jù)集的第一個(gè)問題就是樣本不均衡。網(wǎng)上的公開垃圾數(shù)據(jù)集比如TrashNet塑料瓶和易拉罐的圖片非常多但果皮、廢紙團(tuán)的樣本相對(duì)少。如果直接用這個(gè)數(shù)據(jù)集訓(xùn)練模型對(duì)果皮的識(shí)別效果會(huì)很差。我的做法是自己采集了一部分真實(shí)場(chǎng)景數(shù)據(jù)把桌面背景、光照條件、垃圾形態(tài)都做了一次擴(kuò)充。采集時(shí)我踩了一個(gè)很大的坑只采集了干凈、完整的物體忽略了真實(shí)垃圾往往有變形、壓扁、貼標(biāo)簽、沾污漬等情況。比如一個(gè)壓扁的易拉罐角度稍微偏一點(diǎn)模型就認(rèn)為是塑料瓶。后來我在采集中特意加入了殘缺、遮擋、堆疊、不同光照時(shí)段的樣本識(shí)別率才真正提上來。如果你也想做類似項(xiàng)目建議一開始就收集臟亂差樣本而不是商品展示圖樣本。標(biāo)注環(huán)節(jié)用LabelImg格式選的YOLO的txt格式也就是每個(gè)目標(biāo)一行包括類別id和歸一化的中心點(diǎn)坐標(biāo)、寬高。這里有個(gè)細(xì)節(jié)標(biāo)注框不要緊貼物體邊界。我一開始標(biāo)注時(shí)為了框得準(zhǔn)貼得很緊訓(xùn)練出來的模型預(yù)測(cè)框在推理時(shí)經(jīng)常比實(shí)際物體小一圈導(dǎo)致機(jī)械臂抓取時(shí)定位偏上。后來我重新把標(biāo)注框擴(kuò)大到物體邊界外2%到5%左右問題才解決。原因是YOLO的預(yù)測(cè)框回歸在邊界處不穩(wěn)定稍微留一點(diǎn)余量可以讓框的中心點(diǎn)更準(zhǔn)機(jī)械臂抓取中心位置也更可靠。2.2 模型訓(xùn)練關(guān)鍵參數(shù)與調(diào)優(yōu)筆記我選的預(yù)訓(xùn)練權(quán)重是YOLOv5s訓(xùn)練了大概200輪batch size選16輸入分辨率640x640。官方默認(rèn)的hyp.scratch.yaml里很多超參數(shù)直接可用但有兩個(gè)參數(shù)我單獨(dú)做了調(diào)整一是mosaic增強(qiáng)的概率保持默認(rèn)0.7這個(gè)對(duì)垃圾這類小目標(biāo)很有用二是fliplr水平翻轉(zhuǎn)我調(diào)到了0.2因?yàn)闄C(jī)械臂抓取時(shí)物體從相機(jī)視角看主要是正立和略帶旋轉(zhuǎn)的狀態(tài)太強(qiáng)的水平翻轉(zhuǎn)會(huì)引入不自然的視角。還有一個(gè)容易被忽略的參數(shù)image_weights。這個(gè)參數(shù)如果開啟采樣時(shí)會(huì)優(yōu)先選那些損失較大的圖片能緩解樣本不均衡。我在訓(xùn)練后期開了它val集的mAP提升了大概2個(gè)百分點(diǎn)。不要小看這兩個(gè)點(diǎn)在垃圾識(shí)別這種類別間相似度較高的場(chǎng)景里mAP的微小提升可能就意味著某些易混淆類別能被分對(duì)。訓(xùn)練過程中最需要盯的是PR曲線和混淆矩陣。我最初訓(xùn)練的模型在易拉罐和塑料瓶之間頻繁混淆看混淆矩陣發(fā)現(xiàn)鋁罐在強(qiáng)光下的反光區(qū)域非常像塑料瓶的高光紋理。解決方法是采集數(shù)據(jù)時(shí)特意加入強(qiáng)反光樣本同時(shí)把亮度增強(qiáng)參數(shù)hsv_v調(diào)低從默認(rèn)的0.4降到0.2避免模型把反光當(dāng)成目標(biāo)特征。調(diào)完訓(xùn)練一輪后這兩類的混淆比例降了大概40%。2.3 實(shí)時(shí)推理的性能優(yōu)化TensorRT與模型輕量化訓(xùn)練完的PyTorch模型直接跑推理在1660Ti上大概30ms一幀純看視覺是夠用的但一旦和MoveIt同時(shí)跑CPU、GPU爭(zhēng)搶資源整個(gè)系統(tǒng)的響應(yīng)延遲會(huì)明顯上升。為了給機(jī)械臂規(guī)劃留出余量我把模型導(dǎo)出成TensorRT的FP16 engine。導(dǎo)出流程網(wǎng)上資料很多但有幾個(gè)坑值得提醒一是YOLOv5官方倉(cāng)庫(kù)的export.py在導(dǎo)出TensorRT時(shí)如果CUDA、cuDNN和TensorRT版本不匹配很容易報(bào)錯(cuò)或者導(dǎo)出的engine體積異常。我最后用的組合是CUDA 10.2cuDNN 7.6.5TensorRT 7.1.3在2023年之后這個(gè)組合偏舊但穩(wěn)定性非常好。二是FP16的精度損失在垃圾識(shí)別里完全可接受我實(shí)測(cè)mAP只掉了0.8%推理時(shí)間從30ms降到了12ms。三是TensorRT的engine文件跟推理環(huán)境強(qiáng)綁定換一臺(tái)電腦必須重新導(dǎo)出沒法直接拷著用這個(gè)要提前在交付文檔里寫清楚。還有一個(gè)更輕量的優(yōu)化是降低輸入分辨率。640x640降到480x480推理時(shí)間從12ms降到7msmAP僅下降1.1%。在桌面固定抓取場(chǎng)景下物體距離相機(jī)很近YOLO在低分辨率下同樣能檢測(cè)到所以我覺得這個(gè)trade-off很劃算。最終部署時(shí)我選了480x480輸入這樣即使MoveIt規(guī)劃需要占用大量計(jì)算資源視覺端的幀率也能穩(wěn)定在25FPS以上。3. 機(jī)械臂控制模塊MoveIt運(yùn)動(dòng)規(guī)劃與抓取邏輯的實(shí)現(xiàn)機(jī)械臂控制是整個(gè)系統(tǒng)的執(zhí)行層任何識(shí)別上的誤差最終都會(huì)體現(xiàn)在抓不到或者碰倒物體上。MoveIt本身封裝了很多東西但用好它需要理解幾個(gè)核心概念URDF機(jī)器人模型、規(guī)劃組、運(yùn)動(dòng)學(xué)求解器、規(guī)劃場(chǎng)景和坐標(biāo)系變換。3.1 機(jī)械臂URDF建模與MoveIt配置助手的使用自組機(jī)械臂沒有現(xiàn)成的URDF我是從零開始建的。建模時(shí)最重要的事情是把每個(gè)關(guān)節(jié)的運(yùn)動(dòng)范圍搞對(duì)。拿我用的MG996R舵機(jī)來說名義上是180度但實(shí)際PWM信號(hào)在500到2500微秒之間對(duì)應(yīng)的角度才穩(wěn)定超出這個(gè)范圍舵機(jī)要么堵轉(zhuǎn)要么抖動(dòng)。我當(dāng)時(shí)沒仔細(xì)測(cè)實(shí)際行程直接按180度建模結(jié)果MoveIt規(guī)劃的路徑經(jīng)常讓舵機(jī)到達(dá)物理極限執(zhí)行時(shí)咔咔響。后來用角度尺實(shí)測(cè)了每個(gè)關(guān)節(jié)的極限角度重新改URDF問題才解決。URDF建好后用moveit_setup_assistant生成MoveIt配置包。這個(gè)工具會(huì)引導(dǎo)你定義規(guī)劃組Planning Group、生成碰撞矩陣并檢查URDF的完整性。需要注意的是規(guī)劃組里必須把末端執(zhí)行器也就是夾爪單獨(dú)列成一個(gè)group后面做抓取時(shí)可以直接指定末端目標(biāo)位姿否則MoveIt只控制機(jī)械臂本體夾爪的開合狀態(tài)沒法納入規(guī)劃。還有一個(gè)細(xì)節(jié)moveit_setup_assistant生成的默認(rèn)碰撞矩陣采用的是相鄰連桿不檢查碰撞的自動(dòng)碰撞矩陣。我原本以為這個(gè)就很可靠但實(shí)際有一個(gè)非相鄰連桿在特定姿態(tài)下會(huì)干涉比如大臂在抬高到某個(gè)角度時(shí)會(huì)碰到肩部的一個(gè)支架。所以我后來手動(dòng)給這兩個(gè)連桿添加了碰撞對(duì)讓規(guī)劃器把它們視為不可碰撞才徹底避免了規(guī)劃出的路徑掃過自身支架的情況。3.2 抓取位姿估計(jì)與坐標(biāo)變換TF的坑視覺檢測(cè)出來的是像素坐標(biāo)機(jī)械臂要用的是機(jī)器人基座坐標(biāo)系下的三維坐標(biāo)。兩者之間的橋梁是TF樹。我這邊固定了相機(jī)在機(jī)械臂正上方大約70cm處要求相機(jī)坐標(biāo)系camera_link到機(jī)械臂基座坐標(biāo)系base_link的變換是固定的。這個(gè)變換可以通過手眼標(biāo)定得到也可以直接通過精確測(cè)量安裝位置和角度推導(dǎo)出來。我當(dāng)時(shí)圖省事想直接量尺寸算坐標(biāo)變換結(jié)果誤差大到離譜。為什么相機(jī)安裝的俯仰角只要偏2度在60cm遠(yuǎn)的桌面上位置誤差就能到3cm以上機(jī)械臂夾爪張開寬度才5cm直接抓空。后來老老實(shí)實(shí)做了手眼標(biāo)定。用的事OpenCV的經(jīng)典張正友標(biāo)定板配合eye-in-hand標(biāo)定算法但這里的eye-in-hand其實(shí)是錯(cuò)誤說法因?yàn)橄鄼C(jī)是固定的應(yīng)該是eye-to-hand。網(wǎng)上大量代碼把eih和eth混在一起我一開始照著eye-in-hand的流程走標(biāo)定結(jié)果一直在跳查了半天才發(fā)現(xiàn)坐標(biāo)系關(guān)系搞反了。標(biāo)定流程總結(jié)下來就是機(jī)械臂帶著標(biāo)定板或者相機(jī)帶著標(biāo)定板看你用哪種方法移動(dòng)到多個(gè)不同姿態(tài)采集標(biāo)定板圖像并記錄各個(gè)姿態(tài)下機(jī)械臂末端位姿然后通過AXXB方程求解相機(jī)相對(duì)機(jī)械臂基座的變換。手眼標(biāo)定的誤差非常依賴采樣數(shù)據(jù)的多樣性機(jī)械臂姿態(tài)變化范圍越大、標(biāo)定板在圖像中的位置越分散標(biāo)定結(jié)果越準(zhǔn)確。我只做了一次旋轉(zhuǎn)角范圍較小結(jié)果誤差有5mm后來重新采集了30個(gè)姿態(tài)覆蓋不同高度和不同水平位置誤差穩(wěn)定在1.5mm以內(nèi)。3.3 運(yùn)動(dòng)規(guī)劃與避障基于實(shí)際場(chǎng)景的規(guī)劃器選擇MoveIt默認(rèn)的規(guī)劃器是RRTConnect我一開始照默認(rèn)用。RRTConnect的原理是同時(shí)從起點(diǎn)和終點(diǎn)生長(zhǎng)兩棵樹直到兩棵樹相遇所以它在無障礙場(chǎng)景下規(guī)劃速度非?？?。但缺點(diǎn)是生成的路徑通常比較繞路徑平滑度差。機(jī)械臂執(zhí)行時(shí)會(huì)出現(xiàn)不自然的擺動(dòng)速度控制也沒那么流暢。后來我測(cè)試了OMPL里的RRTstar和STOMP。RRTstar的路徑質(zhì)量明顯好但規(guī)劃時(shí)間動(dòng)不動(dòng)就幾秒在實(shí)時(shí)交互場(chǎng)景里太慢。STOMP需要配置代價(jià)代價(jià)函數(shù)調(diào)起來麻煩。最后我的方案是先讓MoveIt用RRTConnect快速規(guī)劃一條路徑然后對(duì)路徑做一步簡(jiǎn)化再通過MoveIt的平滑處理模塊做軌跡重采樣最終的軌跡既快又平滑。這種快速規(guī)劃后處理的思路效果非常顯著機(jī)械臂運(yùn)動(dòng)過程中的抖動(dòng)基本消除。規(guī)劃場(chǎng)景Planning Scene里的障礙物設(shè)置也是一個(gè)重要環(huán)節(jié)。如果你不告訴MoveIt桌面上哪里有障礙物它規(guī)劃時(shí)只考慮機(jī)械臂自身碰撞可能規(guī)劃出一條直接穿桌子的路徑。我在項(xiàng)目中把桌面、垃圾桶都作為固定障礙物加到了規(guī)劃場(chǎng)景里。做法是在節(jié)點(diǎn)的/planning_scene話題上發(fā)布CollisionObject消息幾何形狀用box表示加上對(duì)應(yīng)的位姿。這樣MoveIt規(guī)劃出的路徑會(huì)自然地繞過垃圾桶而不是把機(jī)械臂的手臂插進(jìn)垃圾桶里。4. 視覺與機(jī)械臂的聯(lián)動(dòng)系統(tǒng)集成的關(guān)鍵代碼路徑與狀態(tài)機(jī)視覺和機(jī)械臂單獨(dú)跑都能工作但把它們連起來才真正暴露問題。這一章重點(diǎn)講ROS節(jié)點(diǎn)怎么組織、坐標(biāo)怎么轉(zhuǎn)換、狀態(tài)機(jī)怎么設(shè)計(jì)這些是系統(tǒng)能否穩(wěn)定跑通的關(guān)鍵。4.1 ROS節(jié)點(diǎn)劃分與消息通信設(shè)計(jì)整個(gè)項(xiàng)目我拆成了四個(gè)ROS節(jié)點(diǎn)camera_node負(fù)責(zé)讀取相機(jī)圖像發(fā)布sensor_msgs/Image和sensor_msgs/CameraInfodetection_node訂閱圖像用TensorRT engine做推理發(fā)布檢測(cè)結(jié)果自定義的消息類型trashDetectionArray包含類別、置信度、像素坐標(biāo)和檢測(cè)框大小controller_node訂閱檢測(cè)結(jié)果做決策生成目標(biāo)位姿調(diào)用MoveIt接口規(guī)劃并執(zhí)行運(yùn)動(dòng)gripper_node訂閱夾爪開合指令控制夾爪舵機(jī)節(jié)點(diǎn)之間用ROS話題通信好處是解耦清晰。detection_node和controller_node之間不需要等待函數(shù)調(diào)用檢測(cè)結(jié)果一發(fā)布控制節(jié)點(diǎn)就會(huì)觸發(fā)回調(diào)。調(diào)試時(shí)還可以用rostopic echo實(shí)時(shí)查看檢測(cè)數(shù)據(jù)非常方便。這里有一個(gè)設(shè)計(jì)細(xì)節(jié)值得說為什么不在detection_node里做坐標(biāo)轉(zhuǎn)換而是把像素坐標(biāo)直接發(fā)布出去因?yàn)閐etection_node不需要關(guān)心機(jī)械臂的坐標(biāo)轉(zhuǎn)換參數(shù)如果相機(jī)換了安裝位置只需要在controller_node里修改變換關(guān)系視覺節(jié)點(diǎn)完全不用動(dòng)。這種低耦合設(shè)計(jì)讓我在后期調(diào)整標(biāo)定時(shí)省了大量時(shí)間。4.2 從檢測(cè)框到抓取點(diǎn)的坐標(biāo)轉(zhuǎn)換流程檢測(cè)到目標(biāo)類別后下一個(gè)問題就是抓哪里。我用最直接的辦法用檢測(cè)框的中心點(diǎn)作為抓取點(diǎn)在圖像上的投影。由于桌面上的垃圾主要是一個(gè)個(gè)平放或稍微傾斜的物體它們的抓取點(diǎn)大致在物體幾何中心。通過相機(jī)內(nèi)參將像素坐標(biāo)轉(zhuǎn)為相機(jī)坐標(biāo)系下的坐標(biāo)再通過手眼標(biāo)定得到的變換矩陣轉(zhuǎn)到機(jī)械臂基座坐標(biāo)系下。公式上其實(shí)很簡(jiǎn)單[X, Y, Z, 1]^T T_eye_to_base * [x_cam, y_cam, z_cam, 1]^T。但真正的問題是Z坐標(biāo)怎么確定。相機(jī)是固定朝下的桌面就在一個(gè)固定的高度我直接設(shè)Z為桌面高度。這樣如果物體有一定高度取點(diǎn)會(huì)偏低但夾爪有足夠的容錯(cuò)空間只要能抓住物體上半部分就行。另一個(gè)重要參數(shù)是抓取姿態(tài)。垃圾不具備固定朝向我用的是豎直向下的姿態(tài)夾爪末端朝向Z軸負(fù)方向垂直于桌面。這樣機(jī)械臂第五軸和第六軸的姿態(tài)是確定的只需要控制XY位置。實(shí)際上我測(cè)試過帶一點(diǎn)傾斜角度去抓取斜坡上的物體但成功率反而不如豎直抓取因?yàn)槲矬w的實(shí)際朝向很難提前判斷。豎直向下抓取配合軟指夾爪對(duì)大多數(shù)扁平垃圾紙團(tuán)、果皮和圓柱垃圾易拉罐都有不錯(cuò)的適應(yīng)性。4.3 狀態(tài)機(jī)設(shè)計(jì)檢測(cè)、定位、抓取、投放的調(diào)度邏輯整個(gè)系統(tǒng)的核心邏輯是一個(gè)狀態(tài)機(jī)用Python實(shí)現(xiàn)狀態(tài)包括IDLE、DETECTED、REACHING、GRASPING、LIFTING、MOVING_TO_BIN、RELEASING、RETURNING。狀態(tài)之間的切換由MoveIt的回調(diào)和夾爪傳感器反饋觸發(fā)。這里我踩的坑是一開始沒有考慮到機(jī)械臂持續(xù)運(yùn)動(dòng)時(shí)的狀態(tài)同步。比如MoveIt執(zhí)行完REACHING狀態(tài)后需要夾爪閉合但夾爪閉合需要時(shí)間必須等夾爪完全閉合后才能進(jìn)入LIFTING狀態(tài)。我最初用固定延時(shí)1秒結(jié)果機(jī)械臂上升太快把物體甩掉了。后來改成夾爪霍爾傳感器檢測(cè)到物體夾緊后再切換到下一狀態(tài)成功率一下子從70%提升到95%。狀態(tài)機(jī)還有一個(gè)關(guān)鍵點(diǎn)是超時(shí)和失敗處理。抓取不是每次都能成功比如物體太滑或者位置太偏。我給GRASPING狀態(tài)加了一個(gè)超時(shí)設(shè)定如果超過3秒還沒有檢測(cè)到夾緊信號(hào)狀態(tài)機(jī)就回到IDLE讓機(jī)械臂重新等待識(shí)別或調(diào)整目標(biāo)點(diǎn)。否則機(jī)械臂會(huì)卡在原地整個(gè)系統(tǒng)就僵住了。5. 實(shí)機(jī)調(diào)試中踩過的坑與排查思路這一章是全文最想讓你仔細(xì)看的因?yàn)檫@些坑我都是在連續(xù)調(diào)試十幾個(gè)小時(shí)之后才找到根因的。每一類問題都值得拿出來單獨(dú)說。5.1 相機(jī)標(biāo)定與手眼標(biāo)定失敗的原因分析手眼標(biāo)定失敗的最常見原因是數(shù)據(jù)采集不夠好而不是算法本身有問題。我第一次標(biāo)定時(shí)機(jī)械臂只轉(zhuǎn)了10來種姿態(tài)而且標(biāo)定板在圖像里的位置幾乎都在中間。標(biāo)定結(jié)果每次運(yùn)行都不一樣旋轉(zhuǎn)矩陣的方差特別大。后來我意識(shí)到手眼標(biāo)定的核心是讓標(biāo)定板在相機(jī)視野的不同區(qū)域都有足夠的約束姿態(tài)變化要足夠大才能解出穩(wěn)定的變換矩陣。另一個(gè)容易踩的坑是標(biāo)定板角點(diǎn)的亞像素提取失敗。光照太強(qiáng)或標(biāo)定板太舊時(shí)OpenCV的findChessboardCorners經(jīng)常返回False或者角點(diǎn)順序錯(cuò)亂。解決辦法是把標(biāo)定板放在陰影下并且事先做一次灰度歸一化。在采集時(shí)保持標(biāo)定板平整不要彎折。否則標(biāo)定出來的矩陣也會(huì)有明顯偏差。標(biāo)定完成后一定要做一次交叉驗(yàn)證把標(biāo)定板放在桌面上幾個(gè)已知位置用機(jī)械臂末端去觸碰標(biāo)定板上的特定角點(diǎn)對(duì)比兩個(gè)位置的實(shí)際坐標(biāo)和通過標(biāo)定矩陣換算出的坐標(biāo)。如果誤差超過3mm就要重新標(biāo)定。這套驗(yàn)證流程看起來多花時(shí)間但能避免后面所有調(diào)試都在錯(cuò)誤誤差前提下的浪費(fèi)時(shí)間。5.2 MoveIt規(guī)劃失敗與碰撞檢測(cè)誤判的處理MoveIt規(guī)劃失敗最典型的報(bào)錯(cuò)是Unable to solve the planning problem原因通常是目標(biāo)位姿在機(jī)械臂的不可達(dá)區(qū)域之外。我畫了機(jī)械臂的工作空間包絡(luò)把所有目標(biāo)抓取點(diǎn)都限制在包絡(luò)之內(nèi)但仍然有一小部分點(diǎn)規(guī)劃失敗。深入排查后發(fā)現(xiàn)原因是末端執(zhí)行器的默認(rèn)方向太苛刻。我指定目標(biāo)位姿時(shí)用了orientation constraint要求夾爪完全垂直于桌面但實(shí)際可達(dá)的抓取姿態(tài)會(huì)略微傾斜。后來我去掉了過于嚴(yán)格的方向約束只限定Z軸方向在正負(fù)5度范圍內(nèi)規(guī)劃成功率從85%提升到了100%。碰撞檢測(cè)誤判是指MoveIt認(rèn)為某個(gè)路徑會(huì)碰撞但目測(cè)明明不會(huì)。這種情況多半是碰撞矩陣?yán)锇瞬辉摪呐鲎矊?duì)或者物體模型尺寸設(shè)置錯(cuò)了。我用的是自組臂有些連桿的STL模型是從網(wǎng)上找的簡(jiǎn)化版幾何形狀和實(shí)物有一定差異。比如末端夾爪的STL模型比實(shí)際夾爪長(zhǎng)了一截導(dǎo)致MoveIt認(rèn)為任何靠近桌面的抓取動(dòng)作都會(huì)碰撞。解決辦法是直接簡(jiǎn)化夾爪碰撞模型用一個(gè)圓柱體代替尺寸略小于實(shí)際夾爪給規(guī)劃器留一點(diǎn)冗余。5.3 實(shí)時(shí)性瓶頸排查CPU占用與推理延遲整個(gè)系統(tǒng)在運(yùn)行時(shí)我用htop和nvtop監(jiān)控資源占用。發(fā)現(xiàn)MoveIt規(guī)劃時(shí)CPU單核占用達(dá)到100%而YOLOv5推理在GPU上只占60%但GPU內(nèi)存卻異常高導(dǎo)致顯存不足報(bào)警。排查后發(fā)現(xiàn)是TensorRT engine的輸入輸出buffer沒有釋放每次推理都重新分配內(nèi)存導(dǎo)致顯存泄漏。修正的方式是初始化時(shí)一次性分配好固定大小的buffer推理時(shí)只復(fù)制數(shù)據(jù)不重新分配。這一個(gè)小修改讓顯存占用從2.1GB降到了0.9GB長(zhǎng)時(shí)間運(yùn)行也不會(huì)再報(bào)錯(cuò)。另一個(gè)瓶頸是ROS話題的傳輸延遲。默認(rèn)的TCPROS在大圖像傳輸時(shí)會(huì)有幾百毫秒的延遲我通過設(shè)置roscpp的傳輸類型為UDPROS解決了這個(gè)問題。核心原理是UDP掉包重傳機(jī)制比TCP輕量適合圖像這種大流量實(shí)時(shí)性要求高的數(shù)據(jù)。實(shí)際測(cè)試下來圖像從相機(jī)到視覺節(jié)點(diǎn)的延遲從180ms降到了35ms這個(gè)提升對(duì)整個(gè)系統(tǒng)的實(shí)時(shí)響應(yīng)非常有幫助。6. 項(xiàng)目源碼與設(shè)計(jì)文檔的使用指引把源碼和設(shè)計(jì)文檔打包交付時(shí)我特意做了目錄整理和README說明因?yàn)槲抑廊绻粚懬宄e人拿到這堆文件很容易卡在環(huán)境配置上。這里我也把源碼結(jié)構(gòu)和使用步驟大致講一下幫你少走彎路。6.1 源碼目錄結(jié)構(gòu)與關(guān)鍵文件說明壓縮包解壓后頂層目錄大概是這樣的trash_sorting_robot/ ├── README.md ├── src/ │ ├── vision/ │ │ ├── yolo_detector.py │ │ ├── export_tensorrt.sh │ │ └── weights/ │ ├── controller/ │ │ ├── state_machine.py │ │ ├── moveit_control.py │ │ └── gripper_control.py │ ├── calibration/ │ │ ├── hand_eye_calibration.py │ │ └── camera_calibration.py │ └── launch/ │ ├── system.launch │ └── moveit_plan.launch ├── docs/ │ ├── 設(shè)計(jì)文檔.pdf │ ├── 操作手冊(cè).md │ └── 實(shí)驗(yàn)數(shù)據(jù)記錄.xlsx └── requirements.txtvision/yolo_detector.py是視覺節(jié)點(diǎn)的核心文件里面包含了TensorRT engine的加載和推理邏輯。如果你沒有TensorRT環(huán)境也可以直接改成PyTorch推理但要注意修改輸入預(yù)處理的部分。controller/moveit_control.py封裝了MoveIt的所有接口包括設(shè)置目標(biāo)位姿、執(zhí)行規(guī)劃、等待執(zhí)行完成等。controller/state_machine.py是完整的狀態(tài)機(jī)實(shí)現(xiàn)里面有一個(gè)TrashSortingStateMachine類你可以在__init__里修改垃圾桶對(duì)應(yīng)的類別列表比如把易拉罐對(duì)應(yīng)到可回收桶、紙團(tuán)對(duì)應(yīng)到紙類桶。calibration/hand_eye_calibration.py是手眼標(biāo)定腳本需要配合標(biāo)定板使用。代碼里有詳細(xì)的注釋按步驟運(yùn)行即可。6.2 環(huán)境部署步驟與ROS版本注意事項(xiàng)環(huán)境依賴寫在了requirements.txt里主要包括Python 3.6.9、PyTorch 1.7.1、TensorRT 7.1.3、OpenCV 4.5.2、ROS Melodic、MoveIt 1.0.3。這些版本組合我是實(shí)測(cè)過的如果完全照抄可以最大程度避免版本沖突。ROS版本這里重點(diǎn)說一下如果你用的是ROS NoeticUbuntu 20.04那么MoveIt版本是2.xAPI和Melodic下的1.x有一些差異主要的坑是moveit_commander的接口在Noetic下沒那么穩(wěn)定建議優(yōu)先使用MoveIt的C API。如果你仍然想用Python API需要切換到moveit_py包或者使用ROS2的moveit2。我這邊因?yàn)闆]有ROS2的需求所以最終選擇了MelodicC接口組合穩(wěn)得很。在編譯過程中如果遇到moveit_ros_planning找不到的問題可以在安裝MoveIt時(shí)把ros-melodic-moveit和ros-melodic-moveit-ros-planning都裝全不要只裝核心包否則會(huì)缺失一些頭文件。6.3 設(shè)計(jì)文檔中不被注意但很實(shí)用的細(xì)節(jié)設(shè)計(jì)文檔里除了常規(guī)的需求分析、系統(tǒng)架構(gòu)、模塊設(shè)計(jì)還有三塊我覺得價(jià)值最大一是實(shí)驗(yàn)數(shù)據(jù)記錄.xlsx其中原始記錄了不同光照、不同角度下識(shí)別成功率的具體數(shù)據(jù)比如在光照充足時(shí)四類垃圾的平均識(shí)別率是97.2%逆光時(shí)降到了88.5%這些數(shù)據(jù)可以幫你預(yù)判實(shí)際使用環(huán)境的表現(xiàn)二是機(jī)械臂末端夾爪的結(jié)構(gòu)設(shè)計(jì)圖紙我用了3D打印的軟性硅膠指套比硬塑膠夾爪對(duì)不規(guī)則垃圾的適應(yīng)性好很多設(shè)計(jì)文檔里有STL文件三是操作手冊(cè).md里關(guān)于硬件上電順序的警告控制器必須先上電再啟動(dòng)ROS節(jié)點(diǎn)否則機(jī)械臂會(huì)處于未使能狀態(tài)導(dǎo)致無法規(guī)劃。這些細(xì)節(jié)單獨(dú)看起來不起眼但都是我在實(shí)測(cè)中累積出來的經(jīng)驗(yàn)如果你只是照著設(shè)計(jì)文檔的框架去做很可能忽略掉這些隱藏信息然后在某個(gè)環(huán)節(jié)卡住很久。7. 后續(xù)擴(kuò)展與個(gè)人心得項(xiàng)目做到這個(gè)程度已經(jīng)能在實(shí)驗(yàn)室穩(wěn)定演示了。但如果你有興趣往更實(shí)用方向走或者想把這套東西改造成更完整的系統(tǒng)我有幾個(gè)思路和一些發(fā)自肺腑的建議。7.1 從單分類到可回收資源分揀的升級(jí)思路目前的系統(tǒng)只能做一次抓一個(gè)的離散抓取吞吐量不高。想提升效率的話可以把視覺模塊升級(jí)為流式檢測(cè)多目標(biāo)排隊(duì)也就是一次識(shí)別畫面里多個(gè)垃圾然后按優(yōu)先級(jí)逐個(gè)規(guī)劃抓取不需要每次抓完都重新識(shí)別整個(gè)場(chǎng)景。這樣能減少機(jī)械臂空等時(shí)間吞吐量理論上能提升50%以上。另外可以嘗試把識(shí)別類別擴(kuò)展到更多細(xì)分比如把塑料瓶細(xì)分為PET、HDPE等材質(zhì)類型。這需要更多的數(shù)據(jù)樣本和更精細(xì)的模型但意義很大直接對(duì)接后端回收產(chǎn)業(yè)鏈。如果你對(duì)垃圾分類感興趣可以往細(xì)粒度材質(zhì)識(shí)別方向深挖這個(gè)方向目前公開數(shù)據(jù)集相對(duì)較少但也是實(shí)際需求比較旺盛的領(lǐng)域。機(jī)械臂部分也可以做動(dòng)態(tài)抓取。目前桌面上的物體是靜止的如果你把物體放到一個(gè)緩慢轉(zhuǎn)動(dòng)的轉(zhuǎn)盤上讓YOLOv5在運(yùn)動(dòng)過程中實(shí)時(shí)識(shí)別并預(yù)測(cè)抓取點(diǎn)MoveIt就需要和視覺聯(lián)動(dòng)做在線軌跡規(guī)劃。這個(gè)復(fù)雜度會(huì)上升好幾個(gè)臺(tái)階但做好了就是一個(gè)完整的視覺伺服項(xiàng)目含金量很高。7.2 關(guān)于學(xué)習(xí)路徑的一些建議做這類項(xiàng)目最忌諱一上來就攤大餅把YOLO、MoveIt、ROS、標(biāo)定、機(jī)械臂建模一起學(xué)很容易被勸退。我的建議是分三步走第一周先跑通YOLOv5官方訓(xùn)練流程用公開數(shù)據(jù)集訓(xùn)練一個(gè)模型重點(diǎn)理解模型接口和推理流程第二周用MoveIt控制機(jī)械臂到達(dá)固定點(diǎn)不做視覺理解TF和規(guī)劃流程第三周才開始把兩者串起來。每走一步都要確保自己能解釋清楚原理再進(jìn)入下一步。還有一個(gè)小建議項(xiàng)目中出現(xiàn)問題時(shí)盡量自己通過rostopic echo、rosrun rqt_graph、rosrun rqt_tf_tree這些工具去定位而不是直接查代碼。因?yàn)楹芏鄦栴}出在運(yùn)行時(shí)的話題連接或坐標(biāo)系關(guān)系上看代碼看不出什么但一看TF樹和數(shù)據(jù)流就一目了然了。我用這套排查方法節(jié)省了大把時(shí)間。最后動(dòng)手做永遠(yuǎn)比看教程學(xué)得快。哪怕你的機(jī)械臂很簡(jiǎn)陋、相機(jī)是幾十塊的USB攝像頭先把一個(gè)最簡(jiǎn)單的識(shí)別-抓取-投放閉環(huán)跑通后面的優(yōu)化才有意義。真實(shí)項(xiàng)目里你學(xué)到的排錯(cuò)能力才是完全不亞于技術(shù)本身的東西。本文還有配套的精品資源點(diǎn)擊獲取