?NXROBO20231

6.智能機器人視覺應(yīng)用

——人臉檢測課程介紹課程安排：

1)OpenCV介紹與安裝2)配置人臉檢測程序

3)運行人臉檢測程序課程目的：實現(xiàn)通過人臉檢測檢測出視野范圍內(nèi)的人臉?NXROBO2023實現(xiàn)效果?NXROBO2023人臉檢測效果OpenCV安裝4?NXROBO2023OpenCV（OpenSourceComputerVisionLibrary）是一個開源的計算機視覺和機器學習軟件庫。它提供了豐富的圖像處理和計算機視覺算法，可以用于開發(fā)各種應(yīng)用程序，包括圖像和視頻處理、目標檢測和識別、人臉識別、圖像分割、運動跟蹤等。ROS中已經(jīng)集成了OpenCV庫和相關(guān)的接口功能包，使用以下命令即可安裝：$sudoaptinstallros-noetic-vision-opencvlibopencv-devpython3-opencv配置人臉檢測程序新建spark_vision文件夾

拷貝face_detector功能包為python文件添加可執(zhí)行權(quán)限?NXROBO2023運行程序啟動攝像頭

?NXROBO2023運行人臉檢測程序$roslaunchcamera_driver_transferd435.launch$cdspark_noetic/$sourcedevel/setup.bash$rosrunface_detectorfacedetect.py檢測程序講解?NXROBO2023#!/usr/bin/envpython3#-*-coding:utf-8-*-importosimportnumpyasnpimportcv2importrospyfromstd_msgs.msgimportStringfromsensor_msgs.msgimportImagefromcv_bridgeimportCvBridge,CvBridgeError#localmodulesfromcommonimportclock,draw_strclassFacedetect():

def__init__(self):#創(chuàng)建訂閱圖像信息的訂閱者，回調(diào)函數(shù)callbackself.image_sub=rospy.Subscriber("/camera/color/image_raw",Image,self.callback)#檢測函數(shù)defdetect(self,img,cascade):'''使用級聯(lián)分類器(CascadeClassifier)對圖像進行目標檢測，返回的結(jié)果rects是一個包含檢測到目標的矩形框(ROI)的列表，每個矩形框表示一個檢測到的目標。每個矩形框由四個值組成：(x,y,width,height)，表示矩形框的左上角坐標和寬度、高度。'''rects=cascade.detectMultiScale(img,scaleFactor=1.3,minNeighbors=4,minSize=(30,30),flags=cv2.CASCADE_SCALE_IMAGE)iflen(rects)==0:return[]#將檢測到的人臉矩形框的坐標轉(zhuǎn)換為左上角坐標和右下角坐標的形式。rects[:,2:]+=rects[:,:2]returnrectsdefdraw_rects(self,img,rects,color):forx1,y1,x2,y2inrects:cv2.rectangle(img,(x1,y1),(x2,y2),color,2)#回調(diào)函數(shù)defcallback(self,data):try:#ROS圖像與OpenCV圖像格式之間進行轉(zhuǎn)換image=CvBridge().imgmsg_to_cv2(data,"bgr8")exceptCvBridgeErrorase:print(e)

#獲取當前腳本的目錄路徑script_dir=os.path.dirname(os.path.abspath(__file__))

#構(gòu)建級聯(lián)分類器文件的相對路徑cascade_path=os.path.join(script_dir,"../data/haarcascades/haarcascade_frontalface_alt.xml")nested_path=os.path.join(script_dir,"../data/haarcascades/haarcascade_eye.xml")#創(chuàng)建級聯(lián)分類器對象cascade=cv2.CascadeClassifier(cascade_path)nested=cv2.CascadeClassifier(nested_path)gray=cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)#圖像灰度化gray=cv2.equalizeHist(gray)#直方圖均衡化t=clock()rects=self.detect(gray,cascade)#檢測圖像中的人臉vis=image.copy()self.draw_rects(vis,rects,(0,255,0))#在圖像上繪制人臉矩形框ifnotnested.empty():forx1,y1,x2,y2inrects:roi=gray[y1:y2,x1:x2]vis_roi=vis[y1:y2,x1:x2]subrects=self.detect(roi.copy(),nested)#檢測人臉區(qū)域中的眼睛self.draw_rects(vis_roi,subrects,(255,0,0))#在眼睛區(qū)域繪制矩形框dt=clock()-tdraw_str(vis,(20,20),'time:%.1fms'%(dt*1000))#在圖像上繪制處理時間cv2.imshow('facedetect',vis)#顯示帶有檢測結(jié)果的圖像cv2.waitKey(1)defmain():rospy.init_node('facedetector',anonymous=True)Facedetect()try:rospy.spin()exceptKeyboardInterrupt:print("Shuttingdown")cv2.destroyAllWindows()if__name__=='__main__':main()?NXROBO20238

2.智能機器人視覺應(yīng)用

——人臉識別課程介紹課程安排：

1)人臉識別介紹2）人臉識別訓練3)人臉識別

