From c2a70c9470523ab9aa9fd41997e93e9d3dda534a Mon Sep 17 00:00:00 2001 From: nitiantuhao <2062405236@qq.com> Date: Sun, 5 Apr 2026 10:15:40 +0800 Subject: [PATCH] 1.0 --- App/VL53L0X_API/platform/vl53_board.c | 48 +++---- App/VL53L0X_API/platform/vl53_board.h | 14 +- .../platform/vl53_calibration_config.h | 8 +- App/est/corridor_ekf.c | 126 +++++++++++++----- App/est/corridor_ekf.h | 1 + App/est/corridor_filter.c | 7 +- App/robot_params.h | 33 +++-- 7 files changed, 148 insertions(+), 89 deletions(-) diff --git a/App/VL53L0X_API/platform/vl53_board.c b/App/VL53L0X_API/platform/vl53_board.c index b49fa91..e2f525e 100644 --- a/App/VL53L0X_API/platform/vl53_board.c +++ b/App/VL53L0X_API/platform/vl53_board.c @@ -4,26 +4,22 @@ #include "robot_params.h" #include "vl53_calibration_config.h" -/* ================= 卡尔曼滤波底层实现 ================= */ -static void vl53_kalman_init(Vl53Kalman_t *kf, float q, float r) { - kf->x = 0.0f; - kf->p = 1.0f; - kf->q = q; - kf->r = r; - kf->initialized = 0; +/* ================= EMA滤波底层实现 ================= */ +static void vl53_ema_init(Vl53EMA_t *ema, float alpha) { + ema->x = 0.0f; + ema->alpha = alpha; + ema->initialized = 0; } -static float vl53_kalman_update(Vl53Kalman_t *kf, float measurement) { - if (kf->initialized == 0) { - kf->x = measurement; - kf->initialized = 1; - return kf->x; +static float vl53_ema_update(Vl53EMA_t *ema, float measurement) { + if (ema->initialized == 0) { + ema->x = measurement; + ema->initialized = 1; + return ema->x; } - kf->p = kf->p + kf->q; - float k = kf->p / (kf->p + kf->r); - kf->x = kf->x + k * (measurement - kf->x); - kf->p = (1.0f - k) * kf->p; - return kf->x; + // EMA公式: x_new = alpha * measurement + (1 - alpha) * x_old + ema->x = ema->alpha * measurement + (1.0f - ema->alpha) * ema->x; + return ema->x; } static const Vl53RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id) @@ -124,8 +120,8 @@ VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET); - /* 初始化卡尔曼滤波器：Q/R 从 robot_params.h 读取 */ - vl53_kalman_init(&board->kf[i], PARAM_VL53_KALMAN_Q, PARAM_VL53_KALMAN_R); + /* 初始化EMA滤波器：alpha 从 robot_params.h 读取 */ + vl53_ema_init(&board->ema[i], PARAM_VL53_EMA_ALPHA); } vTaskDelay(pdMS_TO_TICKS(10)); @@ -191,25 +187,25 @@ VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapsho snapshot->range_status[i] = data.RangeStatus; if (data.RangeStatus == 0u) { - /* 2. 标记有效并按开关决定是否应用卡尔曼滤波 */ + /* 2. 标记有效并按开关决定是否应用EMA滤波 */ snapshot->valid_mask |= (1u << i); -#if PARAM_VL53_USE_KALMAN_FILTER - snapshot->range_mm_filtered[i] = vl53_kalman_update(&board->kf[i], (float)data.RangeMilliMeter); +#if PARAM_VL53_USE_EMA_FILTER + snapshot->range_mm_filtered[i] = vl53_ema_update(&board->ema[i], (float)data.RangeMilliMeter); #else snapshot->range_mm_filtered[i] = (float)data.RangeMilliMeter; - board->kf[i].x = (float)data.RangeMilliMeter; - board->kf[i].initialized = 1u; + board->ema[i].x = (float)data.RangeMilliMeter; + board->ema[i].initialized = 1u; #endif } else { /* 测距失败时，滤波值维持上一次的历史最佳估计不变 */ - snapshot->range_mm_filtered[i] = board->kf[i].x; + snapshot->range_mm_filtered[i] = board->ema[i].x; } VL53L0X_ClearInterruptMask(&board->dev[i], 0u); } } else { /* 如果没准备好，把上一帧的历史值顺延下来，防止读到 0 */ - snapshot->range_mm_filtered[i] = board->kf[i].x; + snapshot->range_mm_filtered[i] = board->ema[i].x; } } return VL53L0X_ERROR_NONE; diff --git a/App/VL53L0X_API/platform/vl53_board.h b/App/VL53L0X_API/platform/vl53_board.h index cbcc50a..39aa4f4 100644 --- a/App/VL53L0X_API/platform/vl53_board.h +++ b/App/VL53L0X_API/platform/vl53_board.h @@ -21,27 +21,25 @@ extern "C" { uint8_t id; } Vl53BoardHwCfg_t; - /* ================= 内部卡尔曼滤波器结构 ================= */ + /* ================= 内部EMA滤波器结构 ================= */ typedef struct { - float x; // 最优估计值 (滤波后的距离) - float p; // 估计误差协方差 - float q; // 过程噪声 - float r; // 测量噪声 + float x; // 当前滤波值 (滤波后的距离) + float alpha; // 平滑系数 (0.0~1.0, 越大响应越快) uint8_t initialized; // 防止第一次开机从0缓慢爬升 - } Vl53Kalman_t; + } Vl53EMA_t; /* ================= 快照数据结构 (对外接口) ================= */ typedef struct { uint32_t tick_ms; uint16_t range_mm[VL53_MAX_DEVS_PER_BOARD]; /* 接口1：原始硬件数据 */ - float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：卡尔曼平滑数据 */ + float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：EMA平滑数据 */ uint8_t range_status[VL53_MAX_DEVS_PER_BOARD]; uint8_t valid_mask; } Vl53BoardSnapshot_t; typedef struct { VL53L0X_Dev_t dev[VL53_MAX_DEVS_PER_BOARD]; - Vl53Kalman_t kf[VL53_MAX_DEVS_PER_BOARD]; /* 每个传感器配备一个专属卡尔曼滤波器 */ + Vl53EMA_t ema[VL53_MAX_DEVS_PER_BOARD]; /* 每个传感器配备一个专属EMA滤波器 */ uint8_t init_mask; uint8_t dev_count; uint32_t timing_budget_us; diff --git a/App/VL53L0X_API/platform/vl53_calibration_config.h b/App/VL53L0X_API/platform/vl53_calibration_config.h index fb640bd..7fe9609 100644 --- a/App/VL53L0X_API/platform/vl53_calibration_config.h +++ b/App/VL53L0X_API/platform/vl53_calibration_config.h @@ -22,13 +22,13 @@ typedef struct { */ static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = { { - .offset_micro_meters = 10000, + .offset_micro_meters = 8000, .xtalk_compensation_rate_mcps = 0, .offset_calibrated = 1, .xtalk_calibrated = 0, }, { - .offset_micro_meters = 10000, + .offset_micro_meters = 8000, .xtalk_compensation_rate_mcps = 0, .offset_calibrated = 1, .xtalk_calibrated = 0, @@ -37,13 +37,13 @@ static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = { static const Vl53RuntimeCalibration_t k_vl53_right_calibration[2] = { { - .offset_micro_meters = 9000, + .offset_micro_meters = 2000, .xtalk_compensation_rate_mcps = 0, .offset_calibrated = 1, .xtalk_calibrated = 0, }, { - .offset_micro_meters = 13000, + .offset_micro_meters = 9000, .xtalk_compensation_rate_mcps = 0, .offset_calibrated = 1, .xtalk_calibrated = 0, diff --git a/App/est/corridor_ekf.c b/App/est/corridor_ekf.c index 508dfb4..f173c9f 100644 --- a/App/est/corridor_ekf.c +++ b/App/est/corridor_ekf.c @@ -46,12 +46,29 @@ static inline float clampf(float val, float lo, float hi) return val; } -/** 对称矩阵拷贝 + 上三角 symmetrize */ +/** 对称矩阵拷贝 + 双向取平均 (减少舍入误差传播) */ static void symmetrize(float M[3][3]) { - M[1][0] = M[0][1]; - M[2][0] = M[0][2]; - M[2][1] = M[1][2]; + float avg; + avg = (M[0][1] + M[1][0]) * 0.5f; + M[0][1] = M[1][0] = avg; + + avg = (M[0][2] + M[2][0]) * 0.5f; + M[0][2] = M[2][0] = avg; + + avg = (M[1][2] + M[2][1]) * 0.5f; + M[1][2] = M[2][1] = avg; +} + +/** 角度归一化到 [-π, π] + * 防止 IMU yaw 累积角度跨越 ±π 时导致新息突变 + */ +static float wrap_angle(float angle) +{ + const float PI = 3.14159265358979323846f; + while (angle > PI) angle -= 2.0f * PI; + while (angle < -PI) angle += 2.0f * PI; + return angle; } /** P 上界保护 */ @@ -63,6 +80,57 @@ static void protect_P(float P[3][3]) } } +/** Joseph 形式协方差更新 (1DOF 标量观测) + * P_new = (I - K*H) * P * (I - K*H)^T + K * R * K^T + * + * 参数: + * P[3][3] - 先验协方差 (输入/输出) + * K[3] - 卡尔曼增益向量 + * H[3] - 观测矩阵 (1x3 行向量) + * R - 观测噪声方差 (标量) + * + * 优点: 保证数值稳定性，即使有舍入误差也能保持 P 正定 + */ +static void joseph_update_P(float P[3][3], const float K[3], const float H[3], float R) +{ + /* 计算 A = (I - K*H) */ + float A[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + A[i][j] = (i == j ? 1.0f : 0.0f) - K[i] * H[j]; + } + } + + /* 计算 A * P */ + float AP[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + AP[i][j] = 0.0f; + for (int k = 0; k < 3; k++) { + AP[i][j] += A[i][k] * P[k][j]; + } + } + } + + /* 计算 A * P * A^T */ + float APAT[3][3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + APAT[i][j] = 0.0f; + for (int k = 0; k < 3; k++) { + APAT[i][j] += AP[i][k] * A[j][k]; // A^T[k][j] = A[j][k] + } + } + } + + /* 计算 K * R * K^T 并加到 APAT */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + P[i][j] = APAT[i][j] + K[i] * R * K[j]; + } + } +} + /** 计算 2x2 对称矩阵的逆 (原地) */ static bool invert_2x2_sym(float S[2][2]) { @@ -302,11 +370,13 @@ void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt) F[2][1] = -vx * sin_th * dt; // ds/de_th F[2][2] = 1.0f; - /* 过程噪声协方差 Q (对角) */ + /* 过程噪声协方差 Q (含耦合项) + * [改进] 添加 e_y 和 e_th 的耦合噪声，反映横向-航向动力学耦合 */ float Q[3][3] = {0}; Q[0][0] = s_cfg.q_ey * dt * dt; Q[1][1] = s_cfg.q_eth * dt * dt; Q[2][2] = s_cfg.q_s * dt * dt; + Q[0][1] = Q[1][0] = s_cfg.q_ey_eth * dt * dt; // 横向-航向耦合 /* 协方差预测: P_pred = F * P * F^T + Q */ float F_P[3][3] = {0}; @@ -324,8 +394,8 @@ void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt) for (int k = 0; k < 3; k++) { P_pred[i][j] += F_P[i][k] * F[j][k]; // F^T: F[j][k] = F[k][j] } + P_pred[i][j] += Q[i][j]; // 加过程噪声 (含非对角项) } - P_pred[i][i] += Q[i][i]; // 加过程噪声 } symmetrize(P_pred); @@ -462,18 +532,10 @@ int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state) s_state.x[1] += K_ey[1] * y_ey; s_state.x[2] += K_ey[2] * y_ey; - float P_new[3][3]; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; 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_new[i][j]; - } - } + /* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T + * H = [1, 0, 0] (仅观测 e_y) */ + float H_ey[3] = {1.0f, 0.0f, 0.0f}; + joseph_update_P(s_state.P, K_ey, H_ey, R_ey); symmetrize(s_state.P); protect_P(s_state.P); @@ -525,11 +587,13 @@ void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool val { if (!s_initialized || !valid) return; - /* 观测值: IMU 相对于走廊参考方向的航向偏差 */ - float z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad; + /* 观测值: IMU 相对于走廊参考方向的航向偏差 + * [改进] 角度归一化防止 ±π 跨越时新息突变 */ + float z_eth_imu = wrap_angle(imu_yaw_rad - imu_yaw_ref_rad); - /* 新息: y = z - h(x), h(x) = e_th = x[1] */ - float y_imu = z_eth_imu - s_state.x[1]; + /* 新息: y = z - h(x), h(x) = e_th = x[1] + * [改进] 再次归一化，防止 e_th 和 z_eth_imu 符号不同时差值超出 [-π, π] */ + float y_imu = wrap_angle(z_eth_imu - s_state.x[1]); /* H = [0, 1, 0] → S = P[1][1] + R_imu */ float R_imu = s_cfg.r_eth_imu; @@ -553,20 +617,10 @@ void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool val s_state.x[1] += K[1] * y_imu; s_state.x[2] += K[2] * y_imu; - /* 协方差更新: P = (I - K*H) * P - * H = [0, 1, 0], 所以 KH[i][j] = K[i] * H[j] = K[i] * δ(j,1) */ - float P_new[3][3]; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - P_new[i][j] = s_state.P[i][j] - K[i] * s_state.P[1][j]; - } - } - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - s_state.P[i][j] = P_new[i][j]; - } - } + /* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T + * H = [0, 1, 0] (仅观测 e_th) */ + float H_imu[3] = {0.0f, 1.0f, 0.0f}; + joseph_update_P(s_state.P, K, H_imu, R_imu); symmetrize(s_state.P); protect_P(s_state.P); diff --git a/App/est/corridor_ekf.h b/App/est/corridor_ekf.h index 5b43dd2..e4e6262 100644 --- a/App/est/corridor_ekf.h +++ b/App/est/corridor_ekf.h @@ -43,6 +43,7 @@ typedef struct { float q_ey; // e_y 过程噪声方差 float q_eth; // e_th 过程噪声方差 float q_s; // s 过程噪声方差 + float q_ey_eth; // [改进] e_y 和 e_th 耦合噪声 (横向-航向动力学耦合) /* 观测噪声协方差 R */ float r_ey; // 横向观测噪声方差 (侧墙) diff --git a/App/est/corridor_filter.c b/App/est/corridor_filter.c index 4508610..1ca6534 100644 --- a/App/est/corridor_filter.c +++ b/App/est/corridor_filter.c @@ -44,9 +44,10 @@ void CorridorFilter_Init(const CorridorFilterConfig_t *config) ekf_cfg.robot_width = config->robot_width; /* 过程噪声 Q —— 统一从 robot_params.h 读取，改参数只改那一个文件 */ - ekf_cfg.q_ey = PARAM_EKF_Q_EY; /* 横向过程噪声 */ - ekf_cfg.q_eth = PARAM_EKF_Q_ETH; /* 航向过程噪声 */ - ekf_cfg.q_s = PARAM_EKF_Q_S; /* 里程过程噪声 */ + ekf_cfg.q_ey = PARAM_EKF_Q_EY; /* 横向过程噪声 */ + ekf_cfg.q_eth = PARAM_EKF_Q_ETH; /* 航向过程噪声 */ + ekf_cfg.q_s = PARAM_EKF_Q_S; /* 里程过程噪声 */ + ekf_cfg.q_ey_eth = PARAM_EKF_Q_EY_ETH; /* [改进] 横向-航向耦合噪声 */ /* 观测噪声 R */ ekf_cfg.r_ey = PARAM_EKF_R_EY; /* 横向观测噪声 (侧墙) */ diff --git a/App/robot_params.h b/App/robot_params.h index 9dc54e3..d5217ff 100644 --- a/App/robot_params.h +++ b/App/robot_params.h @@ -168,6 +168,15 @@ extern "C" { */ #define PARAM_EKF_Q_S 0.1f +/** @brief [改进] 横向-航向耦合过程噪声 [m·rad] + * 含义：e_y 和 e_th 的动力学耦合强度 + * 物理意义：横向偏差会导致航向修正，航向偏差会导致横向漂移 + * 调优方法：从 0 开始，如果发现横向和航向震荡相关可适当增大 + * 典型值：0~0.005 + * 设为 0 则退化为对角 Q 矩阵 + */ +#define PARAM_EKF_Q_EY_ETH 0.0f + /* --- EKF 观测噪声 R (对传感器信任度) --- */ /** @brief [调优] 横向观测噪声方差 [m²] * 含义：VL53L0X 测距的可靠程度 @@ -368,23 +377,23 @@ extern "C" { */ #define PARAM_VL53_TIMING_BUDGET 33000U -/** @brief [调试] 是否启用 VL53L0X 端卡尔曼滤波 +/** @brief [调试] 是否启用 VL53L0X 端EMA滤波 * 1: 使用滤波后的 range_mm_filtered * 0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比 */ -#define PARAM_VL53_USE_KALMAN_FILTER 1 +#define PARAM_VL53_USE_EMA_FILTER 1 -/** @brief [调优] VL53L0X 卡尔曼滤波参数 Q - * 含义：测距过程噪声 - * 典型值：5.0~20.0 +/** @brief [调优] VL53L0X EMA滤波平滑系数 alpha + * 含义：新测量值的权重 (0.0~1.0) + * - 越大响应越快，延迟越低，但抖动越大 + * - 越小输出越平滑，但延迟越高 + * 典型值： + * 0.3 - 平滑优先 (延迟约100ms) + * 0.4 - 折中 (延迟约60ms，推荐) + * 0.5 - 快速响应 (延迟约40ms) + * 0.6 - 极速响应 (延迟约25ms，抖动较大) */ -#define PARAM_VL53_KALMAN_Q 10.0f - -/** @brief [调优] VL53L0X 卡尔曼滤波参数 R - * 含义：测距观测噪声 - * 典型值：10.0~30.0 - */ -#define PARAM_VL53_KALMAN_R 14.1f +#define PARAM_VL53_EMA_ALPHA 0.4f /* --- IMU --- */ /** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad]