?NXROBO20231

6.智能機器人視覺應用

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

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

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

拷貝face_detector功能包為python文件添加可執行權限?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):#創建訂閱圖像信息的訂閱者，回調函數callbackself.image_sub=rospy.Subscriber("/camera/color/image_raw",Image,self.callback)#檢測函數defdetect(self,img,cascade):'''使用級聯分類器(CascadeClassifier)對圖像進行目標檢測，返回的結果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[]#將檢測到的人臉矩形框的坐標轉換為左上角坐標和右下角坐標的形式。rects[:,2:]+=rects[:,:2]returnrectsdefdraw_rects(self,img,rects,color):forx1,y1,x2,y2inrects:cv2.rectangle(img,(x1,y1),(x2,y2),color,2)#回調函數defcallback(self,data):try:#ROS圖像與OpenCV圖像格式之間進行轉換image=CvBridge().imgmsg_to_cv2(data,"bgr8")exceptCvBridgeErrorase:print(e)

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

#構建級聯分類器文件的相對路徑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")#創建級聯分類器對象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)#檢測人臉區域中的眼睛self.draw_rects(vis_roi,subrects,(255,0,0))#在眼睛區域繪制矩形框dt=clock()-tdraw_str(vis,(20,20),'time:%.1fms'%(dt*1000))#在圖像上繪制處理時間cv2.imshow('facedetect',vis)#顯示帶有檢測結果的圖像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.智能機器人視覺應用

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

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

課程目的：實現通過人臉識別識別出訓練過的人臉，并將人臉信息打印到圖片上?NXROBO2023實現效果?NXROBO2023人臉識別效果OpenCV安裝11?NXROBO2023OpenCV提供了用于人臉識別的功能和算法，使開發者能夠實現各種人臉識別應用。人臉識別是一種計算機視覺技術，旨在檢測和識別圖像或視頻中的人臉，并將其與預先存儲的人臉進行比較和匹配。ROS中已經集成了OpenCV庫和相關的接口功能包，使用以下命令即可安裝：（已安裝請忽略）$sudoaptinstallros-noetic-vision-opencvlibopencv-devpython3-opencv配置程序拷貝face_recognizer功能包為python文件添加可執行權限?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函數）?NXROBO2023importcv2importosfromPILimportImageimportnumpyasnpif__name__=='__main__':#獲取當前腳本的目錄路徑script_dir=os.path.dirname(os.path.abspath(__file__))#圖片路徑path=os.path.join(script_dir,"../faces")#獲取圖象數組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函數）?NXROBO2023defgetImageAndLables(path):#存儲人臉數據faceSamples=[]#存儲id數據ids=[]#存儲姓名數據names=[]#存儲圖片信息imagePaths=[os.path.join(path,f)forfinos.listdir(path)]#打印數組imagePathsprint('數據排列：',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')#將圖片轉換為數組，以黑白深淺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])#預防無面容圖片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圖像格式之間進行轉換img=CvBridge().imgmsg_to_cv2(img,"bgr8")exceptCvBridgeErrorase:print(e)#設置成灰度圖片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"，否則將顯示對應的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.智能機器人視覺應用

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

1)hsv顏色空間介紹2)顏色識別程序配置3)運行顏色識別課程目的：實現識別并圈出視野范圍內的目標顏色?NXROBO2023實現效果?NXROBO2023顏色識別效果hsv顏色空間介紹22?NXROBO2023HSV顏色空間比RGB更接近人們對彩色的感知經驗。非常直觀地表達顏色的色調、鮮艷程度和明暗程度，方便進行顏色的對比。HSV表達彩色圖像的方式由三個部分組成：Hue（色調、色相），色相是顏色的基本屬性，用角度度量，取值范圍為0°~360°，從紅色開始按逆時針方向計算；Saturation（飽和度、色彩純凈度），飽和度是指顏色的純度，越高顏色越純，低則逐漸變灰，取0-100%的數值；Value（明度），明度是指顏色的亮暗程度，越高顏色越亮，越低顏色越暗，取0-100%的數值。HSV顏色空間可以用一個圓錐體或一個圓柱體來表示配置程序拷貝color_detection功能包為python文件添加可執行權限?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圖像格式轉換為OpenCV圖像格式exceptCvBridgeErrorase:print(e)hsv=cv.cvtColor(image,cv.COLOR_BGR2HSV)#將圖像轉換為HSV顏色空間mask=cv.inRange(hsv,(71,120,120),(100,255,255))#根據顏色閾值范圍創建掩膜line=cv.getStructuringElement(cv.MORPH_RECT,(15,15),(-1,-1))#創建用于形態學操作的線狀結構元素mask=cv.morphologyEx(mask,cv.MORPH_OPEN,line)#進行形態學開運算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.智能機器人視覺應用

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

