Falling_jasmine/ASER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-03 08:56:26 +08:00
parent 35e7f70e0f
commit 1bd0a73a73
17 changed files with 3147 additions and 1883 deletions
Download Patch File Download Diff File Expand all files Collapse all files

397
Doc/CODE_REVIEW.md
View File

@@ -1,397 +0,0 @@
# ARES 项目代码审查报告
> **审查日期**: 2025 年
> **审查范围**: `/App` 目录下全部业务代码（排除 `VL53L0X_API/core` ST 官方库）
> **审查基准**: HANDOFF.md 中的设计描述与已修复 BUG 列表
> **审查重点**: 残留 BUG、潜在运行时风险、代码质量问题
---
## 审查总结
| 严重等级 | 数量 | 说明 |
|---------|------|------|
| 🔴 严重 (可能导致功能错误/崩溃) | 4 | 运行时会导致错误行为 |
| 🟠 中等 (潜在风险/隐患) | 6 | 特定条件下可能触发问题 |
| 🟡 低 (代码质量/可维护性) | 7 | 不影响功能但应改善 |
**总体评价**: 项目架构设计清晰，HANDOFF.md 中列出的 9 个严重 BUG 和 7 个质量问题均已正确修复。但仍存在若干残留问题，以下逐一分析。
---
## 🔴 严重问题
### S-1: VL53L0X 卡尔曼滤波 Q/R 参数硬编码，未使用 `robot_params.h` 配置
**文件**: `VL53L0X_API/platform/vl53_board.c:84`
```c
/* 初始化卡尔曼滤波器：默认 Q=1.0, R=9.5 */ // ← 注释过期
vl53_kalman_init(&board->kf[i], 10.0f, 14.1f); // ← 硬编码值
```
`robot_params.h` 中定义了 `PARAM_VL53_KALMAN_Q = 10.0f` 和 `PARAM_VL53_KALMAN_R = 14.1f`，但 `vl53_board.c` 没有 `#include "robot_params.h"`，而是直接写死了 `10.0f` 和 `14.1f`。虽然当前值恰好与参数一致，但**修改 `robot_params.h` 中的 `PARAM_VL53_KALMAN_Q/R` 不会生效**，与 BUG-2 (EKF 参数硬编码) 属于同类问题。
**修复建议**:
```c
#include "robot_params.h"
// ...
vl53_kalman_init(&board->kf[i], PARAM_VL53_KALMAN_Q, PARAM_VL53_KALMAN_R);
```
---
### S-2: `LASER_SIMPLE_GetSnapshot()` 返回指针无线程安全保护，存在数据撕裂风险
**文件**: `laser/laser_manager.c:277-279`
```c
const laser_simple_snapshot_t *LASER_SIMPLE_GetSnapshot(void) {
return &g_snapshot; // ← 返回原始指针，无拷贝、无临界区保护
}
```
消费者 `AppTasks_RunLaserTestTask_Impl()` 调用此函数获取指针后直接传给 `Blackboard_UpdateLaser(snap)`。由于 `LaserTask`（内部10ms任务）在 `taskENTER_CRITICAL()` 中**逐通道写入** `g_snapshot.ch[i]`，而消费者拿到的是裸指针，读取可能发生在写入的间隙——比如前两个通道已更新、后两个通道还是旧值——构成**部分撕裂**。
虽然 `Blackboard_UpdateLaser` 内部有临界区保护，但问题出在**读 g_snapshot 的时候没有保护**。当前架构中 `laserTestTask` 以 50ms 周期读取、`LaserTask` 以 10ms 周期写入，两者优先级相同，可能发生抢占。
**影响等级**: 中高。实际撕裂概率取决于临界区外的读取窗口大小。由于 `laser_simple_snapshot_t` 只包含 4 个 `laser_simple_data_t`，单次 `memcpy` 耗时极短，实际风险较低。但从防御性编程角度应修复。
**修复建议**:
```c
void LASER_SIMPLE_GetSnapshotCopy(laser_simple_snapshot_t *out) {
taskENTER_CRITICAL();
*out = g_snapshot;
taskEXIT_CRITICAL();
}
```
---
### S-3: `snc_parse_odom_delta()` 在 ISR 中写 `odom_accum` 无临界区保护
**文件**: `Can/snc_can_app.c:148-173` (在 `SNC_CAN_RxFifo0Callback` 即 CAN ISR 中被调用)
```c
static void snc_parse_odom_delta(const uint8_t *d)
{
// ... 直接写 g_snc_can_app.odom_accum 的各个字段 ...
SNC_OdomDeltaAccum_t *acc = &g_snc_can_app.odom_accum;
acc->fl_accum += (int32_t)snc_read_i16_le(d[0], d[1]);
// ...
if (acc->frame_count < 255U) {
acc->frame_count++;
}
}
```
而消费侧 `SNC_CAN_ConsumeOdomDelta()` 使用 `taskENTER_CRITICAL()` 保护读取和清零。问题是：**`taskENTER_CRITICAL()` 在 Cortex-M 上通过 BASEPRI 屏蔽中断，只屏蔽优先级不高于 `configMAX_SYSCALL_INTERRUPT_PRIORITY` 的中断**。如果 CAN FIFO0 中断的优先级高于此阈值（即数值更小），那么 `snc_parse_odom_delta()` 可能在消费者持有临界区期间执行，导致**竞态条件**：消费者清零后，ISR 立刻写入新数据，然后消费者返回的 `snapshot` 中 `frame_count=0` 但 `accum` 值已非零。
**影响等级**: 取决于 CAN 中断优先级配置。如果 CAN 中断优先级在 FreeRTOS 管理范围内（优先级数值 ≥ `configMAX_SYSCALL_INTERRUPT_PRIORITY`），则 `taskENTER_CRITICAL()` 可以正确屏蔽它，问题不存在。**需要检查 CubeMX 中 FDCAN1 中断优先级配置**。
**修复建议**: 确认 FDCAN1 中断优先级 ≥ `configMAX_SYSCALL_INTERRUPT_PRIORITY`。或在 `snc_parse_odom_delta()` 中也使用 `taskENTER_CRITICAL()`（但 ISR 中应使用 `taskENTER_CRITICAL_FROM_ISR()`）。
---
### S-4: `corridor_msgs.h` 使用 `#pragma pack(push, 1)` 导致 EKF 矩阵和浮点运算性能损失及潜在对齐异常
**文件**: `preproc/corridor_msgs.h:8,61`
```c
#pragma pack(push, 1)
// ...
typedef struct {
float P[3][3]; // 36 字节的浮点矩阵，被强制 1 字节对齐
float innovation[3]; // 12 字节
float mahalanobis_d2;
// ...
} CorridorState_t;
// ...
#pragma pack(pop)
```
`CorridorState_t` 包含大量 `float` 和 `float[3][3]` 数组，被 `#pragma pack(push, 1)` 强制 1 字节对齐。在 Cortex-M7 上：
1. **性能损失**: 未对齐的 float 访问会触发硬件 unaligned access 处理，速度比对齐访问慢数倍。这个结构体在 navTask 的 20ms 循环中被高频读写，影响实时性能。
2. **FPU 指令风险**: 某些 FPU 指令（如 `VLDM`/`VSTM`）**要求 4 字节对齐**，不对齐会触发 UsageFault。虽然 GCC 通常会生成安全的加载指令，但编译器优化可能引入向量化或批量加载指令。
`CorridorObs_t` 和 `RawCmd_t` 也受影响，但内部结构更简单，风险较低。
**修复建议**: `#pragma pack(push, 1)` 仅用于需要匹配物理总线帧格式的结构体（如 `chassis_can_msg.h` 中的 CAN 帧，那是正确的用法）。`corridor_msgs.h` 中的结构体是内存中的数据传递，**不需要 pack(1)**，应删除。
---
## 🟠 中等问题
### M-1: IMU 帧解析状态机缺少帧类型 0x55 之后的 header byte 校验
**文件**: `IMU/hwt101.c:64-68`
```c
if (s_frame_idx == 0 && byte != 0x55) continue;
s_frame[s_frame_idx++] = byte;
if (s_frame_idx >= 11) {
```
当前状态机只检查首字节 `0x55`，然后无条件接收后续 10 字节。如果数据流中出现非帧头的 `0x55`（比如校验和恰好是 `0x55`），会导致帧错位。虽然最后有校验和检查可以过滤错误帧，但会**浪费 10 个字节的解析窗口**，在高频数据流中可能导致短暂的数据更新延迟。
**修复建议**: 在 `s_frame_idx == 1` 时检查第二字节是否为有效帧类型 (`0x51`/`0x52`/`0x53`)，不匹配则 `s_frame_idx = 0` 重新寻帧。
---
### M-2: `nav_script.c` 转向方向始终为正，不支持反向 (顺时针) 180° 转弯
**文件**: `nav/nav_script.c:202,229`
```c
float target_delta = s_cfg.turn_target_angle; // 默认 PI (180度)
// ...
float turn_dir = (target_delta > 0.0f) ? 1.0f : -1.0f;
```
`turn_target_angle` 在 `app_tasks.c` 中始终被初始化为 `3.14159265f`（正值），所以 `turn_dir` 永远为 `1.0f`（逆时针）。如果因场地布局需要顺时针转弯，当前代码无法支持。更关键的是，**两次 180° 转弯方向相同**——第一次到端逆时针转，第二次到端又逆时针转，结果机器人回到出发方向。这可能是设计意图（转完 180° 后继续正向走），但如果走廊空间不对称，固定转向方向可能导致转弯时撞墙。
**修复建议**: 考虑根据当前方向或走廊几何自动选择转向方向，或至少在 `NavScriptConfig_t` 中加一个 `turn_direction` 参数。
---
### M-3: `Odom_GetSpeed()` 在临界区外读取 `s_odom.vx` 和 `s_odom.wz`
**文件**: `Contract/robot_odom.c:122-124`
```c
void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status)
{
if (out_vx != NULL) *out_vx = s_odom.vx; // ← 临界区外
if (out_wz != NULL) *out_wz = s_odom.wz; // ← 临界区外
if (out_status != NULL) {
lock_odom(); // ← 只有 status 在临界区内
// ...
unlock_odom();
}
}
```
`s_odom.vx` 和 `s_odom.wz` 是 `float` 类型，在 Cortex-M7 上 32 位对齐的 float 读写是原子的，所以单独读取不会撕裂。但 `vx` 和 `wz` 之间不是原子的——可能读到旧的 `vx` 和新的 `wz`。不过此函数当前未被任何代码调用（速度通过 `Blackboard_UpdateOdom` 传递），所以实际影响为零。
**修复建议**: 将 `vx/wz` 的读取也放入 `lock_odom()` 中，或标注此函数为 `@deprecated`。
---
### M-4: `SNC_CAN_SendHeartbeat()` 使用零长度数组
**文件**: `Can/snc_can_app.c:194`
```c
HAL_StatusTypeDef SNC_CAN_SendHeartbeat(void)
{
uint8_t data[0]; // ← 零长度数组，C11 标准未定义行为
HAL_StatusTypeDef ret = snc_fdcan_add_tx_std(SNC_CAN_ID_HEARTBEAT, data, 0U);
```
零长度数组 `uint8_t data[0]` 在 C11 标准中是未定义行为（C99 的 flexible array member 语法是 `[]` 且只能在结构体末尾）。虽然 GCC 作为扩展支持它，且 `snc_fdcan_add_tx_std` 传入 `dlc_bytes=0` 不会实际读取 `data`，但这仍属于未定义行为。
**修复建议**: 改为 `uint8_t data[1] = {0};` 或直接传 `NULL`（需确认 HAL 是否接受 NULL data）。
---
### M-5: 安全状态机 (SegFsm) 在脚本覆盖模式下仍然生效，可能干扰转弯等特殊动作
**文件**: `app_tasks.c` navTask 流水线 Step 5-6
```c
/* Step 5: 控制律 */
if (script_out.use_override) {
raw_cmd.v = script_out.override_v; // 脚本指定速度
raw_cmd.w = script_out.override_w;
} else if (script_out.request_corridor) {
CorridorCtrl_Compute(..., &raw_cmd);
}
/* Step 6: 段状态机 -> 安全仲裁 */
SegFsm_Update(&raw_cmd, &obs, &corridor_state, &fsm_out);
```
当脚本处于 `TURN_AT_END` 阶段时，`use_override = true`，输出 `v=0, w=turn_omega`（原地转向）。但此时 EKF 因侧墙观测丢失，`conf` 可能降到 `< 0.1`，触发 `SegFsm` 进入 `E-STOP`，**强制将转向角速度归零**，导致机器人无法完成转弯。
虽然 `E-STOP` 有自动恢复机制（`conf >= 0.5` 时恢复），但在转弯过程中侧墙持续不可见，`conf` 不会恢复，造成**死锁**：转不了弯 → 永远看不到墙 → 永远 E-STOP。
**影响等级**: 实车中度风险。取决于转弯时 EKF 的 IMU 航向观测是否能维持 `conf >= 0.1`。如果 IMU 航向观测能阻止 `conf` 跌破阈值，则不会触发。但这是一个脆弱的依赖。
**修复建议**: 在 `SegFsm_Update()` 中增加一个 bypass 标志，当脚本处于 `use_override` 模式时跳过 E-STOP 判定；或仅在 `request_corridor` 为 true 时检查置信度。
---
### M-6: `process_complementary_laser()` 中 ATK 距离值无上限校验
**文件**: `preproc/corridor_preproc.c:30-73`
```c
static bool process_complementary_laser(const SensorItem_t *stp, const SensorItem_t *atk, float *out_m)
{
// ...
float atk_m = atk->value / 1000.0f;
// ATK 没有像 VL53 那样的量程校验 (0.02~2.0m)
// 如果 ATK 吐出异常大值 (如 65535mm)，会被当作有效数据
```
`process_side_laser()` 对 VL53L0X 数据做了 `[0.02m, 2.0m]` 范围校验，但 `process_complementary_laser()` 对 ATK 和 STP 数据**没有上限校验**。如果 ATK 传感器吐出异常大值（如掉线前最后一帧的乱码），这个值会被当作有效距离传递给安全状态机，可能导致**该停车时不停车**。
**修复建议**: 添加 ATK/STP 的量程校验，如 `atk_m > 0.0f && atk_m <= 8.0f` (ATK 标称量程 4m，留双倍余量)。
---
## 🟡 低优先级 / 代码质量问题
### L-1: `PARAM_IMU_YAW_OFFSET` 声明但未使用
**文件**: `robot_params.h:100`
```c
#define PARAM_IMU_YAW_OFFSET 0.0f // 声明了但代码中未使用
```
HANDOFF.md CAL-4 已标注此问题。如果 IMU 安装有固定偏角，此参数应在 `HWT101_Process()` 或 `corridor_filter.c` 中使用。当前为死代码。
---
### L-2: `corridor_filter.c` 中 `s_imu_yaw_ref_set` 在 Reset/Init 时未重置
**文件**: `est/corridor_filter.c:23-24`
```c
static float s_imu_yaw_ref_rad = 0.0f;
static bool s_imu_yaw_ref_set = false;
```
`CorridorFilter_Init()` 不会重置 `s_imu_yaw_ref_set`。如果比赛中途调用 `NavScript_Reset()` → `CorridorFilter_Init()` 重新初始化，旧的 `s_imu_yaw_ref_rad` 会保留，导致 IMU 航向观测使用过时的参考值。
**修复建议**: 在 `CorridorFilter_Init()` 末尾添加：
```c
s_imu_yaw_ref_rad = 0.0f;
s_imu_yaw_ref_set = false;
```
---
### L-3: `retarget.c` 中 `_write()` 的忙等循环可能阻塞调用任务
**文件**: `retarget.c:44-64`
```c
int _write(int file, char *ptr, int len)
{
while (1) {
result = CDC_Transmit_FS((uint8_t *)ptr, (uint16_t)len);
if (result == USBD_OK) return len;
if ((HAL_GetTick() - start_tick) > 20U) return 0;
if (osKernelGetState() == osKernelRunning) osDelay(1);
}
}
```
如果 USB CDC 端口忙碌（Host 未连接或 buffer 满），`printf` 会阻塞当前任务最多 20ms。对于 `navTask`（20ms 周期）和 `canTxTask`（20ms 周期），一次 `printf` 阻塞就可能导致**整个控制周期跳过**。当前 `App_PrintStatus()` 已被注释掉，但如果未来取消注释或在其他任务中添加 `printf`，可能造成问题。
**修复建议**: 在实时任务中避免使用 `printf`，或将 `_write` 改为非阻塞（丢弃模式）。
---
### L-4: 多个模块重复定义 `clampf()` 静态内联函数
**文件**:
- `est/corridor_ekf.c:42`
- `nav/corridor_ctrl.c:12`
- `nav/nav_script.c:34`
- `nav/segment_fsm.c:12`
四个文件各自定义了相同的 `static inline float clampf()`。虽然由于 `static` 链接属性不会导致链接错误，但违反 DRY 原则。
**修复建议**: 提取到公共头文件 `robot_params.h` 或新建 `utils.h`。
---
### L-5: `CorridorObs_t.valid_mask` 类型为 `uint8_t` 但掩码使用 bit 0-5 共 6 位
**文件**: `preproc/corridor_msgs.h:31` 和 `corridor_preproc.h:10-15`
当前 6 个掩码位刚好在 `uint8_t` 范围内（最大 bit 5 = 0x20），但余量很小。如果未来添加更多传感器（如第二组 VL53），很容易溢出。不紧急但值得注意。
---
### L-6: `vl53_board.c:84` 注释与实际值不一致
```c
/* 初始化卡尔曼滤波器：默认 Q=1.0, R=9.5 */ // ← 注释说 Q=1.0, R=9.5
vl53_kalman_init(&board->kf[i], 10.0f, 14.1f); // ← 实际值 Q=10.0, R=14.1
```
注释是旧版残留，与当前代码不符。
---
### L-7: `nav_script.c:285` 使用 `exit_start_s == 0.0f` 判断是否已触发，但 `s` 初始值就是 0
```c
if (s_internal.exit_start_s == 0.0f) {
s_internal.exit_start_s = state->s;
}
```
如果恰好在 `state->s ≈ 0.0` 时进入 EXIT 阶段（理论上不会，因为已经往返走过垄沟），`exit_start_s` 会被设为 0，然后下次循环 `== 0.0f` 再次为 true，重复赋值但不会出错（`state->s` 每次都差不多）。实际上因为 `memset(&s_internal, 0, ...)` 已将其初始化为 0.0，如果 EXIT 阶段被跳过再重入，可能出现意外行为。
**修复建议**: 使用 `bool exit_triggered` 标志代替浮点数零值判断。
---
## 架构设计审查
### 优点
1. **分层清晰**: 传感器驱动 → 黑板 → 预处理 → EKF → 控制 → 安全 → 指令槽，每层职责明确。
2. **线程安全设计合理**: 黑板的 `taskENTER_CRITICAL` + snapshot 模式有效防止了数据撕裂。
3. **传感器互补融合策略** (`process_complementary_laser`): STP+ATK 的互补逻辑考虑了盲区、单体故障、保守防撞等多种场景，设计周到。
4. **EKF 实现质量高**: 鲁棒 χ² 检验、分级观测更新（1/2/3 DOF）、协方差保护等机制完备。BUG-7（卡尔曼增益矩阵乘法）的修复正确。
5. **参数集中管理**: `robot_params.h` 作为唯一调参入口，大部分参数已正确引用（除 S-1 遗漏）。
6. **里程计累加器设计** (BUG-8 修复): ISR 累加 + 原子取走清零的设计有效解决了漏积分/重复积分问题。
### 可改进方向
1. **缺少 Watchdog**: 系统没有硬件看门狗 (IWDG/WWDG)。如果任何任务死锁或 HardFault，MCU 将永久挂起而不会自动重启。建议在空闲任务中喂看门狗。
2. **缺少运行时错误日志**: 当前的错误处理主要是"静默忽略"或"返回零值"。建议增加一个轻量级错误计数器或环形日志 buffer，方便赛后分析。
3. **EKF 和安全状态机没有联动**: 如 M-5 所述，脚本覆盖模式下安全状态机可能干扰正常流程。建议在 `SegFsmOutput_t` 中增加 bypass 机制。
4. **`laserTestTask` 的 `osDelay(50)` 不精确**: 使用 `osDelay` 而非 `osDelayUntil`，任务执行时间会累加到周期中，导致实际周期 > 50ms。对于激光数据更新频率有影响。
---
## 已修复 BUG 验证
| HANDOFF 编号 | 修复内容 | 验证结果 |
|-------------|---------|---------|
| BUG-1 | IMU wz deg→rad 转换 | ✅ `app_tasks.c:305` 使用 `PARAM_DEG2RAD()` |
| BUG-2 | EKF Q/R/P0 从 params 读取 | ✅ `corridor_filter.c:50-66` 使用 `PARAM_EKF_*` |
| BUG-3 | `SegFsm_Start()` 调用 | ✅ `app_tasks.c:382` |
| BUG-4 | BACKWARD 段使用 d_front | ✅ `nav_script.c:254` |
| BUG-5 | 超时使用配置参数 | ✅ `nav_script.c:157` |
| BUG-6 | EXIT 速度独立参数 | ✅ `nav_script.c:273` 使用 `s_cfg.exit_v` |
| BUG-7 | 完整矩阵乘法 K=PHT*S_inv | ✅ `corridor_ekf.c:576-607` 两步乘法 |
| BUG-8 | 里程计累加器模式 | ✅ `snc_can_app.c` ISR 累加 + `ConsumeOdomDelta` 原子取走 |
| BUG-9 | IMU yaw unwrap + 连续角度 | ✅ `hwt101.c` unwrap 逻辑 + `nav_script.c` 使用 IMU yaw |
| Q-1~Q-7 | 各项代码质量修复 | ✅ 全部验证通过 |
---
## 修复优先级建议
| 优先级 | 编号 | 预估工时 |
|--------|------|---------|
| 立即修复 | S-4 (pack 对齐) | 5 分钟 |
| 立即修复 | S-1 (VL53 KF 参数) | 5 分钟 |
| 赛前修复 | M-5 (安全 FSM bypass) | 30 分钟 |
| 赛前修复 | M-6 (ATK 量程校验) | 10 分钟 |
| 赛前修复 | S-2 (快照线程安全) | 15 分钟 |
| 赛前修复 | L-2 (IMU ref 重置) | 5 分钟 |
| 确认配置 | S-3 (CAN 中断优先级) | 10 分钟 |
| 建议改进 | 其余所有 | 按需 |
---
> **审查结论**: 项目整体质量良好，架构设计规范，历史 BUG 修复彻底。上述 S-4 和 S-1 建议在下次烧录前立即修复；M-5 (安全状态机与脚本冲突) 是实车测试中最可能暴露的问题，建议优先验证。

