Falling_jasmine/ASER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1.0

Browse Source
This commit is contained in:
nitiantuhao
2026-04-04 23:24:36 +08:00
parent 5e0901a4d9
commit 7c2396a282
11 changed files with 132 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
App/est/corridor_ekf.c
View File

@@ -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;

20
App/est/corridor_ekf.h
View File

@@ -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;

32
App/est/corridor_filter.c
View File

@@ -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;
}
}

4
App/est/corridor_filter.h
View File

@@ -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