課程目的：實現spark通過攝像頭識別行人。?NXROBO2023實現效果?NXROBO2023行人檢測效果配置程序拷貝spark_target_following工作空間?NXROBO2023將課程代碼中的spark_target_following工作空間復制到spark中(/home/spark/)實操部分編譯更新環境運行程序$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圖像格式之間進行轉換img=CvBridge().imgmsg_to_cv2(img,"bgr8")exceptCvBridgeErrorase:print(e)#設置成灰度圖片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"，否則將顯示對應的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實現效果33?NXROBO2022課程介紹課程安排： 1）深度攝像頭D435介紹 2）點云與PCL概述 3）nodelet的簡單使用說明 4）實現跟蹤的代碼講解相關知識點：D435深度攝像頭，nodelet，機器視覺，運動控制課程目的：實現spark通過深度圖信息處理進行跟蹤?NXROBO2022深度攝像頭D435介紹35?NXROBO2022深度攝像頭D435深度攝像頭D435介紹36?NXROBO2022深度攝像頭D435深度攝像頭D435技術規格介紹37?NXROBO2022深度攝像頭D435點云概述38?NXROBO2022人的側臉3D點云圖點云PointCloud是三維空間中，表達目標空間分布和目標表面特性的點的集合，點云通常可以從深度相機中直接獲取，也可以從CAD等軟件中生成。點云是在同一空間參考系下表達目標空間分布和目標表面特性的海量點集合，在獲取物體表面每個采樣點的空間坐標后，得到的是點的集合，稱之為“點云”（PointCloud）。點云是用于表示多維點集合的數據結構，通常用于表示三維數據。在3D點云中，這些點通常代表采樣表面的X，Y和Z幾何坐標。PCL概述39?NXROBO2022PCLPCL（PointCloudLibrary）是用于2D/3D圖像和點云處理的大型開源跨平臺的C++編程庫。PCL框架實現了大量點云相關的通用算法和高效的數據結構。內容涉及了點云的獲取、濾波、分割、配準、檢索、特征提取、特征估計，表面重建、識別、模型擬合、追蹤、曲面重建、可視化等等，這些算法可用于從嘈雜的數據中過濾出異常值，將3D點云縫合在一起，分割場景的部分區域，提取關鍵點并計算描述符，根據物體的幾何外觀識別實際物體，創建表面點云并將其可視化。支持多種操作系統，可以在Windows、Linux、MacOSX、Android、部分嵌入式實時系統上運行。基本類型PointCloud40?NXROBO2022PCL的基本數據類型是PointCloud，一個PointCloud是一個C++的模板類，它包含了以下字段：width(int)：指定點云數據集的寬度對于無組織格式的數據集，width代表了所有點的總數對于有組織格式的數據集，width代表了一行中的總點數height(int)：制定點云數據集的高度對于無組織格式的數據集，值為1對于有組織格式的數據集，表示總行數points(std::vector<PointT>)：包含所有PointT類型的點的數據列表衍生類型PointXYZ-floatx,y,zPointXYZI-floatx,y,z,intensityPointXYZRGB-floatx,y,z,rgbPointXYZRGBA-floatx,y,z,uint32_trgbaNormal-floatnormal[3],curvature法線方向，對應的曲率的測量值PointNormal-floatx,y,z,normal[3],curvature采樣點，法線和曲率Histogram-floathistogram[N]用于存儲一般用途的n維直方圖ROS通信是基于Node(節點)的，Node使用方便、易于擴展，可以滿足ROS中大多數應用場景，但是也存在一些局限性，由于一個Node啟動之后獨占一根進程，不同Node之間數據交互其實是不同進程之間的數據交互，當傳輸類似于圖片、點云的大容量數據時，會出現延時與阻塞的情況。ROS中給出的解決方案是:Nodelet，通過Nodelet可以將多個節點集成進一個進程。nodelet軟件包旨在提供在同一進程中運行多個算法(節點)的方式，不同算法之間通過傳遞指向數據的指針來代替了數據本身的傳輸(類似于編程傳值與傳址的區別)，從而實現零成本的數據拷貝。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到主目錄下編譯更新環境運行程序$cd~/spark-class3$catkin_make$source~/spark-class2/devel/setup.bash?NXROBO2022$roslaunchspark_followerbringup.launch課程小結了解深度攝像頭D435了解點云與PCL概述學習nodelet實現機器人跟蹤人行走?NXROBO20227.MoveIt!編程接口課程內容1.MoveIt!的編程接口2.關節空間運動規劃3.笛卡爾空間運動規劃4.碰撞檢測5.運動學插件的配置46?NXROBO20231.MoveIt的編程接口47?NXROBO2023ROS參數服務器A

