507 lines
39 KiB
Markdown
507 lines
39 KiB
Markdown
# 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**。citeturn2search3
|
||
同时其官方数据手册给出典型“Range profile”：High speed 20 ms、Long range 33 ms、High accuracy 200 ms 等 timing budget 配置（对你做“关键动作更稳、巡航更快”的动态配置非常实用）。citeturn2search41
|
||
- 前后向（每端 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 级传感器的性能参考上限。citeturn0search0
|
||
- 结构：**差速四轮** → 上位机输出 (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 及误差差异得到工程依据）。citeturn2search41
|
||
- 沿程 \(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 在导航融合领域是成熟手段；例如武汉大学学报给出“基于创新卡方检验的扩展鲁棒卡尔曼滤波”并说明可有效抑制观测粗差、提高稳定性。citeturn5search1
|
||
同时马氏距离平方在高斯假设下与 χ² 分布的关系，可作为你选择阈值的理论依据。citeturn5search0
|
||
|
||
#### UKF（备选 P1/P2）
|
||
UKF 的优势是减少雅可比推导错误风险，对非线性更稳健；其系统性论述可参考 Julier & Uhlmann 的综述文章（Proceedings of the IEEE, 2004）。citeturn4search8
|
||
但对 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\) 的硬依赖。
|
||
|
||
#### 重定位/恢复动作链（建议用段脚本固化）
|
||
当“走廊观测不可用”或“误差超过安全阈值”时，不要让系统继续前冲；建议固化为可重复执行的恢复链：
|
||
|
||
1. **低速前进 + 侧向对齐**：尝试恢复 \(e_\theta\) 可观测（左右/单侧）。
|
||
2. **原地旋转扫描**：转动寻找两侧墙面，使 \(e_y/e_\theta\) 可观测。
|
||
3. **后退重试或停车保护**：连续失败 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()`（按绝对时间唤醒，适合固定频率周期任务）。citeturn3search34
|
||
在 ISR→任务的事件通知上，任务通知（`vTaskNotifyGiveFromISR`）通常比信号量更轻，FreeRTOS 手册亦将其描述为更快的替代机制。citeturn2search42
|
||
|
||
### 关键消息帧示例（十六进制 + 结构体）
|
||
下面给出 **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 结构体（注意小端序与打包）：
|
||
|
||
```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 现有工程约束下（协议层/中断链路/任务框架尽量不动），解耦的关键是把系统切成三类模块：
|
||
|
||
1. **硬实时链路模块**：唯一职责是“稳定发 0x100”，只从一个“命令槽（Command Slot）”读取最新的 (v,ω)；不做复杂计算、不打印日志。
|
||
2. **可实时模块**：估计、控制、预处理，允许在极端情况下掉周期，但必须提供保底输出与超时策略。
|
||
3. **非实时模块**：日志、回放、仿真接口、参数调试、统计；永不阻塞硬实时。
|
||
|
||
ASER 文档明确：CAN 协议层位置固定，并建议新功能放 App/ 目录，且 canTxTask/monitorTask 已存在。（《aser》1–5 页）
|
||
|
||
### 数据流总览（mermaid）
|
||
```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 能力，为小维滤波提供了硬件基础。citeturn1search1
|
||
|
||
| 模块 | 职责 | 输入（消息/接口） | 输出（消息/接口） | 典型周期 | 最大延迟（建议） | 资源估计（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）**
|
||
1. 固化 **0x100 20 ms** 发送硬实时：把 canTxTask 设为最高优先级，只从 `CmdSlot` 取值；加 watchdog/统计，任何异常也持续发送 vx=0,wz=0 的合法帧。（《通讯协议》2–4 页、8–10 页；《aser》1、5 页）
|
||
2. 接入 CAN Rx：0x181/0x184/0x200 解析快照写入上下文，monitorTask 先只做可视化打印与超时报警。（《通讯协议》4–7 页；《aser》1–5 页）
|
||
3. 侧向 VL53L0X 驱动：先用固定 profile（例如 33 ms 或 30 ms 档），跑出稳定的 `d_Lf,d_Lr,d_Rf,d_Rr`；必要时在关键动作切换到 200 ms 高精度档（官方数据手册给出 profile 与 timing budget）。citeturn2search41
|
||
4. 前后测距驱动：至少保证一个“前向安全距离”可用，先实现安全限速/急停。
|
||
5. 实现派生观测 \(e_y,e_\theta\) 并闭环：先用互补滤波/低通 + 走廊控制律，让车在 40 cm 通道内稳定跑直线与到端停车。
|
||
6. 段脚本最小集：CorridorFollow / TurnInPlace / ExitAndStop（动作少但可跑完全程）。
|
||
|
||
**阶段 P1：鲁棒性与可调参**
|
||
1. 加入健康度、异常检测、降级状态机：单侧可用时切“单侧贴边 + IMU 航向约束”；双侧不可用时低速+停车保护。
|
||
2. 加入轮滑检测与自适应策略（地毯段必需）：轮速差与 IMU/里程计不一致触发降速与融合降权。（地毯存在：见《附件6…比赛规则》2–3 页）
|
||
3. 引入鲁棒 EKF（或对互补滤波加入创新门限）：用 χ²/马氏距离拒绝异常测距，参考创新卡方检验鲁棒 EKF 的工程化做法。citeturn5search1turn5search0
|
||
4. 完善日志与回放：把“每次撞/每次抖动”都能离线复现，支撑快速迭代。
|
||
|
||
**阶段 P2：增强与赛场集成**
|
||
1. K230 输出“结构化低带宽特征”作为增强/降级输入：例如走廊中心偏差、地标识别结果；K230 的多核 RISC‑V 与 KPU 能力适合做推理后输出特征量，而不承担 20 ms 硬实时闭环。citeturn6search3
|
||
2. 若需要局部绕障（比赛道具/土块干扰可能导致异常行为），可做“小范围 DWA 风格速度采样”，但务必以安全层为前提；DWA 原始思想是“在速度空间搜索可停、避障、前进最优的 (v,ω)”。citeturn4search0
|
||
|
||
### 仿真工具与场景建议
|
||
未指定你们必须使用 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 周节奏展示）
|
||
```mermaid
|
||
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 文献。citeturn5search1
|
||
- 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 做“模式-噪声-速度”联动。citeturn2search41
|
||
- **前后测距冗余冲突**：两种前向传感器可能在某些目标材质/角度下给不同值；缓解：做一致性选择（例如取更保守的近距离值）+ 门限剔除。
|
||
- **地毯轮滑导致里程计失真**：规则明确存在地毯（《附件6…比赛规则》2–3 页）。缓解：轮滑检测触发后降低对里程计的信任、降速并强化侧向闭环。
|
||
- **CAN 带宽与实时性**：若日志/调试占用 CPU 或关中断过久，可能导致 0x100 抖动/丢帧；缓解：日志异步队列+独立低优先级 logger task；0x100 发送任务最高优先级，且即使停车也持续发合法帧。（0x100 硬约束：见《通讯协议》2–4 页、8–10 页）
|
||
- **计算资源不足**：若引入过重的规划（全局优化/TEB 等），会侵蚀硬实时；缓解：主线坚持 B 方案小维估计+简单控制；局部避障仅做轻量 DWA 风格采样，且可选。citeturn4search0
|
||
|
||
### 替代/增强方案
|
||
- **A 方案度量定位 EKF/UKF 作为评估与恢复辅助**：在不依赖其做走廊闭环的前提下输出 (x,y,θ) 与协方差，用于段落切换一致性检查与日志评估（《大体方案》4 页）。
|
||
- **K230 视觉作为测距降级备份**：K230 的双核 RISC‑V + KPU 适合推理后输出结构化特征，作为走廊偏差/地标观测源之一。citeturn6search3
|
||
- **局部避障（可选）**：若赛场土块/障碍导致必须绕行，可引入 DWA 思想做速度空间采样；其原始工作在 IEEE Robotics and Automation Magazine 1997 年文章中系统阐述。citeturn4search0
|
||
|
||
## 代码生成与 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`
|
||
|
||
```c
|
||
#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）：citeturn2search3
|
||
- ST VL53L0X 数据手册（timing budget 与 profile：20 ms/33 ms/200 ms 等）：citeturn2search41
|
||
- ST VL53L1X 产品页（up to 4 m、up to 50 Hz）：citeturn0search0
|
||
- STM32H743（480 MHz Cortex‑M7、双精度 FPU、TCM 等特性）：citeturn1search1
|
||
- FreeRTOS `vTaskDelayUntil()`（固定频率周期任务的绝对时间阻塞）：citeturn3search34
|
||
- FreeRTOS `vTaskNotifyGiveFromISR()`（ISR→任务通知）：citeturn2search42
|
||
- 创新 χ² 检验鲁棒 EKF（武汉大学学报）：citeturn5search1
|
||
- 马氏距离与 χ² 门限关系、鲁棒策略示例：citeturn5search0
|
||
- UKF（Julier & Uhlmann 综述条目）：citeturn4search8
|
||
- DWA 原始工作（Fox/Burgard/Thrun 1997）：citeturn4search0
|
||
- K230 官方文档（CPU0 Linux/CPU1 RTOS、KPU INT8/INT16 等）：citeturn6search3 |