diff --git a/.idea/claudeCodeTabState.xml b/.idea/claudeCodeTabState.xml index f16edb6..f615937 100644 --- a/.idea/claudeCodeTabState.xml +++ b/.idea/claudeCodeTabState.xml @@ -7,9 +7,10 @@ diff --git a/App/VL53L1X_API/platform/vl53_calibration_config.h b/App/VL53L1X_API/platform/vl53_calibration_config.h index 44bf10a..d28f3e1 100644 --- a/App/VL53L1X_API/platform/vl53_calibration_config.h +++ b/App/VL53L1X_API/platform/vl53_calibration_config.h @@ -18,7 +18,7 @@ typedef struct { */ static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = { { - .calibrated = 0u, + .calibrated = 1u, .data = { .struct_version = 3970629922u, .customer = { @@ -67,7 +67,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = { } }, { - .calibrated = 0u, + .calibrated = 1u, .data = { .struct_version = 3970629922u, .customer = { @@ -119,7 +119,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = { static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = { { - .calibrated = 0u, + .calibrated = 1u, .data = { .struct_version = 3970629922u, .customer = { @@ -168,7 +168,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = { } }, { - .calibrated = 0u, + .calibrated = 1u, .data = { .struct_version = 3970629922u, .customer = { diff --git a/App/app_tasks.c b/App/app_tasks.c index d139d2f..6c5b501 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -440,6 +440,10 @@ void AppTasks_Init(void) .w_max = PARAM_CTRL_W_MAX, /* 角速度限幅 */ .v_max = PARAM_CTRL_V_MAX, /* 线速度限幅 */ .speed_reduction_k = PARAM_CTRL_SPEED_REDUCTION, /* 调优：弯道减速系数 */ + .exit_front_dist = PARAM_CTRL_EXIT_FRONT_DIST, /* 调优：出沟检测距离 */ + .wall_escape_dist = PARAM_CTRL_WALL_ESCAPE_DIST, + .wall_escape_kp = PARAM_CTRL_WALL_ESCAPE_KP, + .wall_escape_w_max = PARAM_CTRL_WALL_ESCAPE_WMAX, }; CorridorCtrl_Init(&ctrl_cfg); @@ -470,13 +474,25 @@ void AppTasks_Init(void) .reacquire_v = PARAM_GNAV_REACQUIRE_V, .reacquire_conf_thresh = PARAM_GNAV_REACQUIRE_CONF, .reacquire_width_tol = PARAM_GNAV_REACQUIRE_WIDTH_TOL, + .reacquire_min_odom = PARAM_GNAV_REACQUIRE_MIN_ODOM, .reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS, .reacquire_timeout_ms = PARAM_GNAV_REACQUIRE_TIMEOUT, + .align_kp_th = PARAM_GNAV_ALIGN_KP_TH, + .align_kp_y = PARAM_GNAV_ALIGN_KP_Y, + .align_th_tol_rad = PARAM_GNAV_ALIGN_TH_TOL, + .align_y_tol_m = PARAM_GNAV_ALIGN_Y_TOL, + .align_confirm_ticks = PARAM_GNAV_ALIGN_TICKS, + .align_timeout_ms = PARAM_GNAV_ALIGN_TIMEOUT, + .reacquire_min_back_dist = PARAM_GNAV_REACQUIRE_MIN_BACK, .corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST, .corridor_length_max = PARAM_GNAV_CORRIDOR_MAX_LEN, .link_v = PARAM_GNAV_LINK_V, .link_distance = PARAM_GNAV_LINK_DISTANCE, .link_timeout_ms = PARAM_GNAV_LINK_TIMEOUT, + .link_gap_runout = PARAM_GNAV_LINK_GAP_RUNOUT, + .link_wall_target = PARAM_GNAV_LINK_WALL_TARGET, + .link_wall_kp = PARAM_GNAV_LINK_WALL_KP, + .link_wall_blend = PARAM_GNAV_LINK_WALL_BLEND, .exit_v = PARAM_GNAV_EXIT_V, .exit_runout = PARAM_GNAV_EXIT_RUNOUT, .exit_max_dist = PARAM_GNAV_EXIT_MAX_DIST, diff --git a/App/est/corridor_filter.c b/App/est/corridor_filter.c index 1ca6534..839fa33 100644 --- a/App/est/corridor_filter.c +++ b/App/est/corridor_filter.c @@ -146,3 +146,22 @@ void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad) s_imu_yaw_ref_rad = imu_yaw_continuous_rad; s_imu_yaw_ref_set = true; } + +void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad) +{ + if (!s_initialized) return; + + CorridorEKF_ResetHeading(); + s_imu_yaw_ref_rad = imu_yaw_continuous_rad; + s_imu_yaw_ref_set = true; +} + +void CorridorFilter_CorrectIMUReference(float heading_correction_rad) +{ + if (!s_initialized || !s_imu_yaw_ref_set) return; + + /* 修正IMU参考值，用于消除转向系统性偏差 + * 例如：VL53检测到车头偏右2度，heading_correction_rad = -0.035 rad + * 修正后，EKF会认为当前IMU方向才是正确的走廊方向 */ + s_imu_yaw_ref_rad += heading_correction_rad; +} diff --git a/App/est/corridor_filter.h b/App/est/corridor_filter.h index 35a04ba..431c7d4 100644 --- a/App/est/corridor_filter.h +++ b/App/est/corridor_filter.h @@ -61,6 +61,24 @@ extern "C" { */ void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad); + /** + * @brief 仅重建航向参考，不改横向位置 + * + * 用于进入新垄沟后，已经通过侧墙把车头摆正， + * 这时把当前 IMU yaw 设为新的走廊参考，同时仅清零 e_th。 + */ + void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad); + + /** + * @brief 修正IMU航向参考值（用于消除转向系统性偏差） + * + * 用于赛道模式转向后，用VL53检测到的墙壁航向误差修正IMU参考值 + * 只修正参考值，不改变当前状态估计 + * + * @param heading_correction_rad 航向修正量 (rad)，正值表示车头需要左转 + */ + void CorridorFilter_CorrectIMUReference(float heading_correction_rad); + #ifdef __cplusplus } #endif diff --git a/App/nav/corridor_ctrl.c b/App/nav/corridor_ctrl.c index 9132796..fec0cd7 100644 --- a/App/nav/corridor_ctrl.c +++ b/App/nav/corridor_ctrl.c @@ -32,6 +32,17 @@ void CorridorCtrl_Compute(const CorridorState_t *state, return; } + /* ======================================================== + * 出沟保护: 当前激光检测到接近出口时，停止使用左右激光控制 + * 避免出沟时左右激光数据突变导致车身大幅度转向 + * ======================================================== */ + bool near_exit = false; + if ((obs->valid_mask & (1U << 4)) != 0U) { /* 前激光有效 (bit 4) */ + if (obs->d_front <= s_cfg.exit_front_dist) { + near_exit = true; + } + } + /* ======================================================== * 核心控制律: * w_cmd = kp_theta * e_th + kd_theta * (-imu_wz) + kp_y * e_y @@ -40,11 +51,56 @@ void CorridorCtrl_Compute(const CorridorState_t *state, * - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转" * 用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声 * - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来 + * + * 出沟保护: 接近出口时，仅使用航向保持，不使用横向和角度纠偏 * ======================================================== */ - float w_cmd = -(s_cfg.kp_theta * state->e_th - + s_cfg.kd_theta * imu_wz - + s_cfg.kp_y * state->e_y); + float w_cmd; + bool escape_active = false; + if (near_exit) { + /* 接近出口: 仅保持航向惯性，禁用左右激光控制 */ + w_cmd = -(s_cfg.kd_theta * imu_wz); + } else { + /* 正常控制: 完整PD控制律 */ + w_cmd = -(s_cfg.kp_theta * state->e_th + + s_cfg.kd_theta * imu_wz + + s_cfg.kp_y * state->e_y); + + /* ======================================================== + * 近墙脱离保护: + * 当某一侧平均距离已经明显过小，说明车身已经在擦壁或即将擦壁。 + * 此时不能只等 EKF 慢慢回中，直接叠加一个远离墙面的转向保护项。 + * ======================================================== */ + { + bool left_front_ok = ((obs->valid_mask & (1U << 0)) != 0U); + bool left_rear_ok = ((obs->valid_mask & (1U << 1)) != 0U); + bool right_front_ok = ((obs->valid_mask & (1U << 2)) != 0U); + bool right_rear_ok = ((obs->valid_mask & (1U << 3)) != 0U); + float w_escape = 0.0f; + float left_min = 10.0f; + float right_min = 10.0f; + + if (left_front_ok && obs->d_lf < left_min) left_min = obs->d_lf; + if (left_rear_ok && obs->d_lr < left_min) left_min = obs->d_lr; + if (right_front_ok && obs->d_rf < right_min) right_min = obs->d_rf; + if (right_rear_ok && obs->d_rr < right_min) right_min = obs->d_rr; + + if (left_min < s_cfg.wall_escape_dist) { + float err = s_cfg.wall_escape_dist - left_min; + w_escape -= s_cfg.wall_escape_kp * err; /* 左侧很近 -> 轻微右转 */ + escape_active = true; + } + + if (right_min < s_cfg.wall_escape_dist) { + float err = s_cfg.wall_escape_dist - right_min; + w_escape += s_cfg.wall_escape_kp * err; /* 右侧很近 -> 轻微左转 */ + escape_active = true; + } + + w_escape = clampf(w_escape, -s_cfg.wall_escape_w_max, s_cfg.wall_escape_w_max); + w_cmd += w_escape; + } + } /* 角速度限幅：防止 PD 溢出导致原地打转 */ w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max); @@ -59,6 +115,11 @@ void CorridorCtrl_Compute(const CorridorState_t *state, float speed_reduction = s_cfg.speed_reduction_k * fabsf(w_cmd) / s_cfg.w_max; float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction); + /* 近墙脱离时轻微降速，避免“贴着墙还继续冲” */ + if (escape_active) { + v_cmd *= 0.80f; + } + /* 线速度限幅：不允许倒车，不允许超速 */ v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max); @@ -66,11 +127,13 @@ void CorridorCtrl_Compute(const CorridorState_t *state, * 置信度降级保护: * 当滤波器健康度 conf 过低（两边雷达全瞎）， * 说明走廊参照完全丢失，降低线速度防止盲飞 + * + * 注意：阈值不宜过高，否则会过度降级导致控制器失效 * ======================================================== */ - if (state->conf < 0.3f) { + if (state->conf < 0.2f) { /* 健康度极低：速度打三折，保持航向惯性滑行 */ v_cmd *= 0.3f; - } else if (state->conf < 0.6f) { + } else if (state->conf < 0.4f) { /* 健康度较低（单侧退化）：速度打七折 */ v_cmd *= 0.7f; } diff --git a/App/nav/corridor_ctrl.h b/App/nav/corridor_ctrl.h index a87f9fd..b789152 100644 --- a/App/nav/corridor_ctrl.h +++ b/App/nav/corridor_ctrl.h @@ -16,6 +16,11 @@ typedef struct { float w_max; // 角速度输出硬限幅 (rad/s)，超过此值一律削峰 float v_max; // 线速度输出硬限幅 (m/s) float speed_reduction_k; // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|) + + float exit_front_dist; // 出沟检测距离 (m)，前激光小于此值时禁用左右激光控制 + float wall_escape_dist; // 近墙脱离阈值 (m)，小于此值触发直接远离墙面 + float wall_escape_kp; // 近墙脱离增益 (rad/s per m) + float wall_escape_w_max; // 近墙脱离角速度限幅 (rad/s) } CorridorCtrlConfig_t; #ifdef __cplusplus diff --git a/App/nav/global_nav.c b/App/nav/global_nav.c index c9811fa..90a3e5b 100644 --- a/App/nav/global_nav.c +++ b/App/nav/global_nav.c @@ -49,6 +49,12 @@ static struct { /* 重捕获 */ uint8_t reacquire_ok_count; + /* 对齐 */ + uint8_t align_ok_count; + uint8_t wall_heading_stable_count; + float wall_heading_prev_rad; + bool wall_heading_prev_valid; + /* 出场 */ bool exit_vl53_lost; float exit_lost_distance; @@ -60,6 +66,8 @@ static struct { float link_d_front_start; /* 进入连接段时前激光读数 (m) */ bool link_d_front_valid; /* 进入时前激光是否有效 */ uint8_t link_gap_count; /* 非围栏侧 VL53 连续丢失计数 (沟口确认) */ + bool link_gap_seen; /* 是否已经确认看到下一个沟口 */ + float link_gap_seen_odom; /* 看到沟口时的累计里程 */ /* EKF 进度保存 */ float corridor_entry_s; @@ -127,19 +135,20 @@ static bool gap_detected_on_open_side(const CorridorObs_t* obs, TravelDirection_t prev_travel_dir) { if (prev_travel_dir == TRAVEL_DIR_EAST) { - /* 在右端通道，右侧贴围栏 → 检查左侧 VL53 */ + /* 在右端通道，右侧贴围栏 → 检查左侧 VL53 + * 修改为"与"逻辑：前后都丢失才算沟口，避免单个传感器失效导致误判 */ bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF) || (obs->d_lf > 0.5f); /* >50cm 视为沟口 (正常贴壁约10cm) */ bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR) || (obs->d_lr > 0.5f); - return lf_lost || lr_lost; + return lf_lost && lr_lost; /* 前后都丢失才算沟口 */ } else { /* 在左端通道，左侧贴围栏 → 检查右侧 VL53 */ bool rf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RF) || (obs->d_rf > 0.5f); bool rr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RR) || (obs->d_rr > 0.5f); - return rf_lost || rr_lost; + return rf_lost && rr_lost; /* 前后都丢失才算沟口 */ } } @@ -151,26 +160,95 @@ static bool all_side_lost(const CorridorObs_t* obs) return (obs->valid_mask & side_mask) == 0U; } +/** 计算墙壁航向误差（用于转向后微调） */ +static bool compute_wall_heading_error(const CorridorObs_t* obs, float* out_heading_rad) +{ + const float sensor_base = PARAM_SENSOR_BASE_LENGTH; + + bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) && + ((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U); + bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) && + ((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U); + + /* 赛道模式这里要求双侧都有效，单侧/退化数据不允许触发航向摆正。 */ + if (!left_ok || !right_ok) { + return false; + } + + float left_heading = atan2f(obs->d_lr - obs->d_lf, sensor_base); + float right_heading = atan2f(obs->d_rf - obs->d_rr, sensor_base); + + /* 双侧估计必须基本一致，避免沟口边缘/单边异常导致突然大打角。 */ + if (gnav_fabsf(left_heading - right_heading) > PARAM_DEG2RAD(8.0f)) { + return false; + } + + *out_heading_rad = 0.5f * (left_heading + right_heading); + return true; +} + +static void update_wall_heading_stability(bool valid, float heading_rad) +{ + if (!valid) { + s_nav.wall_heading_stable_count = 0; + s_nav.wall_heading_prev_valid = false; + return; + } + + if (s_nav.wall_heading_prev_valid && + gnav_fabsf(heading_rad - s_nav.wall_heading_prev_rad) <= PARAM_DEG2RAD(4.0f)) { + if (s_nav.wall_heading_stable_count < 255U) { + s_nav.wall_heading_stable_count++; + } + } else { + s_nav.wall_heading_stable_count = 1; + } + + s_nav.wall_heading_prev_rad = heading_rad; + s_nav.wall_heading_prev_valid = true; +} + +/* 重捕获已确认后，判断是否需要短暂停车摆正。 + * 除了航向误差本身，还把“明显贴墙/明显偏中心”作为触发条件。 + * 这样即使车头看起来近似平行，但整车已经贴到一侧，也会先停一下再进跟踪。 */ +static bool need_align_after_reacquire(const CorridorObs_t* obs, float wall_heading_error) +{ + bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) && + ((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U); + bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) && + ((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U); + + if (!left_ok || !right_ok) { + return gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad; + } + + { + float d_left = (obs->d_lf + obs->d_lr) * 0.5f; + float d_right = (obs->d_rf + obs->d_rr) * 0.5f; + float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH); + float min_side = (d_left < d_right) ? d_left : d_right; + bool heading_bad = gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad; + bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m); + bool off_center = gnav_fabsf(d_left - d_right) > (2.0f * s_nav.cfg.align_y_tol_m); + + return heading_bad || near_wall || off_center; + } +} + /** 检查重捕获条件 */ static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state) { - /* 条件 1: 至少 3 个侧向传感器有效 */ - uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR | - CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR; - uint8_t active = obs->valid_mask & side_mask; - int count = 0; - for (int i = 0; i < 4; i++) { - if (active & (1U << i)) count++; - } - if (count < 3) return false; - - /* 条件 2: 左右距离和 ≈ 走廊宽度 */ + /* 条件 1: 四个侧向传感器都有效 + * 赛道模式的重捕获要更稳，不能接受 3/4 这种退化几何。 */ bool left_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) && (obs->valid_mask & CORRIDOR_OBS_MASK_LR); bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) && (obs->valid_mask & CORRIDOR_OBS_MASK_RR); - if (left_ok && right_ok) { + if (!left_ok || !right_ok) return false; + + /* 条件 2: 左右距离和 ≈ 走廊宽度，且这里是必检项 */ + { float d_left = (obs->d_lf + obs->d_lr) * 0.5f; float d_right = (obs->d_rf + obs->d_rr) * 0.5f; float total_width = d_left + d_right + PARAM_ROBOT_WIDTH; @@ -201,6 +279,7 @@ static const char* const s_stage_names[] = { "ENTRY_STRAIGHT", "TURN_INTO_CORRIDOR", "REACQUIRE", + "ALIGN", "CORRIDOR_TRACK", "TURN_OUT", "LINK_STRAIGHT", @@ -294,6 +373,10 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board) s_nav.stage_start_ms = s_last_update_ms; s_nav.stage_entry_odom_vx_accum = s_nav.odom_distance_accum; s_nav.reacquire_ok_count = 0; + s_nav.align_ok_count = 0; + s_nav.wall_heading_stable_count = 0; + s_nav.wall_heading_prev_rad = 0.0f; + s_nav.wall_heading_prev_valid = false; switch (next) { case GNAV_ENTRY_STRAIGHT: @@ -339,6 +422,8 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board) s_nav.link_d_front_start = 0.0f; s_nav.link_d_front_valid = false; /* 首拍再记录 */ s_nav.link_gap_count = 0; + s_nav.link_gap_seen = false; + s_nav.link_gap_seen_odom = 0.0f; break; case GNAV_EXIT_STRAIGHT: @@ -501,7 +586,7 @@ void GlobalNav_Update(const CorridorObs_t* obs, break; /* ============================================================ - * 三种转向状态统一处理 + * 转向进入下一条沟 (原地转 90°) * ============================================================ */ case GNAV_TURN_INTO_CORRIDOR: case GNAV_TURN_OUT_OF_CORRIDOR: @@ -511,27 +596,117 @@ void GlobalNav_Update(const CorridorObs_t* obs, /* ============================================================ * 重捕获走廊 + * + * 问题背景： + * 转弯刚完成时，车身可能还在沟口外（端部通道内）， + * 但此时两侧VL53同样能测到两侧垄背端面（~10cm）， + * 宽度和 = 10+10+20 = 40cm，与真正在沟内完全一致。 + * 这导致尚未入沟就满足重捕获条件，切到ALIGN， + * EKF的e_y可能是大偏差，车卡死在入口。 + * + * 修复方案： + * 引入后部激光距离检测：只有后激光距离 > 40cm， + * 说明车身已完全进入沟内，才允许开始确认计数。 + * 这确保四颗VL53全部脱离沟口边缘进入稳定侧壁区域。 * ============================================================ */ case GNAV_REACQUIRE: out->override_v = s_nav.cfg.reacquire_v; - out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, - s_nav.cfg.heading_kp); + out->override_w = 0.0f; /* 不做航向控制，让车自然进沟，ALIGN阶段再摆正 */ out->safety_mode = SAFETY_MODE_STRAIGHT; - if (check_reacquire(obs, state)) { - s_nav.reacquire_ok_count++; - } else { - s_nav.reacquire_ok_count = 0; + { + /* 进沟深度守卫：后激光 + 最小入沟里程 双保险。 + * 尤其对 C1，不能只靠后激光，否则在入口区就可能过早假成功。 */ + bool back_ok = false; + bool odom_ok = (odom_since_entry() >= s_nav.cfg.reacquire_min_odom); + if ((obs->valid_mask & CORRIDOR_OBS_MASK_BACK) != 0U) { + if (obs->d_back > s_nav.cfg.reacquire_min_back_dist) { + back_ok = true; + } + } + + if (back_ok && odom_ok && check_reacquire(obs, state)) { + s_nav.reacquire_ok_count++; + } else { + /* 深度不足 / 里程不足 / 条件未满足 → 计数严格清零， + * 防止在沟口处积累部分计数后误触发。 */ + s_nav.reacquire_ok_count = 0; + } } if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) { - transition_to(GNAV_CORRIDOR_TRACK, board); + float wall_heading_error = 0.0f; + bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error); + update_wall_heading_stability(wall_heading_ok, wall_heading_error); + + /* 重捕获成功后，优先用 side VL53 短暂停车摆正车头。 + * 只有 wall heading 已稳定时才允许进入 ALIGN 或直接放行。 */ + if (wall_heading_ok && s_nav.wall_heading_stable_count >= 4U) { + if (need_align_after_reacquire(obs, wall_heading_error)) { + transition_to(GNAV_ALIGN, board); + } else { + if (board->imu_yaw_continuous.is_valid) { + CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f); + s_nav.heading_ref_deg = board->imu_yaw_continuous.value; + } + transition_to(GNAV_CORRIDOR_TRACK, board); + } + } } if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) { - transition_to(GNAV_ERROR, board); + /* 取消重捕获失败态：超时后不进 ERROR， + * 直接转入短暂停车摆正阶段，让车停下来继续自恢复。 */ + transition_to(GNAV_ALIGN, board); } break; + /* ============================================================ + * 短暂停车摆正航向 (ALIGN) + * + * 只用 side VL53 的前后差来摆正车头，不做横向居中。 + * 目的不是把车居中，而是避免“斜着进沟时前轮先蹭墙， + * 控制器还继续朝墙边打”的情况。 + * ============================================================ */ + case GNAV_ALIGN: { + float wall_heading_error = 0.0f; + bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error); + update_wall_heading_stability(wall_heading_ok, wall_heading_error); + + out->override_v = 0.0f; + out->use_override = true; + out->request_corridor = false; + out->safety_mode = SAFETY_MODE_STRAIGHT; + + if (wall_heading_ok && s_nav.wall_heading_stable_count >= 2U) { + out->override_w = gnav_clampf(-s_nav.cfg.align_kp_th * wall_heading_error, + -0.25f, 0.25f); + } else { + out->override_w = 0.0f; + } + + if (wall_heading_ok && gnav_fabsf(wall_heading_error) < s_nav.cfg.align_th_tol_rad) { + s_nav.align_ok_count++; + } else { + s_nav.align_ok_count = 0; + } + + if (s_nav.align_ok_count >= s_nav.cfg.align_confirm_ticks) { + if (board->imu_yaw_continuous.is_valid) { + CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f); + s_nav.heading_ref_deg = board->imu_yaw_continuous.value; + } + transition_to(GNAV_CORRIDOR_TRACK, board); + } + if (elapsed_ms > s_nav.cfg.align_timeout_ms) { + if (board->imu_yaw_continuous.is_valid) { + CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f); + s_nav.heading_ref_deg = board->imu_yaw_continuous.value; + } + transition_to(GNAV_CORRIDOR_TRACK, board); + } + break; + } + /* ============================================================ * 沟内闭环跟踪 (交给 corridor_ctrl) * ============================================================ */ @@ -540,11 +715,9 @@ void GlobalNav_Update(const CorridorObs_t* obs, out->request_corridor = true; out->safety_mode = SAFETY_MODE_CORRIDOR; - /* 到端检测 */ { bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) { - /* 里程下限保护: 至少走了 1.0m 才允许认定到端，避免假阳性 */ float corridor_odom = odom_since_entry(); if (corridor_odom > 1.0f) { s_nav.corridors_completed++; @@ -552,7 +725,7 @@ void GlobalNav_Update(const CorridorObs_t* obs, } } } - /* 里程超长保护 */ + if (odom_since_entry() > s_nav.cfg.corridor_length_max) { s_nav.corridors_completed++; transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board); @@ -560,91 +733,128 @@ void GlobalNav_Update(const CorridorObs_t* obs, break; /* ============================================================ - * 连接段直行 (纵向端部通道北行, 方案2: 三信号联合判定) + * 连接段直行 (端部通道内，从一条沟到下一条沟) * - * 传感器情况（转向完成后面朝北）： - * - 前激光（朝北）→ 上围栏，d_front 随北行递减 [精度高] - * - 后激光（朝南）→ 下围栏，d_back 随北行递增 [精度高] - * - 围栏侧 VL53 → 始终有效 (~10cm) [不用于判定] - * - 非围栏侧 VL53 → 贴垄背端面时有效，到垄沟开口时丢失 [沟口标志] - * - * 信号定义： - * A: 里程计 odom >= link_distance * 0.85 (打滑衰减, 权重低) - * B: 前激光变化 d_front缩小 >= link_distance * 0.85 (权重高) - * C: 非围栏侧VL53 丢失/跳到>50cm，连续2拍确认 (直接探测沟口, 权重中) - * - * 触发逻辑: B || (A && C) - * - 前激光变化量足够 → 直接触发（最可靠的单一信号） - * - 里程计到位 + VL53探到沟口 → 联合触发（互相校验） + * 控制策略：IMU 决定主航向；单边 VL53 只做离墙保底。 + * 也就是说： + * - 默认纯航向保持 + * - 只有当贴围栏侧距离小于阈值时，才附加一个远离墙面的修正 + * - 不做墙跟随融合，避免单边 VL53 把主方向带偏 * ============================================================ */ - case GNAV_LINK_STRAIGHT: + case GNAV_LINK_STRAIGHT: { out->override_v = s_nav.cfg.link_v; - out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, - s_nav.cfg.heading_kp); out->safety_mode = SAFETY_MODE_STRAIGHT; + float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, + s_nav.cfg.heading_kp); + float w_wall_guard = 0.0f; + const CorridorDescriptor_t* corridor = TrackMap_GetCorridor(s_nav.current_corridor_id); + + if (corridor != NULL && corridor->travel_dir == TRAVEL_DIR_EAST) { + if ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) && + (obs->valid_mask & CORRIDOR_OBS_MASK_RR)) { + float d_right = (obs->d_rf + obs->d_rr) * 0.5f; + if (d_right < s_nav.cfg.link_wall_target) { + float error = s_nav.cfg.link_wall_target - d_right; + w_wall_guard = s_nav.cfg.link_wall_kp * error; + } + } + } else if (corridor != NULL) { + if ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) && + (obs->valid_mask & CORRIDOR_OBS_MASK_LR)) { + float d_left = (obs->d_lf + obs->d_lr) * 0.5f; + if (d_left < s_nav.cfg.link_wall_target) { + float error = s_nav.cfg.link_wall_target - d_left; + w_wall_guard = -(s_nav.cfg.link_wall_kp * error); + } + } + } + + out->override_w = gnav_clampf(w_heading + w_wall_guard, -1.0f, 1.0f); + { bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; - /* 首拍记录前激光基准值 */ if (!s_nav.link_d_front_valid && front_valid) { s_nav.link_d_front_start = obs->d_front; s_nav.link_d_front_valid = true; } - /* ---- 信号 A: 里程计 (考虑打滑, 用 85% 容差) ---- */ bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f; - /* ---- 信号 B: 前激光变化量 (高精度) ---- */ bool laser_ok = false; if (s_nav.link_d_front_valid && front_valid) { float d_front_delta = s_nav.link_d_front_start - obs->d_front; - /* 车身中心到前激光有偏置(FRONT_LASER_OFFSET)，但这里用的是差值，偏置抵消 */ laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f); } - /* ---- 信号 C: 非围栏侧 VL53 沟口检测 (需连续2拍确认) ---- - * - * 判定阈值 0.5m 的来源: - * 正常贴垄背端面时 VL53 读数 ≈ (通道宽/2 - 车宽/2 - VL53内缩) - * = (0.40/2 - 0.20/2 - 0.0) - * = 0.10m - * 到垄沟开口时 VL53 读数 > 1.2m (超出有效距离) 或无效 - * 阈值 0.5m 在两者之间，足够区分 - */ - const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id); - /* 在 LINK_STRAIGHT 阶段，current_corridor_id 仍是刚走完的沟 (尚未更新) - 所以 cd->travel_dir 就是刚走完那条沟的方向，直接用来判断当前在哪个端部通道 */ - bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir); + bool gap_confirmed = false; + if (corridor != NULL) { + bool gap_now = gap_detected_on_open_side(obs, corridor->travel_dir); + if (gap_now) { + if (s_nav.link_gap_count < 255U) s_nav.link_gap_count++; + } else { + s_nav.link_gap_count = 0; + } + gap_confirmed = (s_nav.link_gap_count >= 5U); - if (gap_now) { - if (s_nav.link_gap_count < 255) s_nav.link_gap_count++; - } else { - s_nav.link_gap_count = 0; + if (gap_confirmed && !s_nav.link_gap_seen) { + s_nav.link_gap_seen = true; + s_nav.link_gap_seen_odom = s_nav.odom_distance_accum; + } } - bool gap_confirmed = (s_nav.link_gap_count >= 2); /* 连续2拍 (40ms @ 20ms周期) */ - /* ---- 联合判定: B || (A && C) ---- */ - if (laser_ok || (odom_ok && gap_confirmed)) - { + bool gap_runout_ok = false; + if (s_nav.link_gap_seen) { + gap_runout_ok = (s_nav.odom_distance_accum - s_nav.link_gap_seen_odom) + >= s_nav.cfg.link_gap_runout; + } + + if (odom_ok && (laser_ok || (s_nav.link_gap_seen && gap_runout_ok))) { transition_to(GNAV_TURN_INTO_NEXT, board); } } + if (elapsed_ms > s_nav.cfg.link_timeout_ms) { transition_to(GNAV_ERROR, board); } break; + } /* ============================================================ - * 出场直行 + * 出场直行 (左端通道向南返回) * ============================================================ */ - case GNAV_EXIT_STRAIGHT: + case GNAV_EXIT_STRAIGHT: { out->override_v = s_nav.cfg.exit_v; - out->override_w = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, - s_nav.cfg.heading_kp); out->safety_mode = SAFETY_MODE_STRAIGHT; - /* 检测侧向全丢 */ + float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, + s_nav.cfg.heading_kp); + float w_wall = 0.0f; + bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U; + bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U; + + if (rf_ok || rr_ok) { + float d_right; + + if (rf_ok && rr_ok) { + d_right = (obs->d_rf + obs->d_rr) * 0.5f; + } else if (rf_ok) { + d_right = obs->d_rf; + } else { + d_right = obs->d_rr; + } + + /* 出场阶段在左端通道向南走，右侧仍然是围栏。 + * 用右侧 VL53 直接叠加一个更硬的保墙约束，避免一路蹭墙。 */ + { + float error = s_nav.cfg.link_wall_target - d_right; + w_wall = 1.5f * s_nav.cfg.link_wall_kp * error; + } + } + + out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f); + if (!s_nav.exit_vl53_lost && all_side_lost(obs)) { s_nav.exit_vl53_lost = true; s_nav.exit_lost_distance = s_nav.odom_distance_accum; @@ -655,7 +865,6 @@ void GlobalNav_Update(const CorridorObs_t* obs, transition_to(GNAV_DOCK, board); } } - /* 里程上限保护 */ if (odom_since_entry() >= s_nav.cfg.exit_max_dist) { transition_to(GNAV_DOCK, board); } @@ -663,21 +872,48 @@ void GlobalNav_Update(const CorridorObs_t* obs, transition_to(GNAV_DOCK, board); } break; + } /* ============================================================ * 回停启动区 * ============================================================ */ - case GNAV_DOCK: + case GNAV_DOCK: { out->override_v = s_nav.cfg.dock_v; - out->override_w = 0.0f; out->safety_mode = SAFETY_MODE_STRAIGHT; + float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg, + s_nav.cfg.heading_kp); + float w_wall = 0.0f; + bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U; + bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U; + + if (rf_ok || rr_ok) { + float d_right; + + if (rf_ok && rr_ok) { + d_right = (obs->d_rf + obs->d_rr) * 0.5f; + } else if (rf_ok) { + d_right = obs->d_rf; + } else { + d_right = obs->d_rr; + } + + /* 回停阶段右墙保持更硬： + * IMU 仍然给主方向，但右侧距离约束直接叠加，不再做 50% 混合。 */ + { + float error = s_nav.cfg.link_wall_target - d_right; + w_wall = 1.5f * s_nav.cfg.link_wall_kp * error; + } + } + + out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f); + if (odom_since_entry() >= s_nav.cfg.dock_distance || - elapsed_ms > 5000U) - { + elapsed_ms > 5000U) { transition_to(GNAV_FINISHED, board); } break; + } /* ============================================================ * 终态 diff --git a/App/nav/global_nav.h b/App/nav/global_nav.h index 4779c2b..5bcb003 100644 --- a/App/nav/global_nav.h +++ b/App/nav/global_nav.h @@ -31,6 +31,7 @@ typedef enum { GNAV_ENTRY_STRAIGHT, /* 入场直线 */ GNAV_TURN_INTO_CORRIDOR, /* 转向进入垄沟 (原地转 90°) */ GNAV_REACQUIRE, /* 重捕获走廊 */ + GNAV_ALIGN, /* 捕获成功后停车对齐航线 */ GNAV_CORRIDOR_TRACK, /* 沟内闭环跟踪 */ GNAV_TURN_OUT_OF_CORRIDOR, /* 到端后出沟转向 (原地转 90°) */ GNAV_LINK_STRAIGHT, /* 连接段直行 */ @@ -61,17 +62,31 @@ typedef struct { float reacquire_v; /* 重捕获速度 m/s */ float reacquire_conf_thresh; /* 重捕获置信度阈值 */ float reacquire_width_tol; /* 走廊宽度容差 m */ + float reacquire_min_odom; /* 重捕获最小入沟里程 m */ uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */ uint32_t reacquire_timeout_ms; + /* 对齐 */ + float align_kp_th; /* 对齐航向P增益 (rad/s per rad) */ + float align_kp_y; /* 对齐横向P增益 (rad/s per m) */ + float align_th_tol_rad; /* 对齐航向容差 (rad) */ + float align_y_tol_m; /* 对齐横向容差 (m) */ + uint8_t align_confirm_ticks; /* 对齐确认拍数 */ + uint32_t align_timeout_ms; /* 对齐超时 ms */ + float reacquire_min_back_dist; /* 重捕获最小后激光距离 (m)，用于判断是否真正进沟 */ + /* 沟内 */ float corridor_end_detect_dist; /* 到端检测距离 m */ float corridor_length_max; /* 沟内里程保护上限 m */ /* 连接段 */ - float link_v; /* 连接段速度 m/s */ - float link_distance; /* 连接段标称距离 m */ + float link_v; + float link_distance; uint32_t link_timeout_ms; + float link_gap_runout; /* 检测到沟口后继续前冲距离 (m) */ + float link_wall_target; /* 墙壁跟随目标距离 (m) */ + float link_wall_kp; /* 墙壁跟随P增益 */ + float link_wall_blend; /* 墙壁跟随权重 (0~1) */ /* 出场 */ float exit_v; /* 出场速度 m/s */ diff --git a/App/robot_params.h b/App/robot_params.h index 39c3fca..46569fd 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -162,7 +162,7 @@ extern "C" { * 过大：EKF 认为横向很不稳定，过度依赖观测 -> 震荡 * 过小：EKF 不信任观测，响应慢 */ -#define PARAM_EKF_Q_EY 0.01f +#define PARAM_EKF_Q_EY 0.02f /** @brief [调优] 航向偏差 e_th 过程噪声方差 [rad²] * 含义：航向状态的自然发散速度 @@ -171,7 +171,7 @@ extern "C" { * 过大：EKF 认为航向不稳定，IMU 作用被削弱 * 过小：EKF 过度信任 IMU，雷达观测不起作用 */ -#define PARAM_EKF_Q_ETH 0.001f +#define PARAM_EKF_Q_ETH 0.002f /** @brief [调优] 沿程进度 s 过程噪声方差 [m²] * 含义：里程估计的不确定度 @@ -198,7 +198,7 @@ extern "C" { * 过大：EKF 不信任侧向雷达，横向响应慢 * 过小：EKF 过度信任雷达，对跳变敏感 */ -#define PARAM_EKF_R_EY 0.015f +#define PARAM_EKF_R_EY 0.007f /** @brief [调优] 航向观测噪声方差 [rad²] * 含义：同侧前后雷达差分测角的可程靠度 @@ -215,8 +215,11 @@ extern "C" { * 典型值：0.001~0.01 * 过大：IMU 航向观测作用弱，e_th 靠 wz 积分漂移 * 过小：IMU 航向主导过强，短时抖动可能传入 + * + * [修正] 如果场地本身是歪的，过度信任IMU会导致车持续擦墙 + * 增加到0.01，让EKF更多依赖VL53传感器判断相对航向 */ -#define PARAM_EKF_R_ETH_IMU 0.002f +#define PARAM_EKF_R_ETH_IMU 0.01f /* --- EKF 初始状态 --- */ /** @brief [调优] e_y 初始不确定度 [m²] @@ -255,7 +258,7 @@ extern "C" { * 过大：车头左右摆动 (震荡) * 过小：纠偏太慢，容易撞墙 */ -#define PARAM_CTRL_KP_THETA 2.0f +#define PARAM_CTRL_KP_THETA 2.5f /* 适度增加，提高航向纠偏速度 */ /** @brief [调优] 航向微分增益 kd_theta [s] * 含义：IMU 角速度阻尼项，抑制车头转动速度 @@ -273,7 +276,7 @@ extern "C" { * 过大：横向纠偏过猛，引起震荡 * 过小：偏了拉不回来 */ -#define PARAM_CTRL_KP_Y 4.0f +#define PARAM_CTRL_KP_Y 7.0f /* 提高横向回中速度，适配更高巡航速度 */ /** @brief [调优] 走廊巡航速度 [m/s] * 含义：走廊内正常行驶速度 @@ -302,6 +305,30 @@ extern "C" { */ #define PARAM_CTRL_SPEED_REDUCTION 0.4f +/** @brief [调优] 出沟检测距离 [m] + * 含义：前激光小于此值时禁用左右激光控制，避免出沟时数据突变导致大幅转向 + * 典型值：0.40 (40cm) + * 过大：提前禁用控制，可能导致出沟前偏离 + * 过小：禁用太晚，出沟时仍可能受左右激光突变影响 + */ +#define PARAM_CTRL_EXIT_FRONT_DIST 0.40f + +/** @brief [调优] 近墙脱离阈值 [m] + * 含义：任一侧平均距离小于此值时，直接叠加远离墙面的保护转向 + * 目的：不等 EKF 慢慢回中，先把车从擦墙状态拉开 + */ +#define PARAM_CTRL_WALL_ESCAPE_DIST 0.05f + +/** @brief [调优] 近墙脱离增益 [rad/s per m] + * 含义：近墙保护的附加角速度增益 + */ +#define PARAM_CTRL_WALL_ESCAPE_KP 6.0f + +/** @brief [调优] 近墙脱离角速度限幅 [rad/s] + * 含义：近墙保护项自身的最大角速度，避免一把打太猛 + */ +#define PARAM_CTRL_WALL_ESCAPE_WMAX 0.25f + /* ========================================================= * 【P4】安全阈值与状态机参数 * ========================================================= */ @@ -393,7 +420,7 @@ extern "C" { * 1: 使用滤波后的 range_mm_filtered * 0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比 */ -#define PARAM_VL53_USE_EMA_FILTER 1 +#define PARAM_VL53_USE_EMA_FILTER 0 /** @brief [调优] VL53L0X EMA滤波平滑系数 alpha * 含义：新测量值的权重 (0.0~1.0) @@ -405,7 +432,7 @@ extern "C" { * 0.5 - 快速响应 (延迟约40ms) * 0.6 - 极速响应 (延迟约25ms，抖动较大) */ -#define PARAM_VL53_EMA_ALPHA 0.4f +#define PARAM_VL53_EMA_ALPHA 0.6f /* --- IMU --- */ /** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad] @@ -443,20 +470,34 @@ extern "C" { /* --- 重捕获 --- */ #define PARAM_GNAV_REACQUIRE_V 0.1f /* m/s — 重捕获入沟速度 */ -#define PARAM_GNAV_REACQUIRE_CONF 0.6f /* — 重捕获置信度阈值 */ -#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f /* m — 走廊宽度容差 */ -#define PARAM_GNAV_REACQUIRE_TICKS 10 /* 拍 — 连续确认次数 (5×20ms=100ms) */ +#define PARAM_GNAV_REACQUIRE_CONF 0.4f /* — 重捕获置信度阈值（从0.6降到0.4，更容易成功） */ +#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.08f /* m — 走廊宽度容差（从5cm放宽到8cm） */ +#define PARAM_GNAV_REACQUIRE_MIN_ODOM 0.12f /* m — 最小入沟里程，避免沟口过早假成功 */ +#define PARAM_GNAV_REACQUIRE_TICKS 5 /* 拍 — 连续确认次数，取更稳的 100ms */ #define PARAM_GNAV_REACQUIRE_TIMEOUT 5000U /* ms — 重捕获超时 */ +/* --- 对齐 --- */ +#define PARAM_GNAV_ALIGN_KP_TH 2.0f /* — 对齐航向P增益 (rad/s per rad) */ +#define PARAM_GNAV_ALIGN_KP_Y 4.0f /* — 对齐横向P增益 (rad/s per m) */ +#define PARAM_GNAV_ALIGN_TH_TOL 0.05f /* rad — 对齐航向容差 (~3°) */ +#define PARAM_GNAV_ALIGN_Y_TOL 0.02f /* m — 对齐横向容差 (2cm) */ +#define PARAM_GNAV_ALIGN_TICKS 5 /* 拍 — 对齐确认次数 (5×20ms=100ms) */ +#define PARAM_GNAV_ALIGN_TIMEOUT 3000U /* ms — 对齐超时 */ +#define PARAM_GNAV_REACQUIRE_MIN_BACK 0.40f /* m — 重捕获最小后激光距离，判断是否真正进沟 */ + /* --- 沟内 --- */ -#define PARAM_GNAV_CORRIDOR_MAX_LEN 2.50f /* m — 沟内里程保护上限 */ -#define PARAM_GNAV_CORRIDOR_END_DIST 0.05f /* m — 到端检测距离 */ +#define PARAM_GNAV_CORRIDOR_MAX_LEN 2.70f /* m — 沟内里程保护上限 */ +#define PARAM_GNAV_CORRIDOR_END_DIST 0.10f /* m — 到端检测距离 */ /* --- 连接段 --- */ /* 在纵向端部通道里北行，从一条垄沟入口到下一条入口，距离=垄沟宽40cm+垄背宽30cm=70cm */ #define PARAM_GNAV_LINK_V 0.10f /* m/s — 连接段速度 (纵向通道内，IMU航向保持) */ -#define PARAM_GNAV_LINK_DISTANCE 0.75f /* m — 连接段标称距离 (沟间距) */ +#define PARAM_GNAV_LINK_DISTANCE 0.90f /* m — 连接段标称距离 (沟间距) */ #define PARAM_GNAV_LINK_TIMEOUT 8000U /* ms — 连接段超时 */ +#define PARAM_GNAV_LINK_GAP_RUNOUT 0.06f /* m — 看到下一个沟口后继续前冲距离 */ +#define PARAM_GNAV_LINK_WALL_TARGET 0.10f /* m — 连接段最小离墙距离（小于10cm才触发保底修正） */ +#define PARAM_GNAV_LINK_WALL_KP 3.0f /* — 连接段离墙保底增益 */ +#define PARAM_GNAV_LINK_WALL_BLEND 0.5f /* — 预留给其他通道状态，连接段本身不再使用融合 */ /* --- 出场 --- */ /* C6完成后在左端通道左转朝南，沿纵向通道南行回到入口，距离约390cm */