Files

2026-03-31 23:30:33 +08:00

39 KiB

Raw Blame History

ASER 平台 B 方案走廊相对定位实现与模块化解耦技术报告

执行摘要：本报告围绕你选择的 B 方案（走廊相对定位 Corridor-relative Estimation），在不破坏现有 ASER 工程结构与 STM32H743（上位机）↔ STM32F407（底盘）既有 CAN 协议的前提下，给出一套可落地、可扩展、可“先跑通再变强”的实现路线。核心思想是：不把比赛当作“绝对坐标 SLAM”，而把赛场 40 cm 窄通道视为拓扑段落序列，实时估计 横向偏差 e_y、航向误差 e_θ、沿通道进度 s，并用侧向测距闭环实现“居中/偏置行走 + 对齐”，以对抗 赛场尺寸 ±5% 误差、地毯导致的轮滑、测距偶发失效（比赛规则明确存在地毯模拟松软路面、且尺寸允许误差 ±5%）。同时，将 0x100 速度指令 20 ms 周期硬实时发送作为最高优先级工程约束，任何导航/融合/日志都不得影响其节拍（否则底盘可能进入 SAFE_FAULT）。赛场层面，规则要求 5 分钟限时且未完全驶出与未停在启动区内可判 0 分，因此系统优先级必须是 安全与完赛闭环 > 定位美观 > 路径最优。（赛事通道/出入口 40 cm、启动区、5 分钟限制、尺寸误差 ±5%：见《附件6…比赛规则》1–3 页与 14 页；上位机 CAN 协议关键帧：见《通讯协议》2–4 页、8–10 页；ASER 工程任务/协议层约束：见《aser》1–5 页。）

系统约束与现状梳理

赛场与任务约束对算法与工程的“硬约束”

比赛场地为 390 cm × 300 cm，共有 5 条田垄；围栏与田垄之间、以及相邻田垄之间均为 40 cm 通道（垄沟），出入口宽 40 cm，出入口外侧紧邻 40 cm × 100 cm 启动区；比赛 限时 5 分钟，且规则强调必须驶离场地并停在启动区，否则可能判 0 分；尺寸允许误差 ±5%（意味着通道宽可能约 38–42 cm，任何“贴边”策略都必须考虑裕量）。这些约束直接决定控制目标应是“别蹭边、别失控、能跑完”，而非追求全局米级地图的一致性。（《附件6…比赛规则》1–3 页、14 页）

现有硬件与软件约束

以下为你给定的固定约束（本报告按此设计）：

车体：20×20 cm，轮厚 3 cm；差速四轮结构（四电机）。
控制器三块：
- STM32F407（底盘）：已实现四轮 LADRC，通过 CAN 与上位机通信。
- STM32H743（主控/上位机）：传感器融合、底盘与机械臂控制（现状上位机）。
- K230（视觉中枢）：用于视觉语义/目标等（本报告将其作为增强/降级输入，不进入硬实时链路）。
ASER 工程形态（来自你上传的 ASER 文档）：基于 STM32CubeMX + FreeRTOS（CMSIS-V2），已建立 20 ms canTxTask 与 100 ms monitorTask；CAN 协议层在 App/Can/snc_can_app.c/.h，并明确要求保持既有协议/中断接收链路，新增功能优先放到 App/ 目录以避免再生成覆盖；FDCAN 接收链路保持 IRQ→回调→协议解析的单路径。（《aser》1–5 页）

既有 CAN 协议是“不可破坏”的硬边界

你已确认 H7↔F4 通信协议“已经制定好”，因此本文不再“重新设计协议”，而是把协议当作 契约（contract）：

0x100 为速度命令，DLC=8，包含 vx_x1000、wz_x1000、rolling counter、CRC8；要求 建议 20 ms 周期持续发送，且 不得超过 150 ms 不发送合法 0x100，否则底盘进入 SAFE_FAULT；心跳帧 0x080 不刷新运动看门狗。（《通讯协议》2–4 页、8–10 页）
这些要求决定：所有算法模块必须围绕“20 ms 严格出 v/ω”来组织调度与解耦。

传感器清单、性能与可观测性分析

传感器清单（按你给定配置）

4 电机编码器：提供四轮增量/速度，可构造差速里程计，并可用于同侧前后轮差异的 轮滑检测（工程上很关键）。
高精度单轴 IMU（ω_z 或 θ）：你给定“短时漂移可忽略”。这对 B 方案非常有利：可把“短段航向保持/原地转向闭环”做得很稳，同时简化滤波状态维度（但仍需讨论风险与退路，见后文）。
8 个测距模块：前后左右各 2 个：
- 左右侧向：VL53L0X V2（最远 2 m）。ST 官方产品页说明 VL53L0X 为 ToF 测距，最大可到 2 m。citeturn2search3
  同时其官方数据手册给出典型“Range profile”：High speed 20 ms、Long range 33 ms、High accuracy 200 ms 等 timing budget 配置（对你做“关键动作更稳、巡航更快”的动态配置非常实用）。citeturn2search41
- 前后向（每端 2 个）：STP-23L（7 cm–7.5 m） + ATK‑MS53L1M（4 m）。其中 4 m 量程模块工程上高度可能基于 VL53L1X 类 ToF：ST 官方 VL53L1X 产品页给出“up to 4 m、up to 50 Hz”能力，可作为你对 4 m 级传感器的性能参考上限。citeturn0search0
结构：差速四轮 → 上位机输出 (v, ω) 即可驱动底盘；底盘闭环已由 F407 LADRC 负责。

