龙芯2k0300 - 走马观碑组Gazebo仿真环境搭建（下）

经过前期的努力《龙芯2k0300 - 走马观碑组Gazebo仿真环境搭建（上）》，现在Gazebo仿真环境已经跑通，小车能动了，摄像头也在发布图像，下一步就是让小车能“看见”赛道并自主巡线。car_vision 功能包的核心任务就是：订阅摄像头图像 → 处理图像找到线条 → 计算中线偏差 →  控制算法→  发布角速度、线速度指令。其核心功能：
模块功能说明图像订阅订阅 /camera/image_raw 话题从 Gazebo 摄像头获取实时图像图像预处理灰度化、二值化、去噪将彩色图像转换为便于线条检测的格式线条检测寻找线条中心位置通过图像矩或轮廓检测找到线条的质心坐标偏差计算计算图像中心与线条中心的偏移偏差值决定小车转向方向和幅度PID 控制计算转向角速度、线速度避免小车左右摆动，提高巡线稳定性速度发布发布 /cmd_vel 话题控制小车速度和转向一、car_vision功能包

接下来我们创建car_vision的Python版本的功能包；

1. zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src$ ros2 pkg create --build-type ament_python car_vision

复制代码

运行成功后，终端会显示创建的文件和目录信息。此时， car_vision 功能包目录结构将如下所示：

1. zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src$ tree ./car_vision/
2. ./car_vision/
3. ├── car_vision      # 核心Python模块目录，用于存放Python代码
4. │   └── __init__.py
5. ├── package.xml      # 功能包的描述文件（含依赖信息）
6. ├── resource         # 资源文件夹
7. │   └── car_vision
8. ├── setup.cfg        # setuptools 的配置文件
9. ├── setup.py         # Python 包的安装脚本
10. └── test             # 测试文件夹

复制代码

1.1 子目录

car_vision功能包创建好了，我们需要创建如下子目录；

• launch：存放启动脚本，一键启动完整仿真环境。
  

在终端中执行：

1. zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src$ cd car_vision/
2. zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src/car_vision$ mkdir launch

复制代码

我们需要修改setup.py文件，告诉Python安装哪些资源文件；

