《智能決策綜合實驗》指導書

學院：專業(yè)：分組：同組人：機電工程學院智能制造工程系二〇二五年五月方向二智能優(yōu)化算法求解分揀機械臂路徑規(guī)劃問題一、實驗?zāi)康脑谕瓿芍悄苤圃煺J知基本實驗基礎(chǔ)上進行的綜合性實驗研究。實驗?zāi)康氖菫榱俗寣W生對智能制造設(shè)備級決策有具象化認知。培養(yǎng)學生對實際生產(chǎn)問題的思考能力，將問題轉(zhuǎn)化為數(shù)學語言能力、編程能力和分析決策能力。學生通過仿真環(huán)境構(gòu)建，了解機械臂運動控制原理、視覺識別原理、路徑規(guī)劃原理，運行所學的算法知識及編程語言研究機械臂的路徑規(guī)劃。二、實驗要求1）了解機械臂的運用場景，明確實驗?zāi)康模?）了解機械臂控制原理和視覺識別原理；3）對兩種路徑規(guī)劃問題進行問題描述、數(shù)學建模；4）學會路徑規(guī)劃的常見算法；5）具有在Python編程的能力與靈活運用代碼的能力；4）實驗過程詳盡，步驟合理，結(jié)論正確、分析討論深入。三、實驗內(nèi)容3.1機械臂視覺控制仿真環(huán)境構(gòu)建本實驗以pybullet搭建了機械臂視覺控制仿真環(huán)境。仿真環(huán)境中的機械臂為六自由度機械臂，每個關(guān)節(jié)的電機可以設(shè)置其力矩，速度和位置，從而控制機械臂運動。末端搭載了RGBD相機，通過相機可以獲取相機視野中的彩色圖像及深度圖像。圖1基本試驗場景3.1.1構(gòu)建仿真環(huán)境本實驗可運行“顯示基本場景.py”文件來察看效果。具體代碼含義解析如下：在pybullet環(huán)境中，運行下列語句，可以加載出一個地板和一個立方體。并設(shè)置環(huán)境中的重力場和觀察相機的位置和視角。importimportpybulletaspphysicsClient=p.connect(p.GUI)#orp.DIRECTfornon-graphicalversionp.setAdditionalSearchPath(pybullet_data.getDataPath())#optionallyp.setGravity(0,0,-9.81)#設(shè)置重力p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])#設(shè)置相機視角p.loadURDF("plane.urdf")#加載地板cube_ind=p.loadURDF('cube.urdf',(1.3,0,0),p.getQuaternionFromEuler([0,0,0]))#加載一個方塊3.1.2構(gòu)建仿真環(huán)境，并控制機械臂運動為方便實驗的展開，本實驗提供了一個機械臂類SimRobot，通過該類可以加載一個機械臂。robot=robot=SimRobot(p)運行以下代碼可以加載一個實驗場景。實驗場景中包含了一個桌子和桌子上的一個方形物料。robot.darw_cubes用于繪制方形物料，cube_xyz_list中存儲一個物料位置的列表。##繪制一個環(huán)境desk_high=0.2num_bodies=p.getNumBodies()robot.draw_desk(p,[-1.25,0,0.2],[1,1,desk_high/2])cube_xyz_list=[[-.5,-0.2,desk_high]]cube_id_list=robot.darw_cubes(p,desk_high,[0.02,0.02,0.02],cube_xyz_list)該機械臂控制類可通過以下代碼控制機械臂運動，go_to_rest可控制機械臂運動到等待位姿。goto_joint_pose可控制機械臂的各個關(guān)節(jié)運動到指定角度，該函數(shù)的輸入?yún)?shù)tmp_target_pose是個六維數(shù)組，表示六個關(guān)節(jié)的弧度。go_to_point可控制機械臂末端到達笛卡爾空間中的某位置，該函數(shù)的輸入?yún)?shù)target_pos是一個三維數(shù)組，表示末端在三維空間中的坐標值。robot.go_to_robot.go_to_rest(p)succ_flag=robot.goto_joint_pose(p,tmp_target_pose,tolerence=tolerence)inv_flag,no_collision_flag=robot.go_to_point(p,target_pos,tolerence=0.01)作業(yè)：請在“顯示基本場景.py”基礎(chǔ)上添加上述三條指令，嘗試控制機械臂。3.1.3獲取彩色圖和深度圖，并檢測目標采用函數(shù)set_unfixed_camera_pose將相機位置設(shè)定到相機所在的位置。用函數(shù)get_pointcloud通過相機獲取圖像數(shù)據(jù)，其中rgbImg為彩色圖，depthImg為深度圖。robot.set_unfixed_camera_robot.set_unfixed_camera_pose(p)rgbImg,depthImg,intrinsic=robot.get_pointcloud(p)圖2由彩色圖和深度圖構(gòu)成點云作業(yè)：（附加題）請根據(jù)彩色圖、深度圖，采用視覺檢測方法檢測出方塊及方塊在相機坐標系下的位置。3.1.4目標坐標轉(zhuǎn)換并抓取一個目標在上一步中，算法檢測出了方塊在相機坐標系下的坐標值。但機械臂控制需要在機械臂基座標系下進行。因此，需要先進行坐標轉(zhuǎn)換再進行抓取。運行“坐標變換并抓取一個物體.py”即可實現(xiàn)一個方塊抓取的案例。圖3機械臂抓取point_in_camerapoint_in_camera=np.array([-0.08601628,-0.01412746,0.39477907])point_list_in_robot_base=robot.trans_point_from_camera_to_base([point_in_camera])forobject_posinpoint_list_in_robot_base:robot.pick_one_object(p,object_pos)方塊在相機中的坐標值為point_in_camera，該數(shù)值通過目標檢測得到。將該坐標值帶入函數(shù)trans_point_from_camera_to_base中進行坐標系轉(zhuǎn)換，可得到目標點在機械臂基座標系下的坐標值point_list_in_robot_base。取出坐標值，用pick_one_object函數(shù)控制機械臂運動到方塊位置，實現(xiàn)抓取。作業(yè)：（附加題）請為物料設(shè)置一定新的初始位置，并進行識別。識別后進行坐標系轉(zhuǎn)換并抓取目標。3.2智能算法解決機械臂路徑規(guī)劃問題在智能制造工廠中，機械臂在分揀，搬運，焊接，加工等環(huán)節(jié)扮演重要作用。其中，分揀環(huán)節(jié)要求機械臂抓取多個零件，而搬運環(huán)節(jié)要求機械臂攜帶貨物無碰撞地通過狹窄空間。因此，機械臂路徑規(guī)劃問題分為最短路徑規(guī)劃問題和避障問題。3.2.1最短路徑規(guī)劃問題與模型構(gòu)建在智能制造場景中，通常將要求機械臂抓取視野內(nèi)的所有物體。為實現(xiàn)高效抓取，則要求在遍歷所有物體的前提下規(guī)劃出最短的路徑。該過程與經(jīng)典的旅行商問題相似，因此可以參考旅行商問題的解決辦法。假設(shè)有一系列點[p1,p2,p3,…,p圖結(jié)構(gòu)的建立：圖4圖結(jié)構(gòu)為方便計算，圖結(jié)構(gòu)需要以鄰接矩陣的形式進行表示。橫豎標題列中填入頂點，兩個頂點作為行列索引找到單元格，將邊的Cost填入表中。圖5鄰接矩陣圖6圖結(jié)構(gòu)作業(yè)：請用python建立一個鄰接矩陣?？蓞⒖即a件“繪制圖結(jié)構(gòu).py”，從代碼中找出建立鄰接矩陣的代碼段，并建立圖6的圖結(jié)構(gòu)的鄰接矩陣。3.2.2最短路徑規(guī)劃算法貪心算法是一個側(cè)重局部最優(yōu)的規(guī)劃算法。貪心算法基本步驟：1.1.從問題的某個初始解出發(fā)。2.當可以向求解目標前進一步時，選擇該階段的局部最優(yōu)策略。3.當?shù)竭_新位置時，繼續(xù)尋找局部最優(yōu)解，以此類推，直至終點。4.將所有的部分解綜合起來，得到問題的最終解。運行“用貪心算法解決基于圖連接的旅行商問題.py”，代碼基于圖四中的圖結(jié)構(gòu)，設(shè)計了一條從A點出發(fā)，遍歷所有點的最短路徑。作業(yè)：請分解出代碼中的重要代碼段：建立鄰接矩陣的代碼段；貪心算法的代碼段。并繪制代碼運行的基本流程。退火算法（SimulatedAnnealing）是一種全局優(yōu)化算法，用于在搜索空間中找到最優(yōu)或近似最優(yōu)解。它通過模擬固體退火的過程來搜索解空間，并在搜索過程中允許一定程度的不穩(wěn)定性和隨機性。退火算法基本步驟：1.1.初始解和初始溫度：從搜索空間中隨機生成一個初始解，并設(shè)置一個初始的搜索溫度（初始參數(shù)T）。2.迭代更新：在每次迭代中，根據(jù)當前解進行鄰域搜索，生成一個臨近解，計算當前解與臨近解的目標函數(shù)值之差（ΔE），如果ΔE為負，則接受臨近解作為新的解；如果ΔE為正，則以一定概率接受該臨近解。3.溫度調(diào)度：在每次迭代中，逐步降低搜索溫度，降低接受劣解的概率，使算法更傾向于接受更好的解。4.停止條件：當達到設(shè)定的停止條件（如迭代次數(shù)達到上限、溫度降至閾值等）時，算法結(jié)束，返回得到的最優(yōu)解或接近最優(yōu)解。用于表示退火算法的SASolution類，具體內(nèi)容如下：classclassSASolution(TSP): """ 模擬退火算法解決TSP問題: SA（Simulatedannealing) """ def__init__(self,points_names,map,start_point_name): super().__init__(points_names,map,start_point_name) defsa(self): router=[self.points_names.index(i)foriinself.points_names] distance=self.router_distance(router) a=0.99#降溫系數(shù) turn=1000 e=math.e for_inrange(turn): p1=int(random.random()*self.points_num) p2=int(random.random()*self.points_num) whilep1==p2: p2=int(random.random()*self.points_num) temp=copy.deepcopy(router) temp[p1],temp[p2]=temp[p2],temp[p1] curr_distance=self.router_distance(temp) ifcurr_distance<distance*e**a: distance=curr_distance router=temp #更新降溫系數(shù) a=a*a print("sa總距離：",distance) returnrouter作業(yè)：請參考“用貪心算法解決基于圖連接的旅行商問題.py”，將其中的貪心算法替換為退火算法。3.2.3避障路徑規(guī)劃問題與模型構(gòu)建在上一節(jié)中，采用圖描述路徑規(guī)劃問題，在圖中某個頂點A與其他幾個頂點相連，表示從頂點A出發(fā)移動一次時，只能移動到這幾個相連的頂點上。而在分揀場景中，當沒有障礙物時，空間是連續(xù)的，從A點出發(fā)可以到達空間中任意點。用圖的概念描述，則可以看作頂點之間兩兩相連。AA=(0,1)B=(1,3)C=(4,3)D=(5,1)作業(yè)：假設(shè)有四個點，作為四個頂點，頂點之間兩兩連接，連接兩頂點的邊的參數(shù)為兩頂點之間的距離。請繪制圖結(jié)構(gòu)以及其鄰接矩陣。為方便計算，往往將運動空間劃分為網(wǎng)格。每個單元格可以移動到相鄰的單元格中。將每個單元格視為一個頂點，并將頂點與相鄰頂點相連，則可繪制出一個局部圖。圖7網(wǎng)格化地圖運行代碼“在網(wǎng)格空間中添加障礙物.py”可以獲得圖7中含障礙物的網(wǎng)格化地圖。圖8含障礙物的網(wǎng)格化地圖在有障礙物的情況下，若頂點相鄰的點為障礙物，則兩個點之間不形成連接關(guān)系。為每個網(wǎng)格都繪制一個局部圖，則可以獲得一張圖結(jié)構(gòu)G=(V,E)?？梢圆捎没趫D的路徑規(guī)劃算法來進行避障路徑優(yōu)化。作業(yè)：上一章中目標檢測出的方形物料的位置為cube_xyz_list，由于物料在同一高度上，因此可以將規(guī)劃問題視為二維空間中的規(guī)劃。請參考“在網(wǎng)格空間中添加障礙物.py”為將物料視為障礙物，并繪制網(wǎng)格地圖。desk_highdesk_high=0.2cube_xyz_list=[[-.42,-0.2,desk_high],[-.4,-0.1,desk_high],[-0.39,0.01,desk_high],[-0.41,0.11,desk_high],[-0.50,-0.22,desk_high],[-0.51,-0.12,desk_high],[-0.53,-0.01,desk_high],[-0.49,0.12,desk_high],[-0.58,-0.23,desk_high],[-0.6,-0.13,desk_high],[-0.62,0.02,desk_high],[-0.61,0.13,desk_high]]3.2.4避障路徑規(guī)劃算法A*算法是一種很常用的路徑查找和圖形遍歷算法。它有較好的性能和準確度。其算法原理及偽代碼如下：初始化初始化open_set和close_set；*將起點加入open_set中，并設(shè)置優(yōu)先級為0（優(yōu)先級最高）；*如果open_set不為空，則從open_set中選取優(yōu)先級最高的節(jié)點n：*如果節(jié)點n為終點，則：*從終點開始逐步追蹤parent節(jié)點，一直達到起點；*返回找到的結(jié)果路徑，算法結(jié)束；*如果節(jié)點n不是終點，則：*將節(jié)點n從open_set中刪除，并加入close_set中；*遍歷節(jié)點n所有的鄰近節(jié)點：*如果鄰近節(jié)點m在close_set中，則：*跳過，選取下一個鄰近節(jié)點*如果鄰近節(jié)點m也不在open_set中，則：*設(shè)置節(jié)點m的parent為節(jié)點n*計算節(jié)點m的優(yōu)先級*將節(jié)點m加入open_set中運行文件“A星算法.py”，可以用A星算法實現(xiàn)起始點到重點之間的路徑規(guī)劃。圖9用A星算法實現(xiàn)路徑規(guī)劃作業(yè)：請參考“A星算法.py”為上一個作業(yè)中繪制出的地圖中設(shè)置起始點和終點，并實現(xiàn)路徑規(guī)劃。3.5視覺識別+路徑規(guī)劃算法控制機械臂在實際工況下，路徑規(guī)劃規(guī)劃結(jié)果用于提升機械臂抓取或避障的效率和成功率。在本實驗中，將視覺識別、路徑規(guī)劃和機械臂控制組成一個完整的流程。在仿真環(huán)境下實現(xiàn)機械臂的視覺伺服控制。3.5.1基本流程視覺識別用于感知作業(yè)場景，感知結(jié)果為一系列目標點或障礙物點。依據(jù)這些目標點和障礙物點，可以構(gòu)建路徑規(guī)劃問題。以圖或網(wǎng)格地圖的形式描述路徑規(guī)劃問題，進一步地，將圖或網(wǎng)格地圖以計算機數(shù)據(jù)的形式表達，并采用路徑規(guī)劃算法，實現(xiàn)路徑規(guī)劃。最終用路徑規(guī)劃結(jié)果指導機械臂的運動。完整運行框架參考文件“機械臂視覺伺服框架.py”，該框架中的視覺識別和路徑規(guī)劃算法需自行添加。 圖10視覺+路徑規(guī)劃+機械臂控制流程3.5.2機械臂類的用法為方便開發(fā)，將機械臂及機械場景寫成一個類。其主要功能包含了場景構(gòu)建、機械臂控制和圖像獲取。robot.draw_robot.draw_desk(p,position,length)cube_id_list=robot.darw_cubes(p,desk_high,size,cube_xyz_list)robot.go_to_rest(p)succ_flag=robot.goto_joint_pose(p,tmp_target_pose,tolerence=tolerence)inv_flag,no_collision_flag=robot.go_to_point(p,target_pos,tolerence=0.01)robot.move_one_point(p,object_center)robot.pick_one_object(p,object_pos)robot.set_unfixed_camera_pose(self,p)rgbImg,depthImg,intrinsic=robot.get_pointcloud(p)current_pos=robot.get_current_point(p,robot.EndEffectorIndex)3.5.3代碼框架避障路徑規(guī)劃的參考代碼如下：ffromSimRobotimportSimRobotimporttimeimportpybullet_dataimportpybulletaspfromgrid_obstacle_planimportAStar,Map_from_pointsphysicsClient=p.connect(p.GUI)#orp.DIRECTfornon-graphicalversionp.setGravity(0,0,-9.81)#設(shè)置重力p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])#設(shè)定全局相機視角robot=SimRobot(p)#創(chuàng)建機械臂#繪制一個環(huán)境desk_high=0.2robot.draw_desk(p,[-1.25,0,0.2],[1,1,desk_high/2])cube_xyz_list=[[-.42,-0.2,desk_high]]cube_id_list=robot.darw_cubes(p,desk_high,[0.02,0.02,0.02],cube_xyz_list)robot.go_to_rest(p)#機械臂到準備位置whileTrue:time.sleep(0.01)p.stepSimulation()#更新仿真畫面keys=p.getKeyboardEvents()#等待鍵盤輸入iford("z")inkeysandkeys[ord("z")]&p.KEY_WAS_TRIGGERED:robot.go_to_photo_pose(p)#機械臂到拍照位robot.set_unfixed_camera_pose(p)#將相機設(shè)置到對應(yīng)位置 rgbImg,depthImg,intrinsic=robot.get_pointcloud(p)#獲取圖像數(shù)據(jù)object_center_list=detect(rgbImg,depthImg)#檢測出目標在相機坐標系下的位置base_point_list=robot.trans_point_from_camera_to_base(object_center_list)#坐標轉(zhuǎn)換獲取目標在機械臂基座標系下的位置map_tool=Map_from_points(ba