課程目的：實現(xiàn)通過人臉識別識別出訓練過的人臉，并將人臉信息打印到圖片上?NXROBO2023實現(xiàn)效果?NXROBO2023人臉識別效果OpenCV安裝11?NXROBO2023OpenCV提供了用于人臉識別的功能和算法，使開發(fā)者能夠?qū)崿F(xiàn)各種人臉識別應(yīng)用。人臉識別是一種計算機視覺技術(shù)，旨在檢測和識別圖像或視頻中的人臉，并將其與預(yù)先存儲的人臉進行比較和匹配。ROS中已經(jīng)集成了OpenCV庫和相關(guān)的接口功能包，使用以下命令即可安裝：（已安裝請忽略）$sudoaptinstallros-noetic-vision-opencvlibopencv-devpython3-opencv配置程序拷貝face_recognizer功能包為python文件添加可執(zhí)行權(quán)限?NXROBO2023將課程代碼中的face_recognizer功能包復制到spark_vision中(/home/spark/spark_noetic/src/spark_app/spark_vision/face_recognizer)準備人臉識別圖像13?NXROBO2023圖片命名格式為：id.姓名準備了三張人臉信息圖像，放置在/spark_vision/face_recognizer/faces路徑下訓練人臉圖像14?NXROBO2023運行人臉識別訓練程序train_date.py（/spark_vision/face_recognizer/script）$cd/home/spark/spark_noetic/src/spark_app/spark_vision/face_recognizer/script$python3train_date.py訓練程序講解（頭文件與main函數(shù)）?NXROBO2023importcv2importosfromPILimportImageimportnumpyasnpif__name__=='__main__':#獲取當前腳本的目錄路徑script_dir=os.path.dirname(os.path.abspath(__file__))#圖片路徑path=os.path.join(script_dir,"../faces")#獲取圖象數(shù)組id標簽和姓名faces,ids,names=getImageAndLables(path)#加載識別源recognizer=cv2.face.LBPHFaceRecognizer_create()#訓練recognizer.train(faces,np.array(ids))#保存文件recognizer.write(os.path.join(path,"../data/haarcasades"))訓練程序講解（getImageAndLables函數(shù)）?NXROBO2023defgetImageAndLables(path):#存儲人臉數(shù)據(jù)faceSamples=[]#存儲id數(shù)據(jù)ids=[]#存儲姓名數(shù)據(jù)names=[]#存儲圖片信息imagePaths=[os.path.join(path,f)forfinos.listdir(path)]#打印數(shù)組imagePathsprint('數(shù)據(jù)排列：',imagePaths)#分類器地址cascade_path=os.path.join(path,"../data/haarcascades/haarcascade_frontalface_default.xml")#加載分類器face_detector=cv2.CascadeClassifier(cascade_path)

#遍歷列表中的圖片forimagePathinimagePaths:#打開圖片，并且灰度化PIL有九種不同的模式：1，L,P,RGB,RGBA,CMYK,YCbCr,I,F.PIL_img=Image.open(imagePath).convert('L')#將圖片轉(zhuǎn)換為數(shù)組，以黑白深淺img_numpy=np.array(PIL_img,'uint8')#獲取人臉特征faces=face_detector.detectMultiScale(img_numpy)#獲取每張有圖片的的ID和姓名id=int(os.path.split(imagePath)[1].split('.')[0])name=str(os.path.split(imagePath)[1].split('.')[1])#預(yù)防無面容圖片forx,y,w,hinfaces:ids.append(id)names.append(name)faceSamples.append(img_numpy[y:y+h,x:x+w])#打印臉部特征和IDprint('name:',name)print('fs:',faceSamples)returnfaceSamples,ids,names運行程序啟動攝像頭

?NXROBO2023運行人臉識別程序$roslaunchcamera_driver_transferd435.launch$cdspark_noetic/$sourcedevel/setup.bash$rosrunface_recognizerface_recognition.py識別程序講解?NXROBO2023defface_detect_demo(self,img):try:#ROS圖像與OpenCV圖像格式之間進行轉(zhuǎn)換img=CvBridge().imgmsg_to_cv2(img,"bgr8")exceptCvBridgeErrorase:print(e)#設(shè)置成灰度圖片gary=cv.cvtColor(img,cv.COLOR_BGR2GRAY)#導入人臉檢測訓練好的模型--分類器face_detect=cv.CascadeClassifier(self.cascade_path)#開始檢測face=face_detect.detectMultiScale(gary,1.15,cv.CASCADE_SCALE_IMAGE)

forx,y,w,hinface:cv.rectangle(img,(x,y),(x+w,y+h),color=(0,0,255),thickness=2)#獲取人臉id與置信度ids,confidence=self.recogizer.predict(gary[y:y+h,x:x+w])print('id:',ids,'confidence:',confidence)#如果置信度超過閾值80，則將人臉標記為"unknow"，否則將顯示對應(yīng)的ID。ifconfidence>80:self.warningtime+=1ifself.warningtime>90:self.warningtime=0print("unknowpeople")cv.putText(img,'unknow',(x+10,y-10),cv.FONT_HERSHEY_SIMPLEX,0.75,(0,255,0),1)else:cv.putText(img,str(s[ids-1]),(x+10,y-10),cv.FONT_HERSHEY_SIMPLEX,0.75,(0,255,0),1)cv.imshow('result',img)cv.waitKey(1)?NXROBO202319

3.智能機器人視覺應(yīng)用

——顏色識別課程介紹課程安排：

1)hsv顏色空間介紹2)顏色識別程序配置3)運行顏色識別課程目的：實現(xiàn)識別并圈出視野范圍內(nèi)的目標顏色?NXROBO2023實現(xiàn)效果?NXROBO2023顏色識別效果hsv顏色空間介紹22?NXROBO2023HSV顏色空間比RGB更接近人們對彩色的感知經(jīng)驗。非常直觀地表達顏色的色調(diào)、鮮艷程度和明暗程度，方便進行顏色的對比。HSV表達彩色圖像的方式由三個部分組成：Hue（色調(diào)、色相），色相是顏色的基本屬性，用角度度量，取值范圍為0°~360°，從紅色開始按逆時針方向計算；Saturation（飽和度、色彩純凈度），飽和度是指顏色的純度，越高顏色越純，低則逐漸變灰，取0-100%的數(shù)值；Value（明度），明度是指顏色的亮暗程度，越高顏色越亮，越低顏色越暗，取0-100%的數(shù)值。HSV顏色空間可以用一個圓錐體或一個圓柱體來表示配置程序拷貝color_detection功能包為python文件添加可執(zhí)行權(quán)限?NXROBO2023將課程代碼中的color_detection功能包復制到spark_vision中(/home/spark/spark_noetic/src/spark_app/spark_vision/color_detection)運行程序啟動攝像頭

