企业官网建设流程全解析

这个代码没有做障碍区分都默认是person，做简单的b样条规划简单前方靠中避障路线，大致效果如下：实现功能清单

1. 深度阈值分割生成可通行区域 Mask
   • 筛选深度在 400~2500 mm 的像素作为可行区域。
   • 中值滤波 + 形态学开闭运算，消除深度图噪声、填充空洞，得到干净二值 Mask。
2. 多行横向采样提取可行区域中心点（控制点 raw_points）
   • 在 ROI 区域纵向均匀取 9 条水平线；
   • 每行扫描 Mask，获取可行区域 x 坐标，取均值作为该行道路中心点；
   • 蓝色圆点标记控制点。
3. B 样条插值生成连续平滑路径（核心升级）
   • 对控制点按 y 坐标从小到大排序；
   • 去重 y 值，防止插值报错；
   • make_interp_spline(k=3)3 次 B 样条插值，生成高密度平滑曲线点；
   • 相比多项式拟合：不会发生龙格震荡，弯道曲线更自然。
4. 多帧时序滑动窗口滤波（路径时域平滑）
   • 缓存最近 3 帧 B 样条路径；同索引路径点 x 坐标取平均，抑制单帧抖动（深度噪声、相机抖动）。
5. YOLOv8 目标检测 + 深度图测距
   • YOLO 识别指定类别障碍物：人、小车、摩托车、巴士、卡车；
   • 取障碍物框底部中心点，在深度图取 4×4 窗口求平均深度，抗单点深度噪声；
   • 在图像上绘制障碍物框、类别 + 距离文字。
6. 障碍物挡路判定
   • 计算障碍物像素点到 B 样条规划路径上所有点的最短欧式像素距离；
   • 双条件触发危险告警：
     • 障碍物到中心线像素距离＜35 像素
     • 障碍物真实距离＜1200mm（1.2 米）
   • 满足则标记红色圆点，显示DANGER OBSTACLE!告警。
7. 横向偏差计算（给 PID 循迹控制器）
   • smooth_curve[-1]：离机器人最近的路径点，计算相对图像中心的横向误差error_near；
   • error_mid：路径中段前瞻误差，用于预转向；
8. 可视化调试
   • Road+B-Spline+YOLO+Depth 窗口：RGB 图叠加控制点、红色 B 样条路径、障碍物框、告警文字；
   • Mask 窗口：深度可行区域掩码，方便调深度阈值。

核心技术

1. 深度相机空间可行域分割基于深度值阈值划分可通行空间，不依赖路面颜色纹理，光照鲁棒；缺点：只能识别高度落差，同高度物体无法被 mask 过滤，必须依靠 YOLO 识别。
2. 行扫描质心提取控制点轻量经典算法，横向取可行区域质心，作为 B 样条插值控制点，算力开销低。
3. 3 次 B 样条插值（make_interp_spline）
   • B 样条属于分段插值，局部性好：一个控制点只影响附近一小段曲线，不会全局震荡；
   • k=3 三阶样条，曲线二阶连续，曲率平滑，更适合机器人路径；
   • 前置做unique_y去重，避免 y 坐标重复导致插值报错。
4. 时序滑动窗口滤波对多帧规划路径做同位置点平均，抑制单帧噪声带来路径跳变，属于低通滤波。
5. RGB 目标检测 + 深度图融合测距YOLO 做物体分类，深度图获取物体真实物理距离；在图像像素平面判断障碍物与规划路径的空间关系。
6. ROS2 + cv_bridge 图像消息收发，OpenCV 可视化。

代码目的

实现无地图局部视觉循迹 + 障碍物预警，用于小型移动机器人（你的爬壁机器人 / 巡检小车）局部导航：

1. 实时提取前方可通行区域中心线，输出横向偏差，作为底盘 PID 控制器输入，使机器人沿着道路中心行驶；
2. 识别前方障碍物，测量距离，判断障碍物是否阻挡规划路径，输出危险告警，上层逻辑可做减速、停车、绕行决策；
3. 提供可视化调试界面，方便调参（深度范围、采样层数、安全距离、像素阈值）。

✅ 优势（对比多项式拟合版本）

1. B 样条分段插值，不存在龙格现象，大弯道不会曲线畸变、乱飞；
2. 曲线平滑连续，曲率变化平缓，机器人转向更稳定；
3. 控制点局部影响曲线，个别控制点异常不会破坏整条路径。

⚠️ 现存局限（报告可直接写）

