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 13:31:07 +08:00
parent a54cc3b274
commit 37a1788543
7 changed files with 290 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
App/nav/global_nav.c
View File

@@ -95,6 +95,21 @@ static inline float gnav_fabsf(float x)
return x < 0.0f ? -x : x;
}
typedef struct {
bool sides_complete;
bool near_sat;
bool width_ok;
bool heading_valid;
bool diagonal_conflict;
bool low_yaw_rate;
bool safe_for_align;
bool need_align;
float d_left_avg;
float d_right_avg;
float e_y_m;
float heading_rad;
} CorridorPoseEval_t;
/** 简单 P 控制航向保持，输入偏差 (deg)，输出角速度 (rad/s) */
static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp)
{
@@ -187,6 +202,83 @@ static bool compute_wall_heading_error(const CorridorObs_t* obs, float* out_head
return true;
}
static void evaluate_corridor_pose(const CorridorObs_t* obs,
const RobotBlackboard_t* board,
CorridorPoseEval_t* out_eval)
{
memset(out_eval, 0, sizeof(*out_eval));
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
out_eval->sides_complete = left_ok && right_ok;
if (!out_eval->sides_complete) {
return;
}
out_eval->d_left_avg = (obs->d_lf + obs->d_lr) * 0.5f;
out_eval->d_right_avg = (obs->d_rf + obs->d_rr) * 0.5f;
out_eval->e_y_m = 0.5f * (out_eval->d_right_avg - out_eval->d_left_avg);
{
float total_width = out_eval->d_left_avg + out_eval->d_right_avg + PARAM_ROBOT_WIDTH;
float width_err = gnav_fabsf(total_width - s_nav.cfg.corridor_width);
out_eval->width_ok = (width_err <= s_nav.cfg.reacquire_width_tol);
}
{
float sat_eps = 0.002f;
out_eval->near_sat = (obs->d_lf <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) ||
(obs->d_lr <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) ||
(obs->d_rf <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) ||
(obs->d_rr <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps));
}
{
float near_thresh = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH) - 0.015f;
bool lf_near = obs->d_lf < near_thresh;
bool lr_near = obs->d_lr < near_thresh;
bool rf_near = obs->d_rf < near_thresh;
bool rr_near = obs->d_rr < near_thresh;
/* 对角贴墙模式：大角度进沟时常见，此时 wall heading 几何容易失真。 */
out_eval->diagonal_conflict = (lf_near && rr_near) || (rf_near && lr_near);
}
{
float sensor_base = PARAM_SENSOR_BASE_LENGTH;
float left_heading = atan2f(obs->d_lr - obs->d_lf, sensor_base);
float right_heading = atan2f(obs->d_rf - obs->d_rr, sensor_base);
if (gnav_fabsf(left_heading - right_heading) <= PARAM_DEG2RAD(8.0f)) {
out_eval->heading_rad = 0.5f * (left_heading + right_heading);
out_eval->heading_valid = out_eval->width_ok && !out_eval->near_sat;
}
}
if (board != NULL && board->imu_wz.is_valid) {
out_eval->low_yaw_rate = gnav_fabsf(board->imu_wz.value) < 25.0f;
}
{
float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH);
float min_side = (out_eval->d_left_avg < out_eval->d_right_avg)
? out_eval->d_left_avg : out_eval->d_right_avg;
bool heading_bad = out_eval->heading_valid &&
(gnav_fabsf(out_eval->heading_rad) > s_nav.cfg.align_th_tol_rad);
bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m);
bool off_center = gnav_fabsf(out_eval->e_y_m) > s_nav.cfg.align_y_tol_m;
out_eval->need_align = heading_bad || near_wall || off_center || out_eval->diagonal_conflict;
}
out_eval->safe_for_align = out_eval->heading_valid &&
!out_eval->diagonal_conflict &&
out_eval->low_yaw_rate;
}
static void update_wall_heading_stability(bool valid, float heading_rad)
{
if (!valid) {
@@ -208,33 +300,6 @@ static void update_wall_heading_stability(bool valid, float heading_rad)
s_nav.wall_heading_prev_valid = true;
}
/* 重捕获已确认后，判断是否需要短暂停车摆正。
* 除了航向误差本身，还把“明显贴墙/明显偏中心”作为触发条件。
* 这样即使车头看起来近似平行，但整车已经贴到一侧，也会先停一下再进跟踪。 */
static bool need_align_after_reacquire(const CorridorObs_t* obs, float wall_heading_error)
{
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
if (!left_ok || !right_ok) {
return gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad;
}
{
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH);
float min_side = (d_left < d_right) ? d_left : d_right;
bool heading_bad = gnav_fabsf(wall_heading_error) > s_nav.cfg.align_th_tol_rad;
bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m);
bool off_center = gnav_fabsf(d_left - d_right) > (2.0f * s_nav.cfg.align_y_tol_m);
return heading_bad || near_wall || off_center;
}
}
/** 检查重捕获条件 */
static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
{
@@ -635,22 +700,24 @@ void GlobalNav_Update(const CorridorObs_t* obs,
}
if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
float wall_heading_error = 0.0f;
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error);
update_wall_heading_stability(wall_heading_ok, wall_heading_error);
CorridorPoseEval_t pose_eval;
evaluate_corridor_pose(obs, board, &pose_eval);
update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad);
/* 重捕获成功后，优先用 side VL53 短暂停车摆正车头。
* 只有 wall heading 已稳定时才允许进入 ALIGN 或直接放行。 */
if (wall_heading_ok && s_nav.wall_heading_stable_count >= 4U) {
if (need_align_after_reacquire(obs, wall_heading_error)) {
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);
/* 评估结果说明：
* - 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);
}
}
if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
@@ -668,23 +735,26 @@ void GlobalNav_Update(const CorridorObs_t* obs,
* 控制器还继续朝墙边打”的情况。
* ============================================================ */
case GNAV_ALIGN: {
float wall_heading_error = 0.0f;
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading_error);
update_wall_heading_stability(wall_heading_ok, wall_heading_error);
CorridorPoseEval_t pose_eval;
evaluate_corridor_pose(obs, board, &pose_eval);
update_wall_heading_stability(pose_eval.heading_valid, pose_eval.heading_rad);
out->override_v = 0.0f;
out->use_override = true;
out->request_corridor = false;
out->safety_mode = SAFETY_MODE_STRAIGHT;
if (wall_heading_ok && s_nav.wall_heading_stable_count >= 2U) {
out->override_w = gnav_clampf(-s_nav.cfg.align_kp_th * wall_heading_error,
-0.25f, 0.25f);
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);
} else {
out->override_w = 0.0f;
}
if (wall_heading_ok && gnav_fabsf(wall_heading_error) < s_nav.cfg.align_th_tol_rad) {
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) {
s_nav.align_ok_count++;
} else {
s_nav.align_ok_count = 0;
@@ -697,6 +767,13 @@ 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) {
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);
}
if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
if (board->imu_yaw_continuous.is_valid) {
CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);