diff --git a/App/app_tasks.c b/App/app_tasks.c index 74e9e8c..f599437 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -420,7 +420,9 @@ void AppTasks_Init(void) .sensor_base_length = PARAM_SENSOR_BASE_LENGTH, /* 实测：同侧前后雷达间距 */ .corridor_width = PARAM_CORRIDOR_WIDTH, /* 实测：走廊宽度 */ .y_offset = 0.0f, /* 0 = 绝对居中 */ - .side_sensor_inset = PARAM_VL53_SIDE_INSET, /* 实测：侧向传感器内缩距离 */ + .side_sensor_inset = PARAM_VL53_SIDE_INSET, /* [兼容] 统一内缩距离 */ + .left_sensor_inset = PARAM_VL53_LEFT_INSET, /* [改进A] 左侧独立内缩 (实测后填入!) */ + .right_sensor_inset = PARAM_VL53_RIGHT_INSET, /* [改进A] 右侧独立内缩 (实测后填入!) */ .robot_width = PARAM_ROBOT_WIDTH, /* 实测：车体宽度 */ .alpha_theta = 0.98f, /* 保留兼容，EKF 内部使用 Q/R */ .alpha_y = 0.7f, diff --git a/App/est/corridor_ekf.c b/App/est/corridor_ekf.c index b3481f3..508dfb4 100644 --- a/App/est/corridor_ekf.c +++ b/App/est/corridor_ekf.c @@ -376,23 +376,24 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state) float d_rf = obs->d_rf, d_rr = obs->d_rr; float W = s_cfg.corridor_width; float yoff = s_cfg.y_offset; - float inset = s_cfg.side_sensor_inset; - float Rw = s_cfg.robot_width; + float Rw = s_cfg.robot_width; - /* 传感器居中时的理论读数 (考虑车体宽度和传感器内缩) */ - float d_center = (W - Rw) / 2.0f + inset; + /* [改进A] 分侧传感器内缩补偿 — 消除左右安装不对称引起的系统性偏置 + * 左右各自使用独立的 inset 值计算期望居中读数 d_center */ + float d_center_left = (W - Rw) / 2.0f + s_cfg.left_sensor_inset; + float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset; /* 观测值 (测量) — 仅横向位置，不含航向 */ float z_ey = 0.0f; int valid_sides = 0; if (left_ok) { - z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff; + z_ey += d_center_left - ((d_lf + d_lr) / 2.0f) - yoff; valid_sides++; } if (right_ok) { - z_ey += ((d_rf + d_rr) / 2.0f) - d_center - yoff; + z_ey += ((d_rf + d_rr) / 2.0f) - d_center_right - yoff; valid_sides++; } @@ -427,9 +428,15 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state) float e_y = s_state.x[0]; float y_ey = z_ey - e_y; + /* [改进F] 自适应观测噪声 R: + * 双侧观测: R × 0.5 (噪声互相平均) + * 单侧观测: R × 3.0 (缺少交叉验证, VL53 可信度低时尤需降低信任) + */ float R_ey = s_cfg.r_ey; if (valid_sides == 2) { R_ey *= 0.5f; + } else if (valid_sides == 1) { + R_ey *= 3.0f; } float S_ey = s_state.P[0][0] + R_ey; diff --git a/App/est/corridor_ekf.h b/App/est/corridor_ekf.h index 5920b87..5b43dd2 100644 --- a/App/est/corridor_ekf.h +++ b/App/est/corridor_ekf.h @@ -7,14 +7,18 @@ * - e_th : 航向偏差 (rad) * - s : 沿走廊进度 (m) * - * 观测向量：z = [z_ey, z_eth_L, z_eth_R]^T - * - z_ey : 横向偏差观测 (由左右侧平均距离差计算) - * - z_eth_L: 左侧航向观测 (atan2((d_lr-d_lf), L_s)) - * - z_eth_R: 右侧航向观测 (atan2((d_rf-d_rr), L_s)) + * 观测模型 (方向 B — IMU 主导航向): + * - 侧墙观测: 1DOF 标量更新，仅更新 e_y (横向偏差) + * z_ey = 左右侧 VL53 平均距离差 (支持分侧内缩补偿) + * - IMU 航向观测: 独立 1DOF 标量更新，更新 e_th + * z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad + * - 侧墙航向观测 (z_eth_L/z_eth_R) 已取消: + * VL53 前后差分噪声过大 (±2cm → ~13° 航向噪声)，不适合做航向源 * * 鲁棒机制： * - χ² 马氏距离检验拒绝异常观测 - * - 自适应观测噪声 (根据传感器健康度调整 R 矩阵) + * - 分侧 VL53 内缩补偿 (消除左右安装不对称引起的系统性偏置) + * - 单侧观测时自适应增大 R (降低退化状态下的信任度) * - 协方差上界保护 (防止发散) */ @@ -43,7 +47,7 @@ typedef struct { /* 观测噪声协方差 R */ float r_ey; // 横向观测噪声方差 (侧墙) float r_eth; // 航向观测噪声方差 (侧墙) - float r_eth_imu; // 航向观测噪声方差 (IMU yaw)，建议远大于 r_eth + float r_eth_imu; // 航向观测噪声方差 (IMU yaw)，IMU 准确度高时可设较小值 /* 初始协方差 */ float P0_diag[3]; // 初始 P 对角线 @@ -56,7 +60,9 @@ typedef struct { float sensor_base_length; // 同侧前后雷达间距 L_s float corridor_width; // 走廊标准宽度 float y_offset; // 期望偏置 - float side_sensor_inset; // VL53L0X 侧向传感器内缩距离 (传感器面到车体外边缘) + float side_sensor_inset; // [兼容] 侧向传感器统一内缩距离 (当 left/right 未单独设置时使用) + float left_sensor_inset; // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘, 实测) + float right_sensor_inset; // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘, 实测) float robot_width; // 车体外轮廓宽度 (用于单侧退化时的精确定位) } CorridorEKFConfig_t; diff --git a/App/est/corridor_filter.c b/App/est/corridor_filter.c index 589b076..4508610 100644 --- a/App/est/corridor_filter.c +++ b/App/est/corridor_filter.c @@ -39,6 +39,8 @@ void CorridorFilter_Init(const CorridorFilterConfig_t *config) ekf_cfg.corridor_width = config->corridor_width; ekf_cfg.y_offset = config->y_offset; ekf_cfg.side_sensor_inset = config->side_sensor_inset; + ekf_cfg.left_sensor_inset = config->left_sensor_inset; /* [改进A] 左侧独立内缩 */ + ekf_cfg.right_sensor_inset = config->right_sensor_inset; /* [改进A] 右侧独立内缩 */ ekf_cfg.robot_width = config->robot_width; /* 过程噪声 Q —— 统一从 robot_params.h 读取，改参数只改那一个文件 */ @@ -85,26 +87,34 @@ void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx * 在侧墙观测之后独立执行 1DOF 标量更新。 * * 方向 B 改造：IMU 现在是航向 e_th 的唯一观测来源。 - * 侧墙不再提供航向观测，因此初期置信度可能略低。 - * 将参考值锁定门槛从 0.5 降到 0.3，确保 IMU 航向观测 - * 能尽早介入。 + * 侧墙不再提供航向观测。 * - * 参考值管理策略： - * 首次收到有效 IMU yaw 且 EKF 置信度足够时锁定参考值。 - * 此时 e_th 由 wz 积分驱动，锁定后 yaw 变化量就等于 e_th 变化量。 + * [改进E] 参考值管理策略 (IMU 驱动即时锁定): + * IMU yaw 准确度高，转向完成后 IMU 指向即走廊方向。 + * 不再等 VL53 收敛 (conf >= 0.3)，而是在 EKF reset 后 + * 首次收到有效 IMU yaw 即锁定参考值。 + * 此时 e_th = 0 (刚 reset)，ref = imu_yaw。 */ if (imu_yaw_valid) { - if (!s_imu_yaw_ref_set && out_state->conf >= 0.3f) { - /* 首次锁定：此刻记录 IMU yaw 与当前 e_th 的对应关系 */ - s_imu_yaw_ref_rad = imu_yaw_continuous_rad - out_state->e_th; + if (!s_imu_yaw_ref_set) { + /* [改进E] 即时锁定: 不依赖 VL53 置信度，IMU 有效即锁定 */ + s_imu_yaw_ref_rad = imu_yaw_continuous_rad; /* e_th 刚 reset 为 0，ref = imu_yaw */ s_imu_yaw_ref_set = true; } if (s_imu_yaw_ref_set) { CorridorEKF_UpdateIMUYaw(imu_yaw_continuous_rad, s_imu_yaw_ref_rad, true); - /* 更新输出 (IMU 观测可能微调了 e_th) */ - CorridorEKF_GetState(out_state); + /* [改进D] 只更新状态值 (e_y, e_th, s, P)， + * 保留 CorridorEKF_Update() 计算的 conf, obs_reject_mask, mahalanobis_d2。 + * 原来直接调用 GetState() 会用纯 P_trace 重算 conf 并清零诊断字段。 */ + CorridorState_t imu_updated; + CorridorEKF_GetState(&imu_updated); + out_state->e_y = imu_updated.e_y; + out_state->e_th = imu_updated.e_th; + out_state->s = imu_updated.s; + memcpy(out_state->P, imu_updated.P, sizeof(out_state->P)); + /* conf, obs_reject_mask, mahalanobis_d2 保留自 CorridorEKF_Update() 的值 */ out_state->t_ms = obs->t_ms; } } diff --git a/App/est/corridor_filter.h b/App/est/corridor_filter.h index ee72a23..35a04ba 100644 --- a/App/est/corridor_filter.h +++ b/App/est/corridor_filter.h @@ -8,7 +8,9 @@ typedef struct { float sensor_base_length; // 同侧前后雷达的纵向安装间距 L_s (m) float corridor_width; // 走廊标准宽度 (m)，比赛规则为 0.4m float y_offset; // 期望的偏置行走量 (m)，0 表示绝对居中 - float side_sensor_inset; // VL53L0X 传感器内缩距离 (传感器面到车体外边缘) (m) + float side_sensor_inset; // [兼容] VL53L0X 传感器统一内缩距离 (m)，当分侧值未设置时使用 + float left_sensor_inset; // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘) (m) + float right_sensor_inset; // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘) (m) float robot_width; // 车体外轮廓宽度 (m) float alpha_theta; // 航向互补滤波系数 (0~1) // 【注】因为您的IMU极好，此值建议设为 0.98~0.995 diff --git a/App/nav/global_nav.c b/App/nav/global_nav.c index cfd567c..c9811fa 100644 --- a/App/nav/global_nav.c +++ b/App/nav/global_nav.c @@ -220,8 +220,18 @@ static void execute_turn(const CorridorObs_t* obs, uint32_t now_ms, GlobalNavOutput_t* out) { - float imu_yaw = board->imu_yaw_continuous.is_valid - ? board->imu_yaw_continuous.value : 0.0f; + /* [改进G] IMU 失效安全保护: 没有 IMU 数据时立即停车，不盲转。 + * 超时后会被外部超时保护捕获，进入 GNAV_ERROR。 */ + if (!board->imu_yaw_continuous.is_valid) { + out->override_v = 0.0f; + out->override_w = 0.0f; + out->use_override = true; + out->request_corridor = false; + out->safety_mode = SAFETY_MODE_IDLE; + return; + } + + float imu_yaw = board->imu_yaw_continuous.value; /* 已转过的角度 (取绝对值) */ float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign; @@ -438,8 +448,9 @@ void GlobalNav_Update(const CorridorObs_t* obs, s_last_odom_vx = odom_vx; } + /* [改进G] IMU yaw 提取: 失效时使用参考值，保持航向不变而不是跳到 0 */ float imu_yaw_deg = board->imu_yaw_continuous.is_valid - ? board->imu_yaw_continuous.value : 0.0f; + ? board->imu_yaw_continuous.value : s_nav.heading_ref_deg; /* 默认输出 */ out->use_override = true; diff --git a/App/nav/nav_script.c b/App/nav/nav_script.c index 702e24d..ca8a9ff 100644 --- a/App/nav/nav_script.c +++ b/App/nav/nav_script.c @@ -228,7 +228,8 @@ void NavScript_Update(const CorridorObs_t *obs, /* 检查是否到端 */ bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) { - /* 前向距离足够近 -> 到达垄沟末端，准备转向 */ + /* 前向距离足够近 -> 到达垄沟末端，递增趟数并准备转向 */ + s_internal.pass_count++; s_internal.turn_start_e_th = state->e_th; s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg; s_internal.turn_started = false; @@ -344,12 +345,21 @@ void NavScript_Update(const CorridorObs_t *obs, } if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) { - /* 前向距离足够近 -> 到达另一端，继续 180° 转向循环 */ - s_internal.turn_start_e_th = state->e_th; - s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg; - s_internal.turn_started = false; - s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD; - s_stage = SCRIPT_STAGE_TURN_AT_END; + /* [改进H] 到达另一端: 递增趟数并判断是否退出 + * pass_count=1(入沟首趟), =2(返回), =3(第三趟) → EXIT */ + s_internal.pass_count++; + if (s_internal.pass_count >= 3) { + /* 走了 3 趟 (去-回-去)，进入退出阶段 */ + s_stage = SCRIPT_STAGE_EXIT; + s_internal.exit_start_s = 0.0f; + } else { + /* 继续往返: 转向后进入 FORWARD */ + s_internal.turn_start_e_th = state->e_th; + s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg; + s_internal.turn_started = false; + s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD; + s_stage = SCRIPT_STAGE_TURN_AT_END; + } out->request_corridor = false; } break; diff --git a/App/preproc/corridor_msgs.h b/App/preproc/corridor_msgs.h index a56640f..26ce9cc 100644 --- a/App/preproc/corridor_msgs.h +++ b/App/preproc/corridor_msgs.h @@ -8,7 +8,14 @@ * EKF 状态维度定义 * ========================================================= */ #define EKF_STATE_DIM 3 // [e_y, e_th, s] -#define EKF_OBS_DIM 3 // [z_ey, z_eth_L, z_eth_R] + +/* [改进I] 观测维度 (与实际实现对齐): + * 侧墙观测: 1DOF 标量更新 (z_ey) + * IMU 航向: 独立 1DOF 标量更新 (z_eth_imu)，在 Update 后单独调用 + * 侧墙航向 (z_eth_L/z_eth_R) 已取消 — VL53 差分噪声过大不适合做航向源 + * 旧值 3 对应已取消的 [z_ey, z_eth_L, z_eth_R] 三维观测 + */ +#define EKF_OBS_DIM 1 // 实际: 侧墙 1DOF (z_ey) /* χ² 检验门限 (95% 置信度) */ /* 1 DOF: 3.84, 2 DOF: 5.99, 3 DOF: 7.81 */ diff --git a/App/robot_params.h b/App/robot_params.h index d1f65a7..9dc54e3 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -93,9 +93,25 @@ extern "C" { * 用途：侧向测距补偿，d_actual_to_wall = d_sensor - sensor_inset * 即传感器测到的距离减去这个内缩量才是车体边缘到墙的真实距离 * ⚠ 极其重要：沟道40cm - 车体20cm = 每边仅10cm，1cm的偏差就是10%的误差！ + * 注意：此值作为未分侧设置时的默认值。建议使用下面的分侧参数。 */ #define PARAM_VL53_SIDE_INSET 0.0f +/** @brief [改进A][实测] 左侧 VL53L0X 传感器内缩距离 [m] + * 测量方法：将机器人精确放在走廊正中央(卷尺确认)， + * 理论值 = (走廊宽-车宽)/2 = (0.40-0.20)/2 = 0.10m + * left_inset = 理论值 - 实测左侧前后平均读数 + * 用途：消除左右安装不对称引起的系统性横向偏置(解决"持续偏右/偏左"问题) + * ⚠ 标定后请填入实测值！当前为 0.0 表示未标定。 + */ +#define PARAM_VL53_LEFT_INSET 0.0f + +/** @brief [改进A][实测] 右侧 VL53L0X 传感器内缩距离 [m] + * 测量方法：同上，right_inset = 理论值 - 实测右侧前后平均读数 + * ⚠ 标定后请填入实测值！当前为 0.0 表示未标定。 + */ +#define PARAM_VL53_RIGHT_INSET 0.0f + /* ------------------- 编码器参数 ------------------- */ /** @brief [实测] 编码器每转脉冲数 (CPR) * 测量方法：查阅 F407 固件配置，或实车转动一圈数脉冲 @@ -155,12 +171,13 @@ extern "C" { /* --- EKF 观测噪声 R (对传感器信任度) --- */ /** @brief [调优] 横向观测噪声方差 [m²] * 含义：VL53L0X 测距的可靠程度 - * 调优方法：从 0.001 开始，根据走廊跟踪平滑度调整 - * 典型值：0.001~0.005 + * [改进B] 从 0.002 提高到 0.015，适配 VL53 可信度不高的实际表现 + * 调优方法：VL53 噪声越大，此值越大 + * 典型值：0.005~0.02 * 过大：EKF 不信任侧向雷达，横向响应慢 * 过小：EKF 过度信任雷达，对跳变敏感 */ -#define PARAM_EKF_R_EY 0.002f +#define PARAM_EKF_R_EY 0.015f /** @brief [调优] 航向观测噪声方差 [rad²] * 含义：同侧前后雷达差分测角的可程靠度 @@ -171,14 +188,14 @@ extern "C" { /** @brief [调优] IMU 航向观测噪声方差 [rad²] * 含义：IMU yaw 作为 EKF 航向观测时的噪声 - * 设计意图：IMU yaw 用于长时航向约束 (防止 wz 积分漂移)， - * R 值应明显大于侧墙航向观测 R_ETH，避免干扰侧墙短时精确修正 - * 调优方法：从 0.01 开始，如果 IMU yaw 非常稳定可减小 - * 典型值：0.005~0.05 - * 过大：IMU 航向观测作用弱，等于没接入 - * 过小：IMU 航向主导，侧墙观测被压制 + * [改进C] 从 0.01 降低到 0.002，充分利用 IMU yaw 的高精度。 + * IMU 是航向 e_th 的唯一观测源（侧墙航向已取消），R 值应小。 + * 调优方法：IMU yaw 越稳定，此值可越小 + * 典型值：0.001~0.01 + * 过大：IMU 航向观测作用弱，e_th 靠 wz 积分漂移 + * 过小：IMU 航向主导过强，短时抖动可能传入 */ -#define PARAM_EKF_R_ETH_IMU 0.01f +#define PARAM_EKF_R_ETH_IMU 0.002f /* --- EKF 初始状态 --- */ /** @brief [调优] e_y 初始不确定度 [m²] diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b05242..62e4f33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,7 +35,8 @@ enable_language(C ASM) # NOTE: 所有 App/ 下的源文件统一由下方 file(GLOB_RECURSE) 收集， # 此处不再手写重复列表，避免配置漂移和平台差异（如大小写敏感文件系统）。 add_executable(${CMAKE_PROJECT_NAME} - App/VL53L0X_API/platform/vl53_calibration_config.h) + App/VL53L0X_API/platform/vl53_calibration_config.h + App/VL53L0X_API/platform/vl53l0x_platform_log.h) # Add STM32CubeMX generated sources add_subdirectory(cmake/stm32cubemx) diff --git a/Doc/HANDOFF_v2.md b/Doc/HANDOFF_v2.md index b26c5f1..243546c 100644 --- a/Doc/HANDOFF_v2.md +++ b/Doc/HANDOFF_v2.md @@ -579,7 +579,7 @@ void CorridorFilter_Reset(void) { ```c // App/robot_params.h -#define USE_GLOBAL_NAV 1 // 1=赛道模式 0=单沟测试模式 +#define USE_GLOBAL_NAV 0 // 1=赛道模式 0=单沟测试模式 (当前为单沟测试) ``` ### 10.2 两种模式的差异 @@ -612,7 +612,9 @@ void CorridorFilter_Reset(void) { | `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_VL53_SIDE_INSET` | 0.0 m | 侧向 VL53 统一内缩距离 (兼容) | +| `PARAM_VL53_LEFT_INSET` | 0.0 m | 左侧 VL53 内缩距离 (实测后填入!) | +| `PARAM_VL53_RIGHT_INSET` | 0.0 m | 右侧 VL53 内缩距离 (实测后填入!) | | `PARAM_ENCODER_CPR` | 500 | 编码器每转脉冲数 | ### P2 — EKF 滤波器（调优） @@ -622,9 +624,9 @@ void CorridorFilter_Reset(void) { | `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） | +| `PARAM_EKF_R_EY` | 0.015 | 横向观测噪声 (VL53 可信度低，已调大) | +| `PARAM_EKF_R_ETH` | 0.001 | 航向观测噪声（侧墙，已取消使用） | +| `PARAM_EKF_R_ETH_IMU` | 0.002 | 航向观测噪声（IMU，已调小以充分利用高精度 IMU） | | `PARAM_EKF_P0_EY` | 0.1 | e_y 初始不确定度 | | `PARAM_EKF_P0_ETH` | 0.1 | e_th 初始不确定度 | @@ -633,8 +635,8 @@ void CorridorFilter_Reset(void) { | 参数 | 当前值 | 说明 | |------|--------|------| | `PARAM_CTRL_KP_THETA` | 2.0 | 航向比例增益 | -| `PARAM_CTRL_KD_THETA` | 0.1 | 航向微分增益 | -| `PARAM_CTRL_KP_Y` | 3.0 | 横向比例增益 | +| `PARAM_CTRL_KD_THETA` | 0.4 | 航向微分增益 | +| `PARAM_CTRL_KP_Y` | 4.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 | 最大线速度 | @@ -644,7 +646,7 @@ void CorridorFilter_Reset(void) { | 参数 | 当前值 | 说明 | |------|--------|------| -| `PARAM_SAFE_D_FRONT_STOP` | 0.08 m | 前向停车距离 | +| `PARAM_SAFE_D_FRONT_STOP` | 0.10 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 置信度阈值（CORRIDOR模式） | @@ -653,7 +655,7 @@ void CorridorFilter_Reset(void) { | 参数 | 当前值 | 说明 | |------|--------|------| -| `USE_GLOBAL_NAV` | 1 | 编译开关 | +| `USE_GLOBAL_NAV` | 0 | 编译开关 (当前为单沟测试模式) | | **入场段** | | | | `PARAM_GNAV_ENTRY_V` | 0.08 m/s | 入场速度 | | `PARAM_GNAV_ENTRY_DISTANCE` | 0.30 m | 入场里程上限 (启动区到C1入口仅约10~30cm) |