前言
在全国大学生智能车竞赛（智慧医疗机器人创意赛）中，团队在备赛过程中积累了一些关于惯导、视觉避障及系统优化的经验。为了提升比赛成绩，我们针对网络延迟、上位机辅助、扫码节点及终点校准等关键环节进行了深度调试。以下分享部分核心思路与代码实现。
网络优化
网络延迟是比赛中常见的问题，直接影响上位机与小车通信的稳定性。第一年参赛时曾遇到严重的延迟问题，第二年我们重点优化了网络环境。
建议优先使用有线连接上位机与路由器，避免使用板载无线网卡。在信道选择上，尽量避开拥堵频段。例如，在某些区域赛中，165 信道相对空闲，能显著降低干扰。若现场网络环境复杂，可考虑本地部署轻量级模型作为备用方案，减少对云端 API 的依赖。
图 1 路由器型号参考
图 2 设备外观
上位机辅助处理
在上位机端，可以通过独立的 Python 脚本接收 YOLO 检测结果，并使用 tkinter 绘制障碍物位置，帮助机师快速判断环境。虽然最初尝试过小地图模式，但直接叠加红线提示效果更佳。
import tkinter as tk
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
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
import cv2
import numpy as np
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):
# ... (ROS topic initialization logic)
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) # 680 / 480
canvas.create_line(
int(obst['x'] * 1.41),
b,
int((obst['x'] + obst['w']) * 1.41),
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), (y1-20) if (y1-20) > 0 else 0, text=f"conf:{end['c']:.2f}", fill='cyan')
此外，通过键盘按键可以辅助任务切换和调用 API，例如按下特定键触发返回信号或退出遥操作。
def keyboard_thread(self):
while True:
sleep(0.05)
if keyboard.is_pressed('b') or keyboard.is_pressed('B'):
self.sign4return_pub.publish(self.sign4return_data)
sleep(0.5)
if keyboard.is_pressed('r') or keyboard.is_pressed('R'):
self.sign4return_data['data'] = 5
self.sign4return_pub.publish(self.sign4return_data)
self.sign4return_data['data'] = 0
sleep(0.5)
if keyboard.is_pressed('p') or keyboard.is_pressed('P'):
self.sign4return_data['data'] = 6
self.sign4return_pub.publish(self.sign4return_data)
self.sign4return_data['data'] = 0
sleep(0.5)
if keyboard.is_pressed('j') or keyboard.is_pressed('J'):
self.llm_data['data'] = 1
self.llm_pub.publish(self.llm_data)
sleep(1)
半场扫码优化
USB 相机拍摄的照片清晰度有限，而深度相机在扫码场景下表现更好。建议仅在任务一且小车越过半场（全局坐标 x > 2m）时开启扫码节点，以节省 CPU 资源。
图 3 深度相机扫码效果
图 4 USB 相机扫码效果
以下是扫码节点的简化逻辑：
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"{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 == 0 or msg.data == -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 点
思路 1——利用地图固定元素校准
每次重置里程计后，将起点设为原点。通过逆透视变换计算前方两条线的相对位置，结合已知终点坐标，求解旋转矩阵和平移向量，从而推算出当前视角下的 P 点坐标。
图 6 固定小橙的位置，终点是 (1.9m, -1.5m)
图 7 地图固定元素示意
核心计算公式如下：
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
print(f"(x1, y1): ({x1}, {y1}), (x2, y2): ({x2}, {y2}), (x3, y3): ({x3}, {y3})")
return x3_, y3_
思路 2——YOLO 识别校正
不重置里程计，直接使用 YOLO 识别 P 点，结合单应性矩阵计算全局坐标。此方法无需停车，适合高速场景。
# 单应性矩阵，计算全局坐标
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
训练数据需覆盖各种角度和遮挡情况，建议采集并增强至 2 万张以上。
修改 STM32 源码
STM32 底层控制对车辆性能影响显著。主要调整包括舵机转角多项式系数，确保左右转向对称；提高串口发送频率至 50Hz，波特率设为 921600；关闭非必要外设（CAN、蓝牙）；修改 EKF 配置以匹配新的 IMU 频率。
图 舵机转角限制
图 多项式输入输出
图 右前轮转向角限幅
图 原始多项式结果
图 调整后的多项式
图 对称性验证
图 串口配置
图 外设关闭
图 启动文件修改
图 EKF 频率设置
图 EKF 配置对比
补充：数据处理脚本
为提高效率，建议使用预训练模型自动标注，再人工复核。以下提供删除无效数据、数据增强及分批次打包的脚本。
自动标注脚本
import argparse
import os
import shutil
import time
from pathlib import Path
import torch
import cv2
from models.experimental import attempt_load
from utils.datasets import LoadImages
from utils.utils import non_max_suppression, scale_coords, xyxy2xywh
from utils.torch_utils import select_device, time_synchronized
def auto_annotate(source, weights, output, img_size=640, conf_thres=0.25, iou_thres=0.45, view_img=False):
device = select_device(device)
half = device.type != 'cpu'
model = attempt_load(weights, map_location=device)
names = model.module.names if hasattr(model, 'module') else model.names
dataset = LoadImages(source, img_size=imgsz)
t0 = time.time()
img = torch.zeros((1, 3, imgsz, imgsz), device=device)
_ = model(img.half() if half else img)
for path, img, im0s, _ in dataset:
img = torch.from_numpy(img).to(device)
img = img.half() if half else img.float()
img /= 255.0
if img.ndimension() == 3:
img = img.unsqueeze(0)
t1 = time_synchronized()
pred = model(img, augment=False)[0]
pred = non_max_suppression(pred, conf_thres, iou_thres, classes=None, agnostic=False)
t2 = time_synchronized()
p, im0 = path, im0s.copy()
txt_path = str(Path(output) / Path(p).stem) + ('.txt')
open(txt_path, 'w').close()
gn = torch.tensor(im0.shape)[[1, 0, 1, 0]]
if pred is not None:
for i, det in enumerate(pred):
if det is not None and len(det):
det[:, :4] = scale_coords(img.shape[2:], det[:, 4], im0.shape).round()
with open(txt_path, 'w') as f:
for *xyxy, conf, cls in reversed(det):
xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()
line = "%d %.6f %.6f %.6f %.6f" % (cls, *xywh)
f.write(line + "\n")
print(f'{Path(p).name} done. ({t2 - t1:.3f}s)')
删除无效数据
import os
from pathlib import Path
def remove_invalid_images_labels(image_dir, label_dir):
deleted_images = 0
deleted_labels = 0
for image_file in os.listdir(image_dir):
if image_file.lower().endswith(('.jpg', '.png', '.jpeg')):
image_path = os.path.join(image_dir, image_file)
label_path = os.path.join(label_dir, Path(image_file).stem + '.txt')
if not os.path.exists(label_path):
os.remove(image_path)
deleted_images += 1
print(f"删除图片（无标签）: {image_file}")
else:
with open(label_path, 'r') as f:
content = f.read().strip()
if not content:
os.remove(image_path)
os.remove(label_path)
deleted_images += 1
deleted_labels += 1
print(f"删除无效数据：{image_file} 和对应标签")
print(f"\n操作完成！共删除：{deleted_images} 张图片，{deleted_labels} 个标签")
数据增强
import torch
import torchvision.transforms as T
import torchvision.transforms.functional as TF
from pathlib import Path
import shutil
from PIL import Image
import random
from multiprocessing import Pool
import os
class YOLOAugment:
def __init__(self, output_dir):
self.output_dir = output_dir
Path(f"{output_dir}/images").mkdir(parents=True, exist_ok=True)
Path(f"{output_dir}/labels").mkdir(parents=True, exist_ok=True)
self.img_augment = T.Compose([
T.ColorJitter(brightness=0.3, contrast=0.3, saturation=0.2),
T.GaussianBlur(kernel_size=(3, 7))
])
def apply_augment(self, img_path, label_path, aug_id):
img = Image.open(img_path).convert('RGB')
with open(label_path) as f:
bboxes = [list(map(float, line.strip().split())) for line in f]
img_tensor = TF.to_tensor(img)
bboxes_tensor = torch.tensor(bboxes)
img_tensor = self.img_augment(img_tensor)
stem = Path(img_path).stem
self._save_results(img_tensor, bboxes_tensor, stem, aug_id)
return img, bboxes
def _save_results(self, img_tensor, bboxes, stem, aug_id):
aug_img = TF.to_pil_image(img_tensor)
aug_img.save(f"{self.output_dir}/images/{stem}_aug{aug_id}.jpg")
with open(f"{self.output_dir}/labels/{stem}_aug{aug_id}.txt", 'w') as f:
for bbox in bboxes.numpy():
line = ' '.join(map(str, bbox))
f.write(line + '\n')
def process_file(args):
img_path, label_path, output_dir, aug_per_image = args
augmenter = YOLOAugment(output_dir)
for i in range(1, aug_per_image + 1):
augmenter.apply_augment(img_path, label_path, i)
shutil.copy(img_path, f"{output_dir}/images/{Path(img_path).name}")
shutil.copy(label_path, f"{output_dir}/labels/{Path(label_path).name}")
if __name__ == "__main__":
root_path = os.path.dirname(__file__)
input_dir = os.path.join(root_path, "new1")
output_dir = os.path.join(root_path, "new1_aug")
aug_per_image = 3
num_workers = 4
tasks = []
for img_file in Path(f"{input_dir}/images").glob("*.*"):
if img_file.suffix.lower() in ('.jpg', '.png', '.jpeg'):
label_file = Path(f"{input_dir}/labels/{img_file.stem}.txt")
if label_file.exists():
tasks.append((str(img_file), str(label_file), output_dir, aug_per_image))
print(f"开始增强 {len(tasks)} 张图像...")
with Pool(processes=num_workers) as pool:
pool.map(process_file, tasks)
orig_count = len(tasks)
aug_count = orig_count * aug_per_image
print(f"处理完成！\n- 原始图像保留：{orig_count} 张\n- 增强图像生成：{aug_count} 张\n- 总数据量：{orig_count + aug_count} 张")
分批次打包
import os
import zipfile
import math
from pathlib import Path
def create_task_packs(images_dir, labels_dir, output_dir, tasks=3, label_txt=False):
image_files = sorted([f for f in os.listdir(images_dir) if f.endswith(('.jpg', '.png'))])
label_files = sorted([f for f in os.listdir(labels_dir) if f.endswith('.txt')])
image_stems = {Path(f).stem for f in image_files}
label_stems = {Path(f).stem for f in label_files}
unmatched = image_stems.symmetric_difference(label_stems)
if unmatched:
print(f"⚠️ 警告：发现 {len(unmatched)} 个不匹配文件")
return
total_pairs = len(image_files)
pairs_per_task = math.ceil(total_pairs / tasks)
print(f"数据集统计:\n- 图片数量：{len(image_files)}\n- 标注数量：{len(label_files)}\n- 将分成 {tasks} 个任务包，每个约 {pairs_per_task} 对数据\n")
os.makedirs(output_dir, exist_ok=True)
for task_num in range(1, tasks + 1):
start_idx = (task_num - 1) * pairs_per_task
end_idx = min(start_idx + pairs_per_task, total_pairs)
task_images = image_files[start_idx:end_idx]
task_labels = [Path(f).stem + '.txt' for f in task_images]
zip_path = os.path.join(output_dir, f"task_{task_num}.zip")
print(f"创建任务包 {task_num}:\n- 包含图片：{len(task_images)} 张\n- 包含标注：{len(task_labels)} 个\n- 保存到：{zip_path}")
with zipfile.ZipFile(zip_path, 'w', zipfile.ZIP_DEFLATED) as zipf:
for img in task_images:
img_path = os.path.join(images_dir, img)
zipf.write(img_path, f"images/{img}")
for label in task_labels:
label_path = os.path.join(labels_dir, label)
if os.path.exists(label_path):
zipf.write(label_path, f"labels/{label}")
else:
print(f"⚠️ 缺失标注文件：{label}")
print(f"\n🎉 任务包创建完成！共生成 {tasks} 个压缩包，保存在：{output_dir}")
if __name__ == "__main__":
root_path = os.path.dirname(__file__)
dataset_dir = os.path.join(root_path, "new1")
output_dir = os.path.join(root_path, "package")
label_txt = os.path.join(root_path, "labels.txt")
num_tasks = 4
create_task_packs(
images_dir=os.path.join(dataset_dir, "images"),
labels_dir=os.path.join(dataset_dir, "labels"),
output_dir=output_dir,
tasks=num_tasks,
# label_txt=label_txt,
)
后记
希望这些经验能为后续参赛的同学提供参考。