经过前期的努力《龙芯2k0300 - 走马观碑组Gazebo仿真环境搭建（上）》，现在Gazebo仿真环境已经跑通，小车能动了，摄像头也在发布图像，下一步就是让小车能"看见"赛道并自主巡线。car_vision 功能包的核心任务就是：订阅摄像头图像 → 处理图像找到线条 → 计算中线偏差 → 控制算法→ 发布角速度、线速度指令。其核心功能：
| 模块 | 功能 | 说明 |
|---|---|---|
| 图像订阅 | 订阅 /camera/image_raw 话题 |
从 Gazebo 摄像头获取实时图像 |
| 图像预处理 | 灰度化、二值化、去噪 | 将彩色图像转换为便于线条检测的格式 |
| 线条检测 | 寻找线条中心位置 | 通过图像矩或轮廓检测找到线条的质心坐标 |
| 偏差计算 | 计算图像中心与线条中心的偏移 | 偏差值决定小车转向方向和幅度 |
| PID 控制 | 计算转向角速度、线速度 | 避免小车左右摆动，提高巡线稳定性 |
| 速度发布 | 发布 /cmd_vel 话题 |
控制小车速度和转向 |
一、car_vision功能包
接下来我们创建car_vision的Python版本的功能包；
shell
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src$ ros2 pkg create --build-type ament_python car_vision运行成功后，终端会显示创建的文件和目录信息。此时， car_vision 功能包目录结构将如下所示：
shell
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src$ tree ./car_vision/
./car_vision/
├── car_vision # 核心Python模块目录，用于存放Python代码
│ └── __init__.py
├── package.xml # 功能包的描述文件（含依赖信息）
├── resource # 资源文件夹
│ └── car_vision
├── setup.cfg # setuptools 的配置文件
├── setup.py # Python 包的安装脚本
└── test # 测试文件夹1.1 子目录
car_vision功能包创建好了，我们需要创建如下子目录；
launch：存放启动脚本，一键启动完整仿真环境。
在终端中执行：
shell
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src$ cd car_vision/
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws/src/car_vision$ mkdir launch我们需要修改setup.py文件，告诉Python安装哪些资源文件；
python
import os
from glob import glob
...
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],
...1.2 launch文件
在launch/目录下创建cascaded.launch.py：
python
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
return LaunchDescription([
Node(
package='car_vision',
executable='cascaded_line_follower',
name='cascaded_line_follower',
output='screen',
parameters=[{
# 话题参数
'image_topic': '/camera/image_raw',
'cmd_vel_topic': '/cmd_vel',
# 外环参数（偏差 → 曲率）
'kp_outer': 0.008, # 比例系数
'kd_outer': 0.001, # 微分系数
'max_curvature': 2.0, # 最大曲率 (1/m)
# 内环参数（曲率 → 速度）
'base_speed': 0.30, # 基础速度 (m/s)
'min_speed': 0.12, # 最小速度 (m/s)
'wheel_track': 0.155, # 轮距 (m)
# 调试参数
'debug_view': True
}]
)
])同时修改setup.py，需要在 setup.py 中添加新的入口点：
python
entry_points={
'console_scripts': [
'cascaded_line_follower = car_vision.cascaded_line_follower:main', # 新增
],
},1.3 package.xml
修改package.xml添加以下依赖：
xml
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>opencv2</depend>
<exec_depend>ros2launch</exec_depend>二、巡线算法
这里我们打算采用串级PID方案：偏差 → 曲率 → 角速度 + 线速度，在这里我们引出了一个中间变量，曲率，为什么引入曲率？
曲率（Curvature）是路径弯曲程度的度量，单位为 1/米。曲率越大，路径越弯。对于巡线任务：
| 偏差 | 路径形态 | 曲率 |
|---|---|---|
| 0 | 直线 | 0 |
| 小 | 缓弯 | 小（如 0.5） |
| 大 | 急弯 | 大（如 2.0） |
引入曲率的好处是：曲率是一个物理意义明确的量，可以直接用来计算左右轮的速度差。
内外环的职责：
shell
┌─────────────────────────────────────────────────────────────────┐
│ 外环（路径规划） │
│ 输入：中线偏差（error） │
│ 输出：目标曲率（target_curvature） │
│ 控制算法：P或PD │
│ 作用：决定"我应该走多弯的路径" │
└─────────────────────────────────────────────────────────────────┘
│
▼
┌─────────────────────────────────────────────────────────────────┐
│ 内环（运动控制） │
│ 输入：目标曲率（target_curvature） │
│ 输出：角速度（angular）+ 线速度（linear） │
│ 控制算法：运动学公式 │
│ 作用：决定"如何通过控制车轮实现该曲率" │
└─────────────────────────────────────────────────────────────────┘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，且差速驱动的运动学关系：
shell
ω = (v_right - v_left) / d
v = (v_right + v_left) / 2联立可得：
shell
v_right = v × (1 + κ × d / 2)
v_left = v × (1 - κ × d / 2)2.2 算法实现
在car_vision子目录下创建cascaded_line_follower.py：
python
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge
import cv2
import numpy as np
class CascadedPIDLineFollower(Node):
"""串级PID巡线控制器 - 保持原有控制算法，升级视觉处理"""
def __init__(self):
super().__init__('cascaded_line_follower')
# ========== 参数声明 (保持原有) ==========
self.declare_parameter('image_topic', '/camera/image_raw')
self.declare_parameter('cmd_vel_topic', '/cmd_vel')
# 视觉调试参数
self.declare_parameter('debug_single_step', False) # 新增：单帧调试开关
self.declare_parameter('roi_ratio', 0.5) # 新增：ROI区域比例
# 外环参数
self.declare_parameter('kp_outer', 0.008)
self.declare_parameter('kd_outer', 0.001)
self.declare_parameter('max_curvature', 2.0)
# 内环参数
self.declare_parameter('base_speed', 0.30)
self.declare_parameter('min_speed', 0.12)
self.declare_parameter('wheel_track', 0.155)
self.declare_parameter('debug_view', True)
# 获取参数
self.image_topic = self.get_parameter('image_topic').value
self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
self.kp_outer = self.get_parameter('kp_outer').value
self.kd_outer = self.get_parameter('kd_outer').value
self.max_curvature = self.get_parameter('max_curvature').value
self.base_speed = self.get_parameter('base_speed').value
self.min_speed = self.get_parameter('min_speed').value
self.wheel_track = self.get_parameter('wheel_track').value
self.debug_view = self.get_parameter('debug_view').value
self.debug_single_step = self.get_parameter('debug_single_step').value
self.roi_ratio = self.get_parameter('roi_ratio').value
# 状态变量 (保持原有 + 新增视觉状态)
self.last_error = 0.0
self.last_time = self.get_clock().now()
# 视觉状态机变量
self.vision_state = "NORMAL" # NORMAL: 边缘搜索, LOST: 霍夫找回
self.last_center_x = None # 用于预测搜索起点
self.lost_counter = 0 # 丢线计数器
# 图像处理
self.bridge = CvBridge()
# 创建订阅者和发布者
self.subscription = self.create_subscription(
Image, self.image_topic, self.image_callback, 10
)
self.publisher = self.create_publisher(Twist, self.cmd_vel_topic, 10)
self.get_logger().info('视觉算法已升级：边缘搜索+最小二乘+霍夫变换')
def image_callback(self, msg):
"""图像回调 - 包含状态机逻辑"""
try:
cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
except Exception as e:
self.get_logger().error(f'图像转换失败: {e}')
return
height, width = cv_image.shape[:2]
error = None
debug_img = cv_image.copy()
# ========== 视觉状态机 ==========
if self.vision_state == "NORMAL":
# 1. 正常模式：极速边缘搜索 + 最小二乘
error, debug_img, is_lost = self.process_edge_search(cv_image)
if is_lost:
self.vision_state = "LOST"
self.get_logger().warn("视觉状态: LOST -> 触发霍夫变换")
# 丢线时，保持上一次误差或设为0，防止舵机乱打
error = self.last_error
elif self.vision_state == "LOST":
# 2. 丢线模式：霍夫变换大范围找回
error, debug_img, is_found = self.process_hough_recovery(cv_image)
if is_found:
self.vision_state = "NORMAL"
self.lost_counter = 0
self.get_logger().info("视觉状态: RECOVERY -> 恢复正常")
else:
self.lost_counter += 1
# 如果长时间找不到，可以选择停车
if self.lost_counter > 100:
self.get_logger().error("彻底丢失赛道")
error = 0.0 # 停车逻辑在下方速度为0体现
# 如果 error 为 None (极少情况)，不发布速度
if error is None:
return
# ========== 保持原有的控制算法 ==========
# 2. 计算时间差
current_time = self.get_clock().now()
dt = (current_time - self.last_time).nanoseconds / 1e9
dt = max(0.001, min(0.1, dt))
# 外环：偏差 → 目标曲率
derivative = (error - self.last_error) / dt
target_curvature = self.kp_outer * error + self.kd_outer * derivative
target_curvature = max(-self.max_curvature, min(self.max_curvature, target_curvature))
# 更新状态
self.last_error = error
self.last_time = current_time
# 内环：曲率 → 速度分配
speed_factor = 1.0 - abs(target_curvature) / self.max_curvature
linear_speed = self.base_speed * max(self.min_speed / self.base_speed, speed_factor)
angular_speed = target_curvature * linear_speed
angular_speed = max(-2.0, min(2.0, angular_speed))
# 3. 发布控制指令
self.publish_command(linear_speed, angular_speed)
# 4. 调试信息显示
if self.debug_view:
cv2.putText(debug_img, f'State: {self.vision_state}', (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
cv2.putText(debug_img, f'Error: {error:.0f}', (10, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
cv2.imshow('Cascaded PID Line Follower', debug_img)
# 5. 单帧暂停逻辑
if self.debug_single_step:
# 等待按键，空格(32)继续，ESC(27)退出
key = cv2.waitKey(0) & 0xFF
if key == 27:
cv2.destroyAllWindows()
rclpy.shutdown()
else:
cv2.waitKey(1)
def process_edge_search(self, cv_image):
"""
核心算法：极速边缘搜索 + 最小二乘拟合
返回: error, debug_img, is_lost
"""
height, width = cv_image.shape[:2]
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# 1. 二值化 (使用自适应阈值抗光照干扰)
binary = cv2.adaptiveThreshold(gray, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C,
cv2.THRESH_BINARY_INV, 11, 2)
# 2. ROI 裁剪
roi_y_start = int(height * (1 - self.roi_ratio))
roi = binary[roi_y_start:height, :]
# 3. 极速边缘搜索
points = []
# 预测搜索起点：优先使用上一帧的中心，没有则用图像中心
predict_x = self.last_center_x if self.last_center_x is not None else width // 2
for r in range(0, roi.shape[0], 2): # 隔行扫描进一步提速
y_real = r + roi_y_start
row_data = roi[r, :]
# 动态搜索范围：以预测点为中心，左右各 40 像素
search_start = max(0, int(predict_x - 40))
search_end = min(width, int(predict_x + 40))
# 提取行数据
row_slice = row_data[search_start:search_end]
# 寻找非零点
indices = np.where(row_slice > 0)[0]
if len(indices) > 0:
# 取这一行找到的第一个点作为边缘点
x_real = indices[0] + search_start
points.append([x_real, y_real])
# 更新预测：下一行大概率在这一列附近
predict_x = x_real
# 4. 最小二乘法拟合
is_lost = False
if len(points) > 10: # 点数足够才拟合
points_np = np.array(points, dtype=np.float32)
# fitLine 返回 (vx, vy, x0, y0)
[vx, vy, x0, y0] = cv2.fitLine(points_np, cv2.DIST_L2, 0, 0.01, 0.01)
# 计算底部中心偏差
if abs(vy) > 1e-5:
k = vx / vy
# 计算图像最底部的 x 坐标
check_y = height - 10
# 直线方程 x = (y - y0)/k + x0
center_x = int((check_y - y0) / k + x0) if abs(k) > 1e-5 else int(x0)
# 保存状态供下一帧预测
self.last_center_x = center_x
error = (width // 2) - center_x
# 可视化
cv2.circle(cv_image, (center_x, check_y), 5, (0, 255, 0), -1)
# 画拟合线
# 修复：OpenCV 绘图函数要求点集必须是 int32 类型
pts = points_np.reshape(-1, 1, 2).astype(np.int32)
cv2.polylines(cv_image, [pts], False, (0, 255, 0), 1)
return error, cv_image, False
else:
is_lost = True
else:
is_lost = True
return None, cv_image, is_lost
def process_hough_recovery(self, cv_image):
"""
辅助算法：霍夫变换找回赛道
"""
height, width = cv_image.shape[:2]
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# 1. 边缘检测 (霍夫变换的前置步骤)
edges = cv2.Canny(gray, 50, 150, apertureSize=3)
# 2. 标准霍夫变换
# 注意：如果没有检测到线条，lines 可能是 None
lines = cv2.HoughLines(edges, 1, np.pi / 180, 120)
# 3. 严格检查返回值
if lines is not None and len(lines) > 0:
# 寻找最合理的线，这里简化逻辑：取第一条线
try:
for line in lines:
rho, theta = line[0] # 安全解包
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
# 计算直线与图像底部的交点
x1 = int(x0 + 1000 * (-b))
y1 = int(y0 + 1000 * (a))
x2 = int(x0 - 1000 * (-b))
y2 = int(y0 - 1000 * (a))
# 简单的中心估算
center_x = (x1 + x2) // 2
error = (width // 2) - center_x
cv2.line(cv_image, (x1, y1), (x2, y2), (0, 0, 255), 2)
return error, cv_image, True
except Exception as e:
# 防止解包异常
self.get_logger().warn(f"霍夫变换解析错误: {e}")
# 4. 兜底：如果 lines 是 None 或者解析失败，返回未找到
return None, cv_image, False
def publish_command(self, linear_speed, angular_speed):
"""保持原有的发布逻辑"""
cmd = Twist()
cmd.linear.x = linear_speed
cmd.angular.z = angular_speed
self.publisher.publish(cmd)
def main(args=None):
rclpy.init(args=args)
node = CascadedPIDLineFollower()
try:
rclpy.spin(node)
except KeyboardInterrupt:
node.get_logger().info('节点被手动停止')
finally:
node.destroy_node()
rclpy.shutdown()
cv2.destroyAllWindows()
if __name__ == '__main__':
main()2.3 控制流程图
shell
┌─────────────────────────────────────────────────────────────────────────┐
│ 串级PID控制流程 │
└─────────────────────────────────────────────────────────────────────────┘
┌─────────────────┐
│ 摄像头图像 │
└────────┬────────┘
│
▼
┌─────────────────┐
│ 提取中线偏差 e │
└────────┬────────┘
│
┌────────▼────────┐
│ 外环 PD │
│ κ = kp·e + kd·ė │
└────────┬────────┘
│
┌──────────────┼──────────────┐
│ │ │
▼ ▼ ▼
┌─────────────────────────────────────────────┐
│ 内环 运动学解算 │
│ │
│ 线速度：v = v_base × (1 - |κ|/κ_max) │
│ 角速度：ω = κ × v │
│ │
│ 左轮速度：vL = v - ω × d/2 │
│ 右轮速度：vR = v + ω × d/2 │
└─────────────────────────────────────────────┘
│
▼
┌─────────────────┐
│ 发布 /cmd_vel │
│ (v, ω) │
└─────────────────┘三、编译运行
3.1 编译
在car_ws目录下编译：
shell
zhengyang@ubuntu:~$ cd /opt/2k0300/loongson_2k300_lib/car_ws
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ colcon build --paths src/car_vision
.....
Finished <<< car_description [0.92s]
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source install/setup.sh3.2 运行
启动第一个终端开启Gazebo仿真环境，运行如下命令；
shell
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ source /usr/share/gazebo/setup.sh
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 launch car_gazebo gazebo.launch.py在另一个终端启动巡线节点；
shell
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/car_ws$ ros2 launch car_vision cascaded.launch.py3.3 调试与参数调节
如果需要调节PID参数，可以使用动态参数调节工具：
shell
# 安装 rqt 工具
sudo apt install ros-humble-rqt-reconfigure
# 运行 rqt
ros2 run rqt_reconfigure rqt_reconfigure在界面中选择 /cascaded 节点，可以实时调整各个参数。
3.3.1 调节外环（曲率规划）
第一步：调节外环（曲率规划）；
| 参数 | 作用 | 调节方法 |
|---|---|---|
kp_outer |
曲率对偏差的敏感度 | 从小到大增加，直到小车能跟上弯道 |
kd_outer |
曲率对偏差变化率的响应 | 抑制振荡，设为 kp 的 1/10 左右 |
max_curvature |
最大允许曲率 | 根据最小转弯半径设置：κ_max = 1 / R_min |
F车模的最小转弯半径约0.3-0.4m，所以κ_max ≈ 2.5-3.0。
3.3.2 调节内环（速度分配）
第二步：调节内环（速度分配）；
| 参数 | 作用 | 调节方法 |
|---|---|---|
base_speed |
直道速度 | 从 0.2 开始，稳定后增加 |
min_speed |
急弯最低速度 | 确保过弯时不冲出，约 base_speed 的 1/3 |
3.3.3 实际效果预测
| 场景 | 偏差 | 目标曲率 | 线速度 | 角速度 | 小车行为 |
|---|---|---|---|---|---|
| 直道 | 0 | 0 | 0.30 | 0 | 直行 |
| 缓弯 | 30 | 0.24 | 0.28 | 0.07 | 轻微转向，稍减速 |
| 中弯 | 80 | 0.64 | 0.24 | 0.15 | 明显转向，减速 |
| 急弯 | 150 | 1.20 | 0.18 | 0.22 | 大转向，大幅减速 |
| 发卡弯 | 200 | 1.60 | 0.14 | 0.22 | 极限转向，最慢速度 |