前言
在全国大学生智能车竞赛（智慧医疗机器人创意赛）中，我们团队取得了不错的成绩。作为技术负责人，我想分享一下备赛过程中关于惯导、视觉处理及系统优化的实际思路。比赛竞争激烈，细节决定成败，以下经验希望能帮助到后续参赛的同学。
网络稳定性优化
网络延迟是比赛中常见的痛点，直接影响云端 API 调用和实时控制。第一年参赛时我们也遇到过严重延迟问题，第二年重点解决了这一环节。
硬件选择上，我们使用了华为 BE7 Pro 路由器。在校赛阶段发现其他队伍连接困难，而我们的设备运行稳定。在正式赛场上，我们将信道固定在 165，这个频段通常干扰较少。调试期间若遇到中继模式导致的延迟波动，建议直接切换至本地部署方案，避免依赖不稳定的云端服务。
连接方式上，上位机务必使用有线网络连接路由器，不要依赖板载无线网卡。这能显著降低丢包率和抖动。
上位机辅助可视化
为了帮助机师快速判断障碍物位置，我们在上位机端开发了一个基于 Tkinter 的辅助界面。它接收 YOLO 的检测数据，将障碍物和终点信息绘制在画布上，相当于游戏里的小地图。
核心逻辑是订阅 ROS 话题，解析目标坐标后映射到屏幕像素。例如，对于障碍物，我们计算其在图像中的矩形区域并绘制红线；对于终点，则标记置信度。
import tkinter as tk
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
class LLM2Origincar:
def __init__(self, host, port):
self.ros = None
self.host = host
self.port = port
self.roadblock_list = []
self.end_list = []
self.init_ros()
self.init_topic()
self.init_thread()
self.keep()
def init_topic(self):
# 订阅感知目标话题
self.yolo_sub = Topic(self.ros, '/hobot_dnn_detection', 'ai_msgs/msg/PerceptionTargets', latch=True)
self.yolo_sub.subscribe(self.yolo_sub_callback)
def yolo_sub_callback(self, msg):
self.roadblock_list.clear()
self.end_list.clear()
for target in msg['targets']:
if target['type'] == 'roadblock':
rect = target['rois'][0]['rect']
self.roadblock_list.append({
'x': rect['x_offset'],
'w': rect['width'],
'b': rect['y_offset'] + rect['height'],
})
elif target['type'] == 'end':
rect = target['rois'][0]['rect']
self.end_list.append({
'x': rect['x_offset'],
'y': rect['y_offset'],
'w': rect['width'],
'b': rect['y_offset'] + rect['height'],
'c': target['rois'][0]['confidence'],
})
def keep(self):
try:
while True:
canvas.delete("all")
# 绘制车道线参考
canvas.create_line(141, 0, 141, 680, fill="red", width=1)
canvas.create_line(689, 0, 689, 680, fill="red", width=1)
if self.roadblock_list:
for obst in self.roadblock_list:
b = int(obst['b'] * 1.42) # 高度缩放系数
x_start = int(obst['x'] * 1.41)
x_end = int((obst['x'] + obst['w']) * 1.41)
canvas.create_line(x_start, b, x_end, b, fill="red", width=2)
if self.end_list:
for end in self.end_list:
x1 = int(end['x'] * 1.41)
y1 = int(end['y'] * 1.41)
x2 = int((end['x'] + end['w']) * 1.41)
y2 = int(end['b'] * 1.42)
canvas.create_line(x1, y2, x2, y2, fill="blue", width=1)
canvas.create_text(int((x1+x2)/2), max(0, y1-20), text=f"conf:{end['c']:.2f}", fill='cyan')
except Exception as e:
print(f"Visualization error: {e}")
此外，我们还添加了键盘快捷键来控制任务状态切换，比如连接信号、激活遥操作、退出遥操作等，方便现场调试。
半场扫码优化
小橙小车配备的深度相机在扫码清晰度上远优于 USB 相机。深度相机不仅能提供深度信息，其成像质量也更适合 OCR 识别。
扫码节点不应一直运行，这会消耗大量 CPU。我们设置了触发条件：仅在任务一且小车全局坐标 X 超过 2 米时开启扫码。同时，我们对图像进行了裁剪，去除了底部无效区域，减小了检测模型的输入尺寸，提升了推理速度。
import rclpy
from rclpy.node import Node
import cv2
import numpy as np
from sensor_msgs.msg import Image
from std_msgs.msg import String, Int32
from nav_msgs.msg import Odometry
from origincar_msg.msg import Sign
from cv_bridge import CvBridge
TASK1 = 1
TASK2_WAITFOR_CMD = 2
TASK2 = 3
TASK3 = 4
TASK_STOP = 5
class QrCodeDetection(Node):
def __init__(self):
super().__init__('QRcodeSub')
self.Sign4ReturnSub = self.create_subscription(Int32, 'sign4return', self.sign4return_callback, 10)
self.ImageSub = self.create_subscription(Image, '/aurora/rgb/image_raw', self.image_callback, 10)
self.OdomSub = self.create_subscription(Odometry, '/odom_combined', self.Odom_callback, 10)
self.qrcode_publisher = self.create_publisher(String, "/qrcode_information", 10)
self.info_result = String()
self.sign_publisher = self.create_publisher(Sign, '/sign_switch', 10)
self.sign_msg = Sign()
# 加载微信二维码模型
self.detector = cv2.wechat_qrcode_WeChatQRCode(
"/userdata/WorkSpace/codes/src/qrcode/qrcode/model/detect.prototxt",
"/userdata/WorkSpace/codes/src/qrcode/qrcode/model/detect.caffemodel",
"/userdata/WorkSpace/codes/src/qrcode/qrcode/model/sr.prototxt",
"/userdata/WorkSpace/codes/src/qrcode/qrcode/model/sr.caffemodel"
)
self.bridge = CvBridge()
self.node_run = False
self.task = TASK1
def image_callback(self, msg):
if self.node_run and (self.task == TASK1 or self.task == TASK2):
# 裁剪图像，去除底部无效区域
cv2_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='mono8')[155:, :]
res = self.detector.detectAndDecode(cv2_image)[0]
if res:
self.node_run = False
for r in res:
self.info_result.data = str(r)
self.qrcode_publisher.publish(self.info_result)
self.get_logger().info(f"Detected QR: {self.info_result.data}")
if self.info_result.data == "AntiClockWise":
self.sign_msg.sign_data = 4
elif self.info_result.data == "ClockWise":
self.sign_msg.sign_data = 3
else:
try:
data = int(r)
self.sign_msg.sign_data = 3 if data % 2 else 4
except:
pass
self.sign_publisher.publish(self.sign_msg)
self.info_result.data = "None"
self.sign_msg.sign_data = 0
def sign4return_callback(self, msg):
if msg.data in [0, -1]:
self.task = TASK1
self.node_run = False
elif msg.data == 5:
self.task = TASK2
elif msg.data == 6:
self.task = TASK3
def Odom_callback(self, msg):
if self.task == TASK1 and msg.pose.pose.position.x > 2:
self.node_run = True
准确返回 P 点策略
P 点定位是任务二的关键。我们总结了两种校准思路。
思路一：利用地图固定元素校准
此方法需要重置里程计。每次从通道出来后停在固定位置，以车头方向为 X+，左侧为 Y+，建立局部坐标系。通过逆透视变换（IPM）计算前方两条固定标线的相对位置，结合已知的终点坐标，反推当前车辆相对于终点的位姿。
具体步骤如下：
- 固定车辆位置，拍摄照片，记录终点坐标（如 1.9m, -1.5m）。
- 利用 IPM 计算该视角下两条标线的像素坐标 A, B。
- 在实际行驶中，再次获取标线坐标 A', B'。
- 求解旋转矩阵 R 和平移向量 t，从而计算出当前视角下的终点 P'。
数学推导基于二维平面变换： $$ \Delta A'B' = R \Delta AB $$ $$ R = \Delta A'B' \cdot \Delta AB^{-1} $$ $$ t = A' - RA $$ $$ P' = RP + t $$
Python 实现示例：
def end_point(x1, y1, x2, y2, x3, y3, x1_, y1_, x2_, y2_):
delta_x = x1 - x2
delta_y = y1 - y2
delta_x_ = x1_ - x2_
delta_y_ = y1_ - y2_
den = delta_x ** 2 + delta_y ** 2
a = (delta_x * delta_x_ + delta_y * delta_y_) / den
b = (delta_x * delta_y_ - delta_y * delta_x_) / den
tx = x1_ - a * x1 + b * y1
ty = y1_ - b * x1 - a * y1
x3_ = a * x3 - b * y3 + tx
y3_ = b * x3 + a * y3 + ty
return x3_, y3_
思路二：YOLO 识别校正
此方法无需停车重置里程计，适合高速场景。全程保持里程计运行，利用 YOLO 识别 P 点中心像素，结合单应性矩阵转换到全局坐标。
# 单应性矩阵 H
H = np.array([
[-4.66389128e-04, -2.26288030e-04, -4.92300831e-02],
[7.59821540e-04, 5.20569143e-05, -2.33074608e-01],
[-6.59643252e-04, -7.15022786e-03, 1.00000000e+00],
])
def pixel2global(self, pixel_x, pixel_y):
pixel = np.array([pixel_x, pixel_y, 1], dtype=np.float32)
local = np.dot(H, pixel)
local /= local[2]
local[0] += 0.25
car_cos = np.cos(self.current_pos[2])
car_sin = np.sin(self.current_pos[2])
global_x = self.current_pos[0] + car_cos * local[0] - car_sin * local[1]
global_y = self.current_pos[1] + car_sin * local[0] + car_cos * local[1]
return global_x, global_y
训练数据方面，我们采集了大量包含 P 点的图片，并进行增强。注意区分任务三的白色网格区域，避免误检。
STM32 底层调优
STM32 源码中，舵机转角的多项式计算是关键。我们发现左右转角的限制不对称，导致转向半径不一致。通过调整多项式的二次项系数，使左右最大转角对应的舵机量一致，改善了车辆的对称性。
此外，为了提高惯导频率，我们将串口波特率提升至 921600，发送频率设为 50Hz。同时关闭 CAN、蓝牙等未使用外设，仅保留串口 1 和 3。EKF 配置文件中，imu 的频率也需同步调整为 50Hz。
车型号部分，我们删除了电位器读取逻辑，固定为 Ackerman 底盘，简化了启动流程。
数据处理脚本
为了高效处理数据集，我们编写了自动化脚本，包括自动标注、无效数据清理、数据增强及分批次打包。
自动标注脚本
利用已有模型对图片进行预标注，人工仅需复核，大幅提升效率。
数据清洗与增强
脚本会自动检查图片与标签是否对应，删除空标签或无标签的图片。增强策略包含亮度、对比度调整和高斯模糊，但不包含旋转，以保持赛道特征一致性。
任务分包
支持将数据集按数量切分为多个压缩包，便于团队成员并行标注。
以上脚本均经过验证，可直接用于项目初始化阶段。希望这些经验能为你的备赛之路提供参考。
