工业场景视觉测距实战AGV 导航与避障1. AGV 视觉导航需求AGV 视觉导航功能 ├── 定位视觉 SLAM / 二维码定位 ├── 测距障碍物距离检测 ├── 避障实时障碍物规避 ├── 导航路径规划与跟踪 └── 装载货物对接与装卸2. 障碍物距离检测#!/usr/bin/env python3agv_obstacle.py - AGV 障碍物检测importcv2importnumpyasnpfromultralyticsimportYOLOclassAGVObstacleDetector:AGV 障碍物检测器def__init__(self,model_path,camera_matrix,safety_distance2.0):self.modelYOLO(model_path)self.camera_matrixcamera_matrix self.safety_distancesafety_distance# 安全距离米self.fxcamera_matrix[0,0]self.fycamera_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):检测障碍物并估算距离resultsself.model.predict(image,conf0.3,verboseFalse)obstacles[]forrinresults:forboxinr.boxes:cls_nameself.model.names[int(box.cls)]bboxbox.xyxy[0].tolist()conffloat(box.conf)# 估算距离distanceself._estimate_distance(bbox,cls_name)# 判断是否在安全距离内is_dangerdistanceself.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,y2bbox pixel_heighty2-y1ifcls_nameinself.known_sizes:real_heightself.known_sizes[cls_name]distance(real_height*self.fy)/pixel_heightelse:# 默认估算distance500/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_centerself.camera_matrix[0,2]ifcximg_center-100:return{action:turn_right,reason:obs[class]}elifcximg_center100:return{action:turn_left,reason:obs[class]}else:return{action:stop,reason:obs[class]}return{action:stop,reason:obstacle}if__name____main__:calibnp.load(calibration.npz)detectorAGVObstacleDetector(yolo26n.pt,calib[camera_matrix])capcv2.VideoCapture(0)whileTrue:ret,framecap.read()ifnotret:breakobstaclesdetector.detect_obstacles(frame)commanddetector.get_navigation_command(obstacles)# 绘制forobsinobstacles:x1,y1,x2,y2map(int,obs[bbox])color(0,0,255)ifobs[is_danger]else(0,255,0)cv2.rectangle(frame,(x1,y1),(x2,y2),color,2)labelf{obs[class]}{obs[distance]:.1f}mcv2.putText(frame,label,(x1,y1-10),cv2.FONT_HERSHEY_SIMPLEX,0.6,color,2)# 显示导航命令cv2.putText(frame,fCMD:{command[action]},(10,30),cv2.FONT_HERSHEY_SIMPLEX,1,(0,255,255),2)cv2.imshow(AGV,frame)ifcv2.waitKey(1)0xFFord(q):breakcap.release()cv2.destroyAllWindows()3. 深度相机避障#!/usr/bin/env python3depth_avoidance.py - 深度相机避障importcv2importnumpyasnpimportpyrealsense2asrsclassDepthAvoidance:深度相机避障def__init__(self,safety_distance1.0):self.pipeliners.pipeline()configrs.config()config.enable_stream(rs.stream.depth,640,480,rs.format.z16,30)self.pipeline.start(config)self.safety_distancesafety_distance*1000# m → mmdefcheck_obstacles(self):检查障碍物framesself.pipeline.wait_for_frames()depth_frameframes.get_depth_frame()depthnp.asanyarray(depth_frame.get_data())# 分区域检查h,wdepth.shape regions{left:depth[:,:w//3],center:depth[:,w//3:2*w//3],right:depth[:,2*w//3:],}obstacles{}forname,regioninregions.items():# 忽略 0 值无效深度validregion[region0]iflen(valid)0:min_distancenp.percentile(valid,5)# 5% 分位数obstacles[name]{min_distance:min_distance/1000,# mm → mis_safe:min_distanceself.safety_distance,}returnobstacles,depthdefget_avoidance_command(self,obstacles):避障命令left_safeobstacles.get(left,{}).get(is_safe,True)center_safeobstacles.get(center,{}).get(is_safe,True)right_safeobstacles.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__:avoidanceDepthAvoidance(safety_distance1.0)whileTrue:obstacles,depthavoidance.check_obstacles()commandavoidance.get_avoidance_command(obstacles)print(f障碍物:{obstacles})print(f命令:{command})4. 货物对接测距#!/usr/bin/env python3docking.py - 货物对接importcv2importnumpyasnpclassCargoDocking:货物对接def__init__(self,camera_matrix):self.fxcamera_matrix[0,0]self.fycamera_matrix[1,1]self.cxcamera_matrix[0,2]self.cycamera_matrix[1,2]defdetect_docking_target(self,image,marker_size0.1):检测对接目标ArUco 标记aruco_dictcv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)detectorcv2.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))distancenp.linalg.norm(tvec[0][0])x_offsettvec[0][0][0]y_offsettvec[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}distancetarget[distance]x_offsettarget[x_offset]# 对接精度distance_threshold0.05# 5cmangle_threshold0.05# ~3°ifdistancedistance_thresholdandabs(x_offset)angle_threshold:return{action:dock,status:aligned}elifdistancedistance_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__:calibnp.load(calibration.npz)dockingCargoDocking(calib[camera_matrix])capcv2.VideoCapture(0)whileTrue:ret,framecap.read()ifnotret:breaktargetsdocking.detect_docking_target(frame)iftargets:commanddocking.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)0xFFord(q):breakcap.release()cv2.destroyAllWindows()总结功能传感器精度响应时间障碍物检测单目YOLO±10%30ms避障深度相机±2%30ms货物对接ArUco深度±1cm50ms路径跟踪视觉 SLAM±5cm100ms