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

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;