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 17:09:19 +08:00
parent ee63bee6e5
commit 1ecdf6c187
9 changed files with 201 additions and 320 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

2
App/app_tasks.c
View File

@@ -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 修复：退出直线速度独立参数 */

370
App/est/corridor_ekf.c
View File

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

17
App/est/corridor_ekf.h
View File

@@ -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 设置过程噪声 (运行时可调)
*/

24
App/est/corridor_filter.c
View File

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

12
App/est/corridor_filter.h
View File

@@ -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
#endif // CORRIDOR_FILTER_H

48
App/nav/nav_script.c
View File

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

4
App/robot_params.h
View File

@@ -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]
* 含义：前向雷达低于此值开始线性减速

3
CMakeLists.txt
View File

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