B 方案的可观测性：为什么侧向双点能同时观测 e_y 与 e_θ

走廊相对定位的关键是：只关心“相对墙/垄侧”的横向与航向误差，不强求全局 (x,y)。只要左右两侧各有前后两个测距点（你是每侧 2 个 VL53L0X），就可以构造两个稳定的派生量测：

航向误差 e_θ（相对走廊方向）：利用同侧前后距离差与传感器纵向基线 (L_s)，可用
[ e_{\theta,L} \approx \arctan\left(\frac{d_{Lf}-d_{Lr}}{L_s}\right),\quad e_{\theta,R} \approx \arctan\left(\frac{d_{Rf}-d_{Rr}}{L_s}\right) ] 该构造与你上传的《大体方案》对 B 方案与派生量测的定义一致（《大体方案》4–5 页）。
横向误差 e_y（相对中心线/偏置线）：用左右平均间隙差得到
[ e_y \approx \frac{1}{2}( \bar d_L - \bar d_R) - y_{\text{offset}} ] 其中 (y_{\text{offset}}) 是你可以显式引入的“偏置行走”目标（例如机械臂在右侧外凸时令车辆略偏左，降低擦碰风险）。同样与《大体方案》对 B 方案的描述一致（《大体方案》4–5 页）。

前后向测距（STP-23L / 4 m 模块）主要用于：到端触发（段落切换）、安全制动、以及对沿程 (s) 的事件校正（见后文）。

走廊相对定位与融合算法设计

问题建模与坐标系

定义走廊（或垄沟）局部坐标系 ({C})：

(x_C)：沿走廊前进方向；
(y_C)：走廊横向（向左为正）；
车辆车体坐标 ({B})：(x_B) 前、(y_B) 左。

B 方案输出状态： [ \mathbf{x}c = [e_y,\ e\theta,\ s]^T ] 其中：

(e_y)：车辆参考点（建议用车体几何中心或“控制点”）相对走廊目标线（中心线或偏置线）的横向偏差；
(e_\theta)：车辆航向相对走廊方向的误差；
(s)：沿走廊进度（m），用于段落终止触发/动作编排，不作为“强闭环精确位姿”。

传感器融合总体策略：分层、分频、强约束

建议采用“三层融合与控制”：

层 A（硬实时输出层，20 ms）：必须产出 (v_cmd, ω_cmd) 并发送 CAN 0x100。无论任何模块异常，这层都不能停摆（最多把速度降到 0 并持续发送合法 0x100）。这是由底盘运动看门狗与超时机制决定的：0x080 心跳不刷新看门狗，只有合法 0x100 刷新；超时阈值 150 ms。（《通讯协议》2–4 页、8–10 页）
层 B（相对定位与走廊闭环，建议 50–100 Hz）：估计 (e_y,e_\theta,s)，并形成走廊跟随控制律；允许降级（只用单侧、只用 IMU 航向保持等），但必须在可控范围内给出指令。
层 C（段落状态机/任务决策，10–20 Hz）：执行段脚本（Segment List）、处理到端触发、重定位行为（扫描/后退重试/停车保护）、以及与机械臂/视觉的协同。

这样做的目的：把“安全与完赛”从定位精度中解耦。

状态估计实现方案对比：从“极简互补”到“鲁棒 EKF/UKF”

你明确提出希望覆盖 EKF/UKF/粒子滤波。结合 STM32H743 算力与工程风险，本报告建议按阶段实现：

极简方案：互补滤波 + 鲁棒观测融合（优先推荐做 P0）

利用你给定“短时 IMU 漂移可忽略”，可以先不引入复杂滤波矩阵，直接做：

航向误差 (e_\theta)：
- 预测：(e_{\theta,k|k-1}=e_{\theta,k-1} + \omega_z \Delta t)
- 校正：用侧向差分观测 (e_{\theta,L}, e_{\theta,R}) 做加权融合（按健康度/置信度权重），再与预测做互补融合：
  [ e_\theta \leftarrow \alpha(e_\theta + \omega_z\Delta t) + (1-\alpha)\cdot e_{\theta,\text{meas}} ]
横向误差 (e_y)：
- 直接由 (\frac{1}{2}(\bar d_L-\bar d_R)-y_{\text{offset}}) 给出，并做一阶低通（根据 VL53L0X timing budget 动态调整滤波带宽；20 ms 档噪声较大、200 ms 档更稳但延迟更大，可从数据手册的 profile 及误差差异得到工程依据）。citeturn2search41
沿程 (s)：
- 用编码器里程计积分为主（来自 CAN 0x200 的轮增量 ticks，你需要在 H743 侧累加积分）。（《通讯协议》5–7 页）
- 用前向测距做“事件校正”：当 (d_\text{front}) 小于阈值（接近端部围栏）触发段切换，并把 (s) 校正到“段长 - d_front - 安装偏置”。

优点：实现快、数值稳定、适合先把整车跑通；缺点：难以严格输出协方差，也不便于统一做马氏门限。

鲁棒 EKF（推荐做 P1）

若你希望更系统地融合并获得一致的健康度/协方差，则做小维 EKF：