667
Doc/GLOBAL_NAV_REQUIREMENT.md
View File

@@ -1,667 +0,0 @@
# 固定场地条件下的赛道级导航需求说明
## 1. 问题背景
当前项目已经具备较完整的“单条垄沟内局部导航”能力，包括：
- 侧向测距支撑的走廊横向定位
- IMU / EKF 支撑的航向估计
- 前向距离触发的到端检测
- 原地转向
- 走廊内闭环控制
但正式比赛要求并不是“在一条走廊里走稳”这么简单，而是：
- 遍历全部 `6` 条垄沟
- 在端部完成换沟
- 最终从唯一出入口驶离场地
- 再停回启动区
因此，真正的问题已经从“局部走廊控制”升级成了“赛道级导航”。
本文件针对一个新的核心顾虑做说明：
- 左右 `VL53L0X` 是近场侧向传感器，实际有效距离有限
- 当小车走完第一条通道、完成转向、准备去第二条通道口时，下一条通道口可能仍在数米之外
- 此时无法指望左右侧向 VL53 直接识别远处的下一条通道入口
由此引出新的设计问题：
- 是否需要把当前导航从“局部走廊式”升级为“固定地图下的全局式导航”
本文件只做需求分析与方案建议，不修改现有代码。
## 2. 比赛场地与问题是否成立
根据比赛规则文件 `附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则.md`：
- 场地尺寸：`390cm x 300cm`
- 共有 `5` 条田垄
- 每条田垄：长 `220cm`、宽 `30cm`
- 围栏与田垄之间、相邻田垄之间均为 `40cm` 垄沟
- 实际需要遍历的是 `6` 条垄沟
- 场地尺寸允许误差：`+-5%`
结合 `HANDOFF.md` 的赛道理解：
- 当前赛道本质上是 `6` 段平行窄走廊
- 其间通过端部动作完成换沟
- 最终还要完成出场和停回启动区
因此，你的顾虑是成立的：
- 左右侧向 VL53 的作用域主要是在“贴着垄沟/墙体附近”时建立局部几何约束
- 一旦小车离开当前垄沟、进入端部开阔区或横向换沟区，下一条垄沟入口往往不在 VL53 的可靠观测范围内
- 所以不能把“下一条通道入口识别”这个任务建立在左右 VL53 的远距离探测能力上
这里需要特别澄清一个容易说错的点：
- 赛道级运动并不一定是“在端部开阔区横移很长一段，再去远距离搜索下一条垄沟入口”
- 更符合当前场地理解的运动方式，是一种 **S 型串行入沟**：
1. 从出发区直行到第 1 条垄沟入口附近
2. 第 1 条垄沟在车体一侧（例如右侧）
3. 原地右转 `90°` 对准垄沟后入沟
4. 沿垄沟通过
5. 到端后原地左转 `90°`，进入端部直线连接段
6. 再前进一小段到下一条垄沟入口附近
7. 再原地左转 `90°` 入下一条垄沟
8. 之后重复形成 S 型遍历
也就是说：
- 下一条垄沟并不是完全未知目标
- 它通常位于“端部连接直线段之后的固定相邻位置”
- 问题核心不是远距离搜索，而是 **按固定几何完成 90° 转向 + 短直线推进 + 再次 90° 入沟**
换句话说：
- `VL53` 适合做局部跟墙/居中
- 不适合独立承担赛道级换沟导航
## 3. 当前项目的能力边界
`HANDOFF.md` 已经明确指出：
- 当前项目更像“单垄沟闭环验证系统”
- 还不是完整的 `6` 垄沟遍历赛道导航系统
当前已经具备的能力主要是：
1. 垄沟内定位与控制
2. 到端检测
3. 原地转向
4. 安全停车
当前还缺失的关键能力是：
1. 多垄沟拓扑状态管理
2. 端部换沟策略
3. 下一目标垄沟的判定逻辑
4. 出场与停回启动区的赛道级动作编排
所以，这不是某一个传感器量程的小补丁问题，而是系统层次已经需要从“局部控制”提升到“地图引导下的赛道导航”。
## 4. 为什么仅靠局部传感器闭环不够
### 4.1 局部闭环擅长的任务
局部闭环擅长的是：
- 在 `40cm` 垄沟里保持居中
- 在局部几何约束下控制横向误差和航向误差
- 在接近端部时停车或转向
这些任务的共同特征是：
- 机器人周围可观测到明确的近场结构
- 当前目标由附近环境直接决定
### 4.2 换沟问题的本质不同
从第一条垄沟换到第二条垄沟，不只是“继续沿墙走”，而是要回答以下更高层的问题：
1. 我当前已经完成了第几条垄沟
2. 下一条目标垄沟是哪一条
3. 这次应该左移还是右移
4. 端部 `90°` 转向后需要前进多少连接距离
5. 下一次 `90°` 转向应朝哪一侧入沟
6. 什么时候说明我已经对准并进入了下一条垄沟，可以重新切回局部走廊跟踪模式
这些问题都不是单次近场测距能直接回答的。
它们需要：
- 赛道拓扑信息
- 段落状态
- 距离累计
- 转向后姿态保持
- 已完成进度记忆
所以，换沟本质上是“任务级导航问题”，不是“单纯传感器观测问题”。
## 5. 是否需要全局导航
### 5.1 需要，但不一定是 SLAM 式全局导航
你提出“地图是固定的，是否要改成全局式导航”，我的判断是：
- 需要“全局导航思想”
- 但不需要上“通用移动机器人 SLAM”那种全局导航系统
原因是这个比赛场地有几个重要特征：
1. 地图是固定结构
- 场地拓扑不变
- 垄沟数量固定
- 宽度和相对排列固定
2. 尺寸有误差但不是完全未知
- 规则允许 `+-5%`
- 说明不能死信纯几何标称值
- 但也不代表你需要从零建图
3. 导航目标是离散段落式的
- 进入第 1 条垄沟
- 沿垄沟前进
- 到端
- 换到相邻垄沟
- 重复
- 最后出场并停回启动区
这类问题更适合：
- 固定地图
- 拓扑状态机
- 局部感知闭环
- 少量里程/姿态积分
而不是：
- 实时全局建图
- 通用路径规划
- 自由空间导航
### 5.2 更准确的说法：需要“赛道级固定地图导航”
如果用更工程化的语言描述，真正需要的不是传统 SLAM，而是：
- 固定地图下的赛道级导航
- 或者：拓扑-度量混合导航
它的含义是：
1. 上层知道整张赛道的结构
2. 下层在当前局部段里做高频闭环
3. 段与段之间的切换靠预定义几何、里程推进和事件触发完成
这比“纯局部反应式导航”强很多，但比“完整 SLAM”简单得多，也更符合比赛实际。
## 6. 为什么不建议直接走完整 SLAM / 全局自由导航
### 6.1 赛场结构太规则，SLAM 的收益不高
比赛场地不是开放未知环境，而是高度规则的固定赛道。
如果直接上完整 SLAM，会遇到两个问题：
1. 复杂度远大于实际收益
2. 还要处理对称结构导致的重定位歧义
因为 `6` 条垄沟本身非常相似：
- 走廊宽度相同
- 垄长相同
- 多个局部观测在不同位置上可能长得很像
这对通用 SLAM 来说并不天然友好。
### 6.2 规则里存在地毯与尺寸误差，纯度量导航也不能硬信
比赛规则明确说明：
- 随机会在 `2` 条垄沟铺设地毯模拟松软路面
- 场地尺寸允许 `+-5%` 误差
这意味着：
- 纯里程计距离不能被绝对相信
- 纯固定距离脚本也不能完全相信
所以最合理的方案不是“只靠地图”，而是：
- 地图负责告诉你要去哪一段
- 传感器负责告诉你你是否已经贴近那一段并进入局部闭环条件
## 7. 推荐的导航架构
我建议把系统分成三层。
### 7.1 第 1 层：局部走廊控制层
职责：
- 在垄沟内保持稳定行驶
- 利用侧向 VL53 和 IMU 控制 `e_y`、`e_th`
- 处理贴墙、居中、偏置行走等局部任务
这层继续使用你现在已经有的能力即可。
### 7.2 第 2 层：端部与换沟动作层
职责：
- 识别到端
- 完成 `90°` 转向
- 保持连接段朝向直线推进
- 在下一条垄沟入口处再执行一次 `90°` 转向入沟
- 在接近下一条垄沟入口时重新捕获局部走廊结构
这一层的主要依赖不应再是“直接看到很远处的下一条通道口”，而应是：
- IMU 航向保持
- 编码器里程推进
- 前向/后向长距测距作安全与事件辅助
- 重新捕获两侧结构时切回走廊模式
### 7.3 第 3 层：赛道级拓扑/地图层
职责：
- 记录当前是第几条垄沟
- 决定下一条目标垄沟编号
- 决定换沟方向
- 决定当前所处阶段：入场、走廊、端部、换沟、再入沟、退出、回停
- 在最终阶段规划如何从最后一条垄沟回到唯一出口并停回启动区
这层就是“全局导航思想”的承载层。
## 8. 具体到你的顾虑：第二通道口怎么判定
这个问题不应该被设计成：
- “我在端部一转身，然后用 VL53 去远距离搜索第二通道口在哪里”
更合理的设计应该是：
### 8.1 已知目标法
系统在上层已经知道：
- 当前完成的是第 `i` 条垄沟
- 下一个目标是第 `i+1` 条或第 `i-1` 条垄沟
也就是说，目标不是“搜索未知入口”，而是“按已知地图去下一个入口”。
### 8.2 动作脚本法
在端部动作中执行一段有结构的换沟脚本，例如：
1. 到端停车
2. 原地转 `90°` 到端部连接段朝向
3. 用 IMU 保持该朝向直行一小段
4. 用里程计累计连接段推进距离
5. 到达下一条垄沟入口附近后再原地转 `90°`
6. 一旦两侧 VL53 重新形成新垄沟的局部几何特征，则确认入沟成功
7. 切回局部走廊跟踪
如果按你描述的实际拓扑，更准确的理解是：
- 换沟动作不是“横向平移到另一条沟”
- 而是“端部出沟后走一段连接直线，再 90° 入下一沟”
- 整体轨迹更接近规则的 S 型遍历
### 8.3 重新捕获法
下一条垄沟入口的确认，最可靠的时刻往往不是“远处看见”，而是“进入附近后重新捕获到局部走廊结构”。
也就是说：
- 上层负责把车带到“应该接近下一条垄沟入口”的区域
- 下层负责在近场把入口真正锁住
这比远距离直接识别入口更稳。
## 9. 哪些传感器在赛道级导航里应该扮演什么角色
### 9.1 左右 VL53L0X
适合：
- 垄沟内横向定位
- 局部几何重捕获
- 判断自己是否已经重新进入某条垄沟
不适合：
- 远距离寻找下一条通道口
- 独立承担赛道级换沟导航
### 9.2 IMU
适合：
- 转向控制
- 端部横移/换沟时的航向保持
- 跨局部无墙约束阶段的短时姿态维持
### 9.3 编码器 / 里程计
适合：
- 换沟距离推进
- 已走段长估计
- 作为脚本动作的度量输入
缺点：
- 地毯和打滑会导致累计误差
所以它应作为：
- 主推进量
- 但不是唯一最终确认依据
### 9.4 前后长距测距
当前硬件里前后测距量程远大于 VL53，理论上更适合：
- 到端检测
- 判断前方是否接近围栏
- 在开阔区提供安全保护
- 对某些段落提供事件辅助
它不一定直接告诉你“第二通道口在这”，但可以帮助判断：
- 是否还在端部开阔区
- 是否已经逼近另一侧围栏或终点边界
- 是否应减速/停止/切换动作阶段
## 10. 推荐的总体方案
### 10.1 推荐方向：固定地图 + 拓扑状态机 + 局部闭环
这是我最推荐的方向。
其核心思想是：
- 地图不是在线建出来的，而是事先已知
- 机器人不需要理解“任意世界坐标”
- 机器人只需要知道自己当前位于哪个赛道段、下一步要切到哪个段
可以把整张赛道抽象成若干固定段：
1. 启动区
2. 入口对准段
3. 垄沟 1
4. 端部连接段 1
5. 垄沟 2
6. 端部连接段 2
7. ...
8. 垄沟 6
9. 出场段
10. 启动区停车段
如果按运动学动作再细分，每一个“端部连接段”内部实际上可以拆成：
1. 到端停车
2. 第一次 `90°` 转向
3. 连接直线推进
4. 第二次 `90°` 转向
5. 入沟重捕获
每个段有自己的：
- 目标朝向
- 目标推进方向
- 退出条件
- 允许使用的观测
- 安全策略
### 10.2 这不是“纯全局”，而是“全局指导下的局部控制”
这种方案的优势在于：
- 不会把所有问题都压给近场传感器
- 不会过度依赖里程计的绝对精度
- 保留你当前局部走廊控制代码的大部分价值
- 更贴合固定场地赛事的工程实际
## 11. 不推荐的方案
### 11.1 不推荐：继续纯局部反应式地扩当前逻辑
如果只是继续把当前“单垄沟往返”逻辑往外补，仍然不引入上层赛道状态，那么很容易遇到：
- 转完向不知道该去第几条沟
- 明明应该换沟，却又回到原来那条沟附近
- 因地毯或打滑导致脚本距离错位
- 无法稳定完成最终出场与回停
### 11.2 不推荐：直接上完整 SLAM / 通用全局路径规划
这对当前赛题来说过重，收益不一定匹配复杂度。
## 12. 对当前项目的直接结论
结合当前代码与比赛规则，可以得出以下结论：
1. 你的顾虑成立
- 左右 VL53 无法独立解决“数米外下一条垄沟入口识别”问题
2. 当前项目确实缺少赛道级导航层
- 当前更偏向单垄沟局部验证系统
3. 后续必须补上固定地图下的全局段落管理
- 否则无法可靠完成正式比赛要求的 `6` 垄沟遍历
4. 但不建议走完整 SLAM 路线
- 更合适的是固定地图 + 拓扑状态机 + 局部感知闭环
## 13. 后续实施建议
如果后续开始真正改造导航系统，建议优先级如下：
1. 先定义完整赛道拓扑和段落状态机
2. 明确每一段的进入条件、退出条件、目标朝向和目标距离
3. 让局部走廊控制只负责“在某条已知垄沟里跑稳”
4. 让换沟段由 IMU + 里程计 + 长距测距辅助来完成
5. 用左右 VL53 做“重新捕获下一条垄沟”确认，而不是远距离搜寻入口
6. 最后再考虑是否需要引入更强的全局定位增强手段
### 13.1 建议分 5 步走
结合当前项目现状，建议不要一口气重写整套导航，而是按“先把单段动作语义补完整，再把赛道级层叠上去”的顺序推进。
推荐拆成 `5` 步。
### 第 1 步：先把现有局部导航链路补到“可复用”
这一步不是做全局导航，而是先把现有单垄沟能力整理成后续可调用的稳定基础模块。
优先要补或改的模块：
1. `App/nav/segment_fsm.c/.h`
- 增加“动作语义”或“模式感知”输入
- 区分：走廊前进、原地转向、横向换沟、退出直线
- 解决当前 `TURN_AT_END` 可能被安全层整段按死的问题
2. `App/nav/nav_script.c/.h`
- 不再把它视为最终比赛脚本
- 先把它收敛成“单段动作编排器”或“局部验证脚本”
- 明确哪些能力未来会上移到赛道级状态机
3. `App/preproc/corridor_msgs.h`
- 补充动作模式、段类型、重捕获结果等跨模块消息定义
- 避免后面继续把语义塞进零散布尔量里
4. `App/app_tasks.c`
- 补齐导航启动前的 ready 判定
- 明确 navTask 每周期里：局部控制输出、赛道级输出、安全层输出的优先级
这一阶段的目标不是“遍历 6 条垄沟”，而是：
- 让“走廊跟踪 / 到端 / 原地转向 / 安全仲裁”这几个局部动作变成可被上层稳定调用的基础能力
### 第 2 步：新增赛道级拓扑状态机模块
这一步开始真正引入“全局导航思想”，但仍然基于固定地图，不做 SLAM。
建议新增模块：
1. `App/nav/global_nav_fsm.c/.h`
- 负责赛道级阶段推进
- 记录当前是第几条垄沟
- 决定下一条目标垄沟编号
- 决定下一次是左转入沟还是右转入沟
- 决定当前处于：启动、入沟、走沟、到端、换沟、再入沟、出场、回停 的哪一阶段
2. `App/nav/track_map.c/.h`
- 保存固定赛道的拓扑和名义几何
- 例如：垄沟数量、编号顺序、相邻关系、换沟方向、出口所在侧、启动区相对位置
- 不追求通用地图系统，只做当前赛题所需的固定地图描述
3. `App/preproc/corridor_msgs.h`
- 增加赛道级状态输出结构
- 例如：当前 corridor_id、target_corridor_id、segment_type、progress_state
为什么这一步要单独拆出来：
- 当前项目最大缺口不是局部控制，而是“不知道自己在整张赛道里进行到哪一步”
- 这个职责不能继续堆在 `nav_script.c` 里，否则会越改越像一份超长 if-else 脚本
### 第 3 步：新增换沟动作层模块
这一层负责跨出当前垄沟、去相邻垄沟入口附近、再把局部控制权交还给走廊跟踪层。
建议新增模块：
1. `App/nav/lane_change_executor.c/.h`
- 输入：当前赛道级目标、目标换沟方向、目标段参数
- 输出：本周期期望动作 `v/w`
- 内部实现典型动作序列：
- 到端停车
- 第一次 `90°` 转向到连接段朝向
- 维持航向直线推进
- 按里程推进到预计下一沟入口区域
- 第二次 `90°` 转向入沟
- 减速搜索并等待局部结构重捕获
2. `App/nav/heading_hold.c/.h` 或并入 `lane_change_executor`
- 用 IMU 做连接直线阶段短时航向保持
- 与走廊控制器解耦，避免把“无侧墙阶段”继续硬塞进 `corridor_ctrl.c`
3. `App/nav/reacquire_detector.c/.h`
- 负责判断是否已重新进入一条有效垄沟
- 典型判据：左右 VL53 重新同时形成合理几何、置信度恢复、持续若干拍成立
这一阶段的核心目标是：
- 不依赖“远距离看到下一条垄沟入口”
- 而是“按固定地图推进到预计区域，再由局部传感器完成重捕获确认”
### 第 4 步：补出场与回停模块
前 3 步完成后，系统已经具备：
- 走一条垄沟
- 到端
- 换到相邻垄沟
但正式比赛还需要最后的：
- 从最后一条垄沟驶离
- 回到唯一出口
- 停回启动区
建议新增模块：
1. `App/nav/exit_planner.c/.h`
- 定义从最后有效垄沟切到出场段的固定动作逻辑
- 决定离场时的目标朝向、推进距离、退出条件
2. `App/nav/start_zone_dock.c/.h`
- 负责最终回停启动区
- 可以做成简化版固定脚本，不需要复杂路径规划
这一阶段不要追求炫技，重点是：
- 流程完整
- 可解释
- 可调参
- 能在赛场误差下稳定完成收尾动作
### 第 5 步：最后再做统一调参与验证支撑
当前项目已经有局部参数，但赛道级导航落地后，还需要把“段参数”和“地图参数”系统化管理。
建议补的模块或整理项：
1. `App/robot_params.h`
- 补充赛道级参数
- 例如：换沟名义距离、减速搜索距离、重捕获持续拍数、回停距离等
2. `App/nav/global_nav_debug.c/.h` 或临时调试结构
- 给 CubeMonitor / 日志暴露关键内部量
- 例如：当前 corridor_id、stage、target_heading、reacquire_flag、lane_change_progress
3. `HANDOFF.md` / `GLOBAL_NAV_REQUIREMENT.md`
- 随代码同步更新
- 保证后续调试时文档和实现一致
这一阶段的目标是：
- 把“能跑”变成“能调、能解释、能复现”
### 13.2 各步的交付标准
为了避免后续开发一直停留在“看起来写了很多模块”，建议每一步都设一个明确交付标准。
第 1 步交付标准：
- 原地转向不再被安全层错误清零
- 走廊前进 / 原地转向 / 退出直行三类动作有清晰安全语义
- 局部导航链路在架空测试中行为稳定可解释
第 2 步交付标准：
- 系统能够明确输出“当前第几条垄沟、下一条目标是哪条、当前赛道阶段是什么”
- 不再依赖单个脚本文件隐式记录全局进度
第 3 步交付标准：
- 能从一条垄沟末端稳定切换到相邻垄沟入口附近
- 能通过 VL53 重捕获确认重新入沟
第 4 步交付标准：
- 能在完成全部目标垄沟后可靠离场
- 能完成最终回停启动区
第 5 步交付标准：
- 参数、日志、状态可观测性完整
- 可以支持正式场地反复调参与问题定位
### 13.3 为什么是这个顺序
这个顺序的核心原则是：
1. 先补“动作语义一致性”，再补“赛道级状态记忆”
2. 先解决端部动作可用性，再扩展多垄沟遍历
3. 先做固定地图和拓扑状态机，再考虑任何更重的全局定位增强
如果顺序反过来，例如一开始就写完整 6 垄沟状态机，但底层转向和换沟动作还不稳定，那么上层状态再完整也只会变成“会卡在某一步的复杂脚本”。
因此，最合理的开发路径不是“先把全局状态机写满”，而是：
- 第 1 步：把局部动作做成稳固积木
- 第 2 步：加赛道级任务管理
- 第 3 步：补跨段动作
- 第 4 步：补最终出场与回停
- 第 5 步：统一调参与调试支撑
## 14. 一句话结论
你的问题本质上说明：当前系统已经不能只靠“局部走廊导航”来思考了。
真正需要补的是“固定地图下的赛道级导航层”，而不是盲目把 VL53 当成远距离入口探测器，也不是直接上复杂 SLAM。

