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-12 16:21:49 +08:00
parent 315495bab3
commit b07d1ff9cd
6 changed files with 84 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,7 +18,7 @@ typedef struct {
*/
static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = {
{
.calibrated = 1u,
.calibrated = 0u,
.data = {
.struct_version = 3970629922u,
.customer = {
@@ -67,7 +67,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = {
}
},
{
.calibrated = 1u,
.calibrated = 0u,
.data = {
.struct_version = 3970629922u,
.customer = {
@@ -119,7 +119,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = {
static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = {
{
.calibrated = 1u,
.calibrated = 0u,
.data = {
.struct_version = 3970629922u,
.customer = {
@@ -168,7 +168,7 @@ static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = {
}
},
{
.calibrated = 1u,
.calibrated = 0u,
.data = {
.struct_version = 3970629922u,
.customer = {

1
App/app_tasks.c
View File

@@ -492,6 +492,7 @@ void AppTasks_Init(void)
.link_gap_runout = PARAM_GNAV_LINK_GAP_RUNOUT,
.link_wall_target = PARAM_GNAV_LINK_WALL_TARGET,
.link_wall_kp = PARAM_GNAV_LINK_WALL_KP,
.link_wall_heading_kp = PARAM_GNAV_LINK_WALL_HEADING_KP,
.link_wall_blend = PARAM_GNAV_LINK_WALL_BLEND,
.exit_v = PARAM_GNAV_EXIT_V,
.exit_runout = PARAM_GNAV_EXIT_RUNOUT,

27
App/nav/corridor_ctrl.c
View File

@@ -61,10 +61,33 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
/* 接近出口: 仅保持航向惯性，禁用左右激光控制 */
w_cmd = -(s_cfg.kd_theta * imu_wz);
} else {
/* 正常控制: 完整PD控制律 */
/*
* 简化原则：
* - IMU 只管航向 (e_th + wz)
* - 左右激光只管居中
*
* 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差：
* e_y_direct = (d_right - d_left)/2
* 车在正中时 d_left ~= d_right，因此误差应接近 0。
*
* 若当前帧双侧不完整，再退回滤波器给出的 e_y。
*/
float e_y_ctrl = state->e_y;
bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
((obs->valid_mask & (1U << 1)) != 0U);
bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
((obs->valid_mask & (1U << 3)) != 0U);
if (left_ok && right_ok) {
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
e_y_ctrl = 0.5f * (d_right - d_left);
}
/* 正常控制: IMU 管航向, 左右激光管居中 */
w_cmd = -(s_cfg.kp_theta * state->e_th
+ s_cfg.kd_theta * imu_wz
+ s_cfg.kp_y * state->e_y);
+ s_cfg.kp_y * e_y_ctrl);
/* ========================================================
* 近墙脱离保护:

79
App/nav/global_nav.c
View File

@@ -38,6 +38,7 @@ static struct {
float turn_start_yaw_deg; /* IMU yaw_continuous at turn start */
float turn_target_delta_deg; /* 90° */
int8_t turn_sign; /* +1 (CCW) or -1 (CW) */
float turn_goal_yaw_deg; /* 本次转向完成后的名义目标航向 */
/* 里程 (用里程计积分距离) */
float stage_entry_odom_vx_accum; /* 进入阶段时的里程计累计距离 */
@@ -454,6 +455,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
s_nav.turn_start_yaw_deg = imu_yaw;
s_nav.turn_target_delta_deg = 90.0f;
s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f;
break;
}
case GNAV_TURN_OUT_OF_CORRIDOR: {
@@ -461,6 +463,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
s_nav.turn_start_yaw_deg = imu_yaw;
s_nav.turn_target_delta_deg = 90.0f;
s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f;
break;
}
case GNAV_TURN_INTO_NEXT: {
@@ -469,6 +472,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
s_nav.turn_start_yaw_deg = imu_yaw;
s_nav.turn_target_delta_deg = 90.0f;
s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f;
s_nav.current_corridor_id = next_id;
break;
}
@@ -700,25 +704,9 @@ void GlobalNav_Update(const CorridorObs_t* obs,
}
if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
CorridorPoseEval_t pose_eval;
evaluate_corridor_pose(obs, board, &pose_eval);
update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad);
/* 评估结果说明：
* - need_align: 车头歪/偏心/贴墙/对角近墙，需要先停一下
* - safe_for_align: 当前几何足够健康，允许按姿态估计输出转向
*
* 即使几何暂时不安全，只要 need_align=true 也先进入 ALIGN 停住，
* 避免一边前进一边突然大角度纠正。 */
if (pose_eval.need_align) {
transition_to(GNAV_ALIGN, board);
} else {
if (board->imu_yaw_continuous.is_valid) {
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
}
transition_to(GNAV_CORRIDOR_TRACK, board);
}
/* 简化：重捕获成功后统一停车摆正航向。
* 不再在这里用墙姿态决定“摆不摆正”，避免不同沟表现不一致。 */
transition_to(GNAV_ALIGN, board);
}
if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
/* 取消重捕获失败态：超时后不进 ERROR，
@@ -735,26 +723,35 @@ void GlobalNav_Update(const CorridorObs_t* obs,
* 控制器还继续朝墙边打”的情况。
* ============================================================ */
case GNAV_ALIGN: {
CorridorPoseEval_t pose_eval;
evaluate_corridor_pose(obs, board, &pose_eval);
update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad);
float imu_yaw_err_rad = 0.0f;
bool imu_align_ok = false;
if (board->imu_yaw_continuous.is_valid) {
imu_yaw_err_rad = PARAM_DEG2RAD(s_nav.turn_goal_yaw_deg - board->imu_yaw_continuous.value);
while (imu_yaw_err_rad > 3.14159265f) imu_yaw_err_rad -= 6.2831853f;
while (imu_yaw_err_rad < -3.14159265f) imu_yaw_err_rad += 6.2831853f;
if (gnav_fabsf(imu_yaw_err_rad) < s_nav.cfg.align_th_tol_rad &&
(!board->imu_wz.is_valid || gnav_fabsf(board->imu_wz.value) < 15.0f)) {
imu_align_ok = true;
}
}
out->override_v = 0.0f;
out->use_override = true;
out->request_corridor = false;
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (pose_eval.safe_for_align && s_nav.wall_heading_stable_count >= 2U) {
float w_align = -(s_nav.cfg.align_kp_th * pose_eval.heading_rad
+ s_nav.cfg.align_kp_y * pose_eval.e_y_m);
out->override_w = gnav_clampf(w_align, -0.20f, 0.20f);
if (board->imu_yaw_continuous.is_valid) {
/* imu_yaw_err_rad = target - current
* 误差为正时，需要正角速度(左转/逆时针)去追目标，不能取反。 */
float w_align_imu = 1.2f * imu_yaw_err_rad;
out->override_w = gnav_clampf(w_align_imu, -0.18f, 0.18f);
} else {
out->override_w = 0.0f;
}
if (pose_eval.safe_for_align &&
gnav_fabsf(pose_eval.heading_rad) < s_nav.cfg.align_th_tol_rad &&
gnav_fabsf(pose_eval.e_y_m) < s_nav.cfg.align_y_tol_m) {
if (imu_align_ok) {
s_nav.align_ok_count++;
} else {
s_nav.align_ok_count = 0;
@@ -767,12 +764,12 @@ void GlobalNav_Update(const CorridorObs_t* obs,
}
transition_to(GNAV_CORRIDOR_TRACK, board);
}
if (!pose_eval.safe_for_align && elapsed_ms > 400U) {
if (board->imu_yaw_continuous.is_valid) {
if (elapsed_ms > 400U) {
if (board->imu_yaw_continuous.is_valid && imu_align_ok) {
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
transition_to(GNAV_CORRIDOR_TRACK, board);
}
transition_to(GNAV_CORRIDOR_TRACK, board);
}
if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
if (board->imu_yaw_continuous.is_valid) {
@@ -825,6 +822,7 @@ void GlobalNav_Update(const CorridorObs_t* obs,
float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
s_nav.cfg.heading_kp);
float w_wall_guard = 0.0f;
float w_wall_parallel = 0.0f;
const CorridorDescriptor_t* corridor = TrackMap_GetCorridor(s_nav.current_corridor_id);
if (corridor != NULL && corridor->travel_dir == TRAVEL_DIR_EAST) {
@@ -835,6 +833,14 @@ void GlobalNav_Update(const CorridorObs_t* obs,
float error = s_nav.cfg.link_wall_target - d_right;
w_wall_guard = s_nav.cfg.link_wall_kp * error;
}
/* 右端通道：右侧是围栏。
* 用右前/右后距离差把车身摆成与围栏平行，
* 避免“不蹭墙但车身是歪的”。 */
{
float heading_right = atan2f(obs->d_rf - obs->d_rr, PARAM_SENSOR_BASE_LENGTH);
w_wall_parallel = -s_nav.cfg.link_wall_heading_kp * heading_right;
}
}
} else if (corridor != NULL) {
if ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
@@ -844,10 +850,17 @@ void GlobalNav_Update(const CorridorObs_t* obs,
float error = s_nav.cfg.link_wall_target - d_left;
w_wall_guard = -(s_nav.cfg.link_wall_kp * error);
}
/* 左端通道：左侧是围栏。 */
{
float heading_left = atan2f(obs->d_lr - obs->d_lf, PARAM_SENSOR_BASE_LENGTH);
w_wall_parallel = -s_nav.cfg.link_wall_heading_kp * heading_left;
}
}
}
out->override_w = gnav_clampf(w_heading + w_wall_guard, -1.0f, 1.0f);
w_wall_parallel = gnav_clampf(w_wall_parallel, -0.20f, 0.20f);
out->override_w = gnav_clampf(w_heading + w_wall_guard + w_wall_parallel, -1.0f, 1.0f);
{
bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;

1
App/nav/global_nav.h
View File

@@ -86,6 +86,7 @@ typedef struct {
float link_gap_runout; /* 检测到沟口后继续前冲距离 (m) */
float link_wall_target; /* 墙壁跟随目标距离 (m) */
float link_wall_kp; /* 墙壁跟随P增益 */
float link_wall_heading_kp; /* 单边墙平行修正增益 */
float link_wall_blend; /* 墙壁跟随权重 (0~1) */
/* 出场 */

13
App/robot_params.h
View File

@@ -64,7 +64,7 @@ extern "C" {
* 典型值：根据机械安装，约 0.10~0.15m
* 影响：航向观测精度 z_eth = atan2((d_back-d_front), L_s)
*/
#define PARAM_SENSOR_BASE_LENGTH 0.120f
#define PARAM_SENSOR_BASE_LENGTH 0.0755f
/** @brief [实测] 比赛走廊标准宽度 [m]
* 测量方法：卷尺测量赛场垄沟实际宽度 (规则 40cm，允许±5% 误差)
@@ -85,7 +85,7 @@ extern "C" {
* 典型值：根据机械安装，若与车尾齐平则为 0
* 用途：后向到端距离补偿 d_body_rear = d_sensor - offset
*/
#define PARAM_REAR_LASER_OFFSET 0.0f
#define PARAM_REAR_LASER_OFFSET (-(0.018f))
/** @brief [实测] 侧向 VL53L0X 传感器内缩距离 [m]
* 测量方法：VL53L0X 发射面到车体侧面最外边缘的距离 (向内为正)
@@ -95,7 +95,7 @@ extern "C" {
* ⚠ 极其重要：沟道40cm - 车体20cm = 每边仅10cm，1cm的偏差就是10%的误差！
* 注意：此值作为未分侧设置时的默认值。建议使用下面的分侧参数。
*/
#define PARAM_VL53_SIDE_INSET 0.0f
#define PARAM_VL53_SIDE_INSET (-(0.018f))
/** @brief [改进A][实测] 左侧 VL53L0X 传感器内缩距离 [m]
* 测量方法：将机器人精确放在走廊正中央(卷尺确认)，
@@ -501,9 +501,9 @@ extern "C" {
/* --- 转向 --- */
#define PARAM_GNAV_TURN_OMEGA 1.0f /* rad/s — 转向角速度 */
#define PARAM_GNAV_TURN_TOLERANCE 0.087f /* rad — 转向完成容差 (~5°) */
#define PARAM_GNAV_TURN_DECEL_ZONE 0.5f /* rad — 接近目标时减速区 (~28°) */
#define PARAM_GNAV_TURN_MIN_OMEGA 0.3f /* rad/s — 减速区最低角速度 */
#define PARAM_GNAV_TURN_TOLERANCE 0.052f /* rad — 转向完成容差 (~3°) */
#define PARAM_GNAV_TURN_DECEL_ZONE 0.70f /* rad — 接近目标时减速区 (~40°) */
#define PARAM_GNAV_TURN_MIN_OMEGA 0.18f /* rad/s — 减速区最低角速度 */
#define PARAM_GNAV_TURN_TIMEOUT 8000U /* ms — 单次转向超时 */
/* --- 重捕获 --- */
@@ -535,6 +535,7 @@ extern "C" {
#define PARAM_GNAV_LINK_GAP_RUNOUT 0.08f /* m — 看到下一个沟口后继续前冲距离 */
#define PARAM_GNAV_LINK_WALL_TARGET 0.10f /* m — 连接段最小离墙距离（小于10cm才触发保底修正） */
#define PARAM_GNAV_LINK_WALL_KP 3.0f /* — 连接段离墙保底增益 */
#define PARAM_GNAV_LINK_WALL_HEADING_KP 1.2f /* — 连接段单边墙平行修正增益 */
#define PARAM_GNAV_LINK_WALL_BLEND 0.5f /* — 预留给其他通道状态，连接段本身不再使用融合 */
/* --- 出场 --- */