工业场景视觉测距实战：AGV 导航与避障-洪萨配资

工业场景视觉测距实战：AGV 导航与避障

1. AGV 视觉导航需求

AGV 视觉导航功能： ├── 定位：视觉 SLAM / 二维码定位 ├── 测距：障碍物距离检测 ├── 避障：实时障碍物规避 ├── 导航：路径规划与跟踪 └── 装载：货物对接与装卸

2. 障碍物距离检测

#!/usr/bin/env python3"""agv_obstacle.py - AGV 障碍物检测"""importcv2importnumpyasnpfromultralyticsimportYOLOclassAGVObstacleDetector:"""AGV 障碍物检测器"""def__init__(self,model_path,camera_matrix,safety_distance=2.0):self.model=YOLO(model_path)self.camera_matrix=camera_matrix self.safety_distance=safety_distance# 安全距离（米）self.fx=camera_matrix[0,0]self.fy=camera_matrix[1,1]# 已知物体尺寸self.known_sizes={'person':1.7,'forklift':2.0,'pallet':0.15,'box':0.4,'obstacle':1.0,}defdetect_obstacles(self,image):"""检测障碍物并估算距离"""results=self.model.predict(image,conf=0.3,verbose=False)obstacles=[]forrinresults:forboxinr.boxes:cls_name=self.model.names[int(box.cls)]bbox=box.xyxy[0].tolist()conf=float(box.conf)# 估算距离distance=self._estimate_distance(bbox,cls_name)# 判断是否在安全距离内is_danger=distance<self.safety_distance obstacles.append({'class':cls_name,'bbox':bbox,'confidence':conf,'distance':distance,'is_danger':is_danger,})returnobstaclesdef_estimate_distance(self,bbox,cls_name):"""估算距离"""x1,y1,x2,y2=bbox pixel_height=y2-y1ifcls_nameinself.known_sizes:real_height=self.known_sizes[cls_name]distance=(real_height*self.fy)/pixel_heightelse:# 默认估算distance=500/pixel_heightreturndistancedefget_navigation_command(self,obstacles):"""根据障碍物生成导航命令"""# 检查前方障碍物front_obstacles=[oforoinobstaclesifo['is_danger']]ifnotfront_obstacles:return{'action':'forward','speed':'normal'}# 计算障碍物方向forobsinfront_obstacles:cx=(obs['bbox'][0]+obs['bbox'][2])/2img_center=self.camera_matrix[0,2]ifcx<img_center-100:return{'action':'turn_right','reason':obs['class']}elifcx>img_center+100:return{'action':'turn_left','reason':obs['class']}else:return{'action':'stop','reason':obs['class']}return{'action':'stop','reason':'obstacle'}if__name__=="__main__":calib=np.load("calibration.npz")detector=AGVObstacleDetector("yolo26n.pt",calib["camera_matrix"])cap=cv2.VideoCapture(0)whileTrue:ret,frame=cap.read()ifnotret:breakobstacles=detector.detect_obstacles(frame)command=detector.get_navigation_command(obstacles)# 绘制forobsinobstacles:x1,y1,x2,y2=map(int,obs['bbox'])color=(0,0,255)ifobs['is_danger']else(0,255,0)cv2.rectangle(frame,(x1,y1),(x2,y2),color,2)label=f"{obs['class']}{obs['distance']:.1f}m"cv2.putText(frame,label,(x1,y1-10),cv2.FONT_HERSHEY_SIMPLEX,0.6,color,2)# 显示导航命令cv2.putText(frame,f"CMD:{command['action']}",(10,30),cv2.FONT_HERSHEY_SIMPLEX,1,(0,255,255),2)cv2.imshow("AGV",frame)ifcv2.waitKey(1)&0xFF==ord('q'):breakcap.release()cv2.destroyAllWindows()

3. 深度相机避障

