ROS2 单节点，RGB-D 深度相机局部路径提取 + B 样条平滑路径 + YOLOv8 障碍物检测 + 深度测距，用于移动机器人视觉局部循迹。

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

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

核心技术

深度相机空间可行域分割 基于深度值阈值划分可通行空间，不依赖路面颜色纹理，光照鲁棒；缺点：只能识别高度落差，同高度物体无法被 mask 过滤，必须依靠 YOLO 识别。
行扫描质心提取控制点 轻量经典算法，横向取可行区域质心，作为 B 样条插值控制点，算力开销低。
3 次 B 样条插值（make_interp_spline）
- B 样条属于分段插值，局部性好：一个控制点只影响附近一小段曲线，不会全局震荡；
- k=3 三阶样条，曲线二阶连续，曲率平滑，更适合机器人路径；
- 前置做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）

作用：超过该距离的像素不纳入可行区域，限定机器人前瞻视野
默认：2500（2.5m）
推荐范围：1500 ~ 3000
调参建议：
- 机器人速度慢、小场景：1500~2000
- 想要更远前瞻：2500~3000
不要过大：远距离深度噪声会变多，mask 会破碎，控制点抖动。

3. `roi_top_ratio` ROI 顶部截取比例

作用：图像上方切掉多少比例，只保留下方 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_classes` YOLO 检测类别列表

复制代码

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

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

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

先调深度参数 min_depth / max_depth，观察 Mask 窗口，保证可行区域完整、噪点少；
调整 roi_top_ratio，保证 ROI 刚好框住需要前瞻的区域；
调 layer_num、min_pixel_per_line，观察蓝色控制点是否连续、无杂点；
调试 smooth_window，平衡路径抖动和响应速度；
最后调障碍物参数 safe_distance_mm、path_pixel_thresh，消除误报 / 漏报。

代码如下，可以直接copy使用，后续调参自己解决就行，我的环境太复杂调的参数并不好用，可以参照上面推荐调参步骤自行尝试，然后只是简单学习记录不喜勿喷，谢谢！！!

python 复制代码

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()