ction用戶接口機器人MoveGroupA

ctionPick關節軌跡move_

group_

i

nterf

ace機器人控制器Place（C

++接口）Servi

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

t_

commander（Py

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

3）設置運動約束（可選）4）使用MoveIt!規劃一條到達目標的軌跡5）修改軌跡（如速度等參數）6）執行規劃出的軌跡2.關節空間運動規劃——正向運動學52?NXROBO2023點到點運動：規劃關節空間中起始狀態到終止狀態的軌跡，不考慮機械臂末端的軌跡起始狀態終止狀態2.關節空間運動規劃——正向運動學53?NXROBO202354?NXROBO20232.關節空間運動規劃——正向運動學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()關鍵API調用的步驟1）創建規劃組的控制對象；2）設置關節空間運動的目標位姿；3）運動規劃并控制機械臂運動。機械臂規劃組夾爪規劃組55?NXROBO20232.關節空間運動規劃——正向運動學$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_fk_demo.py運行如下命令：運動效果：機械臂回到原位(home位置)夾子收緊機械臂運動到另一個位置56?NXROBO20232.關節空間運動規劃——逆向運動學57?NXROBO20232.關節空間運動規劃——逆向運動學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)關鍵API調用的步驟1）創建規劃組的控制對象；2）設置參考坐標系；3）設置機械臂起始狀態；4）設置末端執行器的目標位姿；5）運動規劃計算；6）執行控制機械臂運動。機械臂規劃組運行如下命令：運動效果：機械臂回到原位(home位置)機械臂運動到另一個位置58?NXROBO20232.關節空間運動規劃——逆向運動學$roslaunch

marm_moveit_configdemo.launch$rosrun

marm_planning

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

roslaunchmarm_moveit_configdemo.launch工作空間規劃例程$

rosrunmarm_planning

moveit_cartesian_demo.py

_cartesian:=True（走直線）$

rosrunmarm_planning

moveit_cartesian_demo.py

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

plan：規劃出來的運動軌跡

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

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

Information）機器人的關節話題joint_states；深度圖像附著物體碰撞物體moveit_commander（Python接口）關節狀態規劃場景差異機器人傳感器?

傳感器信息GUI（SensorInformation）機器人的傳感器數據；（rviz插件接口）TF監聽規劃場景（可選）機器人狀態發布其他接口?

外界環境信息（World

geometry

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

marm_moveit_configdemo.launch$rosrun

marm_planning

moveit_obstacles_demo.py自主避障規劃例程注意：1)到網上下載功能包robot_marm，放到工作空間的src目錄下，并編譯此包；2)需將所有的python文件的權限設置為“可執行”；3)若ros版本為noetic，還需要將python文件的第一行的python改為python3。70?NXROBO20234.碰撞檢測——避障運動規劃marm_planning/scripts/moveit_obstacles_demo.py71?NXROBO20234.碰撞檢測——避障運動規劃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)到網上下載功能包robot_marm，放到工作空間的src目錄下，并編譯此包；2)需將所有的python文件的權限設置為“可執行”；3)若ros版本為noetic，還需要將python文件的第一行的python改為python3。73?NXROBO20234.碰撞檢測——Pick&Place實例演示與分析創建抓取的目標物體設置目標物體的放置位置74?NXROBO20234.碰撞檢測——Pick&Place實例演示與分析生成抓取姿態Pick75?NXROBO20234.碰撞檢測——Pick&Place實例演示與分析Place76?NXROBO20235.運動學插件的配置——KDLMoveIt!默認使用的運動學求解器，數值求解

優點：可求解封閉情況下逆運動學

缺點：速度慢、可能找不到解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運動規劃軟件提供；

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

比較穩定、速度快，在最新的處理器上能以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創建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"創建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創建插件$

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

roslaunch

marm_description

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

roslaunch

marm_gazebo

arm_world.launch仿真環境與上個例子完全相同1.機器視覺與機械臂應用92?NXROBO2023$roslaunchmarm_moveit_configdemo_gazebo.launchMoveIt

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

基于

OpenCV

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

PCL

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

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