1. import os
2. from glob import glob
4.     ...
6.     data_files=[
7.         ('share/ament_index/resource_index/packages',
8.             ['resource/' + package_name]),
9.         ('share/' + package_name, ['package.xml']),
10.         (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
11.     ],
13.     ...

复制代码

1.2 launch文件

在launch/目录下创建cascaded.launch.py：

1. from launch import LaunchDescription
2. from launch_ros.actions import Node
3. from ament_index_python.packages import get_package_share_directory
4. import os
6. def generate_launch_description():
7.     return LaunchDescription([
8.         Node(
9.             package='car_vision',
10.             executable='cascaded_line_follower',  
11.             name='cascaded_line_follower',
12.             output='screen',
13.             parameters=[{
14.                 # 话题参数
15.                 'image_topic': '/camera/image_raw',
16.                 'cmd_vel_topic': '/cmd_vel',
17.                
18.                 # 外环参数（偏差 → 曲率）
19.                 'kp_outer': 0.008,      # 比例系数
20.                 'kd_outer': 0.001,      # 微分系数
21.                 'max_curvature': 2.0,   # 最大曲率 (1/m)
22.                
23.                 # 内环参数（曲率 → 速度）
24.                 'base_speed': 0.30,     # 基础速度 (m/s)
25.                 'min_speed': 0.12,      # 最小速度 (m/s)
26.                 'wheel_track': 0.155,   # 轮距 (m)
27.                
28.                 # 调试参数
29.                 'debug_view': True
30.             }]
31.         )
32.     ])

复制代码

同时修改setup.py，需要在 setup.py 中添加新的入口点：

1. entry_points={
2.     'console_scripts': [
3.         'cascaded_line_follower = car_vision.cascaded_line_follower:main',  # 新增
4.     ],
5. },

复制代码

1.3 package.xml

修改package.xml添加以下依赖：

1. <depend>rclpy</depend>
2. <depend>sensor_msgs</depend>
3. <depend>geometry_msgs</depend>
4. <depend>cv_bridge</depend>
5. <depend>opencv2</depend>
7. <exec_depend>ros2launch</exec_depend>

复制代码

二、巡线算法

这里我们打算采用串级PID方案：偏差 → 曲率 → 角速度 + 线速度，在这里我们引出了一个中间变量，曲率，为什么引入曲率？
曲率（Curvature）是路径弯曲程度的度量，单位为 1/米。曲率越大，路径越弯。对于巡线任务：
偏差路径形态曲率0直线0小缓弯小（如 0.5）大急弯大（如 2.0）引入曲率的好处是：曲率是一个物理意义明确的量，可以直接用来计算左右轮的速度差。
内外环的职责：

1. ┌─────────────────────────────────────────────────────────────────┐
2. │                        外环（路径规划）                            │
3. │  输入：中线偏差（error）                                           │
4. │  输出：目标曲率（target_curvature）                                │
5. │  控制算法：P或PD                                                  │
6. │  作用：决定“我应该走多弯的路径”                                      │
7. └─────────────────────────────────────────────────────────────────┘
8.                               │
9.                               ▼
10. ┌─────────────────────────────────────────────────────────────────┐
11. │                        内环（运动控制）                            │
12. │  输入：目标曲率（target_curvature）                                │
13. │  输出：角速度（angular）+ 线速度（linear）                          │
14. │  控制算法：运动学公式                                              │
15. │  作用：决定“如何通过控制车轮实现该曲率”                               │
16. └─────────────────────────────────────────────────────────────────┘

复制代码

2.1 数学原理

2.1.1 曲率与轮速差的关系

对于差速驱动的小车：

\[曲率 = （右轮速度 - 左轮速度） / （轮距 \times 线速度）\]
设：

• v = 线速度 (m/s)；
  
• ω = 角速度 (rad/s)；
  
• d = 轮距 (m) = 0.155m（F车模）；
  
• κ = 曲率 (1/m)；
  

则：$ κ = \frac{ω}{ v} $
曲率 = 角速度 / 线速度，这个公式是关键！它建立了曲率、角速度、线速度三者之间的关系。
2.1.2 曲率与偏差的关系（外环）

偏差 e（像素）与曲率 κ 之间存在近似线性关系：

\[κ_{target} = K_p × e + K_d × \frac{de}{dt}\]
这就是外环的PD控制。
2.1.3  由曲率计算轮速差

已知κ = ω / v，且差速驱动的运动学关系：

1. ω = (v_right - v_left) / d
2. v = (v_right + v_left) / 2

复制代码

联立可得：

1. v_right = v × (1 + κ × d / 2)
2. v_left  = v × (1 - κ × d / 2)

复制代码

2.2 算法实现

在car_vision子目录下创建cascaded_line_follower.py：

1. import rclpy
2. from rclpy.node import Node
3. from sensor_msgs.msg import Image
4. from geometry_msgs.msg import Twist
5. from cv_bridge import CvBridge
6. import cv2
7. import numpy as np
9. class CascadedPIDLineFollower(Node):
10.     """串级PID巡线控制器 - 保持原有控制算法，升级视觉处理"""
12.     def __init__(self):
13.         super().__init__('cascaded_line_follower')
15.         # ========== 参数声明 (保持原有) ==========
16.         self.declare_parameter('image_topic', '/camera/image_raw')
17.         self.declare_parameter('cmd_vel_topic', '/cmd_vel')
19.         # 视觉调试参数
20.         self.declare_parameter('debug_single_step', False) # 新增：单帧调试开关
21.         self.declare_parameter('roi_ratio', 0.5)           # 新增：ROI区域比例
23.         # 外环参数
24.         self.declare_parameter('kp_outer', 0.008)
25.         self.declare_parameter('kd_outer', 0.001)
26.         self.declare_parameter('max_curvature', 2.0)
28.         # 内环参数
29.         self.declare_parameter('base_speed', 0.30)
30.         self.declare_parameter('min_speed', 0.12)
31.         self.declare_parameter('wheel_track', 0.155)
33.         self.declare_parameter('debug_view', True)
35.         # 获取参数
36.         self.image_topic = self.get_parameter('image_topic').value
37.         self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
38.         self.kp_outer = self.get_parameter('kp_outer').value
39.         self.kd_outer = self.get_parameter('kd_outer').value
40.         self.max_curvature = self.get_parameter('max_curvature').value
41.         self.base_speed = self.get_parameter('base_speed').value
42.         self.min_speed = self.get_parameter('min_speed').value
43.         self.wheel_track = self.get_parameter('wheel_track').value
44.         self.debug_view = self.get_parameter('debug_view').value
45.         self.debug_single_step = self.get_parameter('debug_single_step').value
46.         self.roi_ratio = self.get_parameter('roi_ratio').value
48.         # 状态变量 (保持原有 + 新增视觉状态)
49.         self.last_error = 0.0
50.         self.last_time = self.get_clock().now()
52.         # 视觉状态机变量
53.         self.vision_state = "NORMAL" # NORMAL: 边缘搜索, LOST: 霍夫找回
54.         self.last_center_x = None    # 用于预测搜索起点
55.         self.lost_counter = 0        # 丢线计数器
57.         # 图像处理
58.         self.bridge = CvBridge()
60.         # 创建订阅者和发布者
61.         self.subscription = self.create_subscription(
62.             Image, self.image_topic, self.image_callback, 10
63.         )
64.         self.publisher = self.create_publisher(Twist, self.cmd_vel_topic, 10)
66.         self.get_logger().info('视觉算法已升级：边缘搜索+最小二乘+霍夫变换')
68.     def image_callback(self, msg):
69.         """图像回调 - 包含状态机逻辑"""
70.         try:
71.             cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
72.         except Exception as e:
73.             self.get_logger().error(f'图像转换失败: {e}')
74.             return
76.         height, width = cv_image.shape[:2]
77.         error = None
78.         debug_img = cv_image.copy()
80.         # ========== 视觉状态机 ==========
81.         if self.vision_state == "NORMAL":
82.             # 1. 正常模式：极速边缘搜索 + 最小二乘
83.             error, debug_img, is_lost = self.process_edge_search(cv_image)
85.             if is_lost:
86.                 self.vision_state = "LOST"
87.                 self.get_logger().warn("视觉状态: LOST -> 触发霍夫变换")
88.                 # 丢线时，保持上一次误差或设为0，防止舵机乱打
89.                 error = self.last_error
91.         elif self.vision_state == "LOST":
92.             # 2. 丢线模式：霍夫变换大范围找回
93.             error, debug_img, is_found = self.process_hough_recovery(cv_image)
95.             if is_found:
96.                 self.vision_state = "NORMAL"
97.                 self.lost_counter = 0
98.                 self.get_logger().info("视觉状态: RECOVERY -> 恢复正常")
99.             else:
100.                 self.lost_counter += 1
101.                 # 如果长时间找不到，可以选择停车
102.                 if self.lost_counter > 100:
103.                      self.get_logger().error("彻底丢失赛道")
104.                      error = 0.0 # 停车逻辑在下方速度为0体现
106.         # 如果 error 为 None (极少情况)，不发布速度
107.         if error is None:
108.              return
110.         # ========== 保持原有的控制算法 ==========
112.         # 2. 计算时间差
113.         current_time = self.get_clock().now()
114.         dt = (current_time - self.last_time).nanoseconds / 1e9
115.         dt = max(0.001, min(0.1, dt))
117.         # 外环：偏差 → 目标曲率
118.         derivative = (error - self.last_error) / dt
119.         target_curvature = self.kp_outer * error + self.kd_outer * derivative
120.         target_curvature = max(-self.max_curvature, min(self.max_curvature, target_curvature))
122.         # 更新状态
123.         self.last_error = error
124.         self.last_time = current_time
126.         # 内环：曲率 → 速度分配
127.         speed_factor = 1.0 - abs(target_curvature) / self.max_curvature
128.         linear_speed = self.base_speed * max(self.min_speed / self.base_speed, speed_factor)
129.         angular_speed = target_curvature * linear_speed
130.         angular_speed = max(-2.0, min(2.0, angular_speed))
132.         # 3. 发布控制指令
133.         self.publish_command(linear_speed, angular_speed)
135.         # 4. 调试信息显示
136.         if self.debug_view:
137.             cv2.putText(debug_img, f'State: {self.vision_state}', (10, 30),
138.                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
139.             cv2.putText(debug_img, f'Error: {error:.0f}', (10, 60),
140.                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
141.             cv2.imshow('Cascaded PID Line Follower', debug_img)
143.         # 5. 单帧暂停逻辑
144.         if self.debug_single_step:
145.             # 等待按键，空格(32)继续，ESC(27)退出
146.             key = cv2.waitKey(0) & 0xFF
147.             if key == 27:
148.                 cv2.destroyAllWindows()
149.                 rclpy.shutdown()
150.         else:
151.             cv2.waitKey(1)
153.     def process_edge_search(self, cv_image):
154.         """
155.         核心算法：极速边缘搜索 + 最小二乘拟合
156.         返回: error, debug_img, is_lost
157.         """
158.         height, width = cv_image.shape[:2]
159.         gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
161.         # 1. 二值化 (使用自适应阈值抗光照干扰)
162.         binary = cv2.adaptiveThreshold(gray, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
163.                                        cv2.THRESH_BINARY_INV, 11, 2)
165.         # 2. ROI 裁剪
166.         roi_y_start = int(height * (1 - self.roi_ratio))
167.         roi = binary[roi_y_start:height, :]
169.         # 3. 极速边缘搜索
170.         points = []
171.         # 预测搜索起点：优先使用上一帧的中心，没有则用图像中心
172.         predict_x = self.last_center_x if self.last_center_x is not None else width // 2
174.         for r in range(0, roi.shape[0], 2): # 隔行扫描进一步提速
175.             y_real = r + roi_y_start
176.             row_data = roi[r, :]
178.             # 动态搜索范围：以预测点为中心，左右各 40 像素
179.             search_start = max(0, int(predict_x - 40))
180.             search_end = min(width, int(predict_x + 40))
182.             # 提取行数据
183.             row_slice = row_data[search_start:search_end]
185.             # 寻找非零点
186.             indices = np.where(row_slice > 0)[0]
187.             if len(indices) > 0:
188.                 # 取这一行找到的第一个点作为边缘点
189.                 x_real = indices[0] + search_start
190.                 points.append([x_real, y_real])
192.                 # 更新预测：下一行大概率在这一列附近
193.                 predict_x = x_real
195.         # 4. 最小二乘法拟合
196.         is_lost = False
197.         if len(points) > 10: # 点数足够才拟合
198.             points_np = np.array(points, dtype=np.float32)
199.             # fitLine 返回 (vx, vy, x0, y0)
200.             [vx, vy, x0, y0] = cv2.fitLine(points_np, cv2.DIST_L2, 0, 0.01, 0.01)
202.             # 计算底部中心偏差
203.             if abs(vy) > 1e-5:
204.                 k = vx / vy
205.                 # 计算图像最底部的 x 坐标
206.                 check_y = height - 10
207.                 # 直线方程 x = (y - y0)/k + x0
208.                 center_x = int((check_y - y0) / k + x0) if abs(k) > 1e-5 else int(x0)
210.                 # 保存状态供下一帧预测
211.                 self.last_center_x = center_x
213.                 error = (width // 2) - center_x
215.                 # 可视化
216.                 cv2.circle(cv_image, (center_x, check_y), 5, (0, 255, 0), -1)
217.                 # 画拟合线
218.                 # 修复：OpenCV 绘图函数要求点集必须是 int32 类型
219.                 pts = points_np.reshape(-1, 1, 2).astype(np.int32)
220.                 cv2.polylines(cv_image, [pts], False, (0, 255, 0), 1)
222.                 return error, cv_image, False
223.             else:
224.                 is_lost = True
225.         else:
226.             is_lost = True
228.         return None, cv_image, is_lost
230.     def process_hough_recovery(self, cv_image):
231.         """
232.         辅助算法：霍夫变换找回赛道
233.         """
234.         height, width = cv_image.shape[:2]
235.         gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
237.         # 1. 边缘检测 (霍夫变换的前置步骤)
238.         edges = cv2.Canny(gray, 50, 150, apertureSize=3)
240.         # 2. 标准霍夫变换
241.         # 注意：如果没有检测到线条，lines 可能是 None
242.         lines = cv2.HoughLines(edges, 1, np.pi / 180, 120)
244.         # 3. 严格检查返回值
245.         if lines is not None and len(lines) > 0:
246.             # 寻找最合理的线，这里简化逻辑：取第一条线
247.             try:
248.                 for line in lines:
249.                     rho, theta = line[0] # 安全解包
250.                     a = np.cos(theta)
251.                     b = np.sin(theta)
252.                     x0 = a * rho
253.                     y0 = b * rho
254.                     # 计算直线与图像底部的交点
255.                     x1 = int(x0 + 1000 * (-b))
256.                     y1 = int(y0 + 1000 * (a))
257.                     x2 = int(x0 - 1000 * (-b))
258.                     y2 = int(y0 - 1000 * (a))
260.                     # 简单的中心估算
261.                     center_x = (x1 + x2) // 2
262.                     error = (width // 2) - center_x
264.                     cv2.line(cv_image, (x1, y1), (x2, y2), (0, 0, 255), 2)
265.                     return error, cv_image, True
266.             except Exception as e:
267.                 # 防止解包异常
268.                 self.get_logger().warn(f"霍夫变换解析错误: {e}")
270.         # 4. 兜底：如果 lines 是 None 或者解析失败，返回未找到
271.         return None, cv_image, False
274.     def publish_command(self, linear_speed, angular_speed):
275.         """保持原有的发布逻辑"""
276.         cmd = Twist()
277.         cmd.linear.x = linear_speed
278.         cmd.angular.z = angular_speed
279.         self.publisher.publish(cmd)
281. def main(args=None):
282.     rclpy.init(args=args)
283.     node = CascadedPIDLineFollower()
284.     try:
285.         rclpy.spin(node)
286.     except KeyboardInterrupt:
287.         node.get_logger().info('节点被手动停止')
288.     finally:
289.         node.destroy_node()
290.         rclpy.shutdown()
291.         cv2.destroyAllWindows()
293. if __name__ == '__main__':
294.     main()

复制代码

2.3 控制流程图

1. ┌─────────────────────────────────────────────────────────────────────────┐
2. │                           串级PID控制流程                                 │
3. └─────────────────────────────────────────────────────────────────────────┘
5.                     ┌─────────────────┐
6.                     │   摄像头图像      │
7.                     └────────┬────────┘
8.                              │
9.                              ▼
10.                     ┌─────────────────┐
11.                     │  提取中线偏差 e   │
12.                     └────────┬────────┘
13.                              │
14.                     ┌────────▼────────┐
15.                     │   外环 PD        │
16.                     │  κ = kp·e + kd·ė │
17.                     └────────┬────────┘
18.                              │
19.               ┌──────────────┼──────────────┐
20.               │              │              │
21.               ▼              ▼              ▼
22.      ┌─────────────────────────────────────────────┐
23.      │              内环 运动学解算                  │
24.      │                                             │
25.      │  线速度：v = v_base × (1 - |κ|/κ_max)        │
26.      │  角速度：ω = κ × v                           │
27.      │                                             │
28.      │  左轮速度：vL = v - ω × d/2                   │
29.      │  右轮速度：vR = v + ω × d/2                   │
30.      └─────────────────────────────────────────────┘
31.                              │
32.                              ▼
33.                     ┌─────────────────┐
34.                     │  发布 /cmd_vel   │
35.                     │  (v, ω)         │
36.                     └─────────────────┘

复制代码

三、编译运行

3.1 编译

在car_ws目录下编译：
[code]zhengyang@ubuntu:~$ cd /opt/2k0300/loongson_2k300_lib/car_wszhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ colcon build --paths src/car_vision.....Finished