458
Doc/HYBRID_NAVIGATION_GUIDE.md
View File

@@ -1,458 +0,0 @@
# 固定地图赛道的混合导航说明
## 1. 文档目的
本文档面向后续接手项目的人，说明当前比赛场景下为什么推荐采用一种“混合导航”方案，而不是单纯依赖局部传感器闭环，也不是直接上通用 SLAM / 全局路径规划。
这里的“混合导航”特指：
- 上层使用固定地图和拓扑状态机
- 中层使用段落动作与事件切换
- 下层使用局部传感器闭环稳定控制
这套思路适用于当前这类：
- 场地结构固定
- 任务流程固定
- 局部几何约束很强
- 但局部传感器看不到全局目标
的比赛型机器人系统。
## 2. 场地与任务本质
根据比赛规则与 `HANDOFF.md`：
- 场地大小为 `390cm x 300cm`
- 内部有 `5` 条田垄
- 实际需要遍历的是 `6` 条垄沟
- 每条垄沟宽 `40cm`
- 每条田垄长 `220cm`
- 只有 `1` 个出入口
- 出入口外有启动区
从导航角度，这不是自由环境中的随机移动问题，而是一个非常明确的固定任务：
1. 从启动区进入场地
2. 进入某一条垄沟
3. 沿垄沟稳定前进并作业
4. 到达端部后完成转向与换沟
5. 依次遍历全部 `6` 条垄沟
6. 最终从唯一出口驶离场地
7. 自主停回启动区
如果把整个任务抽象出来，本质上就是一个 **S 形遍历任务**。
示意如下：
```text
入口 -> 沟1 ↑
↓ 沟2
↑ 沟3
↓ 沟4
↑ 沟5
↓ 沟6 -> 出口
```
这类问题最重要的不是“任意时刻知道自己在全局坐标中的绝对位置”，而是：
- 知道自己当前在第几条沟
- 知道下一条目标沟是哪一条
- 知道当前处于入沟、走沟、到端、换沟、出场还是回停阶段
- 在每个阶段里，用合适的传感器和控制策略去完成当前动作
## 3. 为什么不能只靠局部导航
当前项目已经具备很强的局部导航能力，尤其是在单条垄沟内：
- 侧向测距可以支撑居中或偏置行驶
- IMU 可以提供航向与转角信息
- 前向测距可以做端部触发
- 里程计可以做短时推进量估计
但只靠局部导航会遇到明显边界：
### 3.1 局部导航能解决什么
局部导航适合解决：
- 在窄沟内居中
- 在窄沟内保持姿态稳定
- 接近端部时减速或停车
- 在局部可观测条件下闭环修正误差
### 3.2 局部导航解决不了什么
局部导航不擅长解决：
- 我现在完成了第几条沟
- 下一条目标沟应该是左边还是右边
- 转完向后该走多远才能接近下一条沟入口
- 什么时候该结束整场遍历并朝唯一出口离场
这些问题需要：
- 全局任务记忆
- 地图拓扑信息
- 阶段状态机
- 跨局部观测空窗的动作脚本
所以，仅靠“看到什么就跟什么”的局部反应式导航，不足以稳定跑完整场比赛。
## 4. 为什么也不建议直接上通用 SLAM
有些人看到“局部导航不够”，第一反应会是“那就做全局 SLAM”。
但当前赛题并不适合直接走这条路线。
### 4.1 地图不是未知的
赛场结构高度固定：
- 垄沟数固定
- 相对排列固定
- 出入口固定
- 启动区固定
这意味着没有必要像服务机器人那样，在未知环境中一边探索一边建图。
### 4.2 场地高度对称
`6` 条垄沟在局部上非常相似。
这会让很多通用全局定位方法遇到典型问题：
- 局部观测相似
- 重定位歧义大
- 小误差可能让系统把“第 2 沟”认成“第 3 沟”附近
### 4.3 比赛更看重稳定完赛，而不是地图美观
比赛规则更关心：
- 能不能完整遍历
- 能不能不撞边
- 能不能顺利出场
- 能不能停回启动区
不是在考察一套通用 SLAM 系统的建图效果。
### 4.4 通用 SLAM 工程负担大
如果直接上完整 SLAM，通常还要处理：
- 更复杂的状态与数据关联
- 更高的开发和调参成本
- 更重的算力与调度开销
- 更难解释的失败模式
这和当前项目追求的工程目标并不匹配。
## 5. 什么是“混合导航”
当前场景下推荐的“混合导航”，可以概括成一句话：
**用固定地图决定去哪，用状态机决定现在该做什么，用局部传感器闭环决定这一小段怎么稳稳地过去。**
它不是纯局部，也不是纯全局，而是分层协作。
## 6. 混合导航的三层结构
## 6.1 上层：固定地图与拓扑状态机
上层负责的是“全局任务理解”。
它需要维护的信息包括：
- 当前所在赛道段编号
- 当前已经完成了第几条垄沟
- 下一条目标垄沟是哪一条
- 当前应该左换沟还是右换沟
- 当前处于：启动、入场、走沟、到端、换沟、再入沟、出场、回停 的哪一阶段
这一层不一定关心厘米级位置，而更关心：
- 拓扑顺序
- 阶段推进
- 事件触发
可以把整场任务拆成固定段落，例如：
1. 启动区准备
2. 入口对准
3. 垄沟 1 前进
4. 端部换沟 1
5. 垄沟 2 返回
6. 端部换沟 2
7. 垄沟 3 前进
8. ...
9. 最后一沟结束
10. 出场
11. 回停启动区
这就是“固定地图导航”的核心，不是基于任意坐标规划，而是基于已知赛道结构推进任务。
## 6.2 中层：段落动作与事件切换
中层负责把“上层目标”翻译成可执行动作。
例如在某个阶段，机器人可能执行：
- 入口慢速直行，直到捕获双侧结构
- 沿当前垄沟闭环跟踪
- 到端后原地转 `90°` 或 `180°`
- 保持某个航向横向推进一段距离
- 在接近预计位置后减速，并等待重新捕获新垄沟
- 切回垄沟跟踪模式
这一层的本质是“段脚本 + 事件触发”。
它依赖：
- IMU 姿态
- 里程计推进量
- 前向/后向安全距离
- 局部结构重新捕获结果
这层很重要，因为许多时候机器人会暂时处于“看不到完整走廊结构”的状态，比如端部换沟阶段。
## 6.3 下层：局部传感器闭环
下层负责在小范围内把车稳稳控制住。
典型任务：
- 在垄沟内居中
- 在垄沟内偏向 1/4 宽度行驶
- 根据 IMU 保持航向
- 根据前向测距做减速与停车
这一层应当追求：
- 高频
- 稳定
- 可降级
- 不依赖复杂全局推理
你现在已有的大部分控制能力，都属于这一层。
## 7. 各类传感器在混合导航中的角色
## 7.1 左右 VL53L0X
推荐职责：
- 走廊内横向定位
- 居中/偏置行驶
- 近场重捕获新垄沟
不推荐职责：
- 远距离识别下一条垄沟入口
- 独立承担换沟全流程导航
原因很简单：
- 它们是近场侧向传感器
- 强项是局部几何闭环
- 弱项是远距离赛道级感知
## 7.2 IMU
推荐职责：
- 航向估计主来源
- 转向角度判定
- 无侧墙约束阶段的短时姿态保持
- 换沟阶段的朝向控制
IMU 在混合导航里非常关键，因为它能帮助机器人跨过“局部结构暂时缺失”的区间。
## 7.3 编码器 / 里程计
推荐职责：
- 估算走过了多长距离
- 在换沟动作中提供推进量
- 与状态机结合做段落退出条件
限制：
- 遇到地毯、打滑、轮胎差异时会有误差
因此它适合作为“推进量参考”，但不适合作为唯一定位真值。
## 7.4 前后长距测距
推荐职责：
- 到端检测
- 防撞保护
- 开阔区事件辅助
- 出场或接近边界时的安全约束
这类传感器不一定直接告诉你“下一沟入口就在前方”，但能帮助你判断：
- 是否接近端部
- 是否接近围栏
- 是否该切换动作阶段
## 8. 为什么这种方案适合当前比赛
## 8.1 它符合固定地图的特点
比赛地图结构是已知的。
这意味着：
- 目标垄沟不是未知搜索对象
- 换沟不必靠“发现远处入口”
- 可以由状态机根据当前进度直接推导“下一步该去哪”
## 8.2 它承认局部传感器的边界
混合导航没有让 VL53 去做它不擅长的事情。
它承认：
- VL53 负责局部
- IMU 负责短时姿态保持
- 里程计负责推进量
- 上层状态机负责“全局流程”
这是符合传感器物理特性的分工。
## 8.3 它能兼容尺寸误差与地毯
规则里有两个现实问题：
- 场地尺寸允许 `+-5%`
- 有 `2` 条垄沟会随机铺地毯
这意味着：
- 不能死信地图上的绝对尺寸
- 不能死信纯里程推进
混合导航的好处是：
- 上层地图只给出大方向和段结构
- 局部闭环用实时传感器做最终修正
这样既利用了先验地图，又不会被固定脚本锁死。
## 9. 一个典型的工作流程
下面给出一个典型流程，帮助理解这套导航在比赛中的运行方式。
### 9.1 启动与入场
- 上层状态机进入“入口对准”阶段
- 机器人从启动区朝入口前进
- 一旦左右侧传感器稳定捕获到垄沟结构，切换为垄沟跟踪模式
### 9.2 垄沟内前进
- 下层局部控制根据左右侧测距维持横向位置
- IMU 维持航向
- 前向测距用于到端检测
- 状态机记录当前垄沟编号与方向
### 9.3 到端
- 前向距离达到阈值
- 状态机判定当前垄沟已到端
- 进入转向阶段
### 9.4 换沟
- IMU 控制转向到目标朝向
- 编码器推进预定距离
- 长距测距做安全约束
- 接近预期位置后减速
- 直到左右 VL53 再次捕获新垄沟结构
### 9.5 再入沟
- 一旦检测到新的局部走廊结构
- 状态机确认已进入下一条垄沟
- 切换回局部走廊跟踪
### 9.6 重复直到遍历完成
- 状态机更新“当前第几沟”
- 按 S 形顺序重复以上过程
### 9.7 最终离场与回停
- 当最后一条垄沟完成后
- 状态机切换到出场段
- 利用固定地图与局部感知朝唯一出口离开
- 出场后再执行停回启动区动作
## 10. 与当前项目的关系
当前项目已经具备混合导航中的一部分基础：
- 局部走廊控制
- IMU 航向处理
- 到端检测
- 原地转向
- 段脚本雏形
但当前仍偏向：
- 单垄沟验证
- 局部闭环主导
- 缺少完整的赛道级段落管理
所以后续真正需要补的，并不是完全推翻现有控制，而是：
- 把“局部走廊能力”封装成底层能力
- 在其上补一个完整的固定地图状态机
- 把换沟和出场逻辑系统化
## 11. 一个容易犯的错误
在这类项目里，最容易犯的错误是两种极端：
### 11.1 极端一：把所有问题都交给局部传感器
这会导致：
- 换沟阶段无从判断全局目标
- 容易在端部迷失
- 难以稳定完成多垄沟遍历
### 11.2 极端二：把所有问题都交给“全局定位”
这会导致：
- 系统复杂度暴涨
- 对称环境下定位歧义严重
- 与比赛需求不匹配
混合导航的价值就在于避开这两个极端。
## 12. 推荐结论
对于当前比赛场景，推荐的总体思路是：
1. 用固定地图描述整条 S 形任务路线
2. 用拓扑状态机管理“当前在哪一段、下一步去哪一段”
3. 用 IMU 和里程计支撑跨局部观测空窗的动作执行
4. 用左右 VL53 负责局部垄沟内的高精度横向闭环与重捕获
5. 用前后长距测距做端部识别与安全保护
这就是当前场景最合适的混合导航方案。
## 13. 一句话总结
这类比赛不是“靠一套万能定位算法解决全部问题”，而是“让合适的层做合适的事”：
- 地图负责全局流程
- 状态机负责阶段切换
- 传感器负责局部闭环
- 控制器负责把每一小段稳稳跑完
这就是混合导航的核心价值。