状态：(\mathbf{x}=[e_y,e_\theta,s]^T) 或扩展为 ([e_y,e_\theta,s,b_g,k_s])（陀螺零偏、轮滑比例因子）以吸收轮滑/地毯误差（《大体方案》4 页提到类似扩展思路）。
预测模型（简化差速运动学在走廊局部系下的表达）： [ \begin{aligned} e_{y,k+1} &\approx e_{y,k} + v_k\sin(e_{\theta,k})\Delta t \ e_{\theta,k+1} &\approx e_{\theta,k} + (\omega_{z,k}-b_{g,k})\Delta t \ s_{k+1} &\approx s_k + v_k\cos(e_{\theta,k})\Delta t \end{aligned} ] 其中 (v_k) 来自里程计估计或上一周期命令（两者在轮滑情况下要区别对待）。
量测模型：
- (z_{e_y}=\frac{1}{2}(\bar d_L-\bar d_R)-y_{\text{offset}})
- (z_{e_\theta})：由左/右侧差分观测（可单侧/双侧）
- (z_s)：端部事件约束（触发式更新）
异常值剔除（创新门限/χ²）：
用马氏距离（innovation 的归一化残差）做门限，d² 超限则拒绝该测距更新、降低该传感器健康度，并触发降级/重定位动作。创新卡方检验型鲁棒 EKF 在导航融合领域是成熟手段；例如武汉大学学报给出“基于创新卡方检验的扩展鲁棒卡尔曼滤波”并说明可有效抑制观测粗差、提高稳定性。citeturn5search1
同时马氏距离平方在高斯假设下与 χ² 分布的关系，可作为你选择阈值的理论依据。citeturn5search0

UKF（备选 P1/P2）

UKF 的优势是减少雅可比推导错误风险，对非线性更稳健；其系统性论述可参考 Julier & Uhlmann 的综述文章（Proceedings of the IEEE, 2004）。citeturn4search8
但对 MCU 工程而言，UKF 的实现细节（sigma 点、数值稳定、协方差正定维护）仍需严格测试。建议在 EKF 稳定后再引入 UKF，或仅在仿真/离线回放中验证。

粒子滤波（不建议作为主线）

粒子滤波更适合多峰分布或强非线性/非高斯，但你这里的走廊状态空间维度小、观测约束强、且 MCU 实时性要求更硬，粒子滤波的收益通常不如鲁棒 EKF/UKF 明显。建议仅在“视觉地标 + 多段拓扑歧义”场景出现时再考虑。

控制律：走廊闭环优先，轨迹跟踪为辅

走廊段推荐直接闭环 (e_y,e_\theta) 输出角速度： [ \omega_\text{cmd}=k_\theta e_\theta + k_y e_y ] 线速度： [ v_\text{cmd}=\min(v_\text{ref},\ v_\text{safety}) ] 其中 (v_\text{safety}) 由安全层根据前向距离与最小侧向间隙硬约束裁剪（见下节）。

对于“入场对准、退出与停回启动区”等非典型走廊段，可以上层用段脚本触发“短距离轨迹跟踪”（如 Pure Pursuit），但其输出仍应统一进入 命令仲裁器，最终被安全层裁剪后送给 CAN 0x100。

安全层、异常检测与重定位策略

这是 B 方案能否“比赛不判零”的关键（规则对失控/冲出场地会强制罚下，且未驶出/未停在启动区可能 0 分）。《附件6…比赛规则》1、8–9 页

传感器健康度与一致性检查

建议对每个测距传感器维护 health_score∈[0,1] 与 state∈{OK,SUSPECT,FAIL}，并用以下规则更新（工程可解释、易调参）：

有效性：超量程/无数据/固定值卡死 → 直接 FAIL（可加 N 次确认）。
跳变：(|d_k-d_{k-1}|>\Delta d_\text{max}) 且与冗余传感器不一致 → SUSPECT。
走廊一致性：
- 左右平均距离差过大（(e_y) 过大）接近阈值 → 触发降速/停车；
- 左右和与走廊宽度模型严重不符（考虑 ±5% 尺寸误差）→ 可能进入入口/出口开阔区或测距误读，切换“入口/开阔区模式”。

轮滑检测（地毯场景必须做）

规则明确会在垄沟中随机铺设地毯模拟松软路面（《附件6…比赛规则》2–3 页），强烈建议把轮滑检测当作“安全与融合共同输入”：

同侧前后轮编码器差异过大（四轮提供更多冗余）
(|\Delta\theta_\text{enc} - \Delta\theta_\text{imu}|) 超阈值（IMU 短时漂移可忽略时，这个检测更可靠）

轮滑触发后动作：

降低 (v_\text{max})、限制加速度；
融合层增大过程噪声 (Q) 或降低对里程计的信任；
控制层更依赖侧向闭环（(e_y,e_\theta)），减少对 (s) 的硬依赖。

重定位/恢复动作链（建议用段脚本固化）

当“走廊观测不可用”或“误差超过安全阈值”时，不要让系统继续前冲；建议固化为可重复执行的恢复链：

低速前进 + 侧向对齐：尝试恢复 (e_\theta) 可观测（左右/单侧）。
原地旋转扫描：转动寻找两侧墙面，使 (e_y/e_\theta) 可观测。
后退重试或停车保护：连续失败 N 次后退出（比赛是否允许等待/重试由你们策略决定，但从“避免失控罚下”角度，停车保护是最后底线）。

这类恢复链在你上传的《大体方案》中已有雏形（《大体方案》13 页），建议工程化为状态机（NORMAL/DEGRADED/RECOVERY/STOP_SAFE）。

