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-05 10:15:40 +08:00
parent 46e7fcdbb2
commit c2a70c9470
7 changed files with 148 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
App/VL53L0X_API/platform/vl53_board.c
View File

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

14
App/VL53L0X_API/platform/vl53_board.h
View File

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

8
App/VL53L0X_API/platform/vl53_calibration_config.h
View File

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

126
App/est/corridor_ekf.c
View File

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

1
App/est/corridor_ekf.h
View File

@@ -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; // 横向观测噪声方差 (侧墙)

7
App/est/corridor_filter.c
View File

@@ -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; /* 横向观测噪声 (侧墙) */

33
App/robot_params.h
View File

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