1. 可行区域仅靠深度阈值：同高度障碍物不会被 mask 剔除，完全依赖 YOLO 识别；
2. 障碍物判定是图像平面像素距离，不是相机三维坐标系距离；远距离场景，像素距离不能等价于真实空间距离，会产生误判；
3. 输出的error_near是图像像素误差，没有利用相机内参转换成机器人坐标系下真实米级横向偏移，不能直接用于高精度控制；
4. ROI 固定比例截取图像，机器人俯仰角变化时，ROI 区域会失效；
5. 没有发布 ROS 话题（偏差、障碍物标志、路径），仅可视化，需要额外写 publisher 对接底盘 /rviz；
6. 依赖scipy库，部署环境需要额外安装；YOLO 权重需要手动下载，国内网络无法自动拉取；
7. 障碍物搜索最短距离是暴力遍历路径点，路径点多时会轻微增加耗时（80 个点影响很小）。

技术路线

RGB-D 相机获取 RGB 图像与深度图 → 深度阈值分割生成可通行区域 Mask → 形态学滤波优化 Mask → 多行扫描提取可行区域质心作为 B 样条控制点 → 控制点排序与去重 → 三阶 B 样条插值生成连续平滑路径 → 多帧滑动窗口时域平滑路径 → 计算横向循迹偏差；同时 YOLOv8 检测障碍物，结合深度图获取障碍物真实距离，计算障碍物到规划路径的像素距离，判定是否存在危险障碍物，实现视觉循迹与障碍物预警。

可选扩展方向

1. 增加 ROS 发布器：发布Float32横向偏差、Bool障碍物告警、nav_msgs/Path路径给 RViz 可视化；
2. 引入相机内参，把图像像素坐标转为相机坐标系下三维坐标，得到真实米级横向误差；
3. 障碍物距离判断改用三维空间距离替代二维像素距离，消除远近距离带来的误判；
4. 增加路径曲率输出，用于前瞻转向控制；
5. 增加异常保护：控制点过少、样条插值失败的异常捕获，防止节点崩溃。

路径生成相关参数（核心，优先调试）

1.min_depth最小深度（单位：mm）

• 作用：小于该距离的像素直接判定不可通行，过滤离相机太近的区域（相机近处噪声、机器人本体）
• 影响：太大 → 近处可行区域被切掉；太小 → 引入近距离噪声、机器人本体干扰
• 默认：400
• 推荐范围：300 ~ 600
• 调参建议： 深度相机最小测距是多少就设略大于该值；奥比中光一般 300mm 起，可设 350。

2.max_depth最大深度（单位：mm）

• 作用：超过该距离的像素不纳入可行区域，限定机器人前瞻视野
• 默认：2500（2.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安全距离阈值

• 作用：障碍物小于该物理距离才判定为危险障碍物
• 默认：1200（1.2m）
• 推荐范围：800 ~ 1500
• 调参建议：
  • 低速巡检机器人：1000~1200
  • 需要提前减速、反应慢：1200~1500
  • 空间狭小场景：800~1000

8.path_pixel_thresh障碍物到路径像素距离阈值

• 作用：障碍物像素点与红色中心线的像素距离小于该值，判定障碍物在路径上
• 默认：35
• 推荐范围：20 ~ 50

⚠️ 注意：这是二维图像像素距离，不是真实三维距离。

• 调参建议：
  • 图像分辨率高：可适当放大
  • 容易误报：减小到 20~25
  • 容易漏检挡路障碍物：增大到 40~50

9.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.pt：nano，速度最快，精度最低（当前代码使用）
• yolov8s.pt：small，精度更高，算力消耗更大

笔记本开发：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

这部分代码没有做成参数，需要手动改代码。

推荐调参顺序（实操流程）

1. 先调深度参数min_depth / max_depth，观察 Mask 窗口，保证可行区域完整、噪点少；
2. 调整roi_top_ratio，保证 ROI 刚好框住需要前瞻的区域；
3. 调layer_num、min_pixel_per_line，观察蓝色控制点是否连续、无杂点；
4. 调试smooth_window，平衡路径抖动和响应速度；
5. 最后调障碍物参数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, dtype=np.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, verbose=False) 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_cx+win) y_start = max(0, obs_cy-win) y_end = min(roi_h, obs_cy+win) 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, key=lambda 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_index=True) if len(unique_y) >=4: xs_ctrl = xs_ctrl[idx] ys_ctrl = unique_y spl = make_interp_spline(ys_ctrl, xs_ctrl, k=3) y_min = np.min(ys_ctrl) y_max = np.max(ys_ctrl) y_sample = np.linspace(y_min, y_max, num=80) 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], isClosed=False, color=(0,0,255), thickness=3) # 遍历障碍物，计算障碍物到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, f"NearErr:{error_near:.1f}", (20,30), cv2.FONT_HERSHEY_SIMPLEX,0.7,(255,255,0),2) cv2.putText(viz_img, f"MidErr:{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("Road+B-Spline+YOLO+Depth", 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()