智能决策综合实验指导书【实验三】

《智能决策综合实验》指导书

学院：专业：分组：同组人：机电工程学院智能制造工程系二〇二五年五月方向二智能优化算法求解分拣机械臂路径规划问题一、实验目的在完成智能制造认知基本实验基础上进行的综合性实验研究。实验目的是为了让学生对智能制造设备级决策有具象化认知。培养学生对实际生产问题的思考能力，将问题转化为数学语言能力、编程能力和分析决策能力。学生通过仿真环境构建，了解机械臂运动控制原理、视觉识别原理、路径规划原理，运行所学的算法知识及编程语言研究机械臂的路径规划。二、实验要求1）了解机械臂的运用场景，明确实验目的；2）了解机械臂控制原理和视觉识别原理；3）对两种路径规划问题进行问题描述、数学建模；4）学会路径规划的常见算法；5）具有在Python编程的能力与灵活运用代码的能力；4）实验过程详尽，步骤合理，结论正确、分析讨论深入。三、实验内容3.1机械臂视觉控制仿真环境构建本实验以pybullet搭建了机械臂视觉控制仿真环境。仿真环境中的机械臂为六自由度机械臂，每个关节的电机可以设置其力矩，速度和位置，从而控制机械臂运动。末端搭载了RGBD相机，通过相机可以获取相机视野中的彩色图像及深度图像。图1基本试验场景3.1.1构建仿真环境本实验可运行“显示基本场景.py”文件来察看效果。具体代码含义解析如下：在pybullet环境中，运行下列语句，可以加载出一个地板和一个立方体。并设置环境中的重力场和观察相机的位置和视角。importimportpybulletaspphysicsClient=p.connect(p.GUI)#orp.DIRECTfornon-graphicalversionp.setAdditionalSearchPath(pybullet_data.getDataPath())#optionallyp.setGravity(0,0,-9.81)#设置重力p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])#设置相机视角p.loadURDF("plane.urdf")#加载地板cube_ind=p.loadURDF('cube.urdf',(1.3,0,0),p.getQuaternionFromEuler([0,0,0]))#加载一个方块3.1.2构建仿真环境，并控制机械臂运动为方便实验的展开，本实验提供了一个机械臂类SimRobot，通过该类可以加载一个机械臂。robot=robot=SimRobot(p)运行以下代码可以加载一个实验场景。实验场景中包含了一个桌子和桌子上的一个方形物料。robot.darw_cubes用于绘制方形物料，cube_xyz_list中存储一个物料位置的列表。##绘制一个环境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可控制机械臂的各个关节运动到指定角度，该函数的输入参数tmp_target_pose是个六维数组，表示六个关节的弧度。go_to_point可控制机械臂末端到达笛卡尔空间中的某位置，该函数的输入参数target_pos是一个三维数组，表示末端在三维空间中的坐标值。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)作业：请在“显示基本场景.py”基础上添加上述三条指令，尝试控制机械臂。3.1.3获取彩色图和深度图，并检测目标采用函数set_unfixed_camera_pose将相机位置设定到相机所在的位置。用函数get_pointcloud通过相机获取图像数据，其中rgbImg为彩色图，depthImg为深度图。robot.set_unfixed_camera_robot.set_unfixed_camera_pose(p)rgbImg,depthImg,intrinsic=robot.get_pointcloud(p)图2由彩色图和深度图构成点云作业：（附加题）请根据彩色图、深度图，采用视觉检测方法检测出方块及方块在相机坐标系下的位置。3.1.4目标坐标转换并抓取一个目标在上一步中，算法检测出了方块在相机坐标系下的坐标值。但机械臂控制需要在机械臂基座标系下进行。因此，需要先进行坐标转换再进行抓取。运行“坐标变换并抓取一个物体.py”即可实现一个方块抓取的案例。图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，该数值通过目标检测得到。将该坐标值带入函数trans_point_from_camera_to_base中进行坐标系转换，可得到目标点在机械臂基座标系下的坐标值point_list_in_robot_base。取出坐标值，用pick_one_object函数控制机械臂运动到方块位置，实现抓取。作业：（附加题）请为物料设置一定新的初始位置，并进行识别。识别后进行坐标系转换并抓取目标。3.2智能算法解决机械臂路径规划问题在智能制造工厂中，机械臂在分拣，搬运，焊接，加工等环节扮演重要作用。其中，分拣环节要求机械臂抓取多个零件，而搬运环节要求机械臂携带货物无碰撞地通过狭窄空间。因此，机械臂路径规划问题分为最短路径规划问题和避障问题。3.2.1最短路径规划问题与模型构建在智能制造场景中，通常将要求机械臂抓取视野内的所有物体。为实现高效抓取，则要求在遍历所有物体的前提下规划出最短的路径。该过程与经典的旅行商问题相似，因此可以参考旅行商问题的解决办法。假设有一系列点[p1,p2,p3,…,p图结构的建立：图4图结构为方便计算，图结构需要以邻接矩阵的形式进行表示。横竖标题列中填入顶点，两个顶点作为行列索引找到单元格，将边的Cost填入表中。图5邻接矩阵图6图结构作业：请用python建立一个邻接矩阵。可参考代码件“绘制图结构.py”，从代码中找出建立邻接矩阵的代码段，并建立图6的图结构的邻接矩阵。3.2.2最短路径规划算法贪心算法是一个侧重局部最优的规划算法。贪心算法基本步骤：1.1.从问题的某个初始解出发。2.当可以向求解目标前进一步时，选择该阶段的局部最优策略。3.当到达新位置时，继续寻找局部最优解，以此类推，直至终点。4.将所有的部分解综合起来，得到问题的最终解。运行“用贪心算法解决基于图连接的旅行商问题.py”，代码基于图四中的图结构，设计了一条从A点出发，遍历所有点的最短路径。作业：请分解出代码中的重要代码段：建立邻接矩阵的代码段；贪心算法的代码段。并绘制代码运行的基本流程。退火算法（SimulatedAnnealing）是一种全局优化算法，用于在搜索空间中找到最优或近似最优解。它通过模拟固体退火的过程来搜索解空间，并在搜索过程中允许一定程度的不稳定性和随机性。退火算法基本步骤：1.1.初始解和初始温度：从搜索空间中随机生成一个初始解，并设置一个初始的搜索温度（初始参数T）。2.迭代更新：在每次迭代中，根据当前解进行邻域搜索，生成一个临近解，计算当前解与临近解的目标函数值之差（ΔE），如果ΔE为负，则接受临近解作为新的解；如果ΔE为正，则以一定概率接受该临近解。3.温度调度：在每次迭代中，逐步降低搜索温度，降低接受劣解的概率，使算法更倾向于接受更好的解。4.停止条件：当达到设定的停止条件（如迭代次数达到上限、温度降至阈值等）时，算法结束，返回得到的最优解或接近最优解。用于表示退火算法的SASolution类，具体内容如下：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#降温系数 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 #更新降温系数 a=a*a print("sa总距离：",distance) returnrouter作业：请参考“用贪心算法解决基于图连接的旅行商问题.py”，将其中的贪心算法替换为退火算法。3.2.3避障路径规划问题与模型构建在上一节中，采用图描述路径规划问题，在图中某个顶点A与其他几个顶点相连，表示从顶点A出发移动一次时，只能移动到这几个相连的顶点上。而在分拣场景中，当没有障碍物时，空间是连续的，从A点出发可以到达空间中任意点。用图的概念描述，则可以看作顶点之间两两相连。AA=(0,1)B=(1,3)C=(4,3)D=(5,1)作业：假设有四个点，作为四个顶点，顶点之间两两连接，连接两顶点的边的参数为两顶点之间的距离。请绘制图结构以及其邻接矩阵。为方便计算，往往将运动空间划分为网格。每个单元格可以移动到相邻的单元格中。将每个单元格视为一个顶点，并将顶点与相邻顶点相连，则可绘制出一个局部图。图7网格化地图运行代码“在网格空间中添加障碍物.py”可以获得图7中含障碍物的网格化地图。图8含障碍物的网格化地图在有障碍物的情况下，若顶点相邻的点为障碍物，则两个点之间不形成连接关系。为每个网格都绘制一个局部图，则可以获得一张图结构G=(V,E)。可以采用基于图的路径规划算法来进行避障路径优化。作业：上一章中目标检测出的方形物料的位置为cube_xyz_list，由于物料在同一高度上，因此可以将规划问题视为二维空间中的规划。请参考“在网格空间中添加障碍物.py”为将物料视为障碍物，并绘制网格地图。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避障路径规划算法A*算法是一种很常用的路径查找和图形遍历算法。它有较好的性能和准确度。其算法原理及伪代码如下：初始化初始化open_set和close_set；*将起点加入open_set中，并设置优先级为0（优先级最高）；*如果open_set不为空，则从open_set中选取优先级最高的节点n：*如果节点n为终点，则：*从终点开始逐步追踪parent节点，一直达到起点；*返回找到的结果路径，算法结束；*如果节点n不是终点，则：*将节点n从open_set中删除，并加入close_set中；*遍历节点n所有的邻近节点：*如果邻近节点m在close_set中，则：*跳过，选取下一个邻近节点*如果邻近节点m也不在open_set中，则：*设置节点m的parent为节点n*计算节点m的优先级*将节点m加入open_set中运行文件“A星算法.py”，可以用A星算法实现起始点到重点之间的路径规划。图9用A星算法实现路径规划作业：请参考“A星算法.py”为上一个作业中绘制出的地图中设置起始点和终点，并实现路径规划。3.5视觉识别+路径规划算法控制机械臂在实际工况下，路径规划规划结果用于提升机械臂抓取或避障的效率和成功率。在本实验中，将视觉识别、路径规划和机械臂控制组成一个完整的流程。在仿真环境下实现机械臂的视觉伺服控制。3.5.1基本流程视觉识别用于感知作业场景，感知结果为一系列目标点或障碍物点。依据这些目标点和障碍物点，可以构建路径规划问题。以图或网格地图的形式描述路径规划问题，进一步地，将图或网格地图以计算机数据的形式表达，并采用路径规划算法，实现路径规划。最终用路径规划结果指导机械臂的运动。完整运行框架参考文件“机械臂视觉伺服框架.py”，该框架中的视觉识别和路径规划算法需自行添加。 图10视觉+路径规划+机械臂控制流程3.5.2机械臂类的用法为方便开发，将机械臂及机械场景写成一个类。其主要功能包含了场景构建、机械臂控制和图像获取。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代码框架避障路径规划的参考代码如下：ffromSimRobotimportSimRobotimporttimeimportpybullet_dataimportpybulletaspfromgrid_obstacle_planimportAStar,Map_from_pointsphysicsClient=p.connect(p.GUI)#orp.DIRECTfornon-graphicalversionp.setGravity(0,0,-9.81)#设置重力p.resetDebugVisualizerCamera(cameraDistance=2,cameraYaw=0,cameraPitch=-40,cameraTargetPosition=[0.5,-0.9,0.5])#设定全局相机视角robot=SimRobot(p)#创建机械臂#绘制一个环境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)#将相机设置到对应位置 rgbImg,depthImg,intrinsic=robot.get_pointcloud(p)#获取图像数据object_center_list=detect(rgbImg,depthImg)#检测出目标在相机坐标系下的位置base_point_list=robot.trans_point_from_camera_to_base(object_center_list)#坐标转换获取目标在机械臂基座标系下的位置map_tool=Map_from_points(ba