#!/usr/bin/env python3"""depth_avoidance.py - 深度相机避障"""importcv2importnumpyasnpimportpyrealsense2asrsclassDepthAvoidance:"""深度相机避障"""def__init__(self,safety_distance=1.0):self.pipeline=rs.pipeline()config=rs.config()config.enable_stream(rs.stream.depth,640,480,rs.format.z16,30)self.pipeline.start(config)self.safety_distance=safety_distance*1000# m → mmdefcheck_obstacles(self):"""检查障碍物"""frames=self.pipeline.wait_for_frames()depth_frame=frames.get_depth_frame()depth=np.asanyarray(depth_frame.get_data())# 分区域检查h,w=depth.shape regions={'left':depth[:,:w//3],'center':depth[:,w//3:2*w//3],'right':depth[:,2*w//3:],}obstacles={}forname,regioninregions.items():# 忽略 0 值（无效深度）valid=region[region>0]iflen(valid)>0:min_distance=np.percentile(valid,5)# 5% 分位数obstacles[name]={'min_distance':min_distance/1000,# mm → m'is_safe':min_distance>self.safety_distance,}returnobstacles,depthdefget_avoidance_command(self,obstacles):"""避障命令"""left_safe=obstacles.get('left',{}).get('is_safe',True)center_safe=obstacles.get('center',{}).get('is_safe',True)right_safe=obstacles.get('right',{}).get('is_safe',True)ifcenter_safe:return{'action':'forward'}elifleft_safe:return{'action':'turn_left'}elifright_safe:return{'action':'turn_right'}else:return{'action':'stop'}if__name__=="__main__":avoidance=DepthAvoidance(safety_distance=1.0)whileTrue:obstacles,depth=avoidance.check_obstacles()command=avoidance.get_avoidance_command(obstacles)print(f"障碍物:{obstacles}")print(f"命令:{command}")

4. 货物对接测距

#!/usr/bin/env python3"""docking.py - 货物对接"""importcv2importnumpyasnpclassCargoDocking:"""货物对接"""def__init__(self,camera_matrix):self.fx=camera_matrix[0,0]self.fy=camera_matrix[1,1]self.cx=camera_matrix[0,2]self.cy=camera_matrix[1,2]defdetect_docking_target(self,image,marker_size=0.1):"""检测对接目标（ArUco 标记）"""aruco_dict=cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)detector=cv2.aruco.ArucoDetector(aruco_dict)corners,ids,_=detector.detectMarkers(image)targets=[]ifidsisnotNone:fori,(corner,marker_id)inenumerate(zip(corners,ids.flatten())):# 估计位姿rvec,tvec,_=cv2.aruco.estimatePoseSingleMarkers(corner,marker_size,np.eye(3),np.zeros(5))distance=np.linalg.norm(tvec[0][0])x_offset=tvec[0][0][0]y_offset=tvec[0][0][1]targets.append({'id':marker_id,'distance':distance,'x_offset':x_offset,'y_offset':y_offset,'rvec':rvec[0][0],'tvec':tvec[0][0],})returntargetsdefget_docking_command(self,target):"""生成对接命令"""iftargetisNone:return{'action':'search'}distance=target['distance']x_offset=target['x_offset']# 对接精度distance_threshold=0.05# 5cmangle_threshold=0.05# ~3°ifdistance<distance_thresholdandabs(x_offset)<angle_threshold:return{'action':'dock','status':'aligned'}elifdistance<distance_threshold:return{'action':'adjust_lateral','offset':x_offset}elifabs(x_offset)>angle_threshold:return{'action':'rotate','angle':x_offset}else:return{'action':'approach','distance':distance}if__name__=="__main__":calib=np.load("calibration.npz")docking=CargoDocking(calib["camera_matrix"])cap=cv2.VideoCapture(0)whileTrue:ret,frame=cap.read()ifnotret:breaktargets=docking.detect_docking_target(frame)iftargets:command=docking.get_docking_command(targets[0])print(f"目标:{targets[0]['id']}, 距离:{targets[0]['distance']:.3f}m")print(f"命令:{command}")cv2.imshow("Docking",frame)ifcv2.waitKey(1)&0xFF==ord('q'):breakcap.release()cv2.destroyAllWindows()