“IMU 短时漂移可忽略”的简化与风险

你给定该条件，可以显著简化实现（例如可不立刻估计 (b_g)），但必须明确风险边界：

风险来源：温漂、振动、饱和、安装误差、长时间累计偏置。短时可忽略 ≠ 全程可忽略。
工程缓解：
- 开机静止校零（估计陀螺零偏）；
- 在走廊段用侧向差分观测周期性校正 (e_\theta)（即使不用 EKF，也做互补融合）；
- 若引入 EKF/UKF，建议把 (b_g) 作为可选状态并在“长段/高温漂”时开启。

通讯协议与硬实时调度约束

CAN 报文契约（按现有协议原样遵守）

你上传的《通讯协议》给出了当前固件实装版协议要点（标准帧 11-bit、DLC、字节序、缩放、CRC8 等），这里整理为“实现必须满足的契约”：

0x100 Velocity Command（H743→F407）
- DLC=8；vx,wz 按 int16 = 物理量 × 1000（小端）；包含 rolling_counter 与 CRC8-SAE J1850（对 Byte0~6 计算，放 Byte7）。
- 推荐周期 20 ms；绝不能超过 150 ms 不发送合法 0x100（即使停车也继续发 vx=0,wz=0 的合法帧）；rolling counter 在正常运行时要求与上一帧差值在 1..3，否则拒收。
  （《通讯协议》2–4 页、8–10 页）
0x080 Heartbeat（H743→F407）：仅表示链路活着，不刷新运动看门狗，不能替代 0x100。（《通讯协议》2–3 页、8–10 页）
0x181 Status（F407→H743，20 ms）：包含 system_state、system_health、diag_bits、cmd_age_10ms 等，用于安全降级决策。（《通讯协议》4–5 页）
0x184 Comm Diag（F407→H743，100 ms）：统计 CRC 错误、counter 拒收、bus-off 等，必须纳入诊断与回放。（《通讯协议》6 页）
0x200 Odom Delta（F407→H743，轮询约 60 ms）：四轮增量 ticks；上位机需自行累积。（《通讯协议》5–7 页）

说明：你已表示协议既有且固定，因此本报告不建议在 H7↔F4 CAN 上新增“破坏兼容”的帧。若未来要加扩展帧，建议在不影响现有 ID/节拍/接收逻辑的前提下做“可选附加 ID”，并通过版本位/能力位协商（但这属于后续扩展，不是当前 P0 必需）。

优先级、周期与超时处理（工程实现建议）

最高优先级：can_tx_0x100_task（20 ms，硬实时，禁止被日志/融合阻塞）。
ASER 工程现有结构已采用 20 ms 的 canTxTask 且强调绝对节拍（osDelayUntil）与协议层保持（《aser》1、5 页）。
第二优先级：CAN Rx 中断回调（接收 0x181/0x184/0x200），以“快照/无锁或短临界区”方式写入共享上下文，避免长时间占用 ISR。
第三优先级：控制/估计（50–100 Hz），必须保证“在下一个 0x100 截止前产出最新命令”，但即便估计超时，也要有“上一帧命令 + 安全裁剪”的保底。
监控任务（100 ms）：读取 cmd_age_10ms、diag_bits、comm diag，若发现异常趋势（例如 DIAG_COMM_TIMEOUT、bus-off、连续 CRC 错误风暴）立即触发上位机 STOP_SAFE 模式并把 (v,ω) 置零，但仍持续发合法 0x100。（《通讯协议》8–10 页）

为实现稳定周期，建议使用 FreeRTOS 的 vTaskDelayUntil()（按绝对时间唤醒，适合固定频率周期任务）。citeturn3search34
在 ISR→任务的事件通知上，任务通知（vTaskNotifyGiveFromISR）通常比信号量更轻，FreeRTOS 手册亦将其描述为更快的替代机制。citeturn2search42

关键消息帧示例（十六进制 + 结构体）

下面给出 0x100 的一个可复现实例（示例：vx=0.30 m/s，wz=0.80 rad/s，ctrl_flags=0x01，counter=0x10；CRC8=SAE J1850，对 Byte0~6 计算）：

vx_x1000 = 300 = 0x012C（小端：2C 01）
wz_x1000 = 800 = 0x0320（小端：20 03）
组合 Byte0~6：2C 01 20 03 01 00 10
计算得到 CRC8 = 0xA1
最终 8 字节：
2C 01 20 03 01 00 10 A1

对应 C 结构体（注意小端序与打包）：

#pragma pack(push,1)
typedef struct {
  int16_t vx_x1000;        // Byte0-1,  m/s * 1000
  int16_t wz_x1000;        // Byte2-3,  rad/s * 1000
  uint8_t ctrl_flags;      // Byte4
  uint8_t reserved;        // Byte5, fixed 0
  uint8_t rolling_counter; // Byte6
  uint8_t crc8;            // Byte7, CRC8-SAE J1850 over Byte0..6
} CanCmdVel_0x100_t;
#pragma pack(pop)

（0x100 字段定义、CRC 规则、rolling counter 规则、150 ms 超时：见《通讯协议》2–4 页、8–10 页。）

软件模块化与解耦方案

解耦目标与原则

在 ASER 现有工程约束下（协议层/中断链路/任务框架尽量不动），解耦的关键是把系统切成三类模块：