?NXROBO2023運行顏色識別程序$roslaunchcamera_driver_transferd435.launch$cdspark_noetic/$sourcedevel/setup.bash$rosruncolor_detectioncolor_detection.py識別程序講解?NXROBO2023defcallback(self,data):try:image=CvBridge().imgmsg_to_cv2(data,"bgr8")#將ROS圖像格式轉(zhuǎn)換為OpenCV圖像格式exceptCvBridgeErrorase:print(e)hsv=cv.cvtColor(image,cv.COLOR_BGR2HSV)#將圖像轉(zhuǎn)換為HSV顏色空間mask=cv.inRange(hsv,(71,120,120),(100,255,255))#根據(jù)顏色閾值范圍創(chuàng)建掩膜line=cv.getStructuringElement(cv.MORPH_RECT,(15,15),(-1,-1))#創(chuàng)建用于形態(tài)學操作的線狀結(jié)構(gòu)元素mask=cv.morphologyEx(mask,cv.MORPH_OPEN,line)#進行形態(tài)學開運算contours,hierarchy=cv.findContours(mask,cv.RETR_EXTERNAL,cv.CHAIN_APPROX_SIMPLE)#查找輪廓index=-1max_area=0forcinrange(len(contours)):area=cv.contourArea(contours[c])#計算輪廓的面積ifarea>max_area:#找到最大面積的輪廓max_area=areaindex=cifindex>=0:rect=cv.minAreaRect(contours[index])#擬合最小外接矩形cv.ellipse(image,rect,(0,255,0),2,8)#繪制橢圓cv.circle(image,(32(rect[0][0]),32(rect[0][1])),2,(255,0,0),2,8,0)#繪制圓點cv.imshow("output",image)#顯示圖像cv.waitKey(1)?NXROBO202326

4.智能機器人視覺應(yīng)用

——行人檢測課程介紹課程安排： 1）HOG算法 2）行人檢測程序配置 3）運行行人檢測程序

課程目的：實現(xiàn)spark通過攝像頭識別行人。?NXROBO2023實現(xiàn)效果?NXROBO2023行人檢測效果配置程序拷貝spark_target_following工作空間?NXROBO2023將課程代碼中的spark_target_following工作空間復制到spark中(/home/spark/)實操部分編譯更新環(huán)境運行程序$cd~/spark_target_following$catkin_make$source~/spark_target_following/devel/setup.bash?NXROBO2022$roslaunchspark_followerbringup.launch識別程序講解?NXROBO2023defface_detect_demo(self,img):try:#ROS圖像與OpenCV圖像格式之間進行轉(zhuǎn)換img=CvBridge().imgmsg_to_cv2(img,"bgr8")exceptCvBridgeErrorase:print(e)#設(shè)置成灰度圖片gary=cv.cvtColor(img,cv.COLOR_BGR2GRAY)#導入人臉檢測訓練好的模型--分類器face_detect=cv.CascadeClassifier(self.cascade_path)#開始檢測face=face_detect.detectMultiScale(gary,1.15,cv.CASCADE_SCALE_IMAGE)

