这个代码没有做障碍区分都默认是person做简单的b样条规划简单前方靠中避障路线大致效果如下实现功能清单深度阈值分割生成可通行区域 Mask筛选深度在 400~2500 mm 的像素作为可行区域。中值滤波 形态学开闭运算消除深度图噪声、填充空洞得到干净二值 Mask。多行横向采样提取可行区域中心点控制点 raw_points在 ROI 区域纵向均匀取 9 条水平线每行扫描 Mask获取可行区域 x 坐标取均值作为该行道路中心点蓝色圆点标记控制点。B 样条插值生成连续平滑路径核心升级对控制点按 y 坐标从小到大排序去重 y 值防止插值报错make_interp_spline(k3)3 次 B 样条插值生成高密度平滑曲线点相比多项式拟合不会发生龙格震荡弯道曲线更自然。多帧时序滑动窗口滤波路径时域平滑缓存最近 3 帧 B 样条路径同索引路径点 x 坐标取平均抑制单帧抖动深度噪声、相机抖动。YOLOv8 目标检测 深度图测距YOLO 识别指定类别障碍物人、小车、摩托车、巴士、卡车取障碍物框底部中心点在深度图取 4×4 窗口求平均深度抗单点深度噪声在图像上绘制障碍物框、类别 距离文字。障碍物挡路判定计算障碍物像素点到 B 样条规划路径上所有点的最短欧式像素距离双条件触发危险告警障碍物到中心线像素距离35 像素障碍物真实距离1200mm1.2 米满足则标记红色圆点显示DANGER OBSTACLE!告警。横向偏差计算给 PID 循迹控制器smooth_curve[-1]离机器人最近的路径点计算相对图像中心的横向误差error_nearerror_mid路径中段前瞻误差用于预转向可视化调试RoadB-SplineYOLODepth 窗口RGB 图叠加控制点、红色 B 样条路径、障碍物框、告警文字Mask 窗口深度可行区域掩码方便调深度阈值。核心技术深度相机空间可行域分割基于深度值阈值划分可通行空间不依赖路面颜色纹理光照鲁棒缺点只能识别高度落差同高度物体无法被 mask 过滤必须依靠 YOLO 识别。行扫描质心提取控制点轻量经典算法横向取可行区域质心作为 B 样条插值控制点算力开销低。3 次 B 样条插值make_interp_splineB 样条属于分段插值局部性好一个控制点只影响附近一小段曲线不会全局震荡k3 三阶样条曲线二阶连续曲率平滑更适合机器人路径前置做unique_y去重避免 y 坐标重复导致插值报错。时序滑动窗口滤波对多帧规划路径做同位置点平均抑制单帧噪声带来路径跳变属于低通滤波。RGB 目标检测 深度图融合测距YOLO 做物体分类深度图获取物体真实物理距离在图像像素平面判断障碍物与规划路径的空间关系。ROS2 cv_bridge 图像消息收发OpenCV 可视化。代码目的实现无地图局部视觉循迹 障碍物预警用于小型移动机器人你的爬壁机器人 / 巡检小车局部导航实时提取前方可通行区域中心线输出横向偏差作为底盘 PID 控制器输入使机器人沿着道路中心行驶识别前方障碍物测量距离判断障碍物是否阻挡规划路径输出危险告警上层逻辑可做减速、停车、绕行决策提供可视化调试界面方便调参深度范围、采样层数、安全距离、像素阈值。✅ 优势对比多项式拟合版本B 样条分段插值不存在龙格现象大弯道不会曲线畸变、乱飞曲线平滑连续曲率变化平缓机器人转向更稳定控制点局部影响曲线个别控制点异常不会破坏整条路径。⚠️ 现存局限报告可直接写可行区域仅靠深度阈值同高度障碍物不会被 mask 剔除完全依赖 YOLO 识别障碍物判定是图像平面像素距离不是相机三维坐标系距离远距离场景像素距离不能等价于真实空间距离会产生误判输出的error_near是图像像素误差没有利用相机内参转换成机器人坐标系下真实米级横向偏移不能直接用于高精度控制ROI 固定比例截取图像机器人俯仰角变化时ROI 区域会失效没有发布 ROS 话题偏差、障碍物标志、路径仅可视化需要额外写 publisher 对接底盘 /rviz依赖scipy库部署环境需要额外安装YOLO 权重需要手动下载国内网络无法自动拉取障碍物搜索最短距离是暴力遍历路径点路径点多时会轻微增加耗时80 个点影响很小。技术路线RGB-D 相机获取 RGB 图像与深度图 → 深度阈值分割生成可通行区域 Mask → 形态学滤波优化 Mask → 多行扫描提取可行区域质心作为 B 样条控制点 → 控制点排序与去重 → 三阶 B 样条插值生成连续平滑路径 → 多帧滑动窗口时域平滑路径 → 计算横向循迹偏差同时 YOLOv8 检测障碍物结合深度图获取障碍物真实距离计算障碍物到规划路径的像素距离判定是否存在危险障碍物实现视觉循迹与障碍物预警。可选扩展方向增加 ROS 发布器发布Float32横向偏差、Bool障碍物告警、nav_msgs/Path路径给 RViz 可视化引入相机内参把图像像素坐标转为相机坐标系下三维坐标得到真实米级横向误差障碍物距离判断改用三维空间距离替代二维像素距离消除远近距离带来的误判增加路径曲率输出用于前瞻转向控制增加异常保护控制点过少、样条插值失败的异常捕获防止节点崩溃。路径生成相关参数核心优先调试1.min_depth最小深度单位mm作用小于该距离的像素直接判定不可通行过滤离相机太近的区域相机近处噪声、机器人本体影响太大 → 近处可行区域被切掉太小 → 引入近距离噪声、机器人本体干扰默认400推荐范围300 ~ 600调参建议 深度相机最小测距是多少就设略大于该值奥比中光一般 300mm 起可设 350。2.max_depth最大深度单位mm作用超过该距离的像素不纳入可行区域限定机器人前瞻视野默认25002.5m推荐范围1500 ~ 3000调参建议机器人速度慢、小场景1500~2000想要更远前瞻2500~3000不要过大远距离深度噪声会变多mask 会破碎控制点抖动。3.roi_top_ratioROI 顶部截取比例作用图像上方切掉多少比例只保留下方 ROI 区域做路径计算去掉远处天空、墙面无效区域默认0.4图片上方 40% 丢弃只用下方 60%推荐范围0.2 ~ 0.5调参建议相机朝下安装爬壁机器人贴近壁面增大到 0.4~0.5相机平视减小到 0.2~0.3太大ROI 区域太小前瞻距离变短太小引入上方无效背景mask 噪声变多。4.layer_num纵向采样层数控制点数量作用在 ROI 内取多少条水平线提取道路中心点即 B 样条控制点数量默认9推荐范围5 ~ 13调参建议弯道多适当增加到 11/13曲线拟合更贴合弯道算力紧张、直道场景降到 5/7减少计算不要过大控制点过多会放大噪声过少曲线拟合能力不足弯道失真。5.min_pixel_per_line单行最小可行像素数量作用一行 mask 中可行像素少于这个数就丢弃这一行不生成控制点过滤碎片化噪声默认30推荐范围15 ~ 50调参建议mask 噪声大调大40~50过滤碎点通道窄、可行区域窄调小15~20防止有效行被丢弃时序平滑滤波参数6.smooth_window滑动窗口帧数作用缓存最近 N 帧路径做帧间平均抑制路径抖动默认3推荐范围1 ~ 5调参建议相机抖动大、深度噪声大调到 4~5平滑更强但会引入滞后弯道响应变慢机器人高速、需要快速响应弯道设为 1关闭帧间平滑或 2权衡平滑效果 ↔ 响应延迟。障碍物判定参数安全相关第二优先级调试7.safe_distance_mm安全距离阈值作用障碍物小于该物理距离才判定为危险障碍物默认12001.2m推荐范围800 ~ 1500调参建议低速巡检机器人1000~1200需要提前减速、反应慢1200~1500空间狭小场景800~10008.path_pixel_thresh障碍物到路径像素距离阈值作用障碍物像素点与红色中心线的像素距离小于该值判定障碍物在路径上默认35推荐范围20 ~ 50⚠️ 注意这是二维图像像素距离不是真实三维距离。调参建议图像分辨率高可适当放大容易误报减小到 20~25容易漏检挡路障碍物增大到 40~509.obstacle_classesYOLO 检测类别列表self.obstacle_classes [0, 2, 3, 5, 7] # person, car, motorcycle, bus, truck作用只对指定类别做障碍物判断忽略其余物体调试室内场景保留0人删掉车辆类别2,3,5,7室外通道全部保留增加类别查看 YOLO 类别 id比如增加1自行车10. YOLO 模型权重选择yolov8n.ptnano速度最快精度最低当前代码使用yolov8s.ptsmall精度更高算力消耗更大笔记本开发n 足够如果障碍物漏检严重可以换成 s 版本。mask cv2.medianBlur(mask, 5) kernel np.ones((5, 5), np.uint8) mask cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) mask cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)中值滤波核5、形态学核5×5若 mask 孔洞多增大核到 7若可行区域被腐蚀断裂减小核到 3这部分代码没有做成参数需要手动改代码。推荐调参顺序实操流程先调深度参数min_depth / max_depth观察 Mask 窗口保证可行区域完整、噪点少调整roi_top_ratio保证 ROI 刚好框住需要前瞻的区域调layer_num、min_pixel_per_line观察蓝色控制点是否连续、无杂点调试smooth_window平衡路径抖动和响应速度最后调障碍物参数safe_distance_mm、path_pixel_thresh消除误报 / 漏报。代码如下可以直接copy使用后续调参自己解决就行我的环境太复杂调的参数并不好用可以参照上面推荐调参步骤自行尝试然后只是简单学习记录不喜勿喷谢谢!import rclpy from rclpy.node import Node from sensor_msgs.msg import Image from cv_bridge import CvBridge import cv2 import numpy as np from scipy.interpolate import make_interp_spline from ultralytics import YOLO class RoadCurveNode(Node): def __init__(self): super().__init__(road_curve_yolo_depth_node) self.bridge CvBridge() self.sub_color self.create_subscription( Image, /camera/color/image_raw, self.color_callback, 10 ) self.sub_depth self.create_subscription( Image, /camera/depth/image_raw, self.depth_callback, 10 ) self.rgb_img None self.depth_img None # 参数 self.min_depth 400 self.max_depth 2500 self.roi_top_ratio 0.4 self.layer_num 9 self.min_pixel_per_line 30 self.smooth_window 3 self.path_buffer [] # YOLO障碍物检测 self.yolo_model YOLO(yolov8n.pt) self.obstacle_classes [0, 2, 3, 5, 7] # person, car, motorcycle, bus, truck self.safe_distance_mm 1200 # 安全距离阈值1.2m self.path_pixel_thresh 35 # 障碍物离中心线小于35像素判定挡路 def color_callback(self, msg): self.rgb_img self.bridge.imgmsg_to_cv2(msg, bgr8) def depth_callback(self, msg): if self.rgb_img is None: return self.depth_img self.bridge.imgmsg_to_cv2(msg, 16UC1) h, w self.depth_img.shape[:2] roi_top int(h * self.roi_top_ratio) roi_depth self.depth_img[roi_top:h, :] roi_h, roi_w roi_depth.shape[:2] # 深度可行区域mask mask np.zeros_like(roi_depth, dtypenp.uint8) valid (roi_depth self.min_depth) (roi_depth self.max_depth) mask[valid] 255 mask cv2.medianBlur(mask, 5) kernel np.ones((5, 5), np.uint8) mask cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) mask cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) viz_img cv2.resize(self.rgb_img, (roi_w, roi_h)) # YOLO障碍物检测 获取障碍物深度 results self.yolo_model(viz_img, verboseFalse) danger_obstacle False obstacle_info_list [] for box in results[0].boxes: cls int(box.cls.item()) if cls not in self.obstacle_classes: continue x1,y1,x2,y2 map(int, box.xyxy[0]) # 取框底部中心点靠近地面 obs_cx int((x1 x2)/2) obs_cy int(y2) # 边界保护防止越界 obs_cx np.clip(obs_cx, 0, roi_w-1) obs_cy np.clip(obs_cy, 0, roi_h-1) # 取周围小区域平均深度避免单点噪声 win 4 x_start max(0, obs_cx-win) x_end min(roi_w, obs_cxwin) y_start max(0, obs_cy-win) y_end min(roi_h, obs_cywin) depth_patch roi_depth[y_start:y_end, x_start:x_end] valid_depth_vals depth_patch[depth_patch ! 0] avg_depth_mm 0 if len(valid_depth_vals) 0: avg_depth_mm np.mean(valid_depth_vals) class_name results[0].names[cls] obstacle_info_list.append({cx:obs_cx, cy:obs_cy, depth:avg_depth_mm, cls:class_name, box:[x1,y1,x2,y2]}) # 绘制障碍物框与距离文字 cv2.rectangle(viz_img, (x1,y1), (x2,y2), (0,255,0), 2) cv2.putText(viz_img, f{class_name} {avg_depth_mm:.0f}mm, (x1,y1-5), cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,255,0),2) # 多层采样道路中心点 raw_points [] for i in range(self.layer_num): y roi_h - 1 - int((roi_h / self.layer_num) * i) scan_row mask[y, :] x_valid np.where(scan_row 128)[0] if len(x_valid) self.min_pixel_per_line: cx float(np.mean(x_valid)) raw_points.append((cx, float(y))) cv2.circle(viz_img, (int(cx), y), 4, (255, 0, 0), -1) cv2.line(viz_img, (0, y), (roi_w, y), (80, 80, 80), 1) # B样条拟合曲线 curve_points [] if len(raw_points) 4: raw_sorted sorted(raw_points, keylambda p: p[1]) pts_arr np.array(raw_sorted) xs_ctrl pts_arr[:,0] ys_ctrl pts_arr[:,1] unique_y, idx np.unique(ys_ctrl, return_indexTrue) if len(unique_y) 4: xs_ctrl xs_ctrl[idx] ys_ctrl unique_y spl make_interp_spline(ys_ctrl, xs_ctrl, k3) y_min np.min(ys_ctrl) y_max np.max(ys_ctrl) y_sample np.linspace(y_min, y_max, num80) x_sample spl(y_sample) for x, y in zip(x_sample, y_sample): xi int(np.clip(x, 0, roi_w-1)) yi int(y) curve_points.append((xi, yi)) # 帧间滑动平均平滑 smooth_curve [] if len(curve_points) 2: self.path_buffer.append(curve_points) if len(self.path_buffer) self.smooth_window: self.path_buffer.pop(0) max_len min(len(p) for p in self.path_buffer) for idx in range(max_len): xs_buf [frame[idx][0] for frame in self.path_buffer] y_fixed curve_points[idx][1] smooth_curve.append((int(np.mean(xs_buf)), y_fixed)) # 判断障碍物是否在规划路径上 if len(smooth_curve) 2: curve_np np.array(smooth_curve, np.int32).reshape(-1,1,2) cv2.polylines(viz_img, [curve_np], isClosedFalse, color(0,0,255), thickness3) # 遍历障碍物计算障碍物到B样条曲线最近像素距离 for obs in obstacle_info_list: ox, oy obs[cx], obs[cy] d_min 9999 for (px, py) in smooth_curve: d np.sqrt((ox-px)**2 (oy-py)**2) if d d_min: d_min d # 距离小于阈值 障碍物足够近 → 危险 if d_min self.path_pixel_thresh and obs[depth]0 and obs[depth] self.safe_distance_mm: danger_obstacle True cv2.circle(viz_img, (ox, oy), 6, (0,0,255), -1) near_x smooth_curve[-1][0] mid_x smooth_curve[len(smooth_curve)//2][0] img_center_x roi_w / 2 error_near near_x - img_center_x error_mid mid_x - img_center_x cv2.putText(viz_img, fNearErr:{error_near:.1f}, (20,30), cv2.FONT_HERSHEY_SIMPLEX,0.7,(255,255,0),2) cv2.putText(viz_img, fMidErr:{error_mid:.1f}, (20,60), cv2.FONT_HERSHEY_SIMPLEX,0.7,(255,255,0),2) if danger_obstacle: cv2.putText(viz_img, DANGER OBSTACLE!, (20,90), cv2.FONT_HERSHEY_SIMPLEX,0.7,(0,0,255),2) else: cv2.putText(viz_img, No Curve Road, (20,30), cv2.FONT_HERSHEY_SIMPLEX,1,(0,0,255),2) cv2.imshow(RoadB-SplineYOLODepth, viz_img) cv2.imshow(Mask, mask) cv2.waitKey(1) def main(): rclpy.init() node RoadCurveNode() try: rclpy.spin(node) except KeyboardInterrupt: print(\nExit) node.destroy_node() rclpy.shutdown() cv2.destroyAllWindows() if __name__ __main__: main()