硬实时链路模块：唯一职责是“稳定发 0x100”，只从一个“命令槽（Command Slot）”读取最新的 (v,ω)；不做复杂计算、不打印日志。
可实时模块：估计、控制、预处理，允许在极端情况下掉周期，但必须提供保底输出与超时策略。
非实时模块：日志、回放、仿真接口、参数调试、统计；永不阻塞硬实时。

ASER 文档明确：CAN 协议层位置固定，并建议新功能放 App/ 目录，且 canTxTask/monitorTask 已存在。（《aser》1–5 页）

数据流总览（mermaid）

flowchart LR
  subgraph S[传感器/底盘输入]
    VL53[VL53L0X x4 侧向测距]
    FRONT[前向测距: STP-23L + 4m模块]
    BACK[后向测距: STP-23L + 4m模块]
    IMU[单轴IMU ωz/θ]
    CANRX[CAN Rx IRQ: 0x181/0x184/0x200]
  end

  subgraph P[预处理与健康度]
    PRE[时间戳对齐/滤波/有效性检测/健康度]
    OBS[派生观测构造: e_y_meas, e_theta_meas, d_front, d_back]
  end

  subgraph E[估计与控制]
    EST[走廊相对定位滤波器\n(x=[e_y,e_θ,s] 或扩展)]
    SEG[段脚本解释器 + 拓扑状态机]
    CTRL[走廊控制器/原地转向/入口对准]
    SAFE[安全监督器/降级状态机/命令仲裁]
  end

  subgraph R[硬实时输出]
    CANTX[CAN Tx Task\n0x100 @20ms]
  end

  subgraph L[工具链]
    LOG[日志记录/回放/故障注入]
    SIM[仿真/传感器注入接口]
  end

  VL53 --> PRE
  FRONT --> PRE
  BACK --> PRE
  IMU --> PRE
  CANRX --> PRE

  PRE --> OBS --> EST
  CANRX --> EST

  EST --> CTRL
  SEG --> CTRL
  CTRL --> SAFE --> CANTX

  CANRX --> SAFE
  SAFE --> LOG
  OBS --> LOG
  EST --> LOG
  SIM --> PRE

模块清单、接口消息、周期与最大延迟（必须表格）

下表给出“建议模块化拆分”，并明确每个模块 I/O、周期与延迟预算。注意：开发语言、CPU 负载预算未指定；下表的资源估计按 STM32H743 “小维滤波 + 50–100 Hz 控制”属于轻量任务的常见工程经验给出，最终应以运行时 profile 校准。STM32H743 的 480 MHz Cortex‑M7 与双精度 FPU、TCM RAM 能力，为小维滤波提供了硬件基础。citeturn1search1

模块	职责	输入（消息/接口）	输出（消息/接口）	典型周期	最大延迟（建议）	资源估计（H743）
CAN协议适配层（既有）	按既有协议收发、CRC、上下文维护（禁止破坏兼容）	FDCAN IRQ	`ChassisStatus(0x181)`、`CommDiag(0x184)`、`OdomDelta(0x200)`	IRQ/20ms/轮询	<2 ms 解析写入	低；保持原样
CAN Tx 0x100 硬实时任务	固定 20 ms 发送合法 0x100，任何情况下不断流	`CmdSlot(v,w,flags)`	CAN 0x100	20 ms	<1 ms 抖动	极低；最高优先级
编码器里程计模块	ticks 累加、四轮→差速等效、轮滑检测特征	`OdomDelta(0x200)`	`OdomEst{ds, v, yaw_enc}`、`SlipFeat`	50–100 Hz（或随0x200）	<20 ms	低
IMU采集模块	采集 ωz/θ、时间戳、静止校零	SPI/I2C/串口(未指定)	`ImuZ{wz, dt}`	200–500 Hz（建议）	<5 ms	低
VL53侧向驱动	4 个 VL53L0X 轮询/异步读取、模式切换（20/33/200ms）	I2C	`RangeSideRaw`	20–50 Hz（视 timing budget）	<40 ms	低到中（I2C占用）
前后测距驱动	STP-23L 与 4m 模块读取、冗余校验	UART/I2C(未指定)	`RangeFrontBackRaw`	20–50 Hz	<50 ms	低到中
预处理与健康度	对齐时间戳、滤波、有效性、跳变检测、一致性/走廊模式识别	Raw ranges、IMU、Odom	`CorridorObs{e_y_meas,e_th_meas,d_front,d_back,valid}`、`SensorHealth`	50–100 Hz	<20 ms	低
走廊相对定位滤波	互补/EKF/UKF 输出 e_y/e_θ/s 与置信度	`CorridorObs`、`ImuZ`、`OdomEst`	`CorridorState{e_y,e_th,s,cov/conf}`	50–100 Hz	<20–40 ms	低（小维）
段脚本解释器	Segment List 执行：走廊段/原地转向/退出停车/失败恢复	`CorridorState`、`SensorHealth`	`SegmentCmd{mode,v_ref,y_offset,end_trigger}`	10–20 Hz	<100 ms	低
控制器集合	走廊闭环、原地转向、入口对准等	`SegmentCmd`、`CorridorState`	`RawCmd{v,w}`	50–100 Hz	<20 ms	低
安全监督器/仲裁	急停/限速/降级/模式切换；融合底盘 diag_bits/cmd_age	`RawCmd`、0x181/0x184、ranges	`CmdSlot(v,w,flags)`、`SafetyState`	50–100 Hz	<20 ms（到0x100）	低
日志与回放	记录观测/状态/命令/故障；支持离线回放复现实验	全部关键消息	log文件/串口输出	5–20 Hz（批量写）	不得阻塞实时链路	中（取决于介质）
测试仿真/注入接口	PC 仿真或回放注入 ranges/odom/imu；用于HIL	离线数据/串口/USB	注入到 PRE	非实时	不影响0x100	低

