1.0

App/robot_params.h

@@ -162,7 +162,7 @@ extern "C" {
  *  过大：EKF 认为横向很不稳定，过度依赖观测 -> 震荡
  *  过小：EKF 不信任观测，响应慢
  */
-#define PARAM_EKF_Q_EY              0.01f
+#define PARAM_EKF_Q_EY              0.02f
 
 /** @brief [调优] 航向偏差 e_th 过程噪声方差 [rad²]
  *  含义：航向状态的自然发散速度
@@ -171,7 +171,7 @@ extern "C" {
  *  过大：EKF 认为航向不稳定，IMU 作用被削弱
  *  过小：EKF 过度信任 IMU，雷达观测不起作用
  */
-#define PARAM_EKF_Q_ETH             0.001f
+#define PARAM_EKF_Q_ETH             0.002f
 
 /** @brief [调优] 沿程进度 s 过程噪声方差 [m²]
  *  含义：里程估计的不确定度
@@ -198,7 +198,7 @@ extern "C" {
  *  过大：EKF 不信任侧向雷达，横向响应慢
  *  过小：EKF 过度信任雷达，对跳变敏感
  */
-#define PARAM_EKF_R_EY              0.015f
+#define PARAM_EKF_R_EY              0.007f
 
 /** @brief [调优] 航向观测噪声方差 [rad²]
  *  含义：同侧前后雷达差分测角的可程靠度
@@ -215,8 +215,11 @@ extern "C" {
  *  典型值：0.001~0.01
  *  过大：IMU 航向观测作用弱，e_th 靠 wz 积分漂移
  *  过小：IMU 航向主导过强，短时抖动可能传入
+ *  
+ *  [修正] 如果场地本身是歪的，过度信任IMU会导致车持续擦墙
+ *  增加到0.01，让EKF更多依赖VL53传感器判断相对航向
  */
-#define PARAM_EKF_R_ETH_IMU         0.002f
+#define PARAM_EKF_R_ETH_IMU         0.01f
 
 /* --- EKF 初始状态 --- */
 /** @brief [调优] e_y 初始不确定度 [m²]
@@ -255,7 +258,7 @@ extern "C" {
  *  过大：车头左右摆动 (震荡)
  *  过小：纠偏太慢，容易撞墙
  */
-#define PARAM_CTRL_KP_THETA         2.0f
+#define PARAM_CTRL_KP_THETA         2.5f  /* 适度增加，提高航向纠偏速度 */
 
 /** @brief [调优] 航向微分增益 kd_theta [s]
  *  含义：IMU 角速度阻尼项，抑制车头转动速度
@@ -273,7 +276,7 @@ extern "C" {
  *  过大：横向纠偏过猛，引起震荡
  *  过小：偏了拉不回来
  */
-#define PARAM_CTRL_KP_Y             4.0f
+#define PARAM_CTRL_KP_Y             7.0f  /* 提高横向回中速度，适配更高巡航速度 */
 
 /** @brief [调优] 走廊巡航速度 [m/s]
  *  含义：走廊内正常行驶速度
@@ -302,6 +305,30 @@ extern "C" {
  */
 #define PARAM_CTRL_SPEED_REDUCTION  0.4f
 
+/** @brief [调优] 出沟检测距离 [m]
+ *  含义：前激光小于此值时禁用左右激光控制，避免出沟时数据突变导致大幅转向
+ *  典型值：0.40 (40cm)
+ *  过大：提前禁用控制，可能导致出沟前偏离
+ *  过小：禁用太晚，出沟时仍可能受左右激光突变影响
+ */
+#define PARAM_CTRL_EXIT_FRONT_DIST  0.40f
+
+/** @brief [调优] 近墙脱离阈值 [m]
+ *  含义：任一侧平均距离小于此值时，直接叠加远离墙面的保护转向
+ *  目的：不等 EKF 慢慢回中，先把车从擦墙状态拉开
+ */
+#define PARAM_CTRL_WALL_ESCAPE_DIST 0.05f
+
+/** @brief [调优] 近墙脱离增益 [rad/s per m]
+ *  含义：近墙保护的附加角速度增益
+ */
+#define PARAM_CTRL_WALL_ESCAPE_KP   6.0f
+
+/** @brief [调优] 近墙脱离角速度限幅 [rad/s]
+ *  含义：近墙保护项自身的最大角速度，避免一把打太猛
+ */
+#define PARAM_CTRL_WALL_ESCAPE_WMAX 0.25f
+
 /* =========================================================
  * 【P4】安全阈值与状态机参数
  * ========================================================= */
@@ -393,7 +420,7 @@ extern "C" {
  *  1: 使用滤波后的 range_mm_filtered
  *  0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比
  */
-#define PARAM_VL53_USE_EMA_FILTER 1
+#define PARAM_VL53_USE_EMA_FILTER 0
 
 /** @brief [调优] VL53L0X EMA滤波平滑系数 alpha
  *  含义：新测量值的权重 (0.0~1.0)
@@ -405,7 +432,7 @@ extern "C" {
  *    0.5 - 快速响应 (延迟约40ms)
  *    0.6 - 极速响应 (延迟约25ms，抖动较大)
  */
-#define PARAM_VL53_EMA_ALPHA        0.4f
+#define PARAM_VL53_EMA_ALPHA        0.6f
 
 /* --- IMU --- */
 /** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad]
@@ -443,20 +470,34 @@ extern "C" {
 
 /* --- 重捕获 --- */
 #define PARAM_GNAV_REACQUIRE_V        0.1f   /* m/s   — 重捕获入沟速度 */
-#define PARAM_GNAV_REACQUIRE_CONF     0.6f    /*        — 重捕获置信度阈值 */
-#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f  /* m     — 走廊宽度容差 */
-#define PARAM_GNAV_REACQUIRE_TICKS    10     /* 拍    — 连续确认次数 (5×20ms=100ms) */
+#define PARAM_GNAV_REACQUIRE_CONF     0.4f    /*        — 重捕获置信度阈值（从0.6降到0.4，更容易成功） */
+#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.08f  /* m     — 走廊宽度容差（从5cm放宽到8cm） */
+#define PARAM_GNAV_REACQUIRE_MIN_ODOM 0.12f   /* m     — 最小入沟里程，避免沟口过早假成功 */
+#define PARAM_GNAV_REACQUIRE_TICKS    5      /* 拍    — 连续确认次数，取更稳的 100ms */
 #define PARAM_GNAV_REACQUIRE_TIMEOUT  5000U   /* ms    — 重捕获超时 */
 
+/* --- 对齐 --- */
+#define PARAM_GNAV_ALIGN_KP_TH        2.0f    /*        — 对齐航向P增益 (rad/s per rad) */
+#define PARAM_GNAV_ALIGN_KP_Y         4.0f    /*        — 对齐横向P增益 (rad/s per m) */
+#define PARAM_GNAV_ALIGN_TH_TOL       0.05f   /* rad    — 对齐航向容差 (~3°) */
+#define PARAM_GNAV_ALIGN_Y_TOL        0.02f   /* m      — 对齐横向容差 (2cm) */
+#define PARAM_GNAV_ALIGN_TICKS        5       /* 拍    — 对齐确认次数 (5×20ms=100ms) */
+#define PARAM_GNAV_ALIGN_TIMEOUT      3000U   /* ms    — 对齐超时 */
+#define PARAM_GNAV_REACQUIRE_MIN_BACK 0.40f   /* m     — 重捕获最小后激光距离，判断是否真正进沟 */
+
 /* --- 沟内 --- */
-#define PARAM_GNAV_CORRIDOR_MAX_LEN   2.50f   /* m     — 沟内里程保护上限 */
-#define PARAM_GNAV_CORRIDOR_END_DIST  0.05f   /* m     — 到端检测距离 */
+#define PARAM_GNAV_CORRIDOR_MAX_LEN   2.70f   /* m     — 沟内里程保护上限 */
+#define PARAM_GNAV_CORRIDOR_END_DIST  0.10f   /* m     — 到端检测距离 */
 
 /* --- 连接段 --- */
 /* 在纵向端部通道里北行，从一条垄沟入口到下一条入口，距离=垄沟宽40cm+垄背宽30cm=70cm */
 #define PARAM_GNAV_LINK_V             0.10f   /* m/s   — 连接段速度 (纵向通道内，IMU航向保持) */
-#define PARAM_GNAV_LINK_DISTANCE      0.75f   /* m     — 连接段标称距离 (沟间距) */
+#define PARAM_GNAV_LINK_DISTANCE      0.90f   /* m     — 连接段标称距离 (沟间距) */
 #define PARAM_GNAV_LINK_TIMEOUT       8000U   /* ms    — 连接段超时 */
+#define PARAM_GNAV_LINK_GAP_RUNOUT    0.06f   /* m     — 看到下一个沟口后继续前冲距离 */
+#define PARAM_GNAV_LINK_WALL_TARGET   0.10f   /* m     — 连接段最小离墙距离（小于10cm才触发保底修正） */
+#define PARAM_GNAV_LINK_WALL_KP       3.0f    /*        — 连接段离墙保底增益 */
+#define PARAM_GNAV_LINK_WALL_BLEND    0.5f    /*        — 预留给其他通道状态，连接段本身不再使用融合 */
 
 /* --- 出场 --- */
 /* C6完成后在左端通道左转朝南，沿纵向通道南行回到入口，距离约390cm */