diff --git a/App/VL53L0X_API/platform/vl53_board.c b/App/VL53L0X_API/platform/vl53_board.c index 09b64ef..40d61ec 100644 --- a/App/VL53L0X_API/platform/vl53_board.c +++ b/App/VL53L0X_API/platform/vl53_board.c @@ -2,6 +2,7 @@ #include "FreeRTOS.h" #include "task.h" #include "robot_params.h" +#include "vl53_calibration_config.h" /* ================= 卡尔曼滤波底层实现 ================= */ static void vl53_kalman_init(Vl53Kalman_t *kf, float q, float r) { @@ -25,6 +26,43 @@ static float vl53_kalman_update(Vl53Kalman_t *kf, float measurement) { return kf->x; } +static const Vl53RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id) +{ + switch (id) { + case 0: return &k_vl53_left_calibration[0]; + case 1: return &k_vl53_left_calibration[1]; + case 2: return &k_vl53_right_calibration[0]; + case 3: return &k_vl53_right_calibration[1]; + default: return NULL; + } +} + +static VL53L0X_Error vl53_apply_runtime_calibration(VL53L0X_DEV dev, uint8_t id) +{ + const Vl53RuntimeCalibration_t *cal = vl53_get_runtime_calibration(id); + VL53L0X_Error status; + + if (cal == NULL) return VL53L0X_ERROR_NONE; + + if (cal->offset_calibrated != 0U) { + status = VL53L0X_SetOffsetCalibrationDataMicroMeter(dev, cal->offset_micro_meters); + if (status != VL53L0X_ERROR_NONE) return status; + } + + if (cal->xtalk_calibrated != 0U) { + status = VL53L0X_SetXTalkCompensationRateMegaCps(dev, cal->xtalk_compensation_rate_mcps); + if (status != VL53L0X_ERROR_NONE) return status; + + status = VL53L0X_SetXTalkCompensationEnable(dev, 1u); + if (status != VL53L0X_ERROR_NONE) return status; + } else { + status = VL53L0X_SetXTalkCompensationEnable(dev, 0u); + if (status != VL53L0X_ERROR_NONE) return status; + } + + return VL53L0X_ERROR_NONE; +} + /* ================= ST 官方配置序列 ================= */ static VL53L0X_Error vl53_do_static_init(VL53L0X_DEV dev, uint32_t timing_budget_us) { @@ -95,6 +133,7 @@ VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs if (VL53L0X_PlatformChangeAddress(&board->dev[i], hw_cfgs[i].runtime_addr_8bit) != VL53L0X_ERROR_NONE) continue; if (VL53L0X_DataInit(&board->dev[i]) != VL53L0X_ERROR_NONE) continue; if (vl53_do_static_init(&board->dev[i], board->timing_budget_us) != VL53L0X_ERROR_NONE) continue; + if (vl53_apply_runtime_calibration(&board->dev[i], hw_cfgs[i].id) != VL53L0X_ERROR_NONE) continue; board->init_mask |= (uint8_t)(1u << i); board->dev[i].is_present = 1u; @@ -163,4 +202,4 @@ VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapsho } } return VL53L0X_ERROR_NONE; -} \ No newline at end of file +} diff --git a/App/app_tasks.c b/App/app_tasks.c index d1a56bc..4d35871 100644 --- a/App/app_tasks.c +++ b/App/app_tasks.c @@ -485,7 +485,7 @@ void AppTasks_Init(void) .turn_omega = PARAM_SCRIPT_TURN_OMEGA, /* 调优：转向角速度 */ .corridor_length = 3.0f, /* 备用：垄沟长度估计 */ .entry_align_timeout = PARAM_SCRIPT_ENTRY_TIMEOUT, /* 调优：入口对准超时 */ - .d_entry_exit_front = 0.10f, /* 调优：出入口距离阈值 */ + .d_entry_exit_front = 0.12f, /* 调优：出入口距离阈值 */ .entry_align_v = PARAM_SCRIPT_ENTRY_V, /* 调优：入口对准速度 */ .exit_runout_m = PARAM_SCRIPT_EXIT_RUNOUT, /* 调优：退出后冲出距离 */ .exit_v = PARAM_SCRIPT_EXIT_V, /* P1 修复：退出直线速度独立参数 */ diff --git a/App/est/corridor_ekf.c b/App/est/corridor_ekf.c index fbf6647..b3481f3 100644 --- a/App/est/corridor_ekf.c +++ b/App/est/corridor_ekf.c @@ -217,6 +217,36 @@ void CorridorEKF_Reset(void) s_last_update_ms = 0U; } +void CorridorEKF_ResetHeading(void) +{ + if (!s_initialized) return; + + s_state.x[1] = 0.0f; + + /* 清理航向与其它状态的耦合，避免旧航向误差继续通过协方差传播。 */ + s_state.P[0][1] = 0.0f; + s_state.P[1][0] = 0.0f; + s_state.P[1][2] = 0.0f; + s_state.P[2][1] = 0.0f; + s_state.P[1][1] = s_cfg.P0_diag[1]; +} + +void CorridorEKF_RebaseAfterTurnaround(void) +{ + if (!s_initialized) return; + + /* 同一条走廊掉头后，新的前进方向相反，横向误差符号需要镜像。 */ + s_state.x[0] = -s_state.x[0]; + s_state.x[1] = 0.0f; + + /* e_y 与 e_th 的相关项在掉头后不再可直接沿用，清零重新收敛。 */ + s_state.P[0][1] = 0.0f; + s_state.P[1][0] = 0.0f; + s_state.P[1][2] = 0.0f; + s_state.P[2][1] = 0.0f; + s_state.P[1][1] = s_cfg.P0_diag[1]; +} + void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s) { s_cfg.q_ey = q_ey; @@ -311,11 +341,21 @@ void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt) /* ========================================================= * 观测步 (Update) - 鲁棒 EKF + * + * 设计决策 (方向 B — IMU 主导航向)： + * 侧墙激光仅用于更新横向位置 e_y，不再构建航向观测 z_eth_L/z_eth_R。 + * 航向 e_th 完全由 IMU 主导： + * - 预测步: imu_wz 驱动 e_th 积分 + * - 观测步: CorridorEKF_UpdateIMUYaw() 提供 yaw_continuous 标量约束 + * 侧墙前后差分 (d_lr-d_lf) 的噪声在 ±2cm 误差下过大，不适合做航向主观测。 * ========================================================= */ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state) { if (!s_initialized) return 0; + /* 维护最近一次 EKF 输出对应的观测时间戳，供 GetState() 返回一致结果。 */ + s_last_update_ms = obs->t_ms; + int updated_obs_count = 0; /* 清除新息和拒绝掩码 */ @@ -334,54 +374,29 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state) /* 左右侧横向平均距离 */ float d_lf = obs->d_lf, d_lr = obs->d_lr; float d_rf = obs->d_rf, d_rr = obs->d_rr; - float Ls = s_cfg.sensor_base_length; 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; - /* 传感器居中时的理论读数 (考虑车体宽度和传感器内缩) - * - * 推导 (以左侧为例)： - * 沟道宽 W，车体宽 Rw，传感器内缩 inset - * 居中时： - * 车体左边缘到左墙 = (W - Rw) / 2 - * 传感器到左墙 = (W - Rw) / 2 + inset (传感器比边缘更靠内) - * - * 所以 d_center = (W - Rw) / 2 + inset - * - * 验证：W=0.40, Rw=0.20, inset=0 - * d_center = (0.40-0.20)/2 + 0 = 0.10m ✓ (每边余量10cm) - * - * 单侧公式：e_y = d_center - d_left (左侧传感器越近墙，偏差越大) - * 双侧公式：e_y = [(d_center - d_left) + (d_right - d_center)] / 2 - * = (d_right - d_left) / 2 (d_center 被消掉) - * - * ⚠ 当 inset = 0 且 Rw = 0 时，d_center = W/2，退化回原始行为 - */ - float d_center = (W - Rw) / 2.0f + inset; /* 传感器居中时的理论读数 */ + /* 传感器居中时的理论读数 (考虑车体宽度和传感器内缩) */ + float d_center = (W - Rw) / 2.0f + inset; - /* 观测值 (测量) */ + /* 观测值 (测量) — 仅横向位置，不含航向 */ float z_ey = 0.0f; - float z_eth_L = 0.0f; - float z_eth_R = 0.0f; - int valid_sides = 0; if (left_ok) { - z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff; - z_eth_L = atan2f(d_lr - d_lf, Ls); + z_ey += d_center - ((d_lf + d_lr) / 2.0f) - yoff; valid_sides++; } if (right_ok) { - z_ey += ((d_rf + d_rr) / 2.0f) - d_center - yoff; - z_eth_R = atan2f(d_rf - d_rr, Ls); + z_ey += ((d_rf + d_rr) / 2.0f) - d_center - yoff; valid_sides++; } if (valid_sides == 0) { - /* 两边都失效: 协方差持续膨胀，输出预测值 */ out_state->t_ms = obs->t_ms; out_state->e_y = s_state.x[0]; out_state->e_th = s_state.x[1]; @@ -396,290 +411,69 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state) } } - /* 协方差膨胀 (无观测时的信任衰减) */ - s_state.P[0][0] += s_cfg.q_ey * 5.0f; - s_state.P[1][1] += s_cfg.q_eth * 5.0f; + /* 协方差膨胀 (无观测时的信任衰减) — 仅膨胀 e_y */ + s_state.P[0][0] += s_cfg.q_ey * 5.0f; protect_P(s_state.P); return 0; } - /* 横向观测: 两侧平均 */ if (valid_sides == 2) { z_ey /= 2.0f; } /* ---------------------------------------------------- - * 构建观测向量 z 和预测观测 h(x) + * 1DOF 标量 EKF 更新 — 仅 e_y * ---------------------------------------------------- */ - float e_y = s_state.x[0]; - float e_th = s_state.x[1]; + float e_y = s_state.x[0]; + float y_ey = z_ey - e_y; - /* 预测观测 h(x) */ - float h_ey = e_y; // 横向: z_ey ≈ e_y - float h_eth_L = e_th; // 左侧航向: z_eth_L ≈ e_th - float h_eth_R = e_th; // 右侧航向: z_eth_R ≈ e_th + float R_ey = s_cfg.r_ey; + if (valid_sides == 2) { + R_ey *= 0.5f; + } + float S_ey = s_state.P[0][0] + R_ey; - /* 新息向量 y = z - h(x) */ - float y[3] = {0}; - int obs_idx = 0; - - /* e_y 观测 */ - y[obs_idx++] = z_ey - h_ey; - - /* e_th_L 观测 */ - if (left_ok) y[obs_idx++] = z_eth_L - h_eth_L; - - /* e_th_R 观测 */ - if (right_ok) y[obs_idx++] = z_eth_R - h_eth_R; - - /* ---------------------------------------------------- - * 构建 H 矩阵 (Jacobian of h(x)) - * ---------------------------------------------------- */ - /* H 布局: - * 航向角观测对应列是 1,0,0 (e_y 观测量) - * 航向角观测对应列是 0,1,0 (e_th 观测量) - * s 不被直接观测 - */ - float H[3][3] = {0}; - int H_row = 0; - - /* e_y 行: H = [1, 0, 0] */ - H[H_row][0] = 1.0f; H[H_row][1] = 0.0f; H[H_row][2] = 0.0f; - H_row++; - - /* e_th_L 行: H = [0, 1, 0] */ - if (left_ok) { - H[H_row][0] = 0.0f; H[H_row][1] = 1.0f; H[H_row][2] = 0.0f; - H_row++; + if (fabsf(S_ey) < 1e-8f) { + goto output_result; } - /* e_th_R 行: H = [0, 1, 0] */ - if (right_ok) { - H[H_row][0] = 0.0f; H[H_row][1] = 1.0f; H[H_row][2] = 0.0f; - H_row++; - } + float d2_ey = y_ey * y_ey / S_ey; + max_maha_d2 = d2_ey; - int obs_count = H_row; - - /* ---------------------------------------------------- - * 构建观测噪声协方差 R (根据有效侧数量调整) - * ---------------------------------------------------- */ - float R[3][3] = {0}; - R[0][0] = s_cfg.r_ey; // e_y 的噪声 - - if (left_ok && right_ok) { - /* 双侧: 航向噪声更小 (两个独立观测平均) */ - R[1][1] = s_cfg.r_eth * 0.5f; // e_th_L - R[2][2] = s_cfg.r_eth * 0.5f; // e_th_R - } else { - /* 单侧: 航向噪声较大 */ - if (left_ok) { - R[1][1] = s_cfg.r_eth; - } - if (right_ok) { - R[1][1] = s_cfg.r_eth; - } - } - - /* ---------------------------------------------------- - * 计算新息协方差 S = H * P * H^T + R - * ---------------------------------------------------- */ - float HP[3][3] = {0}; - for (int i = 0; i < obs_count; i++) { - for (int j = 0; j < 3; j++) { - for (int k = 0; k < 3; k++) { - HP[i][j] += H[i][k] * s_state.P[k][j]; - } - } - } - - float S[3][3] = {0}; - for (int i = 0; i < obs_count; i++) { - for (int j = 0; j < 3; j++) { - for (int k = 0; k < 3; k++) { - S[i][j] += HP[i][k] * H[j][k]; // H^T: H[j][k] = H[k][j] - } - } - S[i][i] += R[i][i]; // 加观测噪声 - } - - /* ---------------------------------------------------- - * 计算 S 的逆 - * ---------------------------------------------------- */ - float S_inv[3][3] = {0}; - if (obs_count == 1) { - /* 1 观测: 标量 */ - if (fabsf(S[0][0]) < 1e-8f) { - goto output_result; - } - S_inv[0][0] = 1.0f / S[0][0]; - } else if (obs_count == 2) { - /* 2 观测：2x2 - 拷贝到局部矩阵 */ - float S_2x2[2][2]; - S_2x2[0][0] = S[0][0]; S_2x2[0][1] = S[0][1]; - S_2x2[1][0] = S[1][0]; S_2x2[1][1] = S[1][1]; - - if (!invert_2x2_sym(S_2x2)) { - goto output_result; - } - - /* 拷贝回 S_inv */ - S_inv[0][0] = S_2x2[0][0]; S_inv[0][1] = S_2x2[0][1]; - S_inv[1][0] = S_2x2[1][0]; S_inv[1][1] = S_2x2[1][1]; - } else { - /* 3 观测: 3x3 */ - memcpy(S_inv, S, sizeof(float) * 9); - if (!invert_3x3_cholesky(S_inv)) { - goto output_result; - } - } - - /* ---------------------------------------------------- - * χ² 马氏距离检验 (鲁棒拒绝) - * ---------------------------------------------------- */ - float d2_total = 0.0f; - if (obs_count == 1) { - d2_total = mahalanobis_d2_1dof(y[0], S_inv[0][0]); - } else if (obs_count == 2) { - d2_total = mahalanobis_d2_2dof(y, (const float (*)[2])S_inv); - } else { - d2_total = mahalanobis_d2_3dof(y, S_inv); - } - max_maha_d2 = d2_total; - - /* 单自由度检验: e_y 单独检验 */ - float d2_ey = mahalanobis_d2_1dof(y[0], S_inv[0][0]); if (d2_ey > s_cfg.chi2_1dof) { - reject_mask |= (1U << 0); // 拒绝 e_y + reject_mask |= (1U << 0); + goto output_result; } - /* 1 DOF 门限约 3.84 (95%), 约 6.63 (99%) */ - /* 如果整体 d² 过大，拒绝最可疑的观测 */ - if (obs_count >= 2) { - /* 检验 e_th_L */ - if (left_ok && !(reject_mask & (1U << 0))) { - /* 需要重新计算不含 e_y 的马氏距离 */ - /* 简化: 用 y[1]^2 / S[1][1] 作为 1DOF 近似 */ - if (fabsf(S[1][1]) > 1e-8f) { - float d2_eth_L = y[1] * y[1] / S[1][1]; - if (d2_eth_L > s_cfg.chi2_1dof) { - reject_mask |= (1U << 1); // 拒绝 e_th_L - } - } - } + float S_inv_ey = 1.0f / S_ey; + float K_ey[3]; + K_ey[0] = s_state.P[0][0] * S_inv_ey; + K_ey[1] = s_state.P[1][0] * S_inv_ey; + K_ey[2] = s_state.P[2][0] * S_inv_ey; - /* 检验 e_th_R */ - if (right_ok && !(reject_mask & (1U << 0)) && obs_count >= 3) { - if (fabsf(S[2][2]) > 1e-8f) { - float d2_eth_R = y[2] * y[2] / S[2][2]; - if (d2_eth_R > s_cfg.chi2_1dof) { - reject_mask |= (1U << 2); // 拒绝 e_th_R - } - } - } - } + s_state.x[0] += K_ey[0] * y_ey; + s_state.x[1] += K_ey[1] * y_ey; + s_state.x[2] += K_ey[2] * y_ey; - /* ---------------------------------------------------- - * 计算卡尔曼增益 K = P * H^T * S^(-1) - * P1 修复: 必须做完整矩阵乘法 (P*H^T) * S_inv， - * 而不能只乘 S_inv 的对角项 S_inv[j][j]。 - * 后者会忽略 S 的非对角元素（观测间相关性）， - * 导致卡尔曼增益不正确，影响滤波收敛性。 - * ---------------------------------------------------- */ - float HT[3][3] = {0}; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < obs_count; j++) { - HT[i][j] = H[j][i]; // H^T - } - } - - /* Step 1: PH^T = P * H^T, 结果为 3×obs_count */ - float PHT[3][3] = {0}; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < obs_count; j++) { - for (int k = 0; k < 3; k++) { - PHT[i][j] += s_state.P[i][k] * HT[k][j]; - } - } - } - - /* Step 2: K = PHT * S_inv, 结果为 3×obs_count */ - float K[3][3] = {0}; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < obs_count; j++) { - for (int k = 0; k < obs_count; k++) { - K[i][j] += PHT[i][k] * S_inv[k][j]; - } - } - } - - /* ---------------------------------------------------- - * 跳过被拒绝的观测，更新剩余观测 - * ---------------------------------------------------- */ - int used_obs = 0; - for (int i = 0; i < obs_count; i++) { - uint8_t bit = (i == 0) ? (1U << 0) : ((i == 1) ? (1U << 1) : (1U << 2)); - if (reject_mask & bit) { - /* - * 关键：被拒绝的观测不仅不能更新状态， - * 也不能参与后续 KH/P 更新，否则会错误收缩协方差。 - */ - K[0][i] = 0.0f; - K[1][i] = 0.0f; - K[2][i] = 0.0f; - continue; - } - - /* 状态更新: x += K[:, i] * y[i] */ - s_state.x[0] += K[0][i] * y[i]; - s_state.x[1] += K[1][i] * y[i]; - s_state.x[2] += K[2][i] * y[i]; - used_obs++; - } - - /* ---------------------------------------------------- - * 协方差更新: P = (I - K * H) * P_pred - * 简化 Joseph 形式: P = (I - K*H) * P * (I - K*H)^T + K * R * K^T - * 这里使用简化形式: P = (I - K*H) * P - * ---------------------------------------------------- */ - float KH[3][3] = {0}; + float P_new[3][3]; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - for (int k = 0; k < obs_count; k++) { - KH[i][j] += K[i][k] * H[k][j]; - } - } - } - - float I_KH[3][3]; - I_KH[0][0] = 1.0f - KH[0][0]; I_KH[0][1] = -KH[0][1]; I_KH[0][2] = -KH[0][2]; - I_KH[1][0] = -KH[1][0]; I_KH[1][1] = 1.0f - KH[1][1]; I_KH[1][2] = -KH[1][2]; - I_KH[2][0] = -KH[2][0]; I_KH[2][1] = -KH[2][1]; I_KH[2][2] = 1.0f - KH[2][2]; - - float P_tmp[3][3] = {0}; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - for (int k = 0; k < 3; k++) { - P_tmp[i][j] += I_KH[i][k] * s_state.P[k][j]; - } + P_new[i][j] = s_state.P[i][j] - K_ey[i] * s_state.P[0][j]; } } for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - s_state.P[i][j] = P_tmp[i][j]; + s_state.P[i][j] = P_new[i][j]; } } symmetrize(s_state.P); protect_P(s_state.P); - updated_obs_count = used_obs; + updated_obs_count = 1; output_result: - /* ---------------------------------------------------- - * 填充输出 - * ---------------------------------------------------- */ out_state->t_ms = obs->t_ms; out_state->e_y = s_state.x[0]; out_state->e_th = s_state.x[1]; @@ -694,24 +488,12 @@ output_result: out_state->mahalanobis_d2 = max_maha_d2; out_state->obs_reject_mask = reject_mask; - /* 置信度: 基于协方差迹和拒绝比例 */ float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2]; float conf_from_P = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f); - - /* 根据有效侧数加权 */ float side_factor = (valid_sides == 2) ? 1.0f : 0.7f; - - /* 根据拒绝比例降低置信度 */ - float reject_ratio = 0.0f; - if (obs_count > 0) { - int rejected = 0; - if (reject_mask & (1U << 0)) rejected++; - if (reject_mask & (1U << 1)) rejected++; - if (reject_mask & (1U << 2)) rejected++; - reject_ratio = (float)rejected / (float)obs_count; - } - - out_state->conf = clampf(conf_from_P * side_factor * (1.0f - reject_ratio * 0.5f), 0.0f, 1.0f); + float reject_penalty = (reject_mask & (1U << 0)) ? 0.5f : 1.0f; + + out_state->conf = clampf(conf_from_P * side_factor * reject_penalty, 0.0f, 1.0f); return updated_obs_count; } diff --git a/App/est/corridor_ekf.h b/App/est/corridor_ekf.h index 823b528..5920b87 100644 --- a/App/est/corridor_ekf.h +++ b/App/est/corridor_ekf.h @@ -120,6 +120,23 @@ void CorridorEKF_GetState(CorridorState_t *out); */ void CorridorEKF_Reset(void); +/** + * @brief 仅重置航向相关状态，用于掉头后重新建立走廊朝向基准 + * + * 保留横向位置 e_y 与进度 s，只将 e_th 清零并清理其相关协方差， + * 避免上一趟积累的航向误差在返程首拍继续驱动控制器猛打方向。 + */ +void CorridorEKF_ResetHeading(void); + +/** + * @brief 180° 掉头后重建走廊状态 + * + * 对同一条走廊原地掉头后： + * - 航向误差 e_th 应回到 0 + * - 横向误差 e_y 在新的前进方向下符号需要翻转 + */ +void CorridorEKF_RebaseAfterTurnaround(void); + /** * @brief 设置过程噪声 (运行时可调) */ diff --git a/App/est/corridor_filter.c b/App/est/corridor_filter.c index 2971dfb..589b076 100644 --- a/App/est/corridor_filter.c +++ b/App/est/corridor_filter.c @@ -84,15 +84,18 @@ void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx /* ---- IMU yaw 航向观测更新 ---- * * 在侧墙观测之后独立执行 1DOF 标量更新。 * + * 方向 B 改造：IMU 现在是航向 e_th 的唯一观测来源。 + * 侧墙不再提供航向观测，因此初期置信度可能略低。 + * 将参考值锁定门槛从 0.5 降到 0.3，确保 IMU 航向观测 + * 能尽早介入。 + * * 参考值管理策略： * 首次收到有效 IMU yaw 且 EKF 置信度足够时锁定参考值。 - * 此时 e_th ≈ 0 (刚被侧墙观测修正过)， - * 所以 yaw_ref = imu_yaw_current → z_eth_imu = 0， - * 后续 yaw 的变化量就等于 e_th 的变化量。 + * 此时 e_th 由 wz 积分驱动，锁定后 yaw 变化量就等于 e_th 变化量。 */ if (imu_yaw_valid) { - if (!s_imu_yaw_ref_set && out_state->conf >= 0.5f) { - /* 首次锁定：此刻 e_th 已被侧墙修正到接近 0 */ + 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; s_imu_yaw_ref_set = true; } @@ -121,3 +124,14 @@ void CorridorFilter_Reset(void) s_imu_yaw_ref_rad = 0.0f; s_imu_yaw_ref_set = false; } + +void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad) +{ + if (!s_initialized) return; + + /* 同一条沟原地掉头后：当前朝向成为新参考， + * 同时横向误差符号要镜像，航向误差要回零。 */ + CorridorEKF_RebaseAfterTurnaround(); + s_imu_yaw_ref_rad = imu_yaw_continuous_rad; + s_imu_yaw_ref_set = true; +} diff --git a/App/est/corridor_filter.h b/App/est/corridor_filter.h index bbf94ac..ee72a23 100644 --- a/App/est/corridor_filter.h +++ b/App/est/corridor_filter.h @@ -49,8 +49,18 @@ extern "C" { */ void CorridorFilter_Reset(void); + /** + * @brief 单沟 180° 掉头后重建滤波器参考 + * + * 同一条沟掉头后需要同时： + * - 将当前 IMU yaw 作为新的走廊航向参考 + * - 清零航向误差 e_th + * - 镜像横向误差 e_y 的符号 + */ + void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad); + #ifdef __cplusplus } #endif -#endif // CORRIDOR_FILTER_H \ No newline at end of file +#endif // CORRIDOR_FILTER_H diff --git a/App/nav/nav_script.c b/App/nav/nav_script.c index 145c2be..9271176 100644 --- a/App/nav/nav_script.c +++ b/App/nav/nav_script.c @@ -25,10 +25,15 @@ static struct { float turn_start_imu_yaw_deg; // 转向开始时的 IMU 连续偏航角 (deg) bool turn_started; // 转向是否已开始 float corridor_s_entry; // 进入垄沟时的 s 里程 + float end_rearm_s; // 掉头后到端检测重新使能的起始里程 + bool end_armed; // 到端检测是否已重新使能 + NavScriptStage_t post_turn_stage; // 本次转向完成后要进入的走廊阶段 int pass_count; // 已走过的垄沟数 float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发) } s_internal; +#define SCRIPT_END_REARM_DIST_M 0.12f + /* ========================================================= * 内部辅助函数 * ========================================================= */ @@ -142,6 +147,8 @@ void NavScript_Update(const CorridorObs_t *obs, if (left_ok && right_ok && state->conf >= 0.8f) { /* 两侧雷达都有数据，且置信度高 -> 进入垄沟，开始跟踪 */ s_internal.corridor_s_entry = state->s; + s_internal.end_rearm_s = state->s; + s_internal.end_armed = true; s_internal.pass_count = 1; s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD; out->request_corridor = true; @@ -160,6 +167,8 @@ void NavScript_Update(const CorridorObs_t *obs, * 否则 pass_count 停留在 0，导致后续 TURN_AT_END 判定时 * 多跑一趟走廊（三趟而非文档描述的两趟）。 */ s_internal.corridor_s_entry = state->s; + s_internal.end_rearm_s = state->s; + s_internal.end_armed = true; s_internal.pass_count = 1; s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD; } @@ -174,13 +183,20 @@ void NavScript_Update(const CorridorObs_t *obs, /* 使用走廊控制器 */ out->request_corridor = true; + if (!s_internal.end_armed) { + if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) { + s_internal.end_armed = true; + } + } + /* 检查是否到端 */ bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; - if (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) { + if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) { /* 前向距离足够近 -> 到达垄沟末端，准备转向 */ 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_BACKWARD; s_stage = SCRIPT_STAGE_TURN_AT_END; out->request_corridor = false; } @@ -214,18 +230,10 @@ void NavScript_Update(const CorridorObs_t *obs, if (remaining <= 0.1f) { /* 转向完成 -> 决定下一步 */ - if (s_internal.pass_count < 2) { - /* 只走了一遍，往回走 */ - /* 180° 掉头后，走廊方向基准已经翻转。 - * 必须清空上一趟的 EKF/IMU 航向参考，避免返程首拍把新朝向 - * 误判成大航向误差，导致一恢复闭环就猛打方向。 */ - CorridorFilter_Reset(); - s_internal.pass_count++; - s_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD; - } else { - /* 走了两遍，退出场地 */ - s_stage = SCRIPT_STAGE_EXIT; - } + CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f); + s_internal.end_rearm_s = state->s; + s_internal.end_armed = false; + s_stage = s_internal.post_turn_stage; out->override_v = 0.0f; out->override_w = 0.0f; out->use_override = true; @@ -257,11 +265,21 @@ void NavScript_Update(const CorridorObs_t *obs, /* P1 修复: 原地转 180° 后车头已调转，返回方向即"向前"， * 因此到端检测应使用前向雷达 d_front，而非后向雷达 d_back */ bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U; - if (front_ok && obs->d_front <= s_cfg.d_entry_exit_front) { - /* 前向距离足够近 -> 到达垄沟起始端，转向或退出 */ + + /* 掉头回来时，前向雷达可能还残留近端读数。 + * 必须先离开端墙一小段距离，再允许重新触发到端检测。 */ + if (!s_internal.end_armed) { + if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) { + s_internal.end_armed = true; + } + } + + 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; out->request_corridor = false; } diff --git a/App/robot_params.h b/App/robot_params.h index a58916f..8fa8432 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -235,7 +235,7 @@ extern "C" { * 过大：横向纠偏过猛，引起震荡 * 过小：偏了拉不回来 */ -#define PARAM_CTRL_KP_Y 3.0f +#define PARAM_CTRL_KP_Y 4.0f /** @brief [调优] 走廊巡航速度 [m/s] * 含义：走廊内正常行驶速度 @@ -279,7 +279,7 @@ extern "C" { * 过小：可能撞墙 * 过大：离墙很远就停车，走不完走廊 */ -#define PARAM_SAFE_D_FRONT_STOP 0.08f +#define PARAM_SAFE_D_FRONT_STOP 0.10f /** @brief [调优] 前向减速预警距离 [m] * 含义：前向雷达低于此值开始线性减速 diff --git a/CMakeLists.txt b/CMakeLists.txt index e807da5..1b05242 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,8 @@ enable_language(C ASM) # Create an executable object type # NOTE: 所有 App/ 下的源文件统一由下方 file(GLOB_RECURSE) 收集， # 此处不再手写重复列表，避免配置漂移和平台差异（如大小写敏感文件系统）。 -add_executable(${CMAKE_PROJECT_NAME}) +add_executable(${CMAKE_PROJECT_NAME} + App/VL53L0X_API/platform/vl53_calibration_config.h) # Add STM32CubeMX generated sources add_subdirectory(cmake/stm32cubemx)