接口命名建议：把“CAN 帧”与“内部消息”严格区分，例如 CanCmdVel_0x100_t 与 CmdSlot，避免上层直接依赖 CAN 打包细节，从而实现解耦与可测试性。

实现步骤与里程碑

分阶段实施步骤清单（可操作）

由于赛期时间表“未指定”，下面以“无特定期限”的工程顺序给出建议；若你们有明确赛期，可按周压缩/并行。

阶段 P0：跑通闭环（目标：能稳定走通道、不撞、不中断 0x100）

固化 0x100 20 ms 发送硬实时：把 canTxTask 设为最高优先级，只从 CmdSlot 取值；加 watchdog/统计，任何异常也持续发送 vx=0,wz=0 的合法帧。（《通讯协议》2–4 页、8–10 页；《aser》1、5 页）
接入 CAN Rx：0x181/0x184/0x200 解析快照写入上下文，monitorTask 先只做可视化打印与超时报警。（《通讯协议》4–7 页；《aser》1–5 页）
侧向 VL53L0X 驱动：先用固定 profile（例如 33 ms 或 30 ms 档），跑出稳定的 d_Lf,d_Lr,d_Rf,d_Rr；必要时在关键动作切换到 200 ms 高精度档（官方数据手册给出 profile 与 timing budget）。citeturn2search41
前后测距驱动：至少保证一个“前向安全距离”可用，先实现安全限速/急停。
实现派生观测 (e_y,e_\theta) 并闭环：先用互补滤波/低通 + 走廊控制律，让车在 40 cm 通道内稳定跑直线与到端停车。
段脚本最小集：CorridorFollow / TurnInPlace / ExitAndStop（动作少但可跑完全程）。

阶段 P1：鲁棒性与可调参

加入健康度、异常检测、降级状态机：单侧可用时切“单侧贴边 + IMU 航向约束”；双侧不可用时低速+停车保护。
加入轮滑检测与自适应策略（地毯段必需）：轮速差与 IMU/里程计不一致触发降速与融合降权。（地毯存在：见《附件6…比赛规则》2–3 页）
引入鲁棒 EKF（或对互补滤波加入创新门限）：用 χ²/马氏距离拒绝异常测距，参考创新卡方检验鲁棒 EKF 的工程化做法。citeturn5search1turn5search0
完善日志与回放：把“每次撞/每次抖动”都能离线复现，支撑快速迭代。

阶段 P2：增强与赛场集成

K230 输出“结构化低带宽特征”作为增强/降级输入：例如走廊中心偏差、地标识别结果；K230 的多核 RISC‑V 与 KPU 能力适合做推理后输出特征量，而不承担 20 ms 硬实时闭环。citeturn6search3
若需要局部绕障（比赛道具/土块干扰可能导致异常行为），可做“小范围 DWA 风格速度采样”，但务必以安全层为前提；DWA 原始思想是“在速度空间搜索可停、避障、前进最优的 (v,ω)”。citeturn4search0

仿真工具与场景建议

未指定你们必须使用 ROS/某仿真器，因此给出三档可选：

轻量快速（推荐 P0/P1）：Python/Matlab 自建 2D 走廊仿真（差速模型 + 墙面 ToF 测距模型 + 噪声/丢包/跳变），用于调 (k_y,k_\theta)、异常门限、降级策略；优点是迭代极快、与 B 方案高度匹配。
中等复杂（可选）：Webots/Gazebo 的差速车模型 + 简化距离传感器，验证入口/转向等几何行为。
硬件在环 HIL（推荐 P1/P2）：
- H743 跑真实控制栈；
- 通过“仿真注入接口”把 ranges/imu/odom 注入 PRE 模块（测试编译开关），同时真实发送 CAN 0x100 给 F4，看 F4 是否稳定接受、是否出现 counter/CRC 风暴、cmd_age 是否异常。（《通讯协议》8–10 页）

里程碑甘特图（示例：无特定期限，按 6 周节奏展示）

gantt
  title ASER B方案（走廊相对定位）里程碑示例（无特定期限）
  dateFormat  YYYY-MM-DD
  axisFormat  %m-%d

  section P0 跑通（先安全后精度）
  0x100 20ms硬实时不掉帧 + 基础安全层     :a1, 2026-03-18, 7d
  CAN Rx接入(0x181/0x184/0x200)+里程计     :a2, after a1, 7d
  VL53侧向+前后测距驱动 + e_y/e_θ观测     :a3, after a2, 10d
  走廊控制器 + 最小段脚本(走廊/转向/退出) :a4, after a3, 10d

  section P1 稳定与容错
  健康度/异常检测/降级状态机              :b1, after a4, 7d
  轮滑检测与自适应降速/融合降权           :b2, after b1, 7d
  鲁棒EKF/互补+χ²门限（可选）            :b3, after b2, 7d

  section 集成与验证
  日志回放/自动化测试/故障注入             :c1, after a3, 14d
  HIL联调 + 赛场流程演练 + 参数冻结        :c2, after b3, 14d

性能指标与验证方法

