无人清扫车Demo复盘：征程6上从0到1的感知与控制链路

上个月和团队参加了“挑战杯”无人清扫车赛题，要求用征程6做视觉感知，实现自动清扫、避障、路径规划。从拿到板子到Demo跑通，前前后后折腾了快一个月。这篇把整个项目的感知链路和控制系统复盘一下。

一、硬件配置

· 主控：征程6开发板（双核BPU + 4x Cortex-A55）

· 相机：USB3.0工业相机，1080p@30fps

· 电机驱动：CAN总线连接四路轮毂电机

· 超声波：I2C接口，前/后/左/右各一个

· 电源：24V锂电池 + DC-DC降压模块

二、感知链路

```

图像输入 (1080p)

↓

预处理 (IPU: resize+normalize to 640x640)

↓

BPU推理 (检测模型: 行人/障碍物/垃圾)

↓

后处理 (CPU: NMS + 距离估计 + 可行驶区域分割)

↓

世界坐标系映射 (相机标定 + 逆透视变换)

↓

局部地图 (2D栅格地图 + 动态障碍物标记)

```

检测模型

用YOLOv5-S，在COCO预训练权重上做迁移学习。检测三类目标：行人、障碍物、垃圾。

训练数据：测试场地拍了3000张图，LabelImg标注。垃圾检测最难，目标小、形状不规则、和地面纹理对比度低。

解决方案：

1. 小目标增强：Copy-Paste策略，把小垃圾目标复制到不同背景

2. 多尺度训练：每10个epoch换一次输入尺寸（480x480 -> 640x640 -> 800x800）

3. Focal Loss：解决正负样本不平衡

训练结果：

· 行人：mAP@0.5 = 91.2%

· 障碍物：mAP@0.5 = 87.5%

· 垃圾：mAP@0.5 = 68.3%

模型部署

用天工工具链量化编译。P3检测头（负责小目标）用int16精度：

```yaml

layer_config:

- layer_name: "model\\.24\\.m\\.0"

precision_mode: int16

```

最终模型延迟22ms（45fps），满足30fps实时性要求。

后处理与距离估计

单目深度估计：相机标定 + 逆透视变换（IPM）。

```python

# 透视变换矩阵

src_points = np.float32([[960, 800], [400, 1080], [1520, 1080], [960, 600]])

dst_points = np.float32([[320, 480], [0, 480], [640, 480], [320, 0]])

M = cv2.getPerspectiveTransform(src_points, dst_points)

bird_view = cv2.warpPerspective(image, M, (640, 480))

```

在鸟瞰图里，像素坐标和世界坐标成线性关系，根据检测框在鸟瞰图里的位置估算距离。精度有限（误差约±0.5米），但低速场景够用。

可行驶区域分割

训练轻量分割模型（UNet，MobileNet backbone），输出可行驶区域mask。和检测模型并行跑在两个BPU core上：

```cpp

auto detect_future = std::async(std::launch::async, [&](){

return RunDetection(frame);

});

auto seg_future = std::async(std::launch::async, [&](){

return RunSegmentation(frame);

});

```

三、控制链路

```

局部地图 (障碍物 + 可行驶区域)

↓

路径规划 (A* / Boustrophedon覆盖)

↓

轨迹跟踪 (纯跟踪算法)

↓

电机控制 (差速转向 PID)

↓

车辆动作

```

路径规划：A* + Boustrophedon

清扫车需要覆盖式清扫。把可行驶区域分成条带，蛇形覆盖：

```cpp

for (int stripe = 0; stripe < num_stripes; stripe++) {

int start_x = stripe * SWEEP_WIDTH;

if (stripe % 2 == 0) {

// 奇数条带从上到下

for (int y = 0; y < HEIGHT; y++) {

for (int x = start_x; x < start_x + SWEEP_WIDTH; x++) {

if (map.grid[y][x] == DRIVABLE) {

path.push_back({x, y});

break;

}

}

}

} else {

// 偶数条带从下到上

for (int y = HEIGHT - 1; y >= 0; y--) {

for (int x = start_x + SWEEP_WIDTH - 1; x >= start_x; x--) {

if (map.grid[y][x] == DRIVABLE) {

path.push_back({x, y});

break;

}

}

}

}

}

```

遇到障碍物时，用A*做局部绕行。

轨迹跟踪：纯跟踪算法

