1.0
This commit is contained in:
143
App/VL53L0X_API/MIGRATION_NOTES.md
Normal file
143
App/VL53L0X_API/MIGRATION_NOTES.md
Normal file
@@ -0,0 +1,143 @@
|
||||
# VL53L0X_API Notes
|
||||
|
||||
本文档只说明 `VL53L0X_API` 目录里我们自己写的兼容层分别负责什么，方便后续在旧工程里继续集成。
|
||||
|
||||
## 目录分层
|
||||
|
||||
### `core/`
|
||||
|
||||
这是 ST 官方 API 核心层，主要负责：
|
||||
|
||||
- 设备初始化
|
||||
- 寄存器访问逻辑
|
||||
- 测距流程
|
||||
- 官方校准函数
|
||||
- `PerformRefSpadManagement`
|
||||
- `PerformRefCalibration`
|
||||
- `PerformOffsetCalibration`
|
||||
- `PerformXTalkCalibration`
|
||||
|
||||
这部分一般不要随便改，除非明确要修改 ST 官方行为。
|
||||
|
||||
### `platform/`
|
||||
|
||||
这是我们自己写的兼容层，负责把 ST API 接到 STM32 工程里。
|
||||
|
||||
## 我们自己写的兼容层文件说明
|
||||
|
||||
### `platform/vl53l0x_platform.h`
|
||||
|
||||
平台适配层头文件。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 声明和 STM32 平台相关的接口
|
||||
- 给 ST API 提供底层读写函数声明
|
||||
- 提供总线、GPIO、延时、地址切换相关接口声明
|
||||
|
||||
### `platform/vl53l0x_platform.c`
|
||||
|
||||
平台适配层实现。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 对接 STM32 的 `HAL I2C`
|
||||
- 实现 VL53L0X 所需的寄存器读写
|
||||
- 绑定每颗传感器的 I2C 句柄和地址
|
||||
- 控制 `XSHUT`
|
||||
- 支持运行时改 I2C 地址
|
||||
|
||||
简单说，这一层负责把 ST API 的“平台无关接口”落到我们实际板子上。
|
||||
|
||||
### `platform/vl53_board.h`
|
||||
|
||||
板级管理层头文件。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 定义多颗 VL53 共用的管理结构体
|
||||
- 定义快照结构 `Vl53BoardSnapshot_t`
|
||||
- 定义硬件配置结构 `Vl53BoardHwCfg_t`
|
||||
- 对外暴露板级接口，例如：
|
||||
- `Vl53Board_Init`
|
||||
- `Vl53Board_StartContinuous`
|
||||
- `Vl53Board_StopContinuous`
|
||||
- `Vl53Board_ReadAll`
|
||||
- 当前版本还扩展了校准相关数据结构和接口
|
||||
|
||||
这一层是“上层任务代码直接调用的 API”。
|
||||
|
||||
### `platform/vl53_board.c`
|
||||
|
||||
板级管理层实现。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 管理一组 VL53 传感器
|
||||
- 完成多颗设备的上电、拉低/拉高 `XSHUT`、分配运行地址
|
||||
- 调 ST 官方初始化流程
|
||||
- 启动/停止连续测量
|
||||
- 轮询读取测距结果
|
||||
- 组织成统一快照给上层使用
|
||||
- 对每颗测距值做简单卡尔曼滤波
|
||||
|
||||
如果说 `vl53l0x_platform.c` 是“底层总线适配”，那 `vl53_board.c` 就是“更贴近业务的板级封装”。
|
||||
|
||||
### `platform/vl53_calibration_config.h`
|
||||
|
||||
这是我们自己加的运行时校准配置头。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 集中保存已经确认过的校准值
|
||||
- 让上层在启动后把这些值写回 VL53
|
||||
|
||||
当前主要用于保存：
|
||||
|
||||
- `offset_micro_meters`
|
||||
- `xtalk_compensation_rate_mcps`
|
||||
- 对应的 `*_calibrated` 标志
|
||||
|
||||
这不是 ST 官方自带文件，是我们为了工程集成方便单独加的配置层。
|
||||
|
||||
### `platform/vl53l0x_types.h`
|
||||
|
||||
类型辅助头文件。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 补充平台侧需要的公共类型定义
|
||||
- 给 `platform` 层做类型适配
|
||||
|
||||
### `platform/vl53l0x_platform_log.h`
|
||||
|
||||
日志相关兼容头。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 兼容 ST API 的日志宏
|
||||
- 当前工程里基本属于轻量占位层
|
||||
|
||||
## 推荐修改边界
|
||||
|
||||
如果后续要继续改 VL53 集成，优先修改：
|
||||
|
||||
- `platform/vl53_board.c/.h`
|
||||
- `platform/vl53_calibration_config.h`
|
||||
|
||||
如果只是换板子、换 I2C、换 XSHUT 引脚，优先修改：
|
||||
|
||||
- `platform/vl53l0x_platform.c/.h`
|
||||
|
||||
如果不是明确要动 ST 官方行为，尽量不要去改：
|
||||
|
||||
- `core/`
|
||||
|
||||
## 一句话总结
|
||||
|
||||
我们自己写的兼容层分两层：
|
||||
|
||||
- `vl53l0x_platform.*`：把 ST API 接到 STM32 HAL
|
||||
- `vl53_board.*`：把多颗 VL53 封装成上层任务可直接使用的板级接口
|
||||
|
||||
而 `vl53_calibration_config.h` 是我们额外加的“运行时固定标定值配置文件”。
|
||||
BIN
App/VL53L0X_API/VL53L0X.pdf
Normal file
BIN
App/VL53L0X_API/VL53L0X.pdf
Normal file
Binary file not shown.
397
Doc/CODE_REVIEW.md
Normal file
397
Doc/CODE_REVIEW.md
Normal file
@@ -0,0 +1,397 @@
|
||||
# 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
Normal file
667
Doc/GLOBAL_NAV_REQUIREMENT.md
Normal file
@@ -0,0 +1,667 @@
|
||||
# 固定场地条件下的赛道级导航需求说明
|
||||
|
||||
## 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。
|
||||
773
Doc/HANDOFF.md
Normal file
773
Doc/HANDOFF.md
Normal file
@@ -0,0 +1,773 @@
|
||||
# ARES 项目交接文档
|
||||
|
||||
> **最后更新**: 2025 年
|
||||
> **项目性质**: B类"马铃薯捡拾机器人竞技"走廊导航系统
|
||||
> **本文目标**: 让任何人 (人类或 AI) 能在 30 分钟内理解整个项目并开始工作
|
||||
|
||||
---
|
||||
|
||||
## 目录
|
||||
|
||||
1. [一句话概述](#1-一句话概述)
|
||||
2. [比赛规则速览](#2-比赛规则速览)
|
||||
3. [硬件架构](#3-硬件架构)
|
||||
4. [软件架构总览](#4-软件架构总览)
|
||||
5. [代码目录结构](#5-代码目录结构)
|
||||
6. [数据流水线 (核心)](#6-数据流水线-核心)
|
||||
7. [各模块详解](#7-各模块详解)
|
||||
8. [FreeRTOS 任务一览](#8-freertos-任务一览)
|
||||
9. [CAN 通信协议](#9-can-通信协议)
|
||||
10. [EKF 算法详解](#10-ekf-算法详解)
|
||||
11. [控制器与安全层](#11-控制器与安全层)
|
||||
12. [比赛流程状态机](#12-比赛流程状态机)
|
||||
13. [全部可调参数](#13-全部可调参数)
|
||||
14. [已知问题与待办](#14-已知问题与待办)
|
||||
15. [构建与烧录](#15-构建与烧录)
|
||||
16. [实车调试流程](#16-实车调试流程)
|
||||
17. [文件快速索引](#17-文件快速索引)
|
||||
|
||||
---
|
||||
|
||||
## 1. 一句话概述
|
||||
|
||||
一个运行在 **STM32H743 (FreeRTOS)** 上的走廊跟踪导航系统——通过 **4 个 VL53L0X 侧向测距** 做横向/航向定位（鲁棒 EKF），**4 个前后激光** 做防撞和到端检测，PD 控制器纠偏，安全状态机兜底，最终通过 **CAN 总线** 将速度指令发给 STM32F407 底盘控制器。
|
||||
|
||||
---
|
||||
|
||||
## 2. 比赛规则速览
|
||||
|
||||
| 项目 | 规格 |
|
||||
|------|------|
|
||||
| 场地尺寸 | 390 cm × 300 cm |
|
||||
| 垄沟（走廊）宽度 | **40 cm**（允许 ±5% 误差） |
|
||||
| 田垄数量 | 5 条 |
|
||||
| 垄间模式需遍历 | **6 条垄沟** |
|
||||
| 机器人尺寸 | **20 cm × 20 cm**（含外壳） |
|
||||
| 驱动方式 | 四轮差速 |
|
||||
| 比赛时间 | 5 分钟 |
|
||||
| 任务 | 在走廊中往返行驶，捡拾目标物 |
|
||||
|
||||
**关键约束**: 走廊 40cm - 车体 20cm = 每边仅 **10cm 余量**。1cm 的传感器偏差就是 10% 的误差。
|
||||
|
||||
### 2.1 赛道拓扑理解（垄间模式）
|
||||
|
||||
本项目当前最相关的正式比赛模式是 **垄间作业模式**。
|
||||
|
||||
#### 赛道几何
|
||||
|
||||
- 整个比赛场地尺寸为 `390cm × 300cm`
|
||||
- 场地内部共有 **5 条田垄**
|
||||
- 每条田垄：长 `220cm`、宽 `30cm`、高 `12cm`
|
||||
- 围栏与田垄之间、以及相邻田垄之间，均形成宽 `40cm` 的可通行垄沟
|
||||
- 场地仅有 **1 个出入口**，宽 `40cm`
|
||||
- 出入口外侧紧邻 **比赛启动区**，尺寸 `40cm × 100cm`
|
||||
|
||||
#### 为什么是 6 条垄沟，不是 5 条
|
||||
|
||||
虽然场地内只有 **5 条田垄**，但在垄间模式下，机器人实际需要遍历的是 **6 条垄沟**：
|
||||
|
||||
1. 最左侧围栏与第 1 条田垄之间的垄沟
|
||||
2. 第 1 条田垄与第 2 条田垄之间的垄沟
|
||||
3. 第 2 条田垄与第 3 条田垄之间的垄沟
|
||||
4. 第 3 条田垄与第 4 条田垄之间的垄沟
|
||||
5. 第 4 条田垄与第 5 条田垄之间的垄沟
|
||||
6. 第 5 条田垄与最右侧围栏之间的垄沟
|
||||
|
||||
也就是说：
|
||||
|
||||
- **田垄数 = 5**
|
||||
- **垄间可通行垄沟数 = 6**
|
||||
|
||||
这是赛道理解里最容易搞错的地方。若只按“5 条走廊”建模，会与正式比赛要求不一致。
|
||||
|
||||
#### 垄间模式下的导航任务本质
|
||||
|
||||
在垄间模式下，导航任务不是“单条走廊往返”，而是：
|
||||
|
||||
1. 从启动区进入场地
|
||||
2. 进入某一条垄沟
|
||||
3. 沿垄沟稳定行驶并完成作业
|
||||
4. 到达端部后完成转向和换沟
|
||||
5. 依次遍历 **全部 6 条垄沟**
|
||||
6. 最终从唯一出入口驶离场地
|
||||
7. 自主停在启动区内
|
||||
|
||||
从导航角度看，赛道是一个 **6 段平行窄走廊 + 端部换沟动作 + 单出入口回停** 的组合问题。
|
||||
|
||||
#### 对导航系统的直接要求
|
||||
|
||||
- **窄走廊居中跟踪**
|
||||
走廊宽 `40cm`，若车体宽 `20cm`，理论左右余量各仅 `10cm`
|
||||
- **到端检测**
|
||||
能识别走到垄沟尽头，避免撞围栏
|
||||
- **端部转向**
|
||||
在垄沟末端稳定完成姿态调整
|
||||
- **换沟**
|
||||
从当前垄沟切换到相邻垄沟，而不是原地掉头后回到同一条垄沟
|
||||
- **赛道级状态机**
|
||||
知道当前是第几条垄沟、下一条目标是哪条、何时结束遍历
|
||||
- **最终退出与停区**
|
||||
驶离场地后必须自主停在启动区，否则可能判 0 分
|
||||
|
||||
#### 当前项目与赛道要求的关系
|
||||
|
||||
当前 ARES 项目已经具备：
|
||||
|
||||
- 单条走廊内的相对定位
|
||||
- 走廊跟踪控制
|
||||
- 到端检测
|
||||
- 原地转向
|
||||
- 安全停车与防撞
|
||||
|
||||
但当前脚本状态机更接近：进入一条走廊、沿走廊前进、到端后转向、再走一趟、然后退出。
|
||||
|
||||
它更像 **单垄沟闭环验证系统**，还不是完整的 **6 垄沟遍历赛道导航系统**。
|
||||
|
||||
后续若要适配正式比赛，重点不是重写 EKF，而是补上：
|
||||
|
||||
- 多垄沟遍历状态机
|
||||
- 端部换沟策略
|
||||
- 最终出场与启动区停车逻辑
|
||||
|
||||
---
|
||||
|
||||
## 3. 硬件架构
|
||||
|
||||
### 3.1 双 MCU 体系
|
||||
|
||||
```
|
||||
┌─────────────────────────────────┐ CAN Bus ┌─────────────────────────┐
|
||||
│ STM32H743 (上位机/主控) │ ◄──────────────► │ STM32F407 (底盘控制器) │
|
||||
│ 480MHz Cortex-M7, DP-FPU │ 0x100 TX │ LADRC 电机控制 │
|
||||
│ FreeRTOS + 传感器 + 导航算法 │ 0x181/200 RX │ 四路编码器 + 四路电机 │
|
||||
└─────────────────────────────────┘ └─────────────────────────┘
|
||||
```
|
||||
|
||||
### 3.2 传感器清单
|
||||
|
||||
| 传感器 | 数量 | 用途 | 接口 | 量程 |
|
||||
|--------|------|------|------|------|
|
||||
| **VL53L0X** ToF | 4 (左前/左后/右前/右后) | 侧向墙壁测距，走廊跟踪 | I2C1 + I2C2 | 2cm ~ 2m |
|
||||
| **STP-23L** 激光 | 2 (前/后) | 远距离测距，到端检测 | UART2 / UART4 (230400bps) | 7cm ~ 7.5m |
|
||||
| **ATK-MS53L1M** 激光 | 2 (前/后) | 近距离补盲 (填 STP 的 7cm 盲区) | UART3 / UART6 (115200bps) | ~4m |
|
||||
| **HWT101** IMU | 1 | 单轴陀螺仪 (航向角速度 wz) | UART7 | ±2000°/s |
|
||||
|
||||
### 3.3 VL53L0X 接线
|
||||
|
||||
```
|
||||
左侧 (I2C2): 右侧 (I2C1):
|
||||
左前 LF: XSHUT=PB1, 地址=0x62 右前 RF: XSHUT=PD13, 地址=0x56
|
||||
左后 LR: XSHUT=PC5, 地址=0x64 右后 RR: XSHUT=PD14, 地址=0x58
|
||||
```
|
||||
|
||||
### 3.4 其他 GPIO
|
||||
|
||||
| 引脚 | 功能 |
|
||||
|------|------|
|
||||
| PE3 | CAN 活动 LED (100ms 翻转) |
|
||||
|
||||
---
|
||||
|
||||
## 4. 软件架构总览
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────────────┐
|
||||
│ FreeRTOS 任务层 │
|
||||
│ canTxTask monitorTask navTask laserTestTask vl53Task imuTask │
|
||||
│ (20ms) (100ms) (20ms) (50ms) (100ms) (10ms) │
|
||||
└──────┬──────────┬──────────┬──────────┬───────────────┬──────────┬─────────┘
|
||||
│ │ │ │ │ │
|
||||
│ CmdSlot │ │ Blackboard (全局传感器数据中心) │
|
||||
│ Pop │ │ taskENTER_CRITICAL 保护 │
|
||||
▼ ▼ ▼ │
|
||||
┌─────────┐ ┌────────┐ ┌───────────────────────────────────┐ │
|
||||
│CAN 0x100│ │Odom │ │ 导航流水线 (navTask, 20ms) │ │
|
||||
│→ 底盘 │ │积分 │ │ Preproc → EKF → Script │ │
|
||||
│ │ │→黑板 │ │ → Ctrl → SafetyFSM → CmdSlot │ │
|
||||
└─────────┘ └────────┘ └───────────────────────────────────┘ │
|
||||
│
|
||||
┌───────────────────────────────────────────────────────────┘
|
||||
│
|
||||
各传感器驱动：hwt101.c / vl53_board.c / laser_manager.c
|
||||
→ Blackboard_Update*()
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. 代码目录结构
|
||||
|
||||
```
|
||||
D:\ARES\
|
||||
├── App/ ← 【所有业务代码在这里】
|
||||
│ ├── robot_params.h ← ★ 全局参数配置中心 (唯一的调参入口)
|
||||
│ ├── app_tasks.c/.h ← 所有 FreeRTOS 任务实现 + 系统初始化
|
||||
│ ├── retarget.c/.h ← printf → USB CDC 重定向
|
||||
│ │
|
||||
│ ├── Can/
|
||||
│ │ └── snc_can_app.c/.h ← CAN 协议层 (编解码/CRC8/滤波器)【已冻结，勿动】
|
||||
│ │
|
||||
│ ├── Contract/ ← 模块间数据契约
|
||||
│ │ ├── robot_blackboard.c/.h ← 全局传感器数据黑板 (线程安全)
|
||||
│ │ ├── robot_odom.c/.h ← 差速里程计积分
|
||||
│ │ ├── robot_cmd_slot.c/.h ← 导航→CAN 指令传递槽
|
||||
│ │ └── chassis_can_msg.h ← CAN 帧结构体定义
|
||||
│ │
|
||||
│ ├── est/ ← 状态估计
|
||||
│ │ ├── corridor_ekf.c/.h ← ★ 鲁棒 EKF 核心 (3 状态)
|
||||
│ │ └── corridor_filter.c/.h ← EKF 的兼容包装层
|
||||
│ │
|
||||
│ ├── nav/ ← 导航与控制
|
||||
│ │ ├── corridor_ctrl.c/.h ← PD 走廊控制器
|
||||
│ │ ├── segment_fsm.c/.h ← 安全状态机 (防撞/急停)
|
||||
│ │ └── nav_script.c/.h ← 比赛流程编排
|
||||
│ │
|
||||
│ ├── preproc/ ← 传感器预处理
|
||||
│ │ ├── corridor_preproc.c/.h ← 数据清洗 + 互补融合
|
||||
│ │ └── corridor_msgs.h ← 所有模块间消息结构体
|
||||
│ │
|
||||
│ ├── IMU/
|
||||
│ │ └── hwt101.c/.h ← HWT101 IMU 驱动 (UART DMA)
|
||||
│ │
|
||||
│ ├── laser/
|
||||
│ │ └── laser_manager.c/.h ← 前后激光驱动 (STP+ATK, UART DMA)
|
||||
│ │
|
||||
│ └── VL53L0X_API/
|
||||
│ ├── platform/
|
||||
│ │ ├── vl53_board.c/.h ← 多传感器管理 + 卡尔曼滤波
|
||||
│ │ └── vl53l0x_platform.c/.h ← ST API HAL 适配
|
||||
│ └── core/ ← ST 官方 VL53L0X API (不修改)
|
||||
│
|
||||
├── Core/ ← CubeMX 生成的 HAL 初始化代码
|
||||
├── Drivers/ ← STM32H7 HAL 驱动
|
||||
├── Middlewares/ ← FreeRTOS + USB
|
||||
├── USB_DEVICE/ ← CDC 虚拟串口
|
||||
├── ARES.ioc ← CubeMX 工程文件
|
||||
├── CMakeLists.txt ← 构建入口
|
||||
└── STM32H743XX_FLASH.ld ← 链接脚本
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 6. 数据流水线 (核心)
|
||||
|
||||
这是整个系统的心脏，运行在 `navTask` 中，**每 20ms 执行一次**：
|
||||
|
||||
```
|
||||
Step 1: Blackboard_GetSnapshot(&board) 拍摄传感器快照 (无撕裂)
|
||||
│
|
||||
Step 2: CorridorPreproc_ExtractObs() 数据清洗
|
||||
│ • VL53L0X: mm→m, 范围校验 (0.02~2.0m)
|
||||
│ • STP+ATK: 互补融合 (ATK 填 STP 的 7cm 盲区)
|
||||
│ • 前后激光偏移补偿 (d -= PARAM_FRONT_LASER_OFFSET)
|
||||
│ → 输出: CorridorObs_t {d_lf, d_lr, d_rf, d_rr, d_front, d_back, valid_mask}
|
||||
│
|
||||
Step 3: CorridorFilter_Update() EKF 状态估计
|
||||
│ → Predict(odom_vx, imu_wz, dt) 预测步 (IMU wz + 里程计)
|
||||
│ → Update(obs) 观测步 (VL53 侧向测距)
|
||||
│ → UpdateIMUYaw(yaw_cont, ref) IMU 航向观测 (独立 1DOF 更新)
|
||||
│ → 输出: CorridorState_t {e_y, e_th, s, conf}
|
||||
│
|
||||
Step 4: NavScript_Update() 比赛流程编排
|
||||
│ → 决定当前阶段: 入口对准/走廊跟踪/转向/退出
|
||||
│ → 180° 转弯判定使用 IMU 连续 yaw (非 EKF e_th)
|
||||
│ → 输出: NavScriptOutput_t {use_override, override_v/w, request_corridor}
|
||||
│
|
||||
Step 5: CorridorCtrl_Compute() PD 控制律 (仅在走廊跟踪模式)
|
||||
│ → w = kp_theta·e_th + kd_theta·(-wz) + kp_y·e_y
|
||||
│ → v = v_cruise × (1 - 0.4 × |w/w_max|)
|
||||
│ → 输出: RawCmd_t {v, w}
|
||||
│
|
||||
Step 6: SegFsm_Update() 安全仲裁
|
||||
│ → 前方太近? → 减速/停车
|
||||
│ → 置信度太低? → 紧急停车
|
||||
│ → 输出: SegFsmOutput_t {safe_v, safe_w}
|
||||
│
|
||||
Step 7: CmdSlot_Push(safe_v, safe_w) 推入指令槽 → canTxTask 取走
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 7. 各模块详解
|
||||
|
||||
### 7.1 传感器驱动层
|
||||
|
||||
#### VL53L0X 侧向测距 (`vl53_board.c`)
|
||||
- 上电时所有 XSHUT 拉低，逐个拉高分配 I2C 地址
|
||||
- 测距预算 100ms (100000μs)，连续测距模式
|
||||
- 每个传感器读数经过独立的 **1D 卡尔曼滤波** (Q=10.0, R=14.1)
|
||||
- 输出 `Vl53BoardSnapshot_t` → `Blackboard_UpdateVl53()`
|
||||
|
||||
#### 前后激光测距 (`laser_manager.c`)
|
||||
- DMA 环形缓冲区轮询，内部自有 FreeRTOS 任务 `LaserTsk` (10ms)
|
||||
- **STP-23L**: 二进制协议，含校验和；3 帧中值滤波去尖刺
|
||||
- **ATK-MS53L1M**: ASCII 文本 `"d: 1234\r\n"`；3 帧中值滤波
|
||||
- 互补融合策略:
|
||||
- ATK 距离 < 8cm 时无条件信任 ATK (STP 盲区)
|
||||
- STP 卡在 7cm 时用 ATK
|
||||
- 两者都活着取更近的 (保守防撞)
|
||||
- STP 独活但在盲区 → 报失效 (宁可急停也不信假数据)
|
||||
|
||||
#### IMU (`hwt101.c`)
|
||||
- UART DMA 11 字节二进制帧
|
||||
- 输出: `yaw` (°)、`yaw_continuous` (°) 和 `wz` (°/s)
|
||||
- `yaw`: 原始偏航角，范围 [-180°, +180°)，跨界时跳变
|
||||
- `yaw_continuous`: **unwrap 后的连续偏航角**，消除 ±180° 跳变，可直接做差计算任意转角
|
||||
- 注意: **输出单位是°/s，不是 rad/s**
|
||||
- `HWT101_ZeroYaw()` 会同时重置 unwrap 状态
|
||||
|
||||
### 7.2 数据契约层
|
||||
|
||||
#### 黑板 (`robot_blackboard.c`)
|
||||
- 全局结构体 `g_blackboard`，所有传感器数据汇聚于此
|
||||
- 所有读写都在 `taskENTER_CRITICAL()` 临界区内
|
||||
- 消费者调用 `Blackboard_GetSnapshot()` 获取无撕裂快照
|
||||
- IMU 字段: `imu_yaw` (原始°)、`imu_yaw_continuous` (连续°)、`imu_wz` (°/s)
|
||||
|
||||
#### 里程计 (`robot_odom.c`)
|
||||
- 从 CAN 0x200 获取四轮编码器增量，经 **ISR 侧累加器** 汇聚后由 monitorTask 一次性取走积分
|
||||
- 消费链路（BUG-8 修复后）:
|
||||
```
|
||||
CAN Rx ISR → snc_parse_odom_delta() 每帧累加到 odom_accum（不覆盖）
|
||||
↓
|
||||
monitorTask → SNC_CAN_ConsumeOdomDelta() 原子取走累计值并清零
|
||||
↓
|
||||
Odom_Update() 仅在有新帧时调用（杜绝重复积分）
|
||||
```
|
||||
- 差速运动学:
|
||||
```
|
||||
v_left = (fl_delta + rl_delta) / 2 × (π × 0.060 / 500) / dt
|
||||
v_right = (fr_delta + rr_delta) / 2 × (π × 0.060 / 500) / dt
|
||||
vx = (v_left + v_right) / 2
|
||||
wz = (v_right - v_left) / 0.140
|
||||
```
|
||||
- 3 帧连续零增量 → 强制 vx=wz=0 (静止检测)
|
||||
|
||||
#### 指令槽 (`robot_cmd_slot.c`)
|
||||
- 单槽无锁设计，`CmdSlot_Push()` 写入，`CmdSlot_Pop()` 读取
|
||||
- 超过 100ms 未更新 → canTxTask 自动发零速指令 (看门狗)
|
||||
|
||||
### 7.3 估计层
|
||||
|
||||
详见 [第10节 EKF 算法详解](#10-ekf-算法详解)。
|
||||
|
||||
### 7.4 控制层
|
||||
|
||||
详见 [第11节 控制器与安全层](#11-控制器与安全层)。
|
||||
|
||||
### 7.5 导航层
|
||||
|
||||
详见 [第12节 比赛流程状态机](#12-比赛流程状态机)。
|
||||
|
||||
---
|
||||
|
||||
## 8. FreeRTOS 任务一览
|
||||
|
||||
| 任务名 | 周期 | 优先级 | 栈大小 | 职责 |
|
||||
|--------|------|--------|--------|------|
|
||||
| `canTxTask` | 20ms | AboveNormal (28) | 2048B | CAN 0x100 硬实时发送；指令槽看门狗 |
|
||||
| `navTask` | 20ms | AboveNormal (28) | 4096B | 完整导航流水线 (7步) |
|
||||
| `LaserTsk` (内部) | 10ms | AboveNormal (28) | 4096B | 4路 UART DMA 解析 |
|
||||
| `monitorTask` | 100ms | Normal (24) | 4096B | CAN 健康监控；里程计更新 |
|
||||
| `laserTestTask` | 50ms | Normal (24) | 16384B | 激光轮询 + 推送黑板 |
|
||||
| `vl53Task` | 100ms | Normal (24) | 4096B | VL53L0X 读取 + 推送黑板 |
|
||||
| `imuTask` | 10ms | BelowNormal (20) | 2048B | IMU 解析 + 推送黑板 |
|
||||
| `defaultTask` | — | Normal (24) | 512B | USB CDC 初始化；空闲循环 |
|
||||
|
||||
**全局配置**: Tick 频率 1000Hz, 堆 65536 字节, 最大优先级 56
|
||||
|
||||
---
|
||||
|
||||
## 9. CAN 通信协议
|
||||
|
||||
### 9.1 TX: H743 → F407
|
||||
|
||||
#### 0x100 — 速度指令 (每 20ms 发送)
|
||||
|
||||
| 字节 | 字段 | 类型 | 编码 |
|
||||
|------|------|------|------|
|
||||
| 0-1 | vx | int16 LE | vx (m/s) × 1000 |
|
||||
| 2-3 | wz | int16 LE | wz (rad/s) × 1000 |
|
||||
| 4 | ctrl_flags | uint8 | 控制标志位 |
|
||||
| 5 | reserved | uint8 | 固定 0 |
|
||||
| 6 | rolling_counter | uint8 | 单调递增 |
|
||||
| 7 | crc8 | uint8 | CRC8-SAE-J1850 (bytes[0..6]) |
|
||||
|
||||
**硬性约束**: 必须每 ≤150ms 发一次，否则底盘进入 `SAFE_FAULT`。即使停车也要发 vx=0, wz=0。
|
||||
|
||||
#### 0x080 — 心跳 (每 20ms, DLC=0)
|
||||
|
||||
### 9.2 RX: F407 → H743
|
||||
|
||||
#### 0x181 — 底盘状态 (20ms)
|
||||
|
||||
| 字节 | 字段 |
|
||||
|------|------|
|
||||
| 0 | system_state: 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT |
|
||||
| 1 | system_health: 0=OK, 1=WARNING, 2=FAULT |
|
||||
| 2-5 | diag_bits (uint32 LE): 通信超时/CAN BUS OFF/电机堵转等 |
|
||||
| 6 | cmd_age_10ms: 上次有效指令距今 (×10ms) |
|
||||
| 7 | status_counter |
|
||||
|
||||
#### 0x200 — 里程计增量 (~60ms)
|
||||
|
||||
| 字节 | 字段 |
|
||||
|------|------|
|
||||
| 0-1 | fl_delta_ticks (int16 LE) — 左前轮编码器增量 |
|
||||
| 2-3 | rl_delta_ticks (int16 LE) — 左后轮 |
|
||||
| 4-5 | fr_delta_ticks (int16 LE) — 右前轮 |
|
||||
| 6-7 | rr_delta_ticks (int16 LE) — 右后轮 |
|
||||
|
||||
#### 0x184 — 通信诊断 (100ms)
|
||||
|
||||
CRC 错误计数、计数器拒绝计数、CAN 丢包等统计信息。
|
||||
|
||||
---
|
||||
|
||||
## 10. EKF 算法详解
|
||||
|
||||
### 状态向量
|
||||
|
||||
```
|
||||
x = [e_y, e_th, s]ᵀ
|
||||
```
|
||||
|
||||
| 状态 | 含义 | 单位 |
|
||||
|------|------|------|
|
||||
| `e_y` | 车体中心相对走廊中心线的横向偏差 | m (左偏为正) |
|
||||
| `e_th` | 车头相对走廊方向的航向偏差 | rad (左偏为正) |
|
||||
| `s` | 沿走廊行驶里程 | m |
|
||||
|
||||
### 预测步 (每 20ms)
|
||||
|
||||
```
|
||||
输入: odom_vx (m/s), imu_wz (rad/s), dt (s)
|
||||
|
||||
e_y_new = e_y + vx × sin(e_th) × dt
|
||||
e_th_new = e_th + wz × dt
|
||||
s_new = s + vx × cos(e_th) × dt
|
||||
|
||||
雅可比矩阵 F:
|
||||
[1, vx·cos(e_th)·dt, 0]
|
||||
[0, 1, 0]
|
||||
[0, -vx·sin(e_th)·dt, 1]
|
||||
|
||||
P_pred = F·P·Fᵀ + Q
|
||||
```
|
||||
|
||||
### 观测步
|
||||
|
||||
```
|
||||
d_center = (W - Rw) / 2 + inset
|
||||
= (0.40 - 0.20) / 2 + 0 = 0.10m (车居中时传感器到墙的距离)
|
||||
|
||||
左侧观测:
|
||||
z_ey = d_center - (d_lf + d_lr)/2 横向偏差
|
||||
z_eth_L = atan2(d_lr - d_lf, L_s) 航向偏差
|
||||
|
||||
右侧观测:
|
||||
z_ey = (d_rf + d_rr)/2 - d_center 横向偏差
|
||||
z_eth_R = atan2(d_rf - d_rr, L_s) 航向偏差
|
||||
|
||||
双侧有效时: z_ey 取两侧平均 = (d_right - d_left) / 2
|
||||
→ d_center 被消掉，结果只取决于左右差值
|
||||
|
||||
单侧退化时: z_ey = d_center - d_one_side
|
||||
→ d_center 的准确性至关重要！
|
||||
|
||||
IMU 航向观测 (侧墙更新之后独立执行):
|
||||
z_eth_imu = (imu_yaw_continuous - yaw_ref) × DEG2RAD
|
||||
yaw_ref 在 EKF 置信度 ≥ 0.5 时首次锁定
|
||||
使用 1DOF 标量 EKF 更新，R 值 (PARAM_EKF_R_ETH_IMU) 远大于侧墙 R
|
||||
→ 侧墙有效时 IMU 影响小；侧墙丢失时 IMU 提供航向约束
|
||||
```
|
||||
|
||||
### 鲁棒拒绝
|
||||
|
||||
每个观测独立做 **χ² 马氏距离检验**:
|
||||
- 1 自由度门限 3.84 (95% 置信度)
|
||||
- 超过门限的观测被标记为异常并跳过更新
|
||||
|
||||
### 置信度计算
|
||||
|
||||
```
|
||||
conf = f(协方差迹) × 侧面因子 × (1 - 拒绝比例 × 0.5)
|
||||
侧面因子: 双侧=1.0, 单侧=0.7
|
||||
两侧全失效: 协方差膨胀，conf 趋向 0 → 触发 E-STOP
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 11. 控制器与安全层
|
||||
|
||||
### 11.1 PD 走廊控制器
|
||||
|
||||
```
|
||||
w_cmd = kp_theta × e_th + kd_theta × (-imu_wz) + kp_y × e_y
|
||||
─────────────── ───────────────────── ────────────
|
||||
航向比例纠偏 航向微分阻尼(用IMU) 横向比例纠偏
|
||||
|
||||
w_cmd = clamp(w_cmd, ±1.5 rad/s)
|
||||
|
||||
v_cmd = v_cruise × (1 - 0.4 × |w/w_max|) 弯道减速
|
||||
v_cmd = clamp(v_cmd, 0, 0.3 m/s)
|
||||
|
||||
置信度保护:
|
||||
conf < 0.3 → v × 0.3 (三折)
|
||||
conf < 0.6 → v × 0.7 (七折)
|
||||
```
|
||||
|
||||
**参数**: kp_theta=2.0, kd_theta=0.1, kp_y=3.0, v_cruise=0.15m/s
|
||||
|
||||
### 11.2 安全状态机 (`segment_fsm.c`)
|
||||
|
||||
```
|
||||
conf < 0.1
|
||||
┌──────────────────┐
|
||||
▼ │
|
||||
┌──────┐ Start ┌──────────┐ │ d_front ≤ 0.25m ┌──────────┐
|
||||
│ IDLE ├────────►│ CORRIDOR ├──┴──────────────────►│ APPROACH │
|
||||
└──────┘ └────┬─────┘ ◄────────────────── └────┬─────┘
|
||||
│ d_front > 0.25m │
|
||||
│ d_front ≤ 0.08m
|
||||
conf ≥ 0.5 │
|
||||
┌────┴─────┐ ┌──────▼─────┐
|
||||
│ E-STOP │ │ STOP │
|
||||
│ v=0,w=0 │ │ v=0,w=0 │
|
||||
└──────────┘ └────────────┘
|
||||
```
|
||||
|
||||
| 状态 | 行为 |
|
||||
|------|------|
|
||||
| **CORRIDOR** | 放行控制器输出 |
|
||||
| **APPROACH** | 线性减速: v 从 raw_v 衰减至 0.05m/s (d 从 25cm→8cm) |
|
||||
| **STOP** | 强制零速 |
|
||||
| **E-STOP** | 强制零速；conf ≥ 0.5 时自动恢复 |
|
||||
|
||||
---
|
||||
|
||||
## 12. 比赛流程状态机
|
||||
|
||||
> 注意：以下状态机描述的是 **当前固件已实现的单垄沟往返流程**，用于验证走廊跟踪、到端检测、转向与退出链路。
|
||||
> 它 **不等价于** 正式比赛垄间模式要求的“遍历全部 6 条垄沟”的完整赛道级导航状态机。
|
||||
|
||||
```
|
||||
IDLE → ENTRY_ALIGN → CORRIDOR_FORWARD → TURN_AT_END → CORRIDOR_BACKWARD
|
||||
↓
|
||||
(pass_count ≥ 2)
|
||||
↓
|
||||
EXIT → FINISHED
|
||||
```
|
||||
|
||||
| 阶段 | 触发条件 | 行为 | 退出条件 |
|
||||
|------|----------|------|----------|
|
||||
| **IDLE** | 初始状态 | 零速 | `NavScript_Start()` |
|
||||
| **ENTRY_ALIGN** | Start() | 慢速前进 0.08m/s，等侧向雷达找到墙 | 双侧有效 + conf≥0.8；或超时 30s |
|
||||
| **CORRIDOR_FWD** | 入口对准完成 | 走廊控制器跟踪 | d_front ≤ 0.10m (到端) |
|
||||
| **TURN_AT_END** | 到端 | 原地转 180° (1.0 rad/s, 接近时减速)；使用 IMU 连续 yaw 判定转角 | 转角 ≥ π-0.1 rad |
|
||||
| **CORRIDOR_BWD** | 第1次转完 | 走廊控制器跟踪 | d_back ≤ 0.10m (到端) |
|
||||
| **EXIT** | 第2次转完 | 0.5m/s 直线冲出 | 双侧 VL53 全丢 + 行驶 ≥ 2m |
|
||||
| **FINISHED** | 冲出完成 | 零速停车 | 终态 |
|
||||
|
||||
---
|
||||
|
||||
## 13. 全部可调参数
|
||||
|
||||
所有参数集中在 **`App/robot_params.h`** 中，修改后需重新编译烧录。
|
||||
|
||||
### P0 — 几何参数 (必须实测)
|
||||
|
||||
| 参数名 | 当前值 | 单位 | 说明 |
|
||||
|--------|--------|------|------|
|
||||
| `PARAM_ROBOT_WIDTH` | **0.200** | m | 车体外轮廓宽度 |
|
||||
| `PARAM_ROBOT_LENGTH` | **0.200** | m | 车体外轮廓长度 |
|
||||
| `PARAM_WHEEL_DIAMETER` | **0.060** | m | 驱动轮外径 |
|
||||
| `PARAM_WHEEL_TRACK` | **0.140** | m | 左右轮中心距 |
|
||||
| `PARAM_SENSOR_BASE_LENGTH` | **0.120** | m | 同侧前后 VL53 间距 |
|
||||
| `PARAM_CORRIDOR_WIDTH` | **0.40** | m | 走廊宽度 |
|
||||
| `PARAM_FRONT_LASER_OFFSET` | **0.0** | m | 前激光到车头前缘的内缩距离 |
|
||||
| `PARAM_REAR_LASER_OFFSET` | **0.0** | m | 后激光到车尾后缘的内缩距离 |
|
||||
| `PARAM_VL53_SIDE_INSET` | **0.0** | m | 侧向 VL53 到车体外壳的内缩距离 |
|
||||
| `PARAM_ENCODER_CPR` | **500** | — | 编码器每转脉冲数 |
|
||||
|
||||
### P2 — EKF 滤波器
|
||||
|
||||
| 参数名 | 当前值 | 说明 |
|
||||
|--------|--------|------|
|
||||
| `PARAM_EKF_Q_EY` | **0.01** | 横向过程噪声 (越大越信观测) |
|
||||
| `PARAM_EKF_Q_ETH` | **0.001** | 航向过程噪声 |
|
||||
| `PARAM_EKF_Q_S` | **0.1** | 里程过程噪声 |
|
||||
| `PARAM_EKF_R_EY` | **0.002** | 横向观测噪声 (越大越不信雷达) |
|
||||
| `PARAM_EKF_R_ETH` | **0.001** | 航向观测噪声 (侧墙) |
|
||||
| `PARAM_EKF_R_ETH_IMU` | **0.01** | 航向观测噪声 (IMU yaw)，远大于侧墙，用于长时约束 |
|
||||
| `PARAM_EKF_P0_EY` | **0.1** | e_y 初始不确定度 |
|
||||
| `PARAM_EKF_P0_ETH` | **0.1** | e_th 初始不确定度 |
|
||||
|
||||
### P3 — 控制器
|
||||
|
||||
| 参数名 | 当前值 | 单位 | 说明 |
|
||||
|--------|--------|------|------|
|
||||
| `PARAM_CTRL_KP_THETA` | **2.0** | — | 航向比例增益 |
|
||||
| `PARAM_CTRL_KD_THETA` | **0.1** | — | 航向微分增益 |
|
||||
| `PARAM_CTRL_KP_Y` | **3.0** | — | 横向比例增益 |
|
||||
| `PARAM_CTRL_V_CRUISE` | **0.15** | m/s | 巡航速度 |
|
||||
| `PARAM_CTRL_W_MAX` | **1.5** | rad/s | 最大角速度 |
|
||||
| `PARAM_CTRL_V_MAX` | **0.3** | m/s | 最大线速度 |
|
||||
|
||||
### P4 — 安全与脚本
|
||||
|
||||
| 参数名 | 当前值 | 单位 | 说明 |
|
||||
|--------|--------|------|------|
|
||||
| `PARAM_SAFE_D_FRONT_STOP` | **0.08** | m | 前向强制停车距离 |
|
||||
| `PARAM_SAFE_D_FRONT_APPROACH` | **0.25** | m | 前向减速预警距离 |
|
||||
| `PARAM_SAFE_APPROACH_MIN_V` | **0.05** | m/s | 减速区最低速度 |
|
||||
| `PARAM_SAFE_CONF_ESTOP` | **0.10** | — | E-Stop 置信度阈值 |
|
||||
| `PARAM_SCRIPT_ENTRY_TIMEOUT` | **30000** | ms | 入口对准超时 |
|
||||
| `PARAM_SCRIPT_TURN_OMEGA` | **1.0** | rad/s | 转向角速度 |
|
||||
| `PARAM_SCRIPT_EXIT_RUNOUT` | **2.0** | m | 退出场地后冲距离 |
|
||||
|
||||
### P5 — 传感器驱动
|
||||
|
||||
| 参数名 | 当前值 | 说明 |
|
||||
|--------|--------|------|
|
||||
| `PARAM_VL53_TIMING_BUDGET` | **100000** | VL53L0X 测距预算 (μs) |
|
||||
| `PARAM_VL53_KALMAN_Q` | **10.0** | VL53 卡尔曼过程噪声 |
|
||||
| `PARAM_VL53_KALMAN_R` | **14.1** | VL53 卡尔曼观测噪声 |
|
||||
| `PARAM_IMU_YAW_OFFSET` | **0.0** | IMU 零位偏置 (rad) |
|
||||
|
||||
---
|
||||
|
||||
## 14. 已知问题与待办
|
||||
|
||||
### 严重 (已全部修复 ✅)
|
||||
|
||||
| # | 问题 | 状态 | 修复位置 |
|
||||
|---|------|------|----------|
|
||||
| **BUG-1** | IMU 输出 `wz` 单位是 **°/s**，但 EKF 预测步按 **rad/s** 使用，导致航向积分放大 57.3 倍 | ✅ 已修复 | `app_tasks.c:305` — 加了 `PARAM_DEG2RAD()` 转换 |
|
||||
| **BUG-2** | `corridor_filter.c` 中 EKF 的 Q/R/P0 参数全部硬编码，修改 `robot_params.h` 中的 `PARAM_EKF_*` 不生效 | ✅ 已修复 | `corridor_filter.c:40-56` — 改为读取 `PARAM_EKF_*` 宏 |
|
||||
| **BUG-3** | `AppTasks_Init()` 只调用 `SegFsm_Init()`，未调用 `SegFsm_Start()`，导致安全状态机长期处于 IDLE，输出始终为零速 | ✅ 已修复 | `app_tasks.c:382` — 补加 `SegFsm_Start()` |
|
||||
| **BUG-4** | `CORRIDOR_BACKWARD` 段转 180° 后正向行驶，但到端检测错误使用 `d_back`（后向雷达），应为 `d_front` | ✅ 已修复 | `nav_script.c:CORRIDOR_BACKWARD` — 改为检查 `d_front` |
|
||||
| **BUG-5** | `ENTRY_ALIGN` 段超时保护硬编码了 `30000`，`entry_align_timeout` 参数实际不生效 | ✅ 已修复 | `nav_script.c:155` — 改为 `s_cfg.entry_align_timeout` |
|
||||
| **BUG-6** | `EXIT` 段退出直线速度误用角速度参数 `turn_omega * 0.5f`，调转向速度会意外影响退出速度 | ✅ 已修复 | `nav_script.h` 加 `exit_v` 字段；`nav_script.c:262` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_EXIT_V`；`robot_params.h` 加宏 |
|
||||
| **BUG-7** | EKF 卡尔曼增益计算 `K = P·H^T·S⁻¹` 只乘 `S_inv` 对角项，忽略观测间相关性，增益不正确 | ✅ 已修复 | `corridor_ekf.c:576-607` — 改为完整两步矩阵乘法：先算 `PHT = P·H^T`，再算 `K = PHT·S_inv` |
|
||||
| **BUG-8** | `0x200 Odom Delta` 按"状态型最新值"消费，实为"事件型增量数据"：ISR 直接覆盖旧帧导致**漏积分**（60ms帧率 vs 100ms消费，必然丢帧）；monitorTask 无条件调用 `Odom_Update()` 导致**重复积分**（无新帧时同一份 delta 被积分两次） | ✅ 已修复 | `snc_can_app.h` 新增 `SNC_OdomDeltaAccum_t`；`snc_can_app.c` ISR 改为累加并实现 `SNC_CAN_ConsumeOdomDelta()` 原子取走；`app_tasks.c:monitorTask` 改用新 API，仅在有新帧时调用积分 |
|
||||
| **BUG-9** | IMU `yaw` 输出范围 [-180°, +180°)，跨界时跳变，且项目完全未使用 `yaw`；180° 转弯判定纯依赖 EKF `e_th` (无侧墙观测时靠 wz 积分漂移) | ✅ 已修复 | `hwt101.c` 新增 yaw unwrap → `yaw_continuous`；`robot_blackboard.h` 新增 `imu_yaw_continuous`；`nav_script.c` 转弯判定改用 IMU 连续 yaw 差值；`corridor_ekf.c` 新增 `CorridorEKF_UpdateIMUYaw()` 1DOF 标量观测更新；`corridor_filter.c` 管理 yaw_ref 参考值并在侧墙更新后注入 IMU 航向观测 |
|
||||
|
||||
### 需要实车标定
|
||||
|
||||
| # | 问题 | 说明 |
|
||||
|---|------|------|
|
||||
| **CAL-1** | `PARAM_FRONT_LASER_OFFSET = 0.0` | 如果传感器在车体内部，停车距离会比预期偏大 |
|
||||
| **CAL-2** | `PARAM_REAR_LASER_OFFSET = 0.0` | 同上 |
|
||||
| **CAL-3** | `PARAM_VL53_SIDE_INSET = 0.0` | 如果传感器内缩，单侧退化时会有系统偏差 |
|
||||
| **CAL-4** | `PARAM_IMU_YAW_OFFSET = 0.0` | 声明了但代码中未使用 |
|
||||
|
||||
### 设计风险 / 尚未处理
|
||||
|
||||
| # | 问题 | 当前影响 | 建议方向 |
|
||||
|---|------|----------|----------|
|
||||
| **RISK-1** | `TURN_AT_END` 原地转向阶段与 `SegFsm` 前向防撞逻辑存在潜在冲突 | `nav_script` 在到端后会输出 `v=0, w!=0` 的原地转向命令；但 `segment_fsm` 仍按“前向距离过近 → STOP”处理，可能把转向角速度也清零，导致机器人到端后想转却被安全层按住 | 后续应为安全层引入“动作语义”或“模式感知”，区分走廊前进与原地转向；转向阶段允许 `v=0` 时保留受限 `w`，而不是沿用普通前向防撞全停策略 |
|
||||
|
||||
补充说明：
|
||||
|
||||
- 该问题的根因不是传感器噪声，而是 **脚本层与安全层的仲裁语义不一致**
|
||||
- 侧向 VL53 在端部转向时本来也会偏离正常走廊几何，因此当前已经用 IMU `yaw_continuous` 来判定转角，这部分方向是对的
|
||||
- 真正需要补的是：**转向阶段的专用安全策略**，否则后续扩展到多垄沟遍历时，端部动作会成为卡点
|
||||
|
||||
### 代码质量 (已全部修复 ✅)
|
||||
|
||||
| # | 问题 | 状态 | 修复位置 |
|
||||
|---|------|------|----------|
|
||||
| **Q-1** | `PARAM_CTRL_SPEED_REDUCTION` 未被控制器读取，硬编码了 `0.4f` | ✅ 已修复 | `corridor_ctrl.h` 加字段 `speed_reduction_k`；`corridor_ctrl.c:59` 读取它；`app_tasks.c` 传入 `PARAM_CTRL_SPEED_REDUCTION` |
|
||||
| **Q-2** | `PARAM_SCRIPT_ENTRY_V` 未被脚本读取，硬编码了 `0.08f` | ✅ 已修复 | `nav_script.h` 加字段 `entry_align_v`；`nav_script.c:146` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_ENTRY_V` |
|
||||
| **Q-3** | `PARAM_SCRIPT_EXIT_RUNOUT` 未被脚本读取，硬编码了 `2.0f` | ✅ 已修复 | `nav_script.h` 加字段 `exit_runout_m`；`nav_script.c:275` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_EXIT_RUNOUT` |
|
||||
| **Q-4** | `PARAM_VL53_TIMING_BUDGET` 未被使用，直接传字面量 `100000` | ✅ 已修复 | `app_tasks.c:222,232` 改为传 `PARAM_VL53_TIMING_BUDGET` |
|
||||
| **Q-5** | `exit_start_s` 是函数内 `static` 局部变量，`NavScript_Reset()` 无法清除它 | ✅ 已修复 | 移入 `s_internal` 结构体，`memset(&s_internal, 0, ...)` 统一清零 |
|
||||
| **Q-6** | 转向角度通过 EKF 的 `e_th` 差值测量，不是绝对角度。如果 EKF 发散或走廊参照丢失，转向判断可能不准 | ✅ 已修复 | `nav_script.c` — 改用 IMU `yaw_continuous` (unwrap 后的连续偏航角) 判定转角，不再依赖 EKF `e_th` |
|
||||
| **Q-7** | `g_snc_can_app` 被中断写、被任务读，没有互斥保护 (中等风险) | ✅ 部分修复 | `odom_accum` 的取走/清零已通过 `taskENTER_CRITICAL` 保护（BUG-8 修复）；其余字段（status/rpm/diag）仍无保护，但读写均为原子宽度或单次赋值，风险可接受 |
|
||||
|
||||
---
|
||||
|
||||
## 15. 构建与烧录
|
||||
|
||||
### 构建
|
||||
|
||||
```bash
|
||||
# 确保 gcc-arm-none-eabi 在 PATH 中
|
||||
cmake --preset Debug
|
||||
cmake --build build/Debug
|
||||
```
|
||||
|
||||
- 输出: `build/Debug/ARES.elf`
|
||||
- 工具链: `cmake/gcc-arm-none-eabi.cmake`
|
||||
- C 标准: C11
|
||||
- 浮点 printf: 已开启 (`-u _printf_float`)
|
||||
|
||||
### 烧录
|
||||
|
||||
使用 **STM32CubeProgrammer** 或 **OpenOCD** 烧录 `.elf` 文件到 STM32H743。
|
||||
|
||||
### 注意事项
|
||||
|
||||
- `CMakeLists.txt` 第 38 行有一个大写 `.C` 后缀 (`laser_manager.C`)，在大小写敏感的文件系统上可能会构建失败
|
||||
- MPU Region 1 配置了 `0x30000000` 32KB 为非缓存区，所有 DMA 缓冲区必须放在此区域 (`.dma_buffer` section)
|
||||
- USB CDC 用于 `printf` 输出 (通过 `retarget.c`)
|
||||
|
||||
---
|
||||
|
||||
## 16. 实车调试流程
|
||||
|
||||
### 建议顺序
|
||||
|
||||
```
|
||||
P0: 几何参数 → 用卷尺实测，填入 robot_params.h
|
||||
↓
|
||||
P1: 里程计标定 → 直线跑 10m，对比编码器累积距离
|
||||
↓
|
||||
P2: EKF 调优 → 从保守 Q/R 开始，观察走廊跟踪稳定性
|
||||
↓
|
||||
P3: 控制器调参 → 先调 kp_theta/kd_theta (航向)，再调 kp_y (横向)
|
||||
↓
|
||||
P4: 安全阈值 → 根据实际场地微调停车/减速距离
|
||||
```
|
||||
|
||||
### 常见问题诊断
|
||||
|
||||
| 现象 | 可能原因 | 调整方法 |
|
||||
|------|----------|----------|
|
||||
| 车头左右摆动 | kp_theta 过大 或 kd_theta 过小 | 减小 kp_theta 或增大 kd_theta |
|
||||
| 横向纠偏太慢 | kp_y 过小 | 增大 kp_y |
|
||||
| 到端刹不住 | d_front_stop 过小 或 approach 区间太短 | 增大 PARAM_SAFE_D_FRONT_STOP |
|
||||
| 总是急停 | EKF 置信度低，可能是 VL53 数据不稳定 | 增大 R (降低观测信任) 或检查传感器接线 |
|
||||
| EKF 发散 | Q 过小 (不信观测) 或 R 过小 (过信噪声数据) | 增大 Q 或增大 R |
|
||||
| 单侧靠墙走 | PARAM_VL53_SIDE_INSET 未校准 | 实测传感器内缩距离并填入 |
|
||||
|
||||
---
|
||||
|
||||
## 17. 文件快速索引
|
||||
|
||||
| 你想做什么 | 去看哪个文件 |
|
||||
|-----------|-------------|
|
||||
| 改任何调参数值 | `App/robot_params.h` |
|
||||
| 理解系统初始化 | `App/app_tasks.c` → `AppTasks_Init()` |
|
||||
| 理解导航流水线 | `App/app_tasks.c` → `AppTasks_RunNavTask_Impl()` |
|
||||
| 看 EKF 数学 | `App/est/corridor_ekf.c` |
|
||||
| 看控制律 | `App/nav/corridor_ctrl.c` |
|
||||
| 看安全逻辑 | `App/nav/segment_fsm.c` |
|
||||
| 看比赛编排 | `App/nav/nav_script.c` |
|
||||
| 看传感器预处理 | `App/preproc/corridor_preproc.c` |
|
||||
| 看 CAN 协议 | `App/Can/snc_can_app.c` + `App/Contract/chassis_can_msg.h` |
|
||||
| 看全局数据结构 | `App/Contract/robot_blackboard.c/.h` |
|
||||
| 看里程计 | `App/Contract/robot_odom.c` |
|
||||
| 看激光驱动 | `App/laser/laser_manager.c` |
|
||||
| 看 VL53 驱动 | `App/VL53L0X_API/platform/vl53_board.c` |
|
||||
| 看 IMU 驱动 | `App/IMU/hwt101.c` |
|
||||
| 看消息结构定义 | `App/preproc/corridor_msgs.h` |
|
||||
|
||||
---
|
||||
|
||||
> **给接手者的最后提醒**:
|
||||
> 1. **先修 BUG-1** (IMU 单位转换)，否则 EKF 完全不可用
|
||||
> 2. **再修 BUG-2** (EKF 参数硬编码)，否则调参改了白改
|
||||
> 3. 拿到实车后第一件事：用卷尺量 CAL-1 ~ CAL-3 的三个偏移量
|
||||
> 4. CAN 协议层 (`snc_can_app.c`) 已冻结，**不要修改**
|
||||
> 5. IMU `yaw_continuous` 已接入转弯判定，实车调试时对比 `wz` 积分与 `yaw` 哪个更稳，参见 `IMU_YAW_WZ_ISSUE.md`
|
||||
458
Doc/HYBRID_NAVIGATION_GUIDE.md
Normal file
458
Doc/HYBRID_NAVIGATION_GUIDE.md
Normal file
@@ -0,0 +1,458 @@
|
||||
# 固定地图赛道的混合导航说明
|
||||
|
||||
## 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
Normal file
321
Doc/IMU_YAW_REQUIREMENT.md
Normal file
@@ -0,0 +1,321 @@
|
||||
# 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
|
||||
- 该需求与当前实现存在结构性差异
|
||||
- 从传感器误差特性看，这一需求是合理的
|
||||
- 后续建议将“位置”和“航向”两个估计任务明确拆分，各自交给更适合的传感器主导
|
||||
23
Doc/can通讯协议.md
Normal file
23
Doc/can通讯协议.md
Normal file
@@ -0,0 +1,23 @@
|
||||
FDR-Core 上 位 机 CAN 协 议 说 明 书 （ 当 前 固 件 实 装 版 ） ⽂ 档 状 态 ： Internal / Current 适 ⽤ 固 件 ： 当 前 上 传 的 f4_can_app.c 实 现 说 明 依 据 ： 以 .c 代 码 ⾏ 为为 准 1. ⽬ 标 与 范 围 本 ⽂ 档 定 义上 位 机 与 底 盘 控 制 器 之 间 的 CAN 通 信 协 议 ， 覆 盖 以 下 内 容 ： 上 位 机 向 底 盘 下 发 速 度 命 令 底 盘 向 上 位 机 上 报 状 态 、 轮 速 、 ⾥ 程 和 通 信 诊 断 命 令 安 全 机 制 ： CRC 、 rolling counter 、 命 令 超 时 故 障 / 警 告 状 态 的 上 报 规 则 2. 总 线 与 编 码 约 定 CAN 类 型 ： 标 准 帧 （ 11-bit ID ） 帧 类 型 ： 数 据 帧 （ RTR=0 ） 字 节 序 ： ⼩ 端 缩 放 规 则 ： vx 、 wz ： int16 = 物理量 * 1000 轮 速 ： RPM 直 接按 int16 发 送 ⾥ 程 ： 增 量 ticks ， 按 int16 发 送 CRC ： CRC8-SAE J1850 poly = 0x1D init = 0xFF xorout = 0xFF ⾮ 反 射 当 前 固 件 在 0x100 控 制 帧 上 ， 对 前 7 个 字 节 计 算 CRC ， 再 放 ⼊ Byte7 。
|
||||
|
||||
3. 帧 ID 总 览 ⽅ 向 ID 名 称 实 际 周 期 说 明 Rx 0x080 Heartbeat 上 位 机 ⾃ 定 仅 表 ⽰ 链 路 活 着 ， 不 刷 新 运 动 看 ⻔ 狗 Rx 0x100 Velocity Command 建 议 20ms 速 度 控 制 命 令 ， 必 须 持 续 发 送 Tx 0x181 Status 20ms 状 态 机 / 健 康 等 级 / 诊 断 位 Tx 0x182 Actual RPM 轮 询 ， 约 60ms 实 际 轮 速 Tx 0x183 Target RPM 轮 询 ， 约 60ms ⽬ 标 轮 速 Tx 0x184 Comm Diag 100ms 通 信 统 计 Tx 0x200 Odom Delta 轮 询 ， 约 60ms 四 轮 ⾥ 程 增 量 当 前 固 件 在 CAN_Send_Telemetry_20ms() 中 采 ⽤ 如 下 节 拍 ： 0x181 ： 每 个 20ms 周 期 都 发 0x182 / 0x183 / 0x200 ： 通过 s_telem_slot 三 选 ⼀ 轮 询 发 送 0x184 ： 每 5 个 20ms 周 期 发 ⼀ 次 ， 即 100ms ⼀ 次 4. 上 位 机 -> 底 盘 4.1 0x080 ⼼ 跳 帧 ⽤ 途 ： 仅 表 ⽰ “ 链 路 上 仍 然 有 ⼈ 在 说话 ”
|
||||
|
||||
⾏ 为 ： 不 会 刷 新 运 动 看 ⻔ 狗 不 会 延 续 旧 速 度 命 令 当 前 固 件 不 解 析 其 载 荷 内 容 ， 也不 使 ⽤ DLC 内 容 建 议 ： 可发可 不 发 若 发 ， DLC 可 统 ⼀ 为 0 或 8 4.2 0x100 速 度 控 制 帧 ID ： 0x100 DLC ： 8 数 据 定 义 Byte 类 型 字 段 说 明 0~1 int16 LE vx_x1000 线 速 度 ， 单 位 m/s * 1000 2~3 int16 LE wz_x1000 ⻆ 速 度 ， 单 位 rad/s * 1000 4 uint8 ctrl_flags 控 制 标 志 位 ， 当 前 仅 保 存 ， 不 参 与 控 制决 策 5 uint8 reserved 预 留 ， 固 定 填 0 6 uint8 rolling_counter 滚 动 计 数 器 7 uint8 crc8 对 Byte0~6 做 CRC8-SAE J1850 固 件 接 收 后 会 把 vx 、 wz 恢 复 成 浮 点 值 ， 分别 写 ⼊ ： g_robot_ctrl.target_vx g_robot_ctrl.target_wz
|
||||
|
||||
并 保 存 ctrl_flags 与 rolling counter 。 接 收 判 定 规 则 1. DLC 必 须 等 于 8 2. CRC 必 须 正 确 3. rolling counter 必 须 满 ⾜ 下 列 规 则 ： 若 尚 未 同 步 ， 或 当 前 不 在 SYSTEM_OPERATIONAL ， 则 ⾸ 帧 直 接接 受 正 常 运 ⾏ 时 ， 新 counter 相 对 上 ⼀ 帧差 值 必 须 在 1..3 之 间 否 则 拒 收 发 送 建 议 推 荐 周 期 ： 20ms （ 50Hz ） 最 低 要 求 ： 不 要 超 过 150ms 不 发 送 合 法 0x100 即 使你 想 停 ⻋ ， 也 建 议 继续 周 期 发 送 ： vx = 0 wz = 0 的 合 法 命 令 帧 因 为 当 前 固 件 只 ⽤ “ 合 法 且 新 鲜 ” 的 0x100 喂 命 令 看 ⻔ 狗 ， 超 时 阈 值 是 150ms 。 5. 底 盘 -> 上 位 机 5.1 0x181 状 态 帧 ID ： 0x181 周 期 ： 20ms DLC ： 8 Byte 类 型 字 段 说 明 0 uint8 system_state 0=BOOTING , 1=OPERATIONAL , 2=SAFE_FAULT 1 uint8 system_health 0=OK , 1=WARNING , 2=FAULT
|
||||
|
||||
Byte 类 型 字 段 说 明 2~5 uint32 LE diag_bits 当 前 诊 断 位 图 6 uint8 cmd_age_10ms 距 最 近 ⼀ 次 合 法 0x100 已 过 去 多 少 个 10ms tick 7 uint8 status_counter 状 态 帧 发 送 计 数 器 5.2 0x182 实 际 轮 速 帧 ID ： 0x182 周 期 ： 轮 询 ， 约 60ms DLC ： 8 Byte 类 型 字 段 0~1 int16 LE FL 实 际 RPM 2~3 int16 LE RL 实 际 RPM 4~5 int16 LE FR 实 际 RPM 6~7 int16 LE RR 实 际 RPM 5.3 0x183 ⽬ 标 轮 速 帧 ID ： 0x183 周 期 ： 轮 询 ， 约 60ms DLC ： 8 Byte 类 型 字 段 0~1 int16 LE FL ⽬ 标 RPM 2~3 int16 LE RL ⽬ 标 RPM
|
||||
|
||||
Byte 类 型 字 段 4~5 int16 LE FR ⽬ 标 RPM 6~7 int16 LE RR ⽬ 标 RPM 5.4 0x184 通 信 诊 断 帧 ID ： 0x184 周 期 ： 100ms DLC ： 8 Byte 字 段 说 明 0 valid_cmd_total_lsb 合 法 命 令 累 计 低 8 位 1 crc_error_total_lsb CRC 错 误 累 计 低 8 位 2 counter_reject_total_lsb rolling counter 拒 收 累 计 低 8 位 3 can_tx_drop_total_lsb CAN 发 送 丢 帧 累 计 低 8 位 4 busoff_total_lsb Bus-Off 累 计 低 8 位 5 rx_overrun_total_lsb FIFO overrun 累 计 低 8 位 6 last_accepted_counter 最 近 ⼀ 次 接 受 的 rolling counter 7 err_nibbles ⾼ 4 位 = 连 续 counter 错 误 数 ； 低 4 位 = 连 续 CRC 错 误 数 ； 均 饱 和 到 15 5.5 0x200 ⾥ 程 增 量 帧 ID ： 0x200 周 期 ： 轮 询 ， 约 60ms DLC ： 8
|
||||
|
||||
Byte 类 型 字 段 0~1 int16 LE FL delta ticks 2~3 int16 LE RL delta ticks 4~5 int16 LE FR delta ticks 6~7 int16 LE RR delta ticks 说 明 ： 这 是 时 间 窗 内 增 量 不 是 累 计 总 值 上 位 机 如 需 总 ⾥ 程 / 总 编 码 器 计 数 ， 需 要 ⾃ ⾏ 累 加 积 分 6. 系 统 状 态 与 健 康 等 级 6.1 system_state 值 名 称 含 义 0 SYSTEM_BOOTING 上 电 后 尚 未 收 到 第 ⼀ 帧 合 法 速 度 命 令 1 SYSTEM_OPERATIONAL 正 常 ⼯ 作 2 SYSTEM_SAFE_FAULT 安 全 保 护 ， ⽬ 标 速 度已 强 制 清 零 6.2 system_health 值 名 称 含 义 0 SYSTEM_HEALTH_OK ⽆ 活 动 中 的 警 告 / 故 障 1 SYSTEM_HEALTH_WARNING 有 警 告 ， 但仍 可 ⼯ 作 2 SYSTEM_HEALTH_FAULT 有 明 确 故 障 ， 通 常已 经 或 应 进 ⼊ 保 护
|
||||
|
||||
7. diag_bits 诊 断 位 图 定 义 diag_bits 是 ⼀ 个 32 位 ⼩ 端 位 图 ， ⽬ 前 定 义 如 下 ： bit 宏 名 级 别 含 义 0 DIAG_COMM_TIMEOUT Fatal 速 度 控 制 帧 超 时 1 DIAG_CAN_BUS_OFF Fatal CAN Bus-Off 2 DIAG_CMD_CRC_STORM Fatal 连 续 CRC 错 误 过 多 3 DIAG_CMD_CNT_STORM Fatal 连 续 rolling counter 错 误 过 多 4 DIAG_MOTOR_FL_STALL Fatal 左 前 轮 堵 转 / 失 效 趋 势 5 DIAG_MOTOR_RL_STALL Fatal 左 后 轮 堵 转 / 失 效 趋 势 6 DIAG_MOTOR_FR_STALL Fatal 右 前 轮 堵 转 / 失 效 趋 势 7 DIAG_MOTOR_RR_STALL Fatal 右后 轮 堵 转 / 失 效 趋 势 8 DIAG_CONTROL_SATURATION Warning 控 制 输 出 ⻓ 时 间 顶 满 建 议 上 位 机 把 ： bit0 ~ bit7 视 为 故 障 类 bit8 视 为 警 告 类 8. 安 全 策 略 与 故 障 触 发 条 件 8.1 命 令 超 时 命 令 看 ⻔ 狗 超 时 阈 值 ： 150ms 只 有 合 法 且 新 鲜 的 0x100 才 会 刷 新 0x080 ⼼ 跳 不 会 刷 新 超 时 后 ： 进 ⼊ SAFE_FAULT
|
||||
|
||||
置 位 DIAG_COMM_TIMEOUT 8.2 CRC 错 误 ⻛ 暴 若 连 续 CRC 错 误 计 数 达 到 5 进 ⼊ SAFE_FAULT 置 位 DIAG_CMD_CRC_STORM 8.3 Counter 错 误 ⻛ 暴 若 连 续 counter 错 误 计 数 达 到 5 进 ⼊ SAFE_FAULT 置 位 DIAG_CMD_CNT_STORM 8.4 CAN Bus-Off 若 发 ⽣ Bus-Off 计 数 累 加 进 ⼊ SAFE_FAULT 置 位 DIAG_CAN_BUS_OFF 8.5 电 机 堵 转 固 件 每 10ms 做 ⼀ 次 电 机 堵 转 诊 断 。 若 某 轮 持 续 满 ⾜ 以 下 条 件 ： ⽬ 标 RPM >= 40 实 际 RPM <= 8 控 制 输 出 绝 对 值 >= 850 并 持 续 50 个 10ms tick （ 即 500ms ） ， 则 ： 置 对 应 轮 ⼦ 的 stall 位
|
||||
|
||||
进 ⼊ SAFE_FAULT 8.6 控 制 饱 和 若 存 在 任 ⼀ 轮 持 续 满 ⾜ ： ⽬ 标 RPM >= 30 控 制 输 出 绝 对 值 >= 980 并 持 续 20 个 10ms tick （ 即 200ms ） ， 则 ： 置 位 DIAG_CONTROL_SATURATION 该 项 属 于 Warning ， 不 单 独 强 制 进 ⼊ SAFE_FAULT 。 9. 上 位 机 实 现 建 议 9.1 速 度 命 令 发 送 建 议 上 位 机 按 如 下 ⽅ 式 实 现 ： 周 期 ： 20ms ID ： 0x100 rolling counter ： 每 帧 ⾃ 增 1 ， uint8 ⾃ 然 回 绕 CRC ： 每 次 发 送 前 重 新 计 算 Byte0~6 的 CRC8-SAE J1850 停 ⻋ 时 不 要 停 ⽌ 发命 令 ， 继续 发 ： vx = 0 wz = 0 推 荐 发 送 频 率 推 荐 ： 50Hz （ 20ms ） 可 接 受 ： 100Hz （ 10ms ） 不 建 议 低 于 10Hz 绝 不 能 超 过 150ms 不 发合 法 0x100
|
||||
|
||||
9.2 ⼼ 跳 发 送 ID ： 0x080 可 选 仅 ⽤ 于上 位 机 链 路 监 控 不 要 依 赖 它 维 持 运 动 9.3 状 态 接 收 建 议 重 点 订 阅 ： 0x181 ： 状 态 / 健 康 / 故 障 0x184 ： 通 信 统 计 以 及 按 需 订 阅 ： 0x182 ： 实 际 轮 速 0x183 ： ⽬ 标 轮 速 0x200 ： ⾥ 程 增 量 建 议 上 位 机 ⾄ 少 显 ⽰ ： system_state system_health diag_bits cmd_age_10ms crc_error_total_lsb counter_reject_total_lsb busoff_total_lsb 四 轮 实 际 RPM 四 轮 ⽬ 标 RPM 四 轮 增 量 ticks
|
||||
|
||||
10. 0x100 组 帧 参 考 伪代 码 counter = (counter + 1) & 0xFF vx_i16 = round(vx_mps * 1000) wz_i16 = round(wz_radps * 1000) data[0] = vx_i16 & 0xFF data[1] = (vx_i16 >> 8) & 0xFF data[2] = wz_i16 & 0xFF data[3] = (wz_i16 >> 8) & 0xFF data[4] = ctrl_flags data[5] = 0 data[6] = counter data[7] = crc8_j1850(data[0:7]) send(id=0x100, dlc=8, data=data)
|
||||
61
Doc/map.md
Normal file
61
Doc/map.md
Normal file
@@ -0,0 +1,61 @@
|
||||
```地图
|
||||
比例尺: 1字符 = 10cm x 10cm
|
||||
物理尺寸: X轴(横向)净宽 300cm, Y轴(纵向)净深 390cm
|
||||
|
||||
图例说明:
|
||||
[#] : 赛场实体围栏 (外部边界)
|
||||
[.] : 垄沟/平地通道 (左侧40cm, 右侧40cm, 垄间40cm)
|
||||
[=] : 垄背凸起 (长220cm, 宽30cm)
|
||||
[S] : 比赛启动区 (宽40cm, 深100cm，紧邻左侧通道)
|
||||
|
||||
X: 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
|
||||
Y: ------------------------------------------------------------------
|
||||
0 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
|
||||
1 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
2 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
3 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
4 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
5 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
6 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
7 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
8 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
9 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
10 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
11 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
12 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
13 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
14 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
15 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
16 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
17 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
18 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
19 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
20 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
21 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
22 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
23 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
24 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
25 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
26 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
27 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
28 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
29 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
30 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
31 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
32 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
33 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
34 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
35 | # . . . . = = = = = = = = = = = = = = = = = = = = = = . . . . #
|
||||
36 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
37 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
38 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
39 | # . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . #
|
||||
40 | # S S S S # # # # # # # # # # # # # # # # # # # # # # # # # # #
|
||||
41 S S S S
|
||||
42 S S S S
|
||||
43 S S S S <-- 启动区长度 100cm (共10格深)
|
||||
44 S S S S
|
||||
45 . . . .
|
||||
```
|
||||
|
||||
|
||||
507
Doc/大体方案.md
Normal file
507
Doc/大体方案.md
Normal file
@@ -0,0 +1,507 @@
|
||||
# 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
|
||||
27
Doc/附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则.md
Normal file
27
Doc/附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则.md
Normal file
@@ -0,0 +1,27 @@
|
||||
- 1 - “奥凯杯”第十一届国际大学生智能农业装备创新大赛 B 类“马铃薯捡拾机器人竞技”比赛及评审规则 一、比赛规则要点 1 、 马铃薯捡拾机器人可采用垄间作业或跨垄作业模式。 垄间作业模式， 机器人在比赛过程中需遍历所有 6 条垄沟；跨垄作业模式，机器人作业幅宽 只允许跨一个垄背，且比赛过程中需遍历所有 5 个垄背。 2 、比赛需要作业，每支队伍有两次机会，成绩取最优者。参赛机器人需 进行马铃薯捡拾作业，以作业速度和作业效果进行综合成绩评判。比赛中无 作业动作的参赛机器人，比赛成绩以 0 分计。 3 、比赛限时 5 分钟（含违章加罚比赛用时）。从 100 秒预备时间已到 之后，评委发出“起跑”命令后，机器人开始跨越起跑线开始计时，到参赛 机器人所有部位都离开比赛场地出入口终止计时。 在限定时间内未完成比赛 者，比赛成绩以 0 分计。 4 、 比赛结束后， 参赛机器人须驶离场地， 任何部位不得停留在场地内， 否则视为未完成比赛。离开场地后需要自主停在比赛启动区，未自主停止的 视为未完成比赛，自主停止但未完全停在比赛启动区的视为连续超界。 5 、各参赛单位可以派出多支参赛队。但每支参赛队都必须根据比赛要 求，自行设计（或组装）、制作各自的参赛机器人。限定每支参赛队只能有 1 台机器人参赛。 6 、比赛分专本组和硕博组两类，以队伍中学历最高者为分类依据。
|
||||
|
||||
- 2 - 二、比赛场地及作业要求 1 、 比赛场地 如图 1 所示。比赛场地为 390cm × 300cm 的区域，共有 5 条田垄。 场地四周用高 12cm 的围栏围住，只留有一个宽 40cm 的出入口，围栏 与田垄之间留有宽 40cm 的通道（垄沟），垄长 220cm 、垄宽 30cm 、垄 高 12cm ，相邻田垄之间也留有宽 40cm 的通道（垄沟）。出入口外侧紧 邻比赛场地的 40cm × 100cm 的区域为比赛启动区，尺寸如图 1 所示： 图 1 比赛场地简图 围栏和田垄可采用钢质或木制材料制作， 田垄为灰色， 围栏为黑色； 地面采用爬行垫铺设，爬行垫标准（ 1. 颜色：灰色； 2. 材质： PE ； 3. 尺
|
||||
|
||||
- 3 - 寸：采用 60cm*60cm 标准尺寸进行拼接）。现场比赛时会从 6 条垄 沟中随机抽取两条垄沟，在其中放置宽 40cm 、长 300cm （即与垄沟等 长）的卡其色地毯，以模拟松软路面，地毯材质为丙纶，底部为防滑网 格底，厚度约 5.5mm ，所有场地毛毯布置均相同。如图 2 所示。 图 2 爬行垫及地毯 2 、马铃薯 马铃薯用榉木实心仿真鸽子蛋代替，有棕色和绿色 2 种，分别用 奶黄色和青绿色油漆喷涂，尺寸规格为外径 30mm ，长度 42mm ，重 量约 14g 。具体规格和形状如图 3 所示。
|
||||
|
||||
- 4 - 图 3 马铃薯示意图（单位 mm ） 3 、土块 土块使用黄色原积木代替， 因现实中有各种颜色土块， 为使比赛 更接近现实，故允许土块有色差，尺寸为 1.5cm 的立方体，具体如 图 4 。
|
||||
|
||||
- 5 - 图 4 土块 4 、 作业要求 参赛机器人需自行设计马铃薯载运装置，比赛时需将马铃薯从 田垄上捡拾起来放到小车上，比赛结束时将其运到比赛启动区，期 间马铃薯不能掉落。 5 、比赛场地布置 每条田垄放置若干颗马铃薯和若干颗土块， 在靠近出入口侧的田垄 长度中心线上每隔 20cm 做标记点，马铃薯在标记点处放置，方向为其 处长度方向与田垄长度中心线 45 °范围内任一方向；土块围绕马铃薯 随机放置，放置范围为以标记点为中心点，长 70mm 、高 75mm 的矩形 区域内随机放置。具体如下： ①专本组 ： 每条田垄放置 2 颗棕色马铃薯、 每颗马铃薯附近随机放 置 4 颗土块，同一级别比赛马铃薯放置标记点位置相同。
|
||||
|
||||
- 6 - ②硕博组： 每条田垄放置 2 颗棕色马铃薯、 1 颗绿色马铃薯，绿色 马铃薯为干扰组，不得捡拾，每颗马铃薯附近随机放置 4 颗土块，同一 级别比赛马铃薯放置标记点位置相同，角度在规定范围内随机。 赛前组委会会设计 5 种硕博组场地布置方案并进行密封处理，决 赛现场由裁判或参赛队员现场随机抽取 1 种方案进行布置；其中专本 组绿色马铃薯位置不做任何布置，棕色马铃薯放置标记点位置与硕博 组相同。 三、比赛规则 比赛根据参赛队员中最高学历者设置专本组和硕博组 2 个级别， 分开比赛和统计成绩。每支队伍都有两次机会。 1. 比赛规则 ①根据参赛队伍情况， 设置多块比赛场地。 比赛前组委会会随机为 各参赛队分配一块场地， 各比赛场地内的参赛队抽签确定比赛顺序， 同 一队伍两次机会结束后， 下一队伍开始比赛。 各场地同时进行比赛。 同 一级别比赛的每块比赛场地毛毯、马铃薯的放置位置均相同。 ②比赛成绩根据参赛机器人“作业得分” “走过的通道数” “能 否驶出场地”“能否停在启动区”和“比赛用时”等进行评比。 ③限时 5 分钟内（含违章加罚比赛用时）未完全驶出场地的比 赛成绩为 0 分； ④驶出场地但所有部位都未停止在比赛启动区内（不含启动区 标线）的成绩为 0 分；
|
||||
|
||||
- 7 - 2. 相关概念界定 ● 有效作业： 参赛机器人发现某个须捡拾马铃薯，控制作业装置对 其进行作业， 将马铃薯放置到小车上， 且过程中不掉落， 则视为一次有 效作业。 ● 无效作业： 参赛机器人发现某个须捡拾马铃薯，控制作业装置对 其进行作业， 但未将马铃薯放置到小车上， 或者放到小车上但比赛过程 中掉落，则视为一次无效作业。 ● 漏 捡： 参赛机器人在作业过程中未对某个须捡拾马铃薯进行捡拾 作业，则视为一次漏施。 ● 误捡： 参赛机器人在比赛过程中对绿色马铃薯进行作业，致其发 生位移或捡拾成功，则视为一次误捡。 ● 损伤 马铃薯 ： 参赛机器人或作业装置的任何动作导致某个马铃薯 上产生划痕（长度≥ 1cm ）等肉眼可见损伤 ，视为损伤。 ● 损伤、 损坏 2 颗及以内马铃薯等情况从已得作业分数中扣相应 分数； ● 损坏 马铃薯 ： 参赛机器人或作业装置的任何动作导致使马铃薯发 生结构断裂、或使其形状发生严重变形等，视为损坏。 ● 损伤、损坏 3 颗及以上马铃薯，取消比赛成绩，直接罚下。 ● 违章 ：在比赛过程中，如果机器人或作业装置的任何部分超出了 围栏边界，触碰到了围栏、垄测，则判为违章。违章次数为超界次数和 触碰次数之和，比赛时由工作人员现场判别并统计。 ● 驶离比赛场地 ：在完成作业后，参赛机器人需要从出入口离开比 赛场地， 参赛机器人的任何部位不得停留在场地内 （按照俯视投影方法 判断），否则视为未完成比赛，成绩为 0 分。
|
||||
|
||||
- 8 - ● 停在比赛启动区 ： 参赛机器人从比赛出口驶出后需要停在比赛启 动区内， 未主动停止的视为未完成比赛， 主动停止但未完全停在比赛启 动区的视为 1 次连续超界，按 2 次违章计算惩罚加时 。 ④比赛过程中不允许使用任何形式的遥控装置，如被裁判发现 或被举报查实，立即取消参赛资格。 ⑤如果机器人或作业装置在比赛过程中出现冲出场地、 失控、 部 件损坏、损坏场地（不含马铃薯、土块）等危险情况，则该参赛机器 人将被立即强制罚下，取消所有比赛成绩。 ⑥比赛开始后， 参赛人员不得以任何理由申请重试， 如因机器人 或作业装置故障而无法在规定时间内完成比赛的，本次比赛以失败 论处。 ⑦如果参赛人员不遵守裁判和工作人员的指示、 指令或警告， 或 做出任何有悖于公平竞争精神的行为，裁判有权直接取消该参赛队 的参赛资格。 3 、比赛过程 ①签到： 所有参赛队都必须在规定时间内到赛场签到， 由评委检 查参赛机器人及其作业装置是否符合比赛要求。检查通过后，关闭 参赛机器人电源，并由工作人员将参赛机器人统一放置在备赛区对 应号位。参赛选手之后不得再进行任何调试，违反者以作弊论处， 取消比赛资格。 ②铺设毛毯： 比赛前由裁判随机抽取确定。 同级别赛场的播种板 位置、毛毯铺设位置和铺设方式完全相同。 ③预备： 评委宣布 “ XX 号机器人进行比赛” 后， 工作人员将 XX 号参赛机器人从备赛区取出，放到比赛启动区。评委宣布“预备”
|
||||
|
||||
- 9 - 后，开始预备计时。选手将参赛机器人放到起跑位置，可以给参赛 机器人上电，但参赛机器人的任何部位都不允许超出起跑线。选手 做好起跑准备后告知评委“已就位”。预备时间最长 100 秒。 ④起跑： 评委在参赛选手告知“已就位”之后，评委发出“起 跑”命令后，从小车跨越起始线开始计时；或 100 秒预备时间已到 之后，评委发出“起跑”命令时，开始计时。 参赛选手给参赛机器 人上电（也可提前上电），参赛机器人从比赛启动区出发进入比赛 场地。如在评委发出“起跑”命令之前参赛机器人或其作业装置就 已跨越起跑线则视为抢跑，评委给予警告，并重新起跑。抢跑两次 则比赛以失败论处，计 0 分。 ⑤比赛： 比赛过程中， 由工作人员记录机器人走过的通道数、 作 业得分、违章次数和比赛用时，由裁判确认是否驶出场地和停在启 动区。参赛机器人一旦从出入口驶出，则本次比赛结束。比赛时间 为 5 分钟，参赛机器人超时仍未完成比赛的，比赛也即刻中止。如 果发生机器人冲出场地、部件损坏、损坏场地等情况，裁判有权终 止比赛，且参赛机器人的比赛以失败论处。 ⑥统计和确认成绩： 工作人员统计参赛机器人是否驶出比赛场 地、走过的垄沟 / 垄背数、比赛用时、作业得分 ，参赛选手确认并签 字。如有异议，回放录像确认。 ⑦各参赛队都有两次比赛机会，比赛成绩取最高者。 4 、评分标准 （ 1 ）作业得分计分规则 专本组评分标准
|
||||
|
||||
- 10 - 马铃薯颜色 马铃薯 数量 每颗 分值 评分 标准 棕色马铃薯 10 颗 10 有效作业得 10 分 硕博组评分标准 马铃薯颜色 马铃薯 数量 每颗 分值 评分 标准 棕色马铃薯 10 颗 10 有效作业得 10 分 绿色马铃薯 5 颗 20 绿色为干扰组，不得捡 拾，使其发生位移每颗 扣 5 分，捡拾成功每颗 扣 20 分 土块误捡扣分标准 土块误拾扣分标准 （ 最终误捡土块数量占总土块比例） 0% ≤误捡≤ 10% 10% ＜误捡≤ 30% 30% 误捡≤ 50% 50% ＜误捡 不扣分 扣最终作业得分 20 分 扣最终作业得分 40 分 最终作业得分 0 分 ①无效作业、漏捡：不得分也不扣分。 ②误捡： 捡拾成功（将绿色马铃薯捡拾到小车上）每颗扣 20 分 且次数累加，直至最终作业得分 0 分为止。绿色马铃薯发生位移按 5 分 / 颗，从已得作业分中扣除，直至最终作业得分 0 分为止。 ③ 损伤、损坏 2 颗及以内马铃薯 ，按 20 分 / 颗，从已得作业分中 扣除相应分数，直至最终作业得分 0 分为止 。 ④ 损伤、损坏 3 颗 及以上 马铃薯 ，取消比赛成绩，直接罚下。 ⑤ 超出围栏边界或触碰围栏、垄侧，按 10 秒 / 次，加罚比赛用 时。连续刮擦、推行或触碰，都按两次违章计算（ 20 秒 / 次）。 ⑥ 触碰垄背不加罚比赛用时。
|
||||
|
||||
- 11 - ⑦如果连续超出围栏边界按 20 秒 / 边计算加罚比赛用时。 （ 2 ）比赛成绩 比赛成绩综合考虑作业用时和作业得分，由两者经归一化处理 后相加得到。选手作业用时为 a ，作业得分为 b ，比赛成绩 S 为： S= max min max min max min 100 0.4 100 0.6 a a b b a a b b − − × × + × × − − 式中： 𝑎𝑎 𝑚𝑚𝑚𝑚𝑚𝑚 —— 所有选手中，作业用时的最大值； 𝑎𝑎 𝑚𝑚𝑖𝑖𝑖𝑖 —— 所有选手中，作业用时的最小值； 𝑏𝑏 𝑚𝑚𝑚𝑚𝑚𝑚 —— 所有选手中，作业得分的最大值； 𝑏𝑏 𝑚𝑚 𝑖𝑖𝑖𝑖 —— 所有选手中，作业得分的最小值。 （ 3 ）获奖比例 决赛期间，大赛委员会根据当年参数规模情况决定各档次获奖 作品的数量。 四、参赛要求 1 、参赛机器人 ①参赛机器人应具有自主行走、 垄间穿行、 识别马铃薯和作业的 能力。 ②作业装置可背负在机器人上， 也可由机器人牵引。 机器人和作 业装置的大小和重量不限，但应尽量小巧，以提高作业灵活性。 ③在整个作业过程中，机器人和作业装置的任何部分都不允许 超出围栏边界，也不允许触碰围栏、垄，更不允许破坏比赛场地。 ④比赛场地周围环境无特殊设置，参赛机器人应能承受周围环 境的光线、噪音和电磁干扰。 ⑤同一参赛单位的任意两台参赛机器人都不可以雷同。如被裁
|
||||
|
||||
- 12 - 判质疑雷同，则对该参赛单位的所有类同参赛机器人的队长进行问 辩测试。如被裁判判定为雷同，则取消所有类同参赛机器人的参赛 资格，并判定成绩无效。 2 、参赛团队 ①参赛队员必须为 2025 年 9 月前（含 9 月）正式注册的全日制 非成人教育的普通高等学校和高职高专院校的在校学生，包括专科 生、本科生、硕博研究生。 ②本科及以上院校每个参赛单位最多可派出 8 支参赛队（承办 单位可以多派 5 支队伍），职业院校每个参赛单位最多可派出 5 支 参赛队伍。每位指导教师最多只能指导一支参赛队。 ③每支参赛队都必须根据比赛要求， 自行设计、 制作各自的参赛 机器人。限定每支参赛队只能有 1 台机器人参赛。 ④参赛队员由 2 ～ 5 名学生组成，须为在校专科生、本科生和硕 博研究生，不限学科专业，并指定学历最高者为队长。 ⑤允许最多 2 名队员在准备区内调试机器人。 五、比赛事宜 1 、 比赛过程 （ 1 ）比赛开始前，各队有 100 秒的准备时间，将机器人置于比 赛区域的入口（启动区），并进行必要的调整，机器人可以加电，但 不得运动； （ 2 ）比赛开始，机器人从启动区启动。如在指令前启动机器人 则判为抢跑，给予警告，第二次抢跑的机器人将被罚下； （ 3 ）比赛过程中冲出场地的机器人将直接被罚下，不得重新进 场比赛， 比赛过程中， 如果出现机器人分离， 该机器人被强制罚下。
|
||||
|
||||
- 13 - 2 、 重试及断电 （ 1 ）比赛开始后，任何机器人不得申请重试，如因故障而不能 运动，则自动退出比赛，为了机器人的安全和保护场地，裁判有权 将机器人断电并拿出场外； （ 2 ）如机器人在场上出现故障或失控，裁判有权根据现场情况 要求该机器人断电并拿出场地。 3 、 取消比赛资格 参赛队的下列行为会被取消比赛资格。 （ 1 ） 机器人做出危险动作， 危及场上操作手或裁判、 观众安全； （ 2 ）故意损坏比赛场地、道具； （ 3 ）不遵守裁判发出的命令和警告； （ 4 ）做出任何有悖公平竞争精神的行为。 六、比赛安全 安全是机器人比赛持续发展的最重要问题。 因此， 每位参赛者应 特别重视并有义务按照本节的规定在充分采取安全措施的前提下研 制机器人。 第一，所有机器人的制作不应给队员、裁判、工作人员、观众、 设备和比赛场地造成伤害。如果现场裁判认为机器人的行为对人员 或设备有潜在危险，可以禁止该机器人参赛或随时终止比赛。 第二，机器人的结构设计应该考虑到赛前机器人安全检查的方 便性。 第三，禁止使用燃油驱动的发动机、爆炸物、高压气体（超过 0.8MPa ）等。 第四， 在参赛任何时段， 队员都必须充分注意安全问题。 指导教
|
||||
|
||||
- 14 - 师或教练应该负起安全指导和监督的责任。参赛期间必须考虑工作 人员和场馆内观众的安全。 七、比赛其它事项 第一， 裁判有权对本规则没有规定的任何行为做出裁决。 在有争 议的情况下，裁判长有权做出最终裁决。 第二，比赛场地及道具尺寸的允许误差为 ±5% 。 第三，重要通知和相关附录后续在官方网站发布。 第四， 比赛将根据报名情况确定赛制， 赛制将在比赛前在官方网 站上发布。 第五， 规则如有更新， 比赛将在官方网站上发布， 以比赛开始前 最后发布的规则为准。 第六，鼓励参赛队在规则允许的范围内以自己的方式装饰机器 人。 第七， 比赛过程中不得使用通讯装置操控机器人， 一旦发现， 以 作弊论处。 第八，如果有需要，比赛将在合适的时间要求各参赛队提交机 器人相关资料、进度报告和录像。 第九，规则的最终解释说明权归大赛委员会所有。 第十一届国际大学生智能农业装备创新大赛委员会 2025 年 9 月 10 日
|
||||
Reference in New Issue
Block a user