建议性能指标（赛前可再定阈值）

结合 40 cm 通道与车辆 20 cm 外形，工程上建议把“安全裕量”明确成指标：

走廊横向控制：
- (e_y) RMS（直通道匀速 0.2–0.4 m/s）：建议目标 < 10–20 mm（取决于传感器安装与轮滑程度）；
- (e_y) 峰值：不得触发“最小侧向间隙”保护线。
航向对齐：(e_\theta) RMS < 1–2°（入口/转向后重新进入走廊的恢复时间也应记录）。
端到端延迟：从侧向测距采样到 0x100 生效的闭环延迟建议 < 40 ms（两周期）；0x100 发送抖动应远小于 20 ms 周期。
鲁棒性：单侧 VL53 临时失效（例如 0.5–1 s）仍能保持不碰撞；前向测距失效时必须降速并可安全停车。
完赛相关：在 5 分钟限制下完成“驶出场地 + 停在启动区”动作（规则强调未完成可 0 分）。（《附件6…比赛规则》1 页）

数据采集需求（支撑可复现实验）

建议每条日志至少包含：

时间戳（单调时钟）、段 ID、模式（NORMAL/DEGRADED/RECOVERY/STOP_SAFE）
原始测距（8 路）、有效标志、健康度
派生观测 (e_y,e_\theta,d_\text{front},d_\text{back})
估计状态（(e_y,e_\theta,s) 与协方差/置信度）
输出命令（RawCmd 与最终 CmdSlot）、0x100 counter/CRC 统计
底盘状态（0x181 system_state/health/diag_bits/cmd_age）与 0x184 通信统计
（这些字段在协议中均可获得：见《通讯协议》4–7 页。）

单元测试与整车测试用例示例

单元测试（Host 或 MCU 上跑，推荐“输入→输出可验算”）

UT-EST-001：给定理想走廊、已知 (L_s)、固定 (e_\theta)，构造 (d_{Lf},d_{Lr}) 并验证 (e_{\theta,L}) 反解误差 < 阈值。
UT-GATE-001：给定 EKF 创新与协方差，构造离群点，验证 χ² 门限拒绝逻辑；门限选择可参考创新卡方检验鲁棒 EKF 文献。citeturn5search1
UT-CAN-001：0x100 打包：vx/wz 缩放、小端序、CRC8-J1850 校验与 rolling counter 递增规则；确保 counter 跳变策略不会触发底盘拒收。（《通讯协议》2–4 页）
UT-SAFE-001：前向距离 < d_stop → v=0；cmd_age_10ms 超阈值或 diag_bits 出现致命位 → STOP_SAFE 并持续发 0x100=0。（《通讯协议》4–5 页、8–10 页）

整车测试（建议可回归）

IT-COR-001（入场对准）：启动区→40 cm 出入口→进入第一通道；指标：不触碰、最大横向误差、耗时。（赛场尺寸：见《附件6…比赛规则》2 页）
IT-COR-002（走廊直行）：匀速 0.2/0.3/0.4 m/s，统计 (e_y) RMS/峰值、(e_\theta) RMS。
IT-END-001（到端触发）：接近端部围栏，验证触发准确率与停车距离一致性。
IT-SLIP-001（地毯轮滑）：在地毯段运行，验证轮滑检测触发率、降速策略与不碰撞。（地毯：见《附件6…比赛规则》2–3 页）
IT-DROP-001（侧向遮挡）：遮挡一只 VL53 或一侧两只，验证降级切换时间与稳定性。
IT-CAN-FAULT-001（CAN 抖动/延迟注入）：验证即使系统负载上升也不会超过 150 ms 不发合法 0x100。（《通讯协议》8–10 页）

风险与替代方案

主要风险点与缓解

通道极窄 + 尺寸误差 ±5%：通道宽可能缩到约 38 cm，若车辆外廓/轮子外露估计不足，居中也可能刮擦。缓解：把“最小侧向间隙”作为硬安全约束，并支持按段设置 (y_\text{offset})。（尺寸误差：见《附件6…比赛规则》14 页）
测距盲区/遮挡/高反射干扰：VL53L0X 在高速档噪声更大；缓解：关键动作切高精度 timing budget（200 ms）并降低速度；按 datasheet profile 做“模式-噪声-速度”联动。citeturn2search41
前后测距冗余冲突：两种前向传感器可能在某些目标材质/角度下给不同值；缓解：做一致性选择（例如取更保守的近距离值）+ 门限剔除。
地毯轮滑导致里程计失真：规则明确存在地毯（《附件6…比赛规则》2–3 页）。缓解：轮滑检测触发后降低对里程计的信任、降速并强化侧向闭环。
CAN 带宽与实时性：若日志/调试占用 CPU 或关中断过久，可能导致 0x100 抖动/丢帧；缓解：日志异步队列+独立低优先级 logger task；0x100 发送任务最高优先级，且即使停车也持续发合法帧。（0x100 硬约束：见《通讯协议》2–4 页、8–10 页）
计算资源不足：若引入过重的规划（全局优化/TEB 等），会侵蚀硬实时；缓解：主线坚持 B 方案小维估计+简单控制；局部避障仅做轻量 DWA 风格采样，且可选。citeturn4search0

替代/增强方案