forx,y,w,hinface:cv.rectangle(img,(x,y),(x+w,y+h),color=(0,0,255),thickness=2)#獲取人臉id與置信度ids,confidence=self.recogizer.predict(gary[y:y+h,x:x+w])print('id:',ids,'confidence:',confidence)#如果置信度超過閾值80，則將人臉標記為"unknow"，否則將顯示對應(yīng)的ID。ifconfidence>80:self.warningtime+=1ifself.warningtime>90:self.warningtime=0print("unknowpeople")cv.putText(img,'unknow',(x+10,y-10),cv.FONT_HERSHEY_SIMPLEX,0.75,(0,255,0),1)else:cv.putText(img,str(s[ids-1]),(x+10,y-10),cv.FONT_HERSHEY_SIMPLEX,0.75,(0,255,0),1)cv.imshow('result',img)cv.waitKey(1)5.通過深度圖信息處理讓機器人跟著你走?NXROBO202232實現(xiàn)效果33?NXROBO2022課程介紹課程安排： 1）深度攝像頭D435介紹 2）點云與PCL概述 3）nodelet的簡單使用說明 4）實現(xiàn)跟蹤的代碼講解相關(guān)知識點：D435深度攝像頭，nodelet，機器視覺，運動控制課程目的：實現(xiàn)spark通過深度圖信息處理進行跟蹤?NXROBO2022深度攝像頭D435介紹35?NXROBO2022深度攝像頭D435深度攝像頭D435介紹36?NXROBO2022深度攝像頭D435深度攝像頭D435技術(shù)規(guī)格介紹37?NXROBO2022深度攝像頭D435點云概述38?NXROBO2022人的側(cè)臉3D點云圖點云PointCloud是三維空間中，表達目標空間分布和目標表面特性的點的集合，點云通?？梢詮纳疃认鄼C中直接獲取，也可以從CAD等軟件中生成。點云是在同一空間參考系下表達目標空間分布和目標表面特性的海量點集合，在獲取物體表面每個采樣點的空間坐標后，得到的是點的集合，稱之為“點云”（PointCloud）。點云是用于表示多維點集合的數(shù)據(jù)結(jié)構(gòu)，通常用于表示三維數(shù)據(jù)。在3D點云中，這些點通常代表采樣表面的X，Y和Z幾何坐標。PCL概述39?NXROBO2022PCLPCL（PointCloudLibrary）是用于2D/3D圖像和點云處理的大型開源跨平臺的C++編程庫。PCL框架實現(xiàn)了大量點云相關(guān)的通用算法和高效的數(shù)據(jù)結(jié)構(gòu)。內(nèi)容涉及了點云的獲取、濾波、分割、配準、檢索、特征提取、特征估計，表面重建、識別、模型擬合、追蹤、曲面重建、可視化等等，這些算法可用于從嘈雜的數(shù)據(jù)中過濾出異常值，將3D點云縫合在一起，分割場景的部分區(qū)域，提取關(guān)鍵點并計算描述符，根據(jù)物體的幾何外觀識別實際物體，創(chuàng)建表面點云并將其可視化。支持多種操作系統(tǒng)，可以在Windows、Linux、MacOSX、Android、部分嵌入式實時系統(tǒng)上運行?；绢愋蚉ointCloud40?NXROBO2022PCL的基本數(shù)據(jù)類型是PointCloud，一個PointCloud是一個C++的模板類，它包含了以下字段：width(int)：指定點云數(shù)據(jù)集的寬度對于無組織格式的數(shù)據(jù)集，width代表了所有點的總數(shù)對于有組織格式的數(shù)據(jù)集，width代表了一行中的總點數(shù)height(int)：制定點云數(shù)據(jù)集的高度對于無組織格式的數(shù)據(jù)集，值為1對于有組織格式的數(shù)據(jù)集，表示總行數(shù)points(std::vector<PointT>)：包含所有PointT類型的點的數(shù)據(jù)列表衍生類型PointXYZ-floatx,y,zPointXYZI-floatx,y,z,intensityPointXYZRGB-floatx,y,z,rgbPointXYZRGBA-floatx,y,z,uint32_trgbaNormal-floatnormal[3],curvature法線方向，對應(yīng)的曲率的測量值PointNormal-floatx,y,z,normal[3],curvature采樣點，法線和曲率Histogram-floathistogram[N]用于存儲一般用途的n維直方圖ROS通信是基于Node(節(jié)點)的，Node使用方便、易于擴展，可以滿足ROS中大多數(shù)應(yīng)用場景，但是也存在一些局限性，由于一個Node啟動之后獨占一根進程，不同Node之間數(shù)據(jù)交互其實是不同進程之間的數(shù)據(jù)交互，當傳輸類似于圖片、點云的大容量數(shù)據(jù)時，會出現(xiàn)延時與阻塞的情況。ROS中給出的解決方案是:Nodelet，通過Nodelet可以將多個節(jié)點集成進一個進程。nodelet軟件包旨在提供在同一進程中運行多個算法(節(jié)點)的方式，不同算法之間通過傳遞指向數(shù)據(jù)的指針來代替了數(shù)據(jù)本身的傳輸(類似于編程傳值與傳址的區(qū)別)，從而實現(xiàn)零成本的數(shù)據(jù)拷貝。nodelet概念41?NXROBO2022nodelet語法標準：nodeletloadpkg/Typemanager-Launchanodeletoftypepkg/Typeonmanagermanagernodeletstandalonepkg/Type-Launchanodeletoftypepkg/Typeinastandalonenodenodeletunloadnamemanager-Unloadanodeletanodeletbynamefrommanagernodeletmanager-Launchanodeletmanagernode舉例說明：rosrunnodeletnodeletmanager__name:=spark_base_nodelet_managerrosrunnodeletnodeletloadnxfollower/NxFollowerNodeletspark_base_nodelet_managernodelet語法42?NXROBO2022實操部分復制spark-class3到主目錄下編譯更新環(huán)境運行程序$cd~/spark-class3$catkin_make$source~/spark-class2/devel/setup.bash?NXROBO2022$roslaunchspark_followerbringup.launch課程小結(jié)了解深度攝像頭D435了解點云與PCL概述學習nodelet實現(xiàn)機器人跟蹤人行走?NXROBO20227.MoveIt!編程接口課程內(nèi)容1.MoveIt!的編程接口2.關(guān)節(jié)空間運動規(guī)劃3.笛卡爾空間運動規(guī)劃4.碰撞檢測5.運動學插件的配置46?NXROBO20231.MoveIt的編程接口47?NXROBO2023ROS參數(shù)服務(wù)器A

ction用戶接口機器人MoveGroupA

ctionPick關(guān)節(jié)軌跡move_

group_

i

nterf

ace機器人控制器Place（C

++接口）Servi

ce笛卡爾路徑逆向運動學正向運動學規(guī)劃有效性路徑規(guī)劃Topicmovei

t_

commander（Py

thon接口）點云機器人3D傳感器關(guān)節(jié)狀態(tài)機器人傳感器路徑執(zhí)行GUI規(guī)劃場景（rviz插件接口）附著物體其他接口碰撞物體TF機器人狀態(tài)發(fā)布規(guī)劃場景差異MoveIt!的核心節(jié)點——move_group1.MoveIt的編程接口——C++&Python48?NXROBO2023PythonC++1.MoveIt的編程接口——接口實現(xiàn)流程49?NXROBO20231.MoveIt的編程接口——官方API和基礎(chǔ)教程50?NXROBO20231.MoveIt的編程接口51?NXROBO2023運動規(guī)劃程序步驟：1）連接控制需要的規(guī)劃組2）設(shè)置目標位姿（關(guān)節(jié)空間或笛卡爾空間）

3）設(shè)置運動約束（可選）4）使用MoveIt!規(guī)劃一條到達目標的軌跡5）修改軌跡（如速度等參數(shù)）6）執(zhí)行規(guī)劃出的軌跡2.關(guān)節(jié)空間運動規(guī)劃——正向運動學52?NXROBO2023點到點運動：規(guī)劃關(guān)節(jié)空間中起始狀態(tài)到終止狀態(tài)的軌跡，不考慮機械臂末端的軌跡起始狀態(tài)終止狀態(tài)2.關(guān)節(jié)空間運動規(guī)劃——正向運動學53?NXROBO202354?NXROBO20232.關(guān)節(jié)空間運動規(guī)劃——正向運動學arm=moveit_commander.MoveGroupCommander('arm')joint_positions=[-0.0867,-1.274,0.02832,0.0820,-1.273,-0.003]arm.set_joint_value_target(joint_positions)