321
Doc/IMU_YAW_REQUIREMENT.md
View File

@@ -1,321 +0,0 @@
# IMU 主导航向需求说明
## 1. 背景
当前项目运行场景是小车在走廊/垄沟内行驶，依赖以下传感器进行状态估计与控制：
- HWT101 IMU：提供 `yaw`、`yaw_continuous`、`wz`
- 左右侧 VL53 测距：提供左右前后 4 个侧向距离
- 编码器里程计：提供线速度 `odom_vx`，并参与运动预测
目前系统在 `navTask` 中每 20ms 执行一次状态估计与控制，整体链路见 `HANDOFF.md`。
本说明文档用于明确一个新的需求方向：
- 左右激光测距主要用于横向位置参考
- 航向角估计希望主要依赖 IMU，而不是依赖左右激光前后差分计算得到的航向角
本文档只描述需求、现状和建议，不修改现有代码。
## 2. 当前实现现状
### 2.1 当前 yaw / 航向相关的数据来源
当前系统中的“航向”并不是单一量，而是分成两类：
1. IMU 原始航向信息
- 文件：`App/IMU/hwt101.c`
- IMU 输出：
- `yaw`：原始偏航角，范围 `[-180, 180)`
- `yaw_continuous`：对原始 yaw 做 unwrap 后得到的连续角度
- `wz`：角速度，单位 `deg/s`
2. EKF 中的相对航向误差 `e_th`
- 文件：`App/est/corridor_filter.c`
- 文件：`App/est/corridor_ekf.c`
- `e_th` 表示小车相对当前走廊方向的航向误差，而不是全局绝对航向角
### 2.2 当前 EKF 中航向的计算方式
当前滤波流程如下：
1. 预测步
- 使用 `odom_vx` 和 `imu_wz` 做预测
- 对应代码：`CorridorEKF_Predict(odom_vx, imu_wz, dt)`
2. 侧墙观测更新
- 使用左右侧测距更新横向误差 `e_y`
- 同时也使用左右同侧前后测距差来估计航向 `e_th`
对应观测形式：
- 左侧航向观测：`z_eth_L = atan2(d_lr - d_lf, Ls)`
- 右侧航向观测：`z_eth_R = atan2(d_rf - d_rr, Ls)`
相关代码位置：`App/est/corridor_ekf.c`
3. IMU yaw 观测更新
- 在侧墙更新之后，再用 `imu_yaw_continuous` 做一个独立 1DOF 的航向观测更新
- 该观测形式为：
`z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad`
其中 `imu_yaw_ref_rad` 是在侧墙观测可信时锁定的参考值
相关代码位置：
- `App/est/corridor_filter.c`
- `App/est/corridor_ekf.c`
### 2.3 当前系统对传感器的信任关系
从参数和注释来看，当前系统默认策略是：
- 侧墙激光是走廊内姿态估计的主观测来源
- IMU yaw 是辅助观测，用于长时约束和侧墙观测缺失时兜底
相关参数位于：`App/robot_params.h`
当前默认值：
- `PARAM_EKF_R_EY = 0.002f`
- `PARAM_EKF_R_ETH = 0.001f`
- `PARAM_EKF_R_ETH_IMU = 0.01f`
含义：
- 侧墙航向观测噪声更小，表示当前更信任侧墙推导出的航向角
- IMU yaw 观测噪声更大，表示当前 IMU 在 EKF 中主要是弱约束
### 2.4 当前导航脚本对 IMU yaw 的使用
虽然走廊跟踪阶段的 `e_th` 主要是由 EKF 输出，但 180 度原地转向判定已经直接使用了 IMU 的 `yaw_continuous`。
相关代码位置：`App/nav/nav_script.c`
这说明项目里已经承认一个事实：
- 在“累计转角判定”这类任务上，IMU 连续 yaw 比 EKF 的 `e_th` 更适合做主依据
## 3. 当前方案存在的问题
### 3.1 左右激光更适合测位置，不适合主导航向
用户当前判断是：
- 左右激光测距误差大约在 `+-2cm`
- 这个误差水平对于横向位置参考仍然有价值
- 但对于航向角计算不够稳定，难以直接采纳为主观测
这是一个合理判断。
原因在于，侧墙航向观测本质上来自“同侧前后两个距离的差分”：
- 左侧：`d_lr - d_lf`
- 右侧：`d_rf - d_rr`
差分量本身会放大噪声影响，尤其是在以下条件下：
- 单个传感器误差较大
- 前后基线长度有限
- 墙面不完全平整
- 传感器安装误差存在偏角或偏移
因此，虽然左右激光仍然适合估计：
- 小车是否居中
- 小车偏左还是偏右
- 小车是否位于沟宽的四分之一等目标横向位置
但未必适合继续承担“主航向来源”的角色。
### 3.2 当前结构下，侧墙观测对 e_th 的影响仍然偏强
当前 EKF 的侧墙更新同时包含：
- `e_y` 观测
- `e_th_L` / `e_th_R` 观测
所以只要侧墙数据有效，系统就会直接利用左右前后差分结果去修正航向。
如果侧墙前后差分噪声较大，就可能带来以下问题：
- `e_th` 抖动
- 控制输出 `w` 抖动
- 走廊直行时出现不必要的左右摆动
- IMU 已经给出较平滑航向，但被激光差分估计不断拉扯
## 4. 目标需求
### 4.1 总体需求
希望重新明确传感器分工：
- IMU 主要负责航向角估计
- 左右激光主要负责横向位置参考
更具体地说：
1. 走廊内横向控制
- 左右激光用于判断小车在走廊中的横向位置
- 支持居中行驶
- 支持偏向左/右四分之一位置行驶等策略
2. 走廊内航向控制
- 航向估计应主要依赖 IMU
- 侧墙测距不应继续作为航向主观测来源
3. 转向阶段
- 继续使用 IMU 连续 yaw 作为转角判定主依据
### 4.2 需求表达上的准确表述
如果用更工程化的语言描述该需求，可以表述为：
- “侧墙激光参与横向位置估计，不参与或仅弱参与航向角估计。”
- “航向角 `e_th` 的主来源改为 IMU `wz + yaw_continuous`。”
- “侧墙前后差分得到的航向观测仅作为弱约束、校验项，或直接关闭。”
## 5. 对现有系统的理解结论
基于当前代码实现，可以得出以下判断：
### 5.1 用户的想法与当前实现不一致
当前实现里：
- 左右激光不仅参与横向位置 `e_y`
- 还直接参与航向 `e_th`
而用户期望的是：
- 左右激光只负责横向位置参考
- 航向主要信任 IMU
因此，这不是简单调一个小参数就完全等价的需求，而是状态估计设计思路上的调整。
### 5.2 用户的想法在当前场景下是成立的
若侧墙测距误差确实约为 `+-2cm`，则：
- 用其估计横向偏移仍有意义
- 用其做前后差分计算航向角则很容易噪声偏大
从传感器特性匹配上看，更合理的做法就是：
- 激光负责位置
- IMU 负责航向
### 5.3 需要区分“横向位置”和“航向角”两个子问题
本需求的关键不是“全面抛弃激光”，而是要区分：
- `e_y`：仍可继续信任侧墙测距
- `e_th`：应改为主要信任 IMU
这是本需求最核心的设计点。
## 6. 后续可选改造方向
本节只记录可能的改造方向，不在本次工作中实施。
### 6.1 方向 A：仅通过参数调权，弱化侧墙航向观测
思路：
- 保留现有 EKF 结构不变
- 仅通过增大 `PARAM_EKF_R_ETH`、减小 `PARAM_EKF_R_ETH_IMU` 来让航向估计更偏向 IMU
优点：
- 修改最小
- 风险相对可控
- 可以快速实车验证
缺点：
- 侧墙航向观测仍然存在于主更新流程中
- 只是“变弱”，不是“彻底不参与”
### 6.2 方向 B：结构性调整，侧墙只更新 e_y
思路：
- 修改 EKF 观测模型
- 侧墙测距只用于更新 `e_y`
- `e_th` 仅由 `imu_wz` 预测和 `imu_yaw` 观测约束
优点：
- 最符合本需求原意
- 传感器职责边界清晰
缺点：
- 改动比参数调权大
- 需要重新验证滤波稳定性和控制效果
### 6.3 方向 C：侧墙航向只做低频校验或异常检测
思路：
- 不再把 `z_eth_L/z_eth_R` 作为主 EKF 观测
- 改成仅在长直段、双侧稳定、连续多帧一致时，低频微量校正 IMU 航向
- 或只用于诊断告警，不直接参与状态更新
优点：
- 兼顾 IMU 主导与环境约束
- 有助于抑制纯 IMU 长时漂移
缺点：
- 逻辑更复杂
- 需要额外设计稳定判据
## 7. 推荐结论
如果以当前用户需求为准，推荐设计原则如下：
1. 左右激光负责横向位置，不再主导航向
2. IMU 负责航向主估计
3. 转弯角度继续使用 IMU 连续 yaw 判定
4. 如需保留侧墙航向，也应降为弱约束或校验项，而不是主观测
换句话说，后续如果要正式调整系统，应优先朝这个方向收敛：
- `e_y` 由侧墙激光主导
- `e_th` 由 IMU 主导
## 8. 涉及模块清单
本需求后续若要实施，主要会影响以下模块：
- `App/IMU/hwt101.c`
- IMU yaw / yaw_continuous / wz 来源
- `App/est/corridor_filter.c`
- IMU yaw 参考值与更新调用逻辑
- `App/est/corridor_ekf.c`
- 侧墙航向观测 `z_eth_L/z_eth_R` 的使用方式
- `App/robot_params.h`
- 观测噪声参数调权
- `App/nav/nav_script.c`
- 转弯阶段的 IMU yaw 使用逻辑
## 9. 本文档结论摘要
本文档确认以下几点：
- 当前系统现状：侧墙激光不仅用于横向位置，也参与航向角估计
- 用户需求：侧墙激光只作为位置参考，航向主要信任 IMU
- 该需求与当前实现存在结构性差异
- 从传感器误差特性看，这一需求是合理的
- 后续建议将“位置”和“航向”两个估计任务明确拆分，各自交给更适合的传感器主导