A 方案度量定位 EKF/UKF 作为评估与恢复辅助：在不依赖其做走廊闭环的前提下输出 (x,y,θ) 与协方差，用于段落切换一致性检查与日志评估（《大体方案》4 页）。
K230 视觉作为测距降级备份：K230 的双核 RISC‑V + KPU 适合推理后输出结构化特征，作为走廊偏差/地标观测源之一。citeturn6search3
局部避障（可选）：若赛场土块/障碍导致必须绕行，可引入 DWA 思想做速度空间采样；其原始工作在 IEEE Robotics and Automation Magazine 1997 年文章中系统阐述。citeturn4search0

代码生成与 AI 协作建议

为什么必须“接口契约优先”

你明确提到多数代码将由 AI 生成。要让 AI 代码可控，必须先把系统拆成“可替换模块 + 明确消息契约 + 可回归测试”。否则 AI 很容易在“看似能跑，但破坏实时性/破坏协议/破坏并发安全”的地方踩雷。

ASER 文档中已经明确：协议层与接收链路不要随意改动，新逻辑应放 App/ 并遵守现有任务框架。（《aser》1–5 页）

建议的仓库结构与代码模板（示例）

建议在 App/ 下新增（不动协议层）：

App/sensors/：vl53_driver.c/.h、stp23l_driver.c/.h、ms53_driver.c/.h、imu_z.c/.h
App/preproc/：corridor_obs.c/.h、health_monitor.c/.h
App/est/：corridor_filter.c/.h（互补/EKF/UKF 可切换）
App/nav/：segment_fsm.c/.h、corridor_ctrl.c/.h、safety_supervisor.c/.h
App/log/：logger_task.c/.h、replay.c/.h
App/test/：host 可编译的纯 C 测试（或 Unity/Ceedling/CppUTest）

接口契约头文件（强制先写）：例如 corridor_msgs.h

#pragma pack(push,1)
typedef struct {
  uint32_t t_ms;
  float d_lf, d_lr, d_rf, d_rr;  // meters
  float d_front, d_back;        // meters
  uint8_t valid_mask;           // bitfield
} CorridorObs_t;

typedef struct {
  uint32_t t_ms;
  float e_y;       // meters
  float e_th;      // radians
  float s;         // meters
  float conf;      // 0..1 or covariance proxy
} CorridorState_t;

typedef struct {
  uint32_t t_ms;
  float v;         // m/s
  float w;         // rad/s
  uint8_t flags;
} RawCmd_t;
#pragma pack(pop)

单元测试规范（让 AI 代码“可验收”）

每个模块至少 3 类测试：正常、边界、异常（离群/丢包/超时）。
所有纯算法模块必须可在 Host 编译运行（不依赖 HAL），把硬件依赖封装成接口（如 read_range() 函数指针或 mock）。
协议相关测试必须比对字节级输出：0x100 的 CRC/rolling counter/小端序是典型“AI 容易写错但编译不过不报错”的点。（《通讯协议》2–4 页）

自动化生成与审查流程建议

先由你/团队写“模块接口契约 + 时序约束表 + 禁止事项”（如“0x100 发送任务不得阻塞”），再让 AI 生成实现。
CI 至少包含：
1. Host 单元测试；
2. 静态检查（clang-tidy/cppcheck 任选）；
3. 构建固件；
4. （可选）HIL 脚本：注入传感器回放数据，检查 0x100 发送间隔最大值与 counter 连续性。
Code review checklist（必须人工看）：
- 是否出现长时间关中断/在高优先级任务里打印；
- 是否对共享上下文无保护读写导致撕裂；
- 是否更改了 snc_can_app 协议定义/接收链路（ASER 文档明确不建议破坏）。（《aser》1–5 页）

参考资料（你上传的原始文档）

《附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则》：赛场尺寸、40 cm 通道/出入口、地毯、5 分钟限时、尺寸误差 ±5%（1–3 页、14 页）。
《通讯协议》：现有 H7↔F4 CAN 协议、0x100 20 ms/150 ms 超时、CRC8/rolling counter、0x181/0x184/0x200 定义与诊断位（2–10 页）。
《aser》：ASER 工程结构、FreeRTOS 任务（20 ms/100 ms）、协议层位置与“不要破坏接收链路/协议层”的约束（1–5 页）。
《大体方案》：B 方案状态与派生量测构造、走廊闭环思想、降级/恢复链与实施计划（4–5 页、13–16 页）。

参考资料（官方/原始文档与论文）

ST VL53L0X 产品页（最大 2 m）：citeturn2search3
ST VL53L0X 数据手册（timing budget 与 profile：20 ms/33 ms/200 ms 等）：citeturn2search41
ST VL53L1X 产品页（up to 4 m、up to 50 Hz）：citeturn0search0
STM32H743（480 MHz Cortex‑M7、双精度 FPU、TCM 等特性）：citeturn1search1
FreeRTOS vTaskDelayUntil()（固定频率周期任务的绝对时间阻塞）：citeturn3search34
FreeRTOS vTaskNotifyGiveFromISR()（ISR→任务通知）：citeturn2search42
创新 χ² 检验鲁棒 EKF（武汉大学学报）：citeturn5search1
马氏距离与 χ² 门限关系、鲁棒策略示例：citeturn5search0
UKF（Julier & Uhlmann 综述条目）：citeturn4search8
DWA 原始工作（Fox/Burgard/Thrun 1997）：citeturn4search0
K230 官方文档（CPU0 Linux/CPU1 RTOS、KPU INT8/INT16 等）：citeturn6search3

39 KiB Raw Blame History Unescape Escape