arm.go()gripper=moveit_commander.MoveGroupCommander('gripper')gripper.set_joint_value_target([0.01])gripper.go()關(guān)鍵API調(diào)用的步驟1）創(chuàng)建規(guī)劃組的控制對象；2）設(shè)置關(guān)節(jié)空間運動的目標位姿；3）運動規(guī)劃并控制機械臂運動。機械臂規(guī)劃組夾爪規(guī)劃組55?NXROBO20232.關(guān)節(jié)空間運動規(guī)劃——正向運動學$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_fk_demo.py運行如下命令：運動效果：機械臂回到原位(home位置)夾子收緊機械臂運動到另一個位置56?NXROBO20232.關(guān)節(jié)空間運動規(guī)劃——逆向運動學57?NXROBO20232.關(guān)節(jié)空間運動規(guī)劃——逆向運動學arm=moveit_commander.MoveGroupCommander('arm')reference_frame='base_link'arm.set_pose_reference_frame(reference_frame)arm.set_start_state_to_current_state()target_pose=…end_effector_link=arm.get_end_effector_link()

arm.set_pose_target(target_pose,end_effector_link)traj=arm.plan()

arm.execute(traj)關(guān)鍵API調(diào)用的步驟1）創(chuàng)建規(guī)劃組的控制對象；2）設(shè)置參考坐標系；3）設(shè)置機械臂起始狀態(tài)；4）設(shè)置末端執(zhí)行器的目標位姿；5）運動規(guī)劃計算；6）執(zhí)行控制機械臂運動。機械臂規(guī)劃組運行如下命令：運動效果：機械臂回到原位(home位置)機械臂運動到另一個位置58?NXROBO20232.關(guān)節(jié)空間運動規(guī)劃——逆向運動學$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_ik_demo.py59?NXROBO20233.笛卡爾空間運動規(guī)劃目標狀態(tài)設(shè)置：1)用關(guān)節(jié)角表達；2)用末端執(zhí)行器位姿表達不考慮中間狀態(tài)的約束，如不關(guān)心末端的軌跡關(guān)節(jié)空間的運動規(guī)劃60?NXROBO20233.笛卡爾空間運動規(guī)劃笛卡爾路徑約束，路徑點之間的路徑形狀是一條直線。61?NXROBO2023$

roslaunchmarm_moveit_configdemo.launch工作空間規(guī)劃例程$

rosrunmarm_planning

moveit_cartesian_demo.py

_cartesian:=True（走直線）$

rosrunmarm_planning

moveit_cartesian_demo.py

_cartesian:=False（走曲線）3.笛卡爾空間運動規(guī)劃注意：1)到網(wǎng)上下載功能包robot_marm，放到工作空間的src目錄下，并編譯此包；2)需將所有的python文件的權(quán)限設(shè)置為“可執(zhí)行”；3)若ros版本為noetic，還需要將python文件的第一行的python改為python3。62?NXROBO20233.笛卡爾空間運動規(guī)劃63?NXROBO20233.笛卡爾空間運動規(guī)劃64?NXROBO20233.笛卡爾空間運動規(guī)劃關(guān)鍵API的實現(xiàn)步驟返回值

plan：規(guī)劃出來的運動軌跡

fraction：描述規(guī)劃成功的軌跡在給定路點列表中的覆蓋率[0~1]。如果fraction小于1，說明給定的路點列表沒辦法完整規(guī)劃。65?NXROBO20234.碰撞檢測MoveIt!可以在運動規(guī)劃時檢測碰撞，并規(guī)劃軌跡繞過障礙66?NXROBO20234.碰撞檢測——碰撞檢測算法碰撞檢測是運動規(guī)劃中最耗時的運算之一，往往會占用90%左右的時間為了降低計算量，可在Moveit!SetupAssistant工具中設(shè)置碰撞檢測矩陣(ACM,AllowedCollisionMatrix)進行優(yōu)化Moveit!使用CollisionWorld對象進行碰撞檢測采用FCL(FlexibleCollisionLibrary)或PCD(ProximityCollisionDetection)功能包實現(xiàn)；67?NXROBO20234.碰撞檢測——規(guī)劃場景監(jiān)聽信息用戶接口機器人Topic?

狀態(tài)信息點云move_group_interface（C++）接口機器人3D傳感器（State

Information）機器人的關(guān)節(jié)話題joint_states；深度圖像附著物體碰撞物體moveit_commander（Python接口）關(guān)節(jié)狀態(tài)規(guī)劃場景差異機器人傳感器?

傳感器信息GUI（SensorInformation）機器人的傳感器數(shù)據(jù)；（rviz插件接口）TF監(jiān)聽規(guī)劃場景（可選）機器人狀態(tài)發(fā)布其他接口?

外界環(huán)境信息（World

geometry

information）通過傳感器建立的周圍環(huán)境信息。規(guī)劃場景模塊的結(jié)構(gòu)68?NXROBO20234.碰撞檢測——可視化插件添加障礙添加障礙物的方式通過moveit_rviz插件通過程序接口手工添加通過傳感器采集數(shù)據(jù)自動添加69?NXROBO20234.碰撞檢測——避障運動規(guī)劃$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_obstacles_demo.py自主避障規(guī)劃例程注意：1)到網(wǎng)上下載功能包robot_marm，放到工作空間的src目錄下，并編譯此包；2)需將所有的python文件的權(quán)限設(shè)置為“可執(zhí)行”；3)若ros版本為noetic，還需要將python文件的第一行的python改為python3。70?NXROBO20234.碰撞檢測——避障運動規(guī)劃marm_planning/scripts/moveit_obstacles_demo.py71?NXROBO20234.碰撞檢測——避障運動規(guī)劃marm_planning/scripts/moveit_obstacles_demo.py72?NXROBO20234.碰撞檢測——Pick&Place實例演示與分析$roslaunch

