From b07d1ff9cd76d2e59530e439b3fdf1a90cafd5b2 Mon Sep 17 00:00:00 2001 From: nitiantuhao <2062405236@qq.com> Date: Sun, 12 Apr 2026 16:21:49 +0800 Subject: [PATCH] 1.0 --- .../platform/vl53_calibration_config.h | 8 +- App/app_tasks.c | 1 + App/nav/corridor_ctrl.c | 27 ++++++- App/nav/global_nav.c | 79 +++++++++++-------- App/nav/global_nav.h | 1 + App/robot_params.h | 13 +-- 6 files changed, 84 insertions(+), 45 deletions(-) diff --git a/App/VL53L1X_API/platform/vl53_calibration_config.h b/App/VL53L1X_API/platform/vl53_calibration_config.h index d28f3e1..44bf10a 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 = 1u, + .calibrated = 0u, .data = { .struct_version = 3970629922u, .customer = { @@ -67,7 +67,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = { } }, { - .calibrated = 1u, + .calibrated = 0u, .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 = 1u, + .calibrated = 0u, .data = { .struct_version = 3970629922u, .customer = { @@ -168,7 +168,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = { } }, { - .calibrated = 1u, + .calibrated = 0u, .data = { .struct_version = 3970629922u, .customer = { diff --git a/App/app_tasks.c b/App/app_tasks.c index 6c5b501..3f1d079 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -492,6 +492,7 @@ void AppTasks_Init(void) .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_heading_kp = PARAM_GNAV_LINK_WALL_HEADING_KP, .link_wall_blend = PARAM_GNAV_LINK_WALL_BLEND, .exit_v = PARAM_GNAV_EXIT_V, .exit_runout = PARAM_GNAV_EXIT_RUNOUT, diff --git a/App/nav/corridor_ctrl.c b/App/nav/corridor_ctrl.c index e0112ce..c1a4cf9 100644 --- a/App/nav/corridor_ctrl.c +++ b/App/nav/corridor_ctrl.c @@ -61,10 +61,33 @@ void CorridorCtrl_Compute(const CorridorState_t *state, /* 接近出口: 仅保持航向惯性，禁用左右激光控制 */ w_cmd = -(s_cfg.kd_theta * imu_wz); } else { - /* 正常控制: 完整PD控制律 */ + /* + * 简化原则： + * - IMU 只管航向 (e_th + wz) + * - 左右激光只管居中 + * + * 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差： + * e_y_direct = (d_right - d_left)/2 + * 车在正中时 d_left ~= d_right，因此误差应接近 0。 + * + * 若当前帧双侧不完整，再退回滤波器给出的 e_y。 + */ + float e_y_ctrl = state->e_y; + bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) && + ((obs->valid_mask & (1U << 1)) != 0U); + bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) && + ((obs->valid_mask & (1U << 3)) != 0U); + + if (left_ok && right_ok) { + float d_left = (obs->d_lf + obs->d_lr) * 0.5f; + float d_right = (obs->d_rf + obs->d_rr) * 0.5f; + e_y_ctrl = 0.5f * (d_right - d_left); + } + + /* 正常控制: IMU 管航向, 左右激光管居中 */ w_cmd = -(s_cfg.kp_theta * state->e_th + s_cfg.kd_theta * imu_wz - + s_cfg.kp_y * state->e_y); + + s_cfg.kp_y * e_y_ctrl); /* ======================================================== * 近墙脱离保护: diff --git a/App/nav/global_nav.c b/App/nav/global_nav.c index 43f9e96..7baf931 100644 --- a/App/nav/global_nav.c +++ b/App/nav/global_nav.c @@ -38,6 +38,7 @@ static struct { float turn_start_yaw_deg; /* IMU yaw_continuous at turn start */ float turn_target_delta_deg; /* 90° */ int8_t turn_sign; /* +1 (CCW) or -1 (CW) */ + float turn_goal_yaw_deg; /* 本次转向完成后的名义目标航向 */ /* 里程 (用里程计积分距离) */ float stage_entry_odom_vx_accum; /* 进入阶段时的里程计累计距离 */ @@ -454,6 +455,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board) s_nav.turn_sign = (int8_t)cd->entry_turn_dir; s_nav.turn_start_yaw_deg = imu_yaw; s_nav.turn_target_delta_deg = 90.0f; + s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f; break; } case GNAV_TURN_OUT_OF_CORRIDOR: { @@ -461,6 +463,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board) s_nav.turn_sign = (int8_t)cd->exit_turn_dir; s_nav.turn_start_yaw_deg = imu_yaw; s_nav.turn_target_delta_deg = 90.0f; + s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f; break; } case GNAV_TURN_INTO_NEXT: { @@ -469,6 +472,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board) s_nav.turn_sign = (int8_t)cd->entry_turn_dir; s_nav.turn_start_yaw_deg = imu_yaw; s_nav.turn_target_delta_deg = 90.0f; + s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f; s_nav.current_corridor_id = next_id; break; } @@ -700,25 +704,9 @@ void GlobalNav_Update(const CorridorObs_t* obs, } if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) { - CorridorPoseEval_t pose_eval; - evaluate_corridor_pose(obs, board, &pose_eval); - update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad); - - /* 评估结果说明： - * - need_align: 车头歪/偏心/贴墙/对角近墙，需要先停一下 - * - safe_for_align: 当前几何足够健康，允许按姿态估计输出转向 - * - * 即使几何暂时不安全，只要 need_align=true 也先进入 ALIGN 停住， - * 避免一边前进一边突然大角度纠正。 */ - if (pose_eval.need_align) { - 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); - } + /* 简化：重捕获成功后统一停车摆正航向。 + * 不再在这里用墙姿态决定“摆不摆正”，避免不同沟表现不一致。 */ + transition_to(GNAV_ALIGN, board); } if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) { /* 取消重捕获失败态：超时后不进 ERROR， @@ -735,26 +723,35 @@ void GlobalNav_Update(const CorridorObs_t* obs, * 控制器还继续朝墙边打”的情况。 * ============================================================ */ case GNAV_ALIGN: { - CorridorPoseEval_t pose_eval; - evaluate_corridor_pose(obs, board, &pose_eval); - update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad); + float imu_yaw_err_rad = 0.0f; + bool imu_align_ok = false; + + if (board->imu_yaw_continuous.is_valid) { + imu_yaw_err_rad = PARAM_DEG2RAD(s_nav.turn_goal_yaw_deg - board->imu_yaw_continuous.value); + while (imu_yaw_err_rad > 3.14159265f) imu_yaw_err_rad -= 6.2831853f; + while (imu_yaw_err_rad < -3.14159265f) imu_yaw_err_rad += 6.2831853f; + + if (gnav_fabsf(imu_yaw_err_rad) < s_nav.cfg.align_th_tol_rad && + (!board->imu_wz.is_valid || gnav_fabsf(board->imu_wz.value) < 15.0f)) { + imu_align_ok = true; + } + } out->override_v = 0.0f; out->use_override = true; out->request_corridor = false; out->safety_mode = SAFETY_MODE_STRAIGHT; - if (pose_eval.safe_for_align && s_nav.wall_heading_stable_count >= 2U) { - float w_align = -(s_nav.cfg.align_kp_th * pose_eval.heading_rad - + s_nav.cfg.align_kp_y * pose_eval.e_y_m); - out->override_w = gnav_clampf(w_align, -0.20f, 0.20f); + if (board->imu_yaw_continuous.is_valid) { + /* imu_yaw_err_rad = target - current + * 误差为正时，需要正角速度(左转/逆时针)去追目标，不能取反。 */ + float w_align_imu = 1.2f * imu_yaw_err_rad; + out->override_w = gnav_clampf(w_align_imu, -0.18f, 0.18f); } else { out->override_w = 0.0f; } - if (pose_eval.safe_for_align && - gnav_fabsf(pose_eval.heading_rad) < s_nav.cfg.align_th_tol_rad && - gnav_fabsf(pose_eval.e_y_m) < s_nav.cfg.align_y_tol_m) { + if (imu_align_ok) { s_nav.align_ok_count++; } else { s_nav.align_ok_count = 0; @@ -767,12 +764,12 @@ void GlobalNav_Update(const CorridorObs_t* obs, } transition_to(GNAV_CORRIDOR_TRACK, board); } - if (!pose_eval.safe_for_align && elapsed_ms > 400U) { - if (board->imu_yaw_continuous.is_valid) { + if (elapsed_ms > 400U) { + if (board->imu_yaw_continuous.is_valid && imu_align_ok) { 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); } - transition_to(GNAV_CORRIDOR_TRACK, board); } if (elapsed_ms > s_nav.cfg.align_timeout_ms) { if (board->imu_yaw_continuous.is_valid) { @@ -825,6 +822,7 @@ void GlobalNav_Update(const CorridorObs_t* obs, 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; + float w_wall_parallel = 0.0f; const CorridorDescriptor_t* corridor = TrackMap_GetCorridor(s_nav.current_corridor_id); if (corridor != NULL && corridor->travel_dir == TRAVEL_DIR_EAST) { @@ -835,6 +833,14 @@ void GlobalNav_Update(const CorridorObs_t* obs, float error = s_nav.cfg.link_wall_target - d_right; w_wall_guard = s_nav.cfg.link_wall_kp * error; } + + /* 右端通道：右侧是围栏。 + * 用右前/右后距离差把车身摆成与围栏平行， + * 避免“不蹭墙但车身是歪的”。 */ + { + float heading_right = atan2f(obs->d_rf - obs->d_rr, PARAM_SENSOR_BASE_LENGTH); + w_wall_parallel = -s_nav.cfg.link_wall_heading_kp * heading_right; + } } } else if (corridor != NULL) { if ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) && @@ -844,10 +850,17 @@ void GlobalNav_Update(const CorridorObs_t* obs, float error = s_nav.cfg.link_wall_target - d_left; w_wall_guard = -(s_nav.cfg.link_wall_kp * error); } + + /* 左端通道：左侧是围栏。 */ + { + float heading_left = atan2f(obs->d_lr - obs->d_lf, PARAM_SENSOR_BASE_LENGTH); + w_wall_parallel = -s_nav.cfg.link_wall_heading_kp * heading_left; + } } } - out->override_w = gnav_clampf(w_heading + w_wall_guard, -1.0f, 1.0f); + w_wall_parallel = gnav_clampf(w_wall_parallel, -0.20f, 0.20f); + out->override_w = gnav_clampf(w_heading + w_wall_guard + w_wall_parallel, -1.0f, 1.0f); { bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; diff --git a/App/nav/global_nav.h b/App/nav/global_nav.h index 5bcb003..5f2a45c 100644 --- a/App/nav/global_nav.h +++ b/App/nav/global_nav.h @@ -86,6 +86,7 @@ typedef struct { float link_gap_runout; /* 检测到沟口后继续前冲距离 (m) */ float link_wall_target; /* 墙壁跟随目标距离 (m) */ float link_wall_kp; /* 墙壁跟随P增益 */ + float link_wall_heading_kp; /* 单边墙平行修正增益 */ float link_wall_blend; /* 墙壁跟随权重 (0~1) */ /* 出场 */ diff --git a/App/robot_params.h b/App/robot_params.h index 3a5f044..70bea30 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -64,7 +64,7 @@ extern "C" { * 典型值：根据机械安装，约 0.10~0.15m * 影响：航向观测精度 z_eth = atan2((d_back-d_front), L_s) */ -#define PARAM_SENSOR_BASE_LENGTH 0.120f +#define PARAM_SENSOR_BASE_LENGTH 0.0755f /** @brief [实测] 比赛走廊标准宽度 [m] * 测量方法：卷尺测量赛场垄沟实际宽度 (规则 40cm，允许±5% 误差) @@ -85,7 +85,7 @@ extern "C" { * 典型值：根据机械安装，若与车尾齐平则为 0 * 用途：后向到端距离补偿 d_body_rear = d_sensor - offset */ -#define PARAM_REAR_LASER_OFFSET 0.0f +#define PARAM_REAR_LASER_OFFSET (-(0.018f)) /** @brief [实测] 侧向 VL53L0X 传感器内缩距离 [m] * 测量方法：VL53L0X 发射面到车体侧面最外边缘的距离 (向内为正) @@ -95,7 +95,7 @@ extern "C" { * ⚠ 极其重要：沟道40cm - 车体20cm = 每边仅10cm，1cm的偏差就是10%的误差！ * 注意：此值作为未分侧设置时的默认值。建议使用下面的分侧参数。 */ -#define PARAM_VL53_SIDE_INSET 0.0f +#define PARAM_VL53_SIDE_INSET (-(0.018f)) /** @brief [改进A][实测] 左侧 VL53L0X 传感器内缩距离 [m] * 测量方法：将机器人精确放在走廊正中央(卷尺确认)， @@ -501,9 +501,9 @@ extern "C" { /* --- 转向 --- */ #define PARAM_GNAV_TURN_OMEGA 1.0f /* rad/s — 转向角速度 */ -#define PARAM_GNAV_TURN_TOLERANCE 0.087f /* rad — 转向完成容差 (~5°) */ -#define PARAM_GNAV_TURN_DECEL_ZONE 0.5f /* rad — 接近目标时减速区 (~28°) */ -#define PARAM_GNAV_TURN_MIN_OMEGA 0.3f /* rad/s — 减速区最低角速度 */ +#define PARAM_GNAV_TURN_TOLERANCE 0.052f /* rad — 转向完成容差 (~3°) */ +#define PARAM_GNAV_TURN_DECEL_ZONE 0.70f /* rad — 接近目标时减速区 (~40°) */ +#define PARAM_GNAV_TURN_MIN_OMEGA 0.18f /* rad/s — 减速区最低角速度 */ #define PARAM_GNAV_TURN_TIMEOUT 8000U /* ms — 单次转向超时 */ /* --- 重捕获 --- */ @@ -535,6 +535,7 @@ extern "C" { #define PARAM_GNAV_LINK_GAP_RUNOUT 0.08f /* m — 看到下一个沟口后继续前冲距离 */ #define PARAM_GNAV_LINK_WALL_TARGET 0.10f /* m — 连接段最小离墙距离（小于10cm才触发保底修正） */ #define PARAM_GNAV_LINK_WALL_KP 3.0f /* — 连接段离墙保底增益 */ +#define PARAM_GNAV_LINK_WALL_HEADING_KP 1.2f /* — 连接段单边墙平行修正增益 */ #define PARAM_GNAV_LINK_WALL_BLEND 0.5f /* — 预留给其他通道状态，连接段本身不再使用融合 */ /* --- 出场 --- */