1500
Doc/实施方案.md Normal file
View File

File diff suppressed because it is too large Load Diff

549
Doc/混合导航方案.md Normal file
View File

@@ -0,0 +1,549 @@
# 混合导航方案
## 1. 文档目的
本文档用于明确本项目后续正式比赛版导航应采用的总体方案。
目标不是重写当前全部导航代码，而是：
1. 保留现有“垄沟内局部闭环控制”能力
2. 在其上补齐赛道级状态机与段间动作编排
3. 让机器人能够按照固定地图完成 6 条垄沟的 S 型遍历
4. 最终从唯一出口驶离并停回启动区
本文档强调的是“混合导航”：
- 上层使用固定地图和拓扑状态机决定现在该去哪
- 中层使用动作执行器完成转向、连接段推进、再入沟
- 下层使用现有局部传感器闭环完成沟内稳定行驶
它不是纯局部反应式导航，也不是通用 SLAM。
## 2. 已知场地理解
根据 `Doc/map.md`：
- 场地净尺寸约为 `300cm x 390cm`
- 内部有 `5` 条田垄
- 因围栏与田垄、田垄与田垄之间均有通道，所以可通行垄沟实际为 `6` 条
- 启动区位于场地下侧靠左，外接唯一入口
- 各条垄沟是横向分布的长通道
- 垄沟间通过左右两端的短连接段串起来
因此，比赛中的真实轨迹不是“在端部横移搜索下一条沟”，而是：
1. 从启动区进入场地
2. 沿入口直线段前进
3. 到第 1 条垄沟入口附近
4. 原地转 `90°` 入沟
5. 沿垄沟通过
6. 到端后原地转 `90°`
7. 走一小段连接直线
8. 再原地转 `90°` 入下一条垄沟
9. 重复以上动作，形成 **S 型遍历**
10. 全部垄沟完成后离场并回停启动区
## 3. 为什么要做混合导航
当前项目已经具备较强的局部能力：
- 4 路侧向 VL53 做走廊观测
- IMU 提供 `wz` 和 `yaw_continuous`
- EKF / Filter 输出 `e_y`、`e_th`、`conf`
- `corridor_ctrl` 输出沟内控制指令
- `segment_fsm` 负责安全裁剪
- `nav_script` 能做单段脚本验证
但这套能力本质上仍然偏向：
- “单条垄沟怎么跑稳”
- 而不是
- “整张赛道下一步该去哪里”
正式比赛需要解决的核心问题有：
1. 当前正在第几条垄沟
2. 下一条应该进入哪条垄沟
3. 当前应该左转还是右转
4. 什么时候从沟内控制切到端部动作
5. 什么时候从端部动作切回沟内控制
6. 什么时候结束全部遍历并离场
7. 离场后如何回停到启动区
这些问题无法只靠局部测距瞬时值回答，必须引入上层任务状态。
## 4. 混合导航的核心思想
本项目推荐采用：
**固定地图 + 赛道级状态机 + 局部闭环控制**
其中：
- 固定地图负责描述赛道结构
- 状态机负责描述任务推进
- 局部闭环负责把当前这一小段走稳
整体思路是：
- 用地图回答“接下来去哪”
- 用状态机回答“现在该做什么动作”
- 用传感器闭环回答“这一段怎么安全稳定地过去”
## 5. 三层架构
### 5.1 上层：赛道级导航层
职责：
- 记录当前 `corridor_id`
- 决定下一个目标 `target_corridor_id`
- 决定当前阶段
- 决定下一步是左转还是右转
- 在所有阶段之间推进任务
这层不直接控制车轮，只输出“当前应该执行哪种段动作”。
### 5.2 中层：段动作执行层
职责：
- 入场直线推进
- `90°` 原地转向
- 连接段直线推进
- 再次 `90°` 入沟
- 出场段动作
- 回停启动区动作
这层输出当前周期的期望 `v/w`，但仍需经过安全层裁剪。
### 5.3 下层：局部闭环控制层
职责：
- 在垄沟内保持居中
- 控制 `e_y` 和 `e_th`
- 提供局部重捕获判据
- 在当前段可观测时给出稳定闭环
这一层尽量复用现有实现，不重复发明轮子。
## 6. 当前代码与未来架构的对应关系
现有代码可保留并复用的部分：
1. `App/preproc/`
- 继续负责传感器清洗与观测构造
2. `App/est/`
- 继续负责 `e_y / e_th / conf` 估计
3. `App/nav/corridor_ctrl.c`
- 继续负责沟内局部控制
4. `App/Contract/robot_blackboard.*`
- 继续作为全局传感器快照中心
5. `App/Contract/robot_cmd_slot.*`
- 继续作为导航输出到 CAN 的命令槽
6. `App/nav/segment_fsm.*`
- 保留为安全层，但后续必须增加“动作语义感知”
当前不应再承担最终比赛全局职责的部分：
1. `App/nav/nav_script.c`
- 当前更像“单垄沟验证脚本”
- 不适合继续膨胀成完整赛道导航总控
因此后续应新增赛道级模块，而不是把全部逻辑继续堆进 `nav_script.c`。
## 7. 推荐状态机建模
建议把赛道任务拆成以下大阶段：
1. `START_ZONE`
- 启动区待发
2. `ENTRY_STRAIGHT`
- 从启动区经唯一入口进入场地
- 沿左侧入口直线段前进
3. `TURN_INTO_CORRIDOR`
- 到目标垄沟入口后原地转 `90°`
- 对准目标垄沟
4. `CORRIDOR_TRACK`
- 沟内闭环跟踪
- 使用现有 `corridor_ctrl`
5. `TURN_OUT_AT_END`
- 到达当前垄沟末端
- 原地转 `90°` 转向连接段
6. `LINK_STRAIGHT`
- 沿端部连接段直行一小段
- 用 IMU 保持航向
- 用里程计或事件触发控制推进
7. `TURN_INTO_NEXT_CORRIDOR`
- 原地转 `90°`
- 对准下一条垄沟
8. `REACQUIRE_CORRIDOR`
- 低速确认两侧 VL53 是否重新形成合理走廊结构
- 成功后切回 `CORRIDOR_TRACK`
9. `EXIT_FIELD`
- 全部垄沟完成后，朝唯一出口离场
10. `DOCK_START_ZONE`
- 回到启动区并停车
11. `FINISHED`
- 比赛结束
## 8. 赛道级核心状态量
建议赛道级层显式维护以下变量：
- `current_corridor_id`
- `target_corridor_id`
- `total_corridor_count = 6`
- `travel_direction`
- `turn_side`
- `stage`
- `stage_progress`
- `next_turn_is_left`
- `is_final_exit_phase`
- `reacquire_confirm_count`
其中最关键的是：
- 当前在第几条沟
- 下一条是哪条沟
- 这次入沟应该左转还是右转
- 当前处于哪个动作阶段
## 9. 传感器参数与角色分工
### 9.1 左右 VL53L0X
已知参数：
- 每侧 2 个，共 4 个
- 主要用于侧向测距
- 精确测量距离按当前工程经验取 **1.2m 以内**
- 当前已由人工完成标定，但单点测距仍存在约 **±1cm** 的偏差
适合：
- 沟内居中
- 入沟重捕获确认
不适合：
- 作为 `yaw / e_th` 的主观测来源
- 远距离搜索下一条沟入口
- 独立完成赛道级导航
设计含义：
- `VL53` 是近场几何约束传感器
- 只能在“已经接近某条沟”时帮你锁住这条沟
- 不能把“下一条沟在哪里”这个问题压给它
- 由于单点误差量级约为 `±1cm`，同侧前后差分法对噪声非常敏感
- 因此不推荐继续用 `VL53` 前后差分直接计算 `yaw`，航向应主要依赖 `IMU`
### 9.2 前后 STP-23L
已知参数：
- 前后各 1 个
- 有效测距范围 **7cm ~ 7.5m**
适合：
- 到端检测
- 前后安全边界监测
- 开阔区边界辅助判定
- 某些段落的事件触发
不适合：
- 独立判断当前位于哪条垄沟
设计含义：
- `STP` 是远距离边界感知传感器
- 它适合回答“前面/后面还有多远”“是否接近端部或围栏”
- 不适合承担精细入沟定位
### 9.3 前后 ATK-MS53L1M
已知参数：
- 前后各 1 个
- 有效测距范围 **4cm ~ 3.9m**
适合：
- 近距离补盲
- 填补 STP 在近端盲区的不足
- 近场防撞保护
设计含义：
- `ATK` 不是主导航传感器
- 它的核心价值是让前后边界感知在近距离不断层
- 在转向、再入沟、靠近围栏时很重要
### 9.4 IMU
适合：
- 原地转 `90°`
- 连接段航向保持
- 无侧墙阶段的短时姿态约束
### 9.5 编码器 / 里程计
适合：
- 连接段推进量估计
- 段落推进计量
- 动作超时和距离上限保护
注意：
- 地毯和打滑会影响绝对精度
- 不能单独作为最终入沟确认依据
## 10. 各传感器在混合导航中的分工原则
建议按下面的分工使用传感器：
1. 沟内阶段
- 主用：左右 `VL53` 做横向约束，`IMU` 做航向约束
- 辅助：前后激光仅做安全和到端检测
2. 转向阶段
- 主用：`IMU yaw_continuous`
- 辅助：前后激光做安全保护
3. 连接段阶段
- 主用：`IMU + 里程计`
- 辅助：前后 `STP/ATK` 做边界与防撞
4. 再入沟阶段
- 主用：左右 `VL53`
- 辅助：`IMU` 做姿态稳定，前后激光做安全兜底
一句话总结：
- `VL53` 负责“锁住局部走廊”
- `IMU` 负责“航向约束和跨过无墙约束阶段”
- `里程计` 负责“推进量”
- `STP/ATK` 负责“边界和安全”
## 10.1 关于航向观测的专项说明
当前侧向 `VL53L0X` 虽然已经完成标定，但单点测距仍有约 `±1cm` 偏差。
这个精度对于：
- 居中控制
- 左右偏移判断
- 重新捕获一条沟
通常是够用的。
但如果把它直接用于航向估计，例如用同侧前后距离差去推导 `yaw / e_th`，会遇到两个问题：
1. 同侧前后差分属于“小量减小量”，对噪声天然敏感
2. 当前 `±1cm` 的单点误差已经足以让差分航向观测明显抖动
因此推荐原则是：
- `VL53` 负责横向约束和重捕获
- `IMU wz + yaw_continuous` 负责航向估计与转向控制
- 不再把 `VL53` 作为 `yaw` 主观测
## 11. 动作执行原则
### 11.1 沟内阶段
- 主要依赖侧向 VL53 做横向闭环，IMU 做航向闭环
- 使用 `corridor_ctrl`
- 安全层负责限速和急停
### 11.2 转向阶段
- 主要依赖 IMU `yaw_continuous`
- 目标是稳定完成 `90°`
- 安全层不能再沿用普通“前方太近则整段全停”的逻辑
- 必须允许 `v=0, w!=0` 的受限原地转向
### 11.3 连接段阶段
- 主要依赖 IMU 保持连接段朝向
- 使用里程计推进
- 接近预计入口后降速
- 前后 `STP/ATK` 负责边界辅助与防撞
- 进入重捕获阶段等待局部结构恢复
### 11.4 重捕获阶段
判据建议包括：
- 左右两侧 VL53 同时有效
- 左右几何关系符合 40cm 垄沟模型
- `conf` 高于阈值
- 持续若干拍成立
只有重捕获成功后，才允许切回沟内闭环。
## 12. 推荐新增模块
建议新增以下模块。
### 12.1 `App/nav/global_nav_fsm.c/.h`
职责：
- 维护整场比赛任务阶段
- 管理 `corridor_id`
- 决定下一步目标段
- 向下游发布当前动作类型
### 12.2 `App/nav/track_map.c/.h`
职责：
- 固化比赛地图拓扑
- 保存各条垄沟、连接段、入口、出口的相对关系
- 提供“当前完成哪条后下一条是谁”的规则查询
### 12.3 `App/nav/lane_transition.c/.h`
职责：
- 执行端部出沟、连接段推进、再入沟
- 内部管理两个 `90°` 转向和一段连接直线
### 12.4 `App/nav/reacquire_detector.c/.h`
职责：
- 负责判断是否已重新进入目标垄沟
- 对 VL53 几何结构和 `conf` 做持续判定
### 12.5 `App/nav/heading_hold.c/.h`
职责：
- 在无侧墙阶段提供短时航向保持
- 可独立实现，也可并入 `lane_transition`
### 12.6 `App/nav/exit_dock.c/.h`
职责：
- 负责最终离场与启动区停车
## 13. 推荐修改的现有模块
### 13.1 `segment_fsm`
必须补：
- 动作模式输入
- 区分：
- 沟内前进
- 原地转向
- 连接段推进
- 出场段直线
- 否则正式比赛阶段会在端部动作上卡死
### 13.2 `nav_script`
建议定位调整为：
- 临时验证脚本
- 单段测试脚本
- 或过渡期动作编排器
不建议继续作为最终赛道总控。
### 13.3 `corridor_msgs`
应补充：
- 赛道级阶段枚举
- 动作模式枚举
- 重捕获结果结构
- 赛道级状态输出结构
## 14. 推荐实施顺序
### 第 1 步：补底层动作语义
先修好：
- 原地转向安全逻辑
- 局部控制与安全层语义一致性
- 局部测试模式与可观测性
目标：
- 让“走沟、转向、连接直行”都能单独稳定测试
### 第 2 步：加入赛道级状态机
新增：
- `global_nav_fsm`
- `track_map`
目标：
- 系统明确知道“当前第几沟、下一沟是谁、这次该左转还是右转”
### 第 3 步：加入段间动作执行器
新增：
- `lane_transition`
- `heading_hold`
- `reacquire_detector`
目标：
- 从一条沟末端稳定过渡到下一条沟入口并重新入沟
### 第 4 步：补最终出场与回停
新增：
- `exit_dock`
目标：
- 让整场流程闭环，不只是在 6 条沟之间来回
### 第 5 步：统一参数、日志和调试接口
目标：
- 可调
- 可观测
- 可复现
- 可在实地快速定位问题
## 15. 一句话结论
本项目后续不应继续按“单沟脚本补丁”方式扩展。
正确方向应是：
**用固定地图描述赛道，用赛道级状态机管理 S 型遍历，用动作执行器完成两次 90° 转向与连接段推进，再用现有局部闭环完成每一条垄沟内的稳定行驶。**