marm_moveit_configdemo.launchPick&Place例程$rosrun

marm_planning

moveit_pick_and_place_demo.py注意：1)到網(wǎng)上下載功能包robot_marm，放到工作空間的src目錄下，并編譯此包；2)需將所有的python文件的權(quán)限設(shè)置為“可執(zhí)行”；3)若ros版本為noetic，還需要將python文件的第一行的python改為python3。73?NXROBO20234.碰撞檢測——Pick&Place實例演示與分析創(chuàng)建抓取的目標物體設(shè)置目標物體的放置位置74?NXROBO20234.碰撞檢測——Pick&Place實例演示與分析生成抓取姿態(tài)Pick75?NXROBO20234.碰撞檢測——Pick&Place實例演示與分析Place76?NXROBO20235.運動學插件的配置——KDLMoveIt!默認使用的運動學求解器，數(shù)值求解

優(yōu)點：可求解封閉情況下逆運動學

缺點：速度慢、可能找不到解77?NXROBO20235.運動學插件的配置——TRAC-IK78?NXROBO20235.運動學插件的配置——MoveIt!TRAC-IK配置方法安裝配置$sudoaptinstallros-noetic-trac-ik-kinematics-plugin$rosed

"$MYROBOT_NAME"_moveit_config/config/kinematics.yaml測試$sudo

marm_moveit_configdemo.launch79?NXROBO20235.運動學插件的配置——IKFast

IKFast，由Rosen

Diankov編寫的OpenRAVE運動規(guī)劃軟件提供；

可以求解任意復雜運動鏈的運動學方程（解析解），并產(chǎn)生特定語言的文件（如C++）后供使用；

比較穩(wěn)定、速度快，在最新的處理器上能以5微秒的速度完成運算。80?NXROBO20235.運動學插件的配置——MoveIt!IKFast配置方法$

git$cdopenrave-installation$./install-dependencies.sh$./install-osg.sh$./install-fcl.sh$./install-openrave.sh安裝openrave-installation$

sudoaptinstallcmakeg++gitipython3minizippython3-devpython3-h5pypython3-numpypython3-scipy

$sudoaptinstalllibassimp-devlibavcodec-devlibavformat-devlibboost-all-devlibboost-date-time-devlibbullet-devlibfaac-devlibglew-devlibgsm1-devliblapack-devliblog4cxx-devlibmpfr-devlibode-devlibogg-devlibpcrecpp0v5libpcre3-devlibqhull-devlibsoqt520libsoqt520-devlibswscale-devlibvorbis-devlibx264-devlibxml2-devlibxvidcore-dev安裝程序與依賴庫81?NXROBO20235.運動學插件的配置——MoveIt!IKFast配置方法$git$cdOpenSceneGraph$gitreset--hard{OSG_COMMIT}$mkdirbuild$cdbuild$cmake-DDESIRED_QT_VERSION=4..$sudomakeinstall_ld_conf$make-j`nproc`$sudomakeinstall安裝OpenSceneGraph-3.4$gitclone$cdfcl$gitreset--hard0.5.0$mkdirbuild$cdbuild$cmake..$make-j`nproc`$sudomakeinstall安裝FlexibleCollisionLibrary0.5.082?NXROBO20235.運動學插件的配置——MoveIt!IKFast配置方法$sudoaptinstallpython3-pip$sudopip3installsympy$sudoaptremovepython3-mpmath安裝Python工具安裝IKFast和OpenRave功能包$gitclone-bproductions:///rdiankov/openrave.git$cdopenrave$gitreset--hardRAVE_COMMIT(RAVE_COMMIT:cmake-DODE_USE_MULTITHREAD=ON-DOSG_DIR=/usr/local/lib64/\-DUSE_PYBIND11_PYTHON_BINDINGS:BOOL=TRUE-DBoost_NO_BOOST_CMAKE=1..)$mkdirbuild$cdbuild創(chuàng)建collada文件$exportMYROBOT_NAME="marm"$rosrunxacroxacro--inorder-o"$MYROBOT_NAME".urdf"$MYROBOT_NAME".xacro$rosruncollada_urdfurdf_to_collada"$MYROBOT_NAME".urdf"$MYROBOT_NAME".dae$exportIKFAST_PRECISION="5"$cp"$MYROBOT_NAME".dae"$MYROBOT_NAME".backup.dae$rosrunmoveit_kinematicsround_collada_numbers.py"$MYROBOT_NAME".dae"$MYROBOT_NAME".dae"$IKFAST_PRECISION"創(chuàng)建dae文件83?NXROBO20235.運動學插件的配置——MoveIt!IKFast配置方法$openrave-robot.py

"$MYROBOT_NAME".dae--infolinks查看生成的模型$openrave

"$MYROBOT_NAME".dae84?NXROBO20235.運動學插件的配置——MoveIt!IKFast配置方法$

export

PLANNING_GROUP="arm"$

export

BASE_LINK="1"生成程序文件$

export

EEF_LINK="11"$

exportIKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp$

python`openrave-config--python-dir`/openravepy/_openravepy_/ikfast.py

--robot="$MYROBOT_NAME".dae--iktype=transform6d

--baselink="$BASE_LINK"

--eelink="$EEF_LINK"

--savefile="$IKFAST_OUTPUT_PATH"$

exportMOVEIT_IK_PLUGIN_PKG="$MYROBOT_NAME"_ikfast_"$PLANNING_GROUP"_plugin$

cd~/catkin_ws/src創(chuàng)建插件$

catkin_create_pkg

"$MOVEIT_IK_PLUGIN_PKG"$

rosrun

