如何在 ROS 中实现基于视觉的 PID 自动循迹？

在 **ROS（Robot Operating System）** 中实现基于视觉的 PID 自动循迹，需要将以下几个模块整合： 1. **摄像头图像获取**（通过 `usb_cam` 或机器人自带的摄像头节点） 2. **图像处理**（使用 OpenCV 识别路径中心） 3. **偏移量计算** 4. **PID 控制器** 5. **发布控制指令**（如速度和转向角，通常通过 `/cmd_vel` 发布 `Twist` 消息） --- ## 一、ROS 实现架构图（逻辑结构） ``` [摄像头图像] → [图像处理节点（OpenCV）] → [计算偏移量] → [PID控制器] → [发布/cmd_vel] ``` --- ## 二、ROS 节点实现（Python + OpenCV + PID） ### 1. 创建 ROS 包（如果尚未创建） ```bash cd ~/catkin_ws/src catkin_create_pkg vision_pid rospy cv_bridge sensor_msgs geometry_msgs cd .. catkin_make source devel/setup.bash ``` ### 2. Python 节点代码（`vision_pid_node.py`） ```python #!/usr/bin/env python import rospy import cv2 import numpy as np from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError from geometry_msgs.msg import Twist class PIDController: def __init__(self, Kp, Ki, Kd): self.Kp = Kp self.Ki = Ki self.Kd = Kd self.last_error = 0 self.integral = 0 def compute(self, error, dt=0.01): self.integral += error * dt derivative = (error - self.last_error) / dt output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative self.last_error = error return output class VisionPIDController: def __init__(self): rospy.init_node('vision_pid_controller', anonymous=True) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback) self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) self.pid = PIDController(Kp=0.08, Ki=0.001, Kd=0.03) self.setpoint = 0 # 图像中心为0偏移 self.image_center = 320 # 假设图像宽度为640 def image_callback(self, data): try: frame = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) return # 简单的图像处理：识别路径中心 deviation = self.detect_path_center(frame) # 计算PID输出 control_signal = self.pid.compute(deviation) # 构造控制命令 cmd = Twist() cmd.linear.x = 0.2 # 固定前进速度 cmd.angular.z = -float(control_signal) / 100 # 控制转向 self.cmd_pub.publish(cmd) # 可视化 height, width, _ = frame.shape center_x = width // 2 detected_x = center_x + int(deviation) cv2.line(frame, (center_x, height - 50), (center_x, height), (0, 255, 0), 2) cv2.line(frame, (detected_x, height - 50), (detected_x, height), (0, 0, 255), 2) cv2.imshow("Vision PID", frame) cv2.waitKey(1) def detect_path_center(self, frame): # 简单的路径识别：灰度 + 阈值 + 找中心 gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) _, binary = cv2.threshold(gray, 200, 255, cv2.THRESH_BINARY_INV) contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) if contours: c = max(contours, key=cv2.contourArea) M = cv2.moments(c) if M["m00"] != 0: cx = int(M["m10"] / M["m00"]) return cx - self.image_center # 偏移量 return 0 # 未识别到路径时返回0偏移 if __name__ == '__main__': try: VisionPIDController() rospy.spin() except rospy.ROSInterruptException: pass ``` --- ## 三、代码说明 ### 1. `PIDController` 类 - 标准的 PID 控制逻辑 - 根据偏移量计算转向控制信号 ### 2. `VisionPIDController` 类 - 使用 `cv_bridge` 将 ROS 图像消息转换为 OpenCV 图像 - 使用简单的图像处理方法识别路径中心（白色线条） - 计算偏移量并传入 PID 控制器 - 输出控制信号通过 `/cmd_vel` 发布 `Twist` 消息控制机器人运动 ### 3. `detect_path_center()` 函数 - 灰度化、二值化、找最大轮廓、计算质心 - 返回与图像中心的偏移量 --- ## 四、运行流程 1. 启动摄像头（例如使用 `usb_cam`）： ```bash roslaunch usb_cam usb_cam.launch ``` 2. 启动图像显示（可选）： ```bash rqt_image_view ``` 3. 运行你的节点： ```bash rosrun vision_pid vision_pid_node.py ``` --- ## 五、优化建议 - 使用 **颜色识别（HSV）** 提高路径识别精度 - 添加 **ROI（Region of Interest）** 区域裁剪 - 使用 **Canny边缘检测 + 霍夫变换** 替代简单轮廓识别 - 在 ROS 中使用 **`dynamic_reconfigure`** 实时调节 PID 参数 - 使用 **ROS 参数服务器** 保存 PID 参数 --- ##