```cpp

class PurePursuit {

float wheelbase_;

float lookahead_;

public:

std::pair ComputeControl(const VehicleState& state,

const std::vector& path) {

// 找lookahead point

Point target = FindLookaheadPoint(state, path);

// 转换到车辆坐标系

float dx = target.x - state.x;

float dy = target.y - state.y;

float local_x = dx * cos(state.theta) + dy * sin(state.theta);

float local_y = -dx * sin(state.theta) + dy * cos(state.theta);

// 计算曲率

float curvature = 2 * local_y / (lookahead_ * lookahead_);

// 差速转向

float v_left = state.v - curvature * wheelbase_ / 2;

float v_right = state.v + curvature * wheelbase_ / 2;

return {v_left, v_right};

}

};

```

超声波融合

超声波补盲（摄像头看不到正下方和侧面近距离障碍物）。

```cpp

class UltrasonicFusion {

const float EMERGENCY_THRESHOLD = 0.3f; // 30cm

public:

bool IsEmergency(const UltrasonicData& us) {

return us.front < EMERGENCY_THRESHOLD ||

us.back < EMERGENCY_THRESHOLD ||

us.left < EMERGENCY_THRESHOLD ||

us.right < EMERGENCY_THRESHOLD;

}

void AdjustControl(std::pair& speeds, const UltrasonicData& us) {

float min_dist = std::min({us.front, us.back, us.left, us.right});

if (min_dist < 1.0f) {

float scale = min_dist / 1.0f;

speeds.first *= scale;

speeds.second *= scale;

}

}

};

```

四、踩坑记录

坑1：CAN总线延迟

默认250kbps波特率，一条控制消息2ms。控制周期50ms，但端到端延迟达到80ms（采集30ms + BPU推理22ms + 后处理10ms + CAN传输2ms + 控制计算5ms + 电机响应10ms）。

解决：CAN波特率提到500kbps，控制周期降到20ms。端到端延迟压到45ms。

坑2：相机和BPU帧同步

USB相机V4L2阻塞采集，30fps就是33ms一帧。BPU推理+后处理32ms，两者刚好错开，实际感知帧率只有15fps。

解决：改成双缓冲异步采集。一个线程专门做V4L2采集放到ring buffer，感知线程取最新帧处理。

坑3：地面反光导致分割失效

光滑水泥地中午阳光直射时反光严重，分割模型把反光区域识别成不可行驶区域。

解决：预处理加CLAHE增强对比度：

```cpp

cv::Mat PreprocessForSegmentation(const cv::Mat& frame) {

cv::Mat lab, l_channel, enhanced;

cv::cvtColor(frame, lab, cv::COLOR_BGR2Lab);

std::vector channels;

cv::split(lab, channels);

l_channel = channels[0];

cv::Ptr clahe = cv::createCLAHE(2.0, cv::Size(8, 8));

clahe->apply(l_channel, enhanced);

channels[0] = enhanced;

cv::merge(channels, lab);

cv::cvtColor(lab, result, cv::COLOR_Lab2BGR);

return result;

}

```

坑4：电池电压波动

电机启动/停止引起电池电压波动（24V跌到20V）。BPU对电压敏感，低于阈值时推理报错。

解决：加DC-DC稳压模块稳到24V。软件层加电压监控线程：

```cpp

void VoltageMonitorThread() {

while (running) {

float voltage = ReadBatteryVoltage();

if (voltage < 21.0f) {

max_speed_ = 1.0f; // 降速

}

if (voltage < 19.0f) {

EnterSafeState(); // 停车

}

std::this_thread::sleep_for(std::chrono::seconds(1));

}

}

```

五、最终效果

· 感知帧率：30fps

· BPU推理延迟：22ms

· 端到端控制延迟：45ms

· 清扫覆盖率：85%

· 避障成功率：行人100%，静态障碍物95%，垃圾80%

比赛成绩：省级二等奖。扣分点在垃圾检测精度不高（68% mAP导致漏扫），以及复杂场景下路径规划不够智能。

六、注意事项总结

1. 感知是基础：检测+分割+深度估计链路要跑通，小目标检测最难。

2. BPU性能要榨干：O3+pipeline，小目标头int16，双核并行跑检测和分割。

3. 控制延迟要压：CAN波特率、双缓冲采集、异步推理。

4. 多传感器融合：超声波补盲、视觉主感知，超声波紧急情况优先。

5. 工程细节决定成败：地面反光、电压波动、帧同步这些小问题不解决系统跑不稳。