moveit_kinematicscreate_ikfast_moveit_plugin.py

"$MYROBOT_NAME""$PLANNING_GROUP""$MOVEIT_IK_PLUGIN_PKG""$IKFAST_OUTPUT_PATH"85?NXROBO20235.運動學插件的配置——MoveIt!IKFast配置方法$catkin_make（工作空間根路徑下）$rosed"$MYROBOT_NAME"_moveit_config/config/kinematics.yaml修MoveIt!配置文件測試IKFast插件$sudomarm_moveit_config

demo.launch*模型發(fā)生變化后，IKFast插件也要重新生成8.綜合應(yīng)用開發(fā)課程內(nèi)容87?NXROBO20231.機器視覺與機械臂應(yīng)用2.Pick&Place中的關(guān)鍵技術(shù)3.uArm編程控制實驗4.Spark+uArm抓取實驗1.機器視覺與機械臂應(yīng)用88?NXROBO2023機器視覺作業(yè)環(huán)境感知目標定位目標識別目標跟蹤……機械臂關(guān)節(jié)空間點到點的運動運動笛卡爾空間點到點運動規(guī)劃碰撞檢測障礙物環(huán)境下的運動規(guī)劃……+=智能的機械臂應(yīng)用自主避障的運動自主pick&place操作自主拋光打磨自主焊接……1.機器視覺與機械臂應(yīng)用89?NXROBO2023運行下列命令：$

roslaunch

marm_description

view_marm_with_kinect.launch1.機器視覺與機械臂應(yīng)用90?NXROBO2023marm_description/urdf/marm.urdf1.機器視覺與機械臂應(yīng)用91?NXROBO2023$

roslaunch

marm_gazebo

arm_world.launch仿真環(huán)境與上個例子完全相同1.機器視覺與機械臂應(yīng)用92?NXROBO2023$roslaunchmarm_moveit_configdemo_gazebo.launchMoveIt

！視覺集成1.機器視覺與機械臂應(yīng)用93?NXROBO2023運動控制1.機器視覺與機械臂應(yīng)用94?NXROBO2023marm_moveit_config/launch/demo_gazebo.launch(1)1.機器視覺與機械臂應(yīng)用95?NXROBO2023marm_moveit_config/launch/demo_gazebo.launch(2)1.機器視覺與機械臂應(yīng)用96?NXROBO2023marm_moveit_config/config/sensors_3d.yamlmarm_moveit_config/launch/marm_moveit_sensor_manager.launch.xml97?NXROBO2023pickplacepickplace能否通過攝像頭獲取pick和place的位姿呢？2.Pick&Place中的關(guān)鍵技術(shù)98?NXROBO2023視覺與控制的集成2.Pick&Place中的關(guān)鍵技術(shù)2.Pick&Place中的關(guān)鍵技術(shù)99?NXROBO2023100?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)1)攝像頭標定（內(nèi)參、外參）2)物體識別3)抓取姿態(tài)分析4)機器人運動規(guī)劃101?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)1)攝像頭標定102?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)1)攝像頭標定103?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)

基于

OpenCV

的圖像定位2)物體識別104?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)基于

PCL

的點云信息處理基于點云的物體識別2)物體識別105?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)3)抓取姿態(tài)分析106?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)agile_grasp(點云)：graspit(仿真)moveit_simple_grasps(簡單物體)

3)抓取姿態(tài)分析107?NXROBO20232.Pick&Place中的關(guān)鍵技術(shù)運動規(guī)劃108?NXROBO20233.uArm編程控制實驗——MoveIt!控制參見：109?NXROBO20233.uArm編程控制實驗——MoveIt!控制

$

roslaunch

swiftpro

pro_control.launchMoveIt!

控制

$

roslaunch

pro_moveit_config

demo.launch

$

rosrun

uarm_planning

moveit_fk_demo.py110?NXROBO20233.uArm編程控制實驗——MoveIt!控制roslaunch

swiftpro

pro_control.launchroslaunchpro_moveit_configdemo.launch111?NXROBO20233.uArm編程控制實驗——SDK接口moveit_fk_demo.py112?NXROBO20234.Spark+uArm抓取實驗——操作步驟Spark

+

uArm

