《从零搭建无人船控制系统》系列第五篇

第五章船的眼睛们 — 多传感器数据采集架构一条无人船身上有 RTK、IMU、雷达、LiDAR 四种传感器。四种协议、四种频率、四种数据格式。怎么设计采集架构让它们互不干扰坏了还能自动降级1. 四个传感器一张表传感器 协议 频率 数据量/帧 任务优先级 作用 ────────────────────────────────────────────────────────── RTK NMEA 0183 10 Hz ~200 字节 4 厘米级定位 IMU I2C 100 Hz 14 字节 4 加速度角速度 雷达 HEX帧 33 Hz 14 字节 4 多目标检测速度 LiDAR HEX帧 240 Hz 92 字节 4 点云障碍轮廓频率差 24 倍LiDAR 240Hz vs RTK 10Hz数据量差 7 倍LiDAR 92B vs IMU 14B但共用同一个 CPU。怎么设计2. 每个传感器一个 FreeRTOS 任务// 标准化传感器任务的骨架structSensorTask{TaskHandle_t handle;uint32_tperiod_ms;// 任务周期uint32_ttimeout_ms;// 离线超时void*data_global;// 全局数据指针bool online;};// 以 LiDAR 为例:staticvoidlidar_task_entry(void*arg){// ① 初始化 UARTcomSetBaud(LIDAR_UART_PORT,LIDAR_UART_BAUD);comClearRxFifo(LIDAR_UART_PORT);TickType_t wakexTaskGetTickCount();while(1){// ② 排空 UART 环形缓冲uint16_tlen;while((lencomGetBuf(LIDAR_UART_PORT,rx_buf,sizeof(rx_buf)))0){process_bytes(rx_buf,len);// 帧状态机 → 解析 → 推入代价地图}// ③ 超时检测: 500ms 没数据 → 离线if(now-last_update500){onlinefalse;}// ④ IWDG 心跳IWDG_MonitorTask_Heartbeat(xTaskGetCurrentTaskHandle());// ⑤ 周期等待vTaskDelayUntil(wake,pdMS_TO_TICKS(10));// 100Hz}}为什么不用一个大任务轮询所有传感器因为一个传感器超时不应该影响其他传感器。如果 RTK 的 GPS 信号丢了100ms 超时LiDAR 还在正常采。大任务轮询的话RTK 的 100ms 超时阻塞了整个循环LiDAR 跟着一起丢帧。3. 生产者-消费者模式传感器任务是生产者——写入全局数据。运动控制任务是消费者——读取全局数据做决策。// 生产者 (radar_task, 33Hz)g_radar_data.min_distance2.5f;g_radar_data.max_threat0.8f;g_radar_data.data_validtrue;g_radar_data.last_update_mstime_base_get_tick_ms();// 消费者 (motion_control, 50Hz)if(g_radar_data.data_valid(now-g_radar_data.last_update_ms)200){// 雷达数据新鲜, 使用它avoid_update(g_radar_data.min_distance,g_radar_data.min_angle);}else{// 雷达离线, 只用 LiDAR}关键消费者不阻塞等生产者。如果传感器离线消费者用上次有效值或标记无效继续工作——船不能因为一个传感器坏了就停下来。4. 传感器健康监控sensor_health.c是一个独立的 10Hz 任务专门检查传感器的健康状况检测项: GPS: freeze检测 — 连续3秒坐标不变 → FAULT glitch检测 — 瞬时跳变50m → DEGRADED HDOP检测 — HDOP2.0 → DEGRADED IMU: timeout检测 — 100ms无数据 → STALE stall检测 — 连续30s STALE → FAULT 饱和检测 — accel8g → FAULT LiDAR: 点数骤降 — 30%典型值 → 天气DEGRADED/SEVERE 超时检测 — 500ms无数据 → OFFLINE Radar: 超时检测 — 500ms无数据 → OFFLINE健康状态的输出不是好/坏两个状态而是 4 级OK → DEGRADED → STALE → FAULT 可以信 有点问题 很久没更新 彻底坏了不同级别对应不同的系统降级策略。5. 传感器降级坏了怎么办场景1: GPS 信号丢失 (INS 模式) GPS OK → GPS丢失 → 切入INS模式 INS模式: EKF纯IMU预测航向速度靠IMU推算 → 10秒内GPS恢复 → 切回原模式 → 10秒超时 → EMERGENCY急停 场景2: IMU 故障 (GPS_ONLY 模式) IMU OK → IMU故障 → 切入GPS_ONLY模式 GPS_ONLY: 用GPS原始航向速度, 跳过EKF → IMU恢复 → 切回原模式 场景3: GPSIMU同时故障 → 直接EMERGENCY急停 → 两个眼睛都瞎了, 船不能走了 场景4: 雷达正常但LiDAR雨天衰减 → 传感器融合自动切换权重 → 好天气: LiDAR 0.7 雷达 0.3 → 恶劣天气: 雷达 0.8 LiDAR 0.2 → DWA安全距离 2m核心设计思想不是一个传感器坏了就急停而是试试只有一个传感器能不能继续走。只有一个 GPS 也能导航精度差但能用只有一个雷达也能避障看得模糊但比瞎了好。6. 四种传感器的时间对齐四种传感器频率不同EKF 怎么融合它们IMU: 100Hz → EKF predict (每10ms推一步状态) RTK: 10Hz → EKF update (有GPS新帧时才更新) 雷达: 33Hz → 避障模块直接读, 不进EKF LiDAR:240Hz → 代价地图直接更新, 不进EKF 关键: EKF predict 和 update 是异步的 predict: 每50Hz控制周期都跑 (靠IMU gyroaccel推) update: 只有收到新GPS数据才跑 (更新位置航向速度)这是松耦合融合——每个传感器按自己的频率干活不要求时间戳严格对齐。7. 踩过的坑坑 1GPS 跳零导致 EKF 发散GPS 偶尔输出lat0, lng0接收机冷启动或信号闪烁。EKF 以为船瞬移到了非洲海岸协方差爆炸。修复nav_ekf_update_position前检查坐标是否在合理范围±1°以内。坑 2IMU 没初始化就喂数据IMU 任务在 MPU6050 还没 calibrate 完成时就发布了数据。EKF 吃到了全零的陀螺仪数据以为船在完美静止→ 实际上船在漂。修复快照字段加imu_ok标记timestamp 非零IMU 未初始化时yaw_rate 0而不是垃圾数据。坑 3传感器离线后 data_valid 没及时清雷达离线 5 秒后data_valid还是 true避障模块还在用 5 秒前的障碍位置——障碍物 5 秒前在 12 米外现在可能已经在 2 米外了。修复每个传感器任务内部有超时检测now - last_update timeout超时自动标记无效。8. 下一章预告第六章6-State EKF — 不用矩阵求逆的卡尔曼滤波标准卡尔曼需要 6×6 矩阵求逆Cortex-M7 算一次要几千周期。我们用顺序卡尔曼技巧——每次只观测一维矩阵退化为标量除法。数学原理 C 代码。本文是《从零搭建无人船控制系统》系列第五篇。项目地址[煜坤 YuKun]开源准备中。