1.0

App/nav/corridor_ctrl.c

@@ -0,0 +1,83 @@
+#include "corridor_ctrl.h"
+#include <math.h>
+#include <stdbool.h>
+
+/* ====================== 内部状态 ====================== */
+static CorridorCtrlConfig_t s_cfg;
+static bool s_initialized = false;
+
+/* 辅助：浮点数限幅 */
+static inline float clampf(float val, float lo, float hi)
+{
+    if (val < lo) return lo;
+    if (val > hi) return hi;
+    return val;
+}
+
+void CorridorCtrl_Init(const CorridorCtrlConfig_t *config)
+{
+    s_cfg = *config;
+    s_initialized = true;
+}
+
+void CorridorCtrl_Compute(const CorridorState_t *state,
+                          const CorridorObs_t *obs,
+                          float imu_wz,
+                          RawCmd_t *out_cmd)
+{
+    if (!s_initialized) {
+        out_cmd->v = 0.0f;
+        out_cmd->w = 0.0f;
+        out_cmd->flags = 0U;
+        return;
+    }
+
+    /* ========================================================
+     * 核心控制律:
+     *   w_cmd = kp_theta * e_th  +  kd_theta * (-imu_wz)  +  kp_y * e_y
+     *
+     * - kp_theta * e_th : 比例项，车头偏了就回转
+     * - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
+     *   用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
+     * - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
+     * ======================================================== */
+
+    float w_cmd = s_cfg.kp_theta * state->e_th
+                + s_cfg.kd_theta * (-imu_wz)
+                + s_cfg.kp_y    * state->e_y;
+
+    /* 角速度限幅：防止 PD 溢出导致原地打转 */
+    w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max);
+
+    /* ========================================================
+     * 线速度策略:
+     *   基础巡航速度 v_cruise，
+     *   当角速度偏大时适当降速（弯道减速），保持运动学协调
+     *   公式: v = v_cruise * (1 - k * |w/w_max|)
+     *   k 取 0.3~0.5 较保守
+     * ======================================================== */
+    float speed_reduction = s_cfg.speed_reduction_k * fabsf(w_cmd) / s_cfg.w_max;
+    float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction);
+
+    /* 线速度限幅：不允许倒车，不允许超速 */
+    v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max);
+
+    /* ========================================================
+     * 置信度降级保护:
+     *   当滤波器健康度 conf 过低（两边雷达全瞎），
+     *   说明走廊参照完全丢失，降低线速度防止盲飞
+     * ======================================================== */
+    if (state->conf < 0.3f) {
+        /* 健康度极低：速度打三折，保持航向惯性滑行 */
+        v_cmd *= 0.3f;
+    } else if (state->conf < 0.6f) {
+        /* 健康度较低（单侧退化）：速度打七折 */
+        v_cmd *= 0.7f;
+    }
+
+    /* 输出赋值 */
+    out_cmd->t_ms  = state->t_ms;
+    out_cmd->v     = v_cmd;
+    out_cmd->w     = w_cmd;
+    out_cmd->flags = 0U;
+}