抓取113?NXROBO20234.Spark+uArm抓取實驗——演示效果9.智能機器人倉儲應(yīng)用場景開發(fā)課程內(nèi)容課程背景場景程序分析和設(shè)計機械臂與攝像頭標定手動控制場景程序代碼視覺處理程序啟動機器人驅(qū)動程序與功能程序?NXROBO2023115課程背景課程背景?NXROBO2023117倉儲空間利用率是倉儲運營成本的重要組成部分，為了提升倉儲庫容率，節(jié)省空間儲存成本，各行業(yè)倉庫通常都會選擇將貨物進行逐層向上的自然堆疊處理，因此貨物自然堆疊是實現(xiàn)內(nèi)部物流自動化必須攻克的剛需場景之一。實踐模擬自動化倉庫貨物堆疊場景，使用智能機器人spark，場地為3m*3m的平面，場地圖如下所示，貨物采用的是9個10cm*10cm*10cm的藍色方塊，均放置在場地中央。機器人通過手動操控的方式，從起始位置出發(fā)到場地中央，從中吸取藍色方塊回至起始位置，將所有方塊吸取并放置在起始區(qū)域中，實現(xiàn)貨物的堆疊。該場景融合了機器人移動控制、SLAM建圖導航、計算機視覺、機械臂抓取等技術(shù)，通過該場景可以很好地將前幾章的知識進行實踐運用。場景程序分析和設(shè)計?NXROBO2023119場景程序分析和設(shè)計設(shè)計鍵盤控制程序，通過設(shè)定鍵盤按鍵發(fā)布對應(yīng)的底盤運動話題，如前進、后退、左轉(zhuǎn)、右轉(zhuǎn)、抓取、釋放等等；設(shè)計視覺處理程序，將攝像頭視野范圍中的方塊進行定位，識別其中抓取成功概率最高的藍色方塊，獲取其中心點位置；設(shè)計機械臂抓取與釋放程序，根據(jù)藍色方塊的位置，通過攝像頭與機械臂標定后，確定機械臂抓取位置，根據(jù)實際放置情況，設(shè)定不同的釋放高度。機械臂與攝像頭標定?NXROBO2023121機械臂與攝像頭標定打開攝像頭與機械臂標定程序。?NXROBO2023122機械臂與攝像頭標定此時打開了這個界面，我們需要將機械臂的末端橙色部分對著圖中綠色方框，可以適當調(diào)整攝像頭的拍攝角度來確定位置。?NXROBO2023123機械臂與攝像頭標定點擊旁邊的終端，按任意鍵確定。這時，機械臂開始與攝像頭標定。機械臂與攝像頭標定?NXROBO2023124這個是比較好的標定過程，周圍沒有其他顏色干擾，所以要注意周圍光線、環(huán)境的影響，同時也要注意比較相關(guān)的參數(shù)，因為使用的是一元線性回歸，圖像位置與機械臂位置會有相對固定的定值增減，所以在標定過程中，觀察圖像的x軸與y軸的變化量是否相對穩(wěn)定。標定完成之后，退出即可。手動控制場景程序代碼手動控制場景程序代碼?NXROBO2023126首先打開我們手動控制場景程序，teleop.py文件。手動控制場景程序代碼?NXROBO2023127首先是初始化的配置，導入相關(guān)庫，創(chuàng)建發(fā)布者對象，定義抓取和放置的標志位。手動控制場景程序代碼?NXROBO2023128這里是機械臂相關(guān)的話題訂閱，首先創(chuàng)建機械臂訂閱者對象，訂閱機械臂的狀態(tài)，判斷當時是抓取狀態(tài)還是放置狀態(tài)。手動控制場景程序代碼?NXROBO2023129這里就是鍵盤控制函數(shù)的相關(guān)初始化參數(shù)操作，速度的設(shè)置等等，此處銜接我們之前學的運動控制程序。手動控制場景程序代碼?NXROBO2023130手動控制場景程序代碼跟我們之前學習的運動控制代碼有一部分類似，也算是拓展，其中這里增加了機械臂抓取和機械臂放置的功能，通過鍵盤的g或h實現(xiàn)，會發(fā)送抓取或者放置信號給機器人，同學們也可以開發(fā)其他的鍵值。視覺處理程序視覺處理程序?NXROBO2023132首先打開我們的視覺處理程序grasp_d435.py。視覺處理程序?NXROBO2023133導入相關(guān)庫文件，這里更多的是圖像處理相關(guān)的，其中也自定義了一個消息類型，這個是跟機械臂狀態(tài)相關(guān)的。視覺處理程序?NXROBO2023134這里也是做一些初始化操作，初始化各個坐標點、標志位，同時也創(chuàng)建一些發(fā)布者、訂閱者對象。視覺處理程序?NXROBO2023135這里是機械臂抓取的回調(diào)函數(shù)，當機械臂發(fā)布了抓取的消息后，機器人會進入這個回調(diào)函數(shù)，機器人會轉(zhuǎn)圈，攝像頭會一直發(fā)布圖像信息，圖像處理回調(diào)函數(shù)也會運行。如果發(fā)現(xiàn)目標物品，停下抓取。視覺處理程序?NXROBO2023136這部分主要是有機械臂抓取相關(guān)的程序，這部分，會與我們之前機械臂與攝像頭標定的部分有關(guān)，根據(jù)機械臂的位置，結(jié)合視覺識別目標的位置，完成抓取。視覺處理程序?NXROBO2023137通過設(shè)置機械臂相關(guān)的位置高度，實現(xiàn)提起物品高度，放置物品高度。?NXROBO2023138視覺處理程序先將訂閱到的圖像格式轉(zhuǎn)換成opencv能夠處理的格式，使用“CvBridge().imgmsg_to_cv2”。然后通過opencv的方法，實現(xiàn)對物品顏色的識別，同時確定要抓取物品的位置。?NXROBO2023139視覺處理程序通過主邏輯程序，完成機器人視覺抓取邏輯功能。啟動機器人驅(qū)動程序與功能程序?NXROBO2023141啟動機器人驅(qū)動程序與功能程序結(jié)合前面課程的學習，若需要將機器人的各個驅(qū)動（底盤驅(qū)動、雷達驅(qū)動、攝像機驅(qū)動等等）一起啟動的話，可以使用launch文件進行整合編寫，那么接下來創(chuàng)建一個launch文件將所需啟動的節(jié)點驅(qū)動包含其中，創(chuàng)建名為teleop2grasp的launch文件。?NXROBO2023142啟動機器人驅(qū)動程序和功能程序使用<include>標簽啟動driver_bringup.launch文件，該文件包含了底盤的啟動節(jié)點、機器人的描述文件、攝像機驅(qū)動，機器人的描述文件，即機器人的模型文件可以在rviz上顯示機器人模型。?NXROBO2023143啟動機器人驅(qū)動程序和功能程序啟動swiftpro功能包中的pro_control_nomoveit.launch，機械臂開始訂閱運動控制話題與吸盤開關(guān)話題。啟動lidar_driver_transfer功能包中的雷達啟動文件，具體的啟動文件根據(jù)雷達的型號進行選擇，雷達開始掃描并發(fā)布雷達數(shù)據(jù)。?NXROBO2023144啟動機器人驅(qū)動程序和功能程序使用<node>標簽啟動teleop.py程序，然后根據(jù)攝像頭型號的不同，啟動對應(yīng)的視覺處理程序grasp_d435.py。啟動成功后，就