588 lines
24 KiB
C
588 lines
24 KiB
C
/**
|
||
* @file robot_params.h
|
||
* @brief ARES 机器人全局参数配置中心
|
||
*
|
||
* ============================================
|
||
* 实车调参前必读
|
||
* ============================================
|
||
* 1. 所有长度单位默认为 米 (m)，角度为 弧度 (rad)，速度为 m/s
|
||
* 2. 带"实测"标记的参数必须用卷尺/编码器实际测量后填写
|
||
* 3. 带"调优"标记的参数需要根据实车运行效果从小往大调整
|
||
* 4. 修改本文件后需要重新编译烧录
|
||
*
|
||
* 调参顺序建议:
|
||
* P0: 几何参数 (传感器位置、轮子尺寸) -> 必须实测
|
||
* P1: 里程计参数 -> 实车跑 10 米对比编码器/实际距离
|
||
* P2: 走廊滤波器 -> 从保守值开始逐步调优
|
||
* P3: 控制器增益 -> 实车走廊测试，从低增益开始
|
||
* P4: 安全阈值 -> 根据实际场地微调
|
||
*/
|
||
|
||
#ifndef ROBOT_PARAMS_H
|
||
#define ROBOT_PARAMS_H
|
||
|
||
#include <stdint.h>
|
||
#include <stdbool.h>
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
/* =========================================================
|
||
* 【P0】机械/几何参数 (必须实测，开机前填写!)
|
||
* ========================================================= */
|
||
|
||
/* ------------------- 车体几何 ------------------- */
|
||
/** @brief [实测] 车体外轮廓宽度 [m] (左右方向，含外壳)
|
||
* 测量方法：卷尺测量车体最宽处
|
||
* 典型值：20cm -> 0.20
|
||
* 用途：侧向安全裕量计算，传感器偏移补偿
|
||
*/
|
||
#define PARAM_ROBOT_WIDTH 0.200f
|
||
|
||
/** @brief [实测] 车体外轮廓长度 [m] (前后方向，含外壳)
|
||
* 测量方法：卷尺测量车体最长处
|
||
* 典型值：20cm -> 0.20
|
||
* 用途：前后安全距离补偿
|
||
*/
|
||
#define PARAM_ROBOT_LENGTH 0.200f
|
||
|
||
/** @brief [实测] 驱动轮标称直径 [m]
|
||
* 测量方法：卷尺测量轮胎外径 (非轮毂)
|
||
* 典型值：60mm 轮胎 -> 0.060
|
||
*/
|
||
#define PARAM_WHEEL_DIAMETER 0.080f
|
||
|
||
/** @brief [实测] 左右轮中心距 [m]
|
||
* 测量方法：卷尺测量左右驱动轮中心之间的距离
|
||
* 典型值：车体 20cm 宽，轮胎每边厚 3cm -> 约 0.14m
|
||
*/
|
||
#define PARAM_WHEEL_TRACK 0.130f
|
||
|
||
/** @brief [实测] 同侧前后 VL53L0X 雷达纵向间距 [m]
|
||
* 测量方法：卷尺测量同侧前后两个 VL53L0X 的中心距离
|
||
* 典型值：根据机械安装，约 0.10~0.15m
|
||
* 影响：航向观测精度 z_eth = atan2((d_back-d_front), L_s)
|
||
*/
|
||
#define PARAM_SENSOR_BASE_LENGTH 0.0755f
|
||
|
||
/** @brief [实测] 比赛走廊标准宽度 [m]
|
||
* 测量方法：卷尺测量赛场垄沟实际宽度 (规则 40cm，允许±5% 误差)
|
||
* 典型值：0.40 (但建议实测后填写)
|
||
*/
|
||
#define PARAM_CORRIDOR_WIDTH 0.40f
|
||
|
||
/** @brief [实测] 前向激光雷达安装中心偏置 [m]
|
||
* 测量方法：雷达发射面到车头最前端的距离 (内缩为正)
|
||
* 典型值：根据机械安装，若与车头齐平则为 0
|
||
* 用途：前向到端距离补偿 d_body_front = d_sensor - offset
|
||
* 注意：这里定义的是传感器到车头前缘的距离，不是到几何中心
|
||
*/
|
||
#define PARAM_FRONT_LASER_OFFSET 0.0f
|
||
|
||
/** @brief [实测] 后向激光雷达安装中心偏置 [m]
|
||
* 测量方法：雷达发射面到车尾最后端的距离 (内缩为正)
|
||
* 典型值：根据机械安装，若与车尾齐平则为 0
|
||
* 用途：后向到端距离补偿 d_body_rear = d_sensor - offset
|
||
*/
|
||
#define PARAM_REAR_LASER_OFFSET (-(0.018f))
|
||
|
||
/** @brief [实测] 侧向 VL53L0X 传感器内缩距离 [m]
|
||
* 测量方法：VL53L0X 发射面到车体侧面最外边缘的距离 (向内为正)
|
||
* 典型值：传感器与外壳齐平则为 0，嵌入 1cm 则为 0.01
|
||
* 用途：侧向测距补偿，d_actual_to_wall = d_sensor - sensor_inset
|
||
* 即传感器测到的距离减去这个内缩量才是车体边缘到墙的真实距离
|
||
* ⚠ 极其重要：沟道40cm - 车体20cm = 每边仅10cm，1cm的偏差就是10%的误差！
|
||
* 注意：此值作为未分侧设置时的默认值。建议使用下面的分侧参数。
|
||
*/
|
||
#define PARAM_VL53_SIDE_INSET (-(0.018f))
|
||
|
||
/** @brief [改进A][实测] 左侧 VL53L0X 传感器内缩距离 [m]
|
||
* 测量方法：将机器人精确放在走廊正中央(卷尺确认)，
|
||
* 理论值 = (走廊宽-车宽)/2 = (0.40-0.20)/2 = 0.10m
|
||
* left_inset = 理论值 - 实测左侧前后平均读数
|
||
* 用途：消除左右安装不对称引起的系统性横向偏置(解决"持续偏右/偏左"问题)
|
||
* ⚠ 标定后请填入实测值！当前为 0.0 表示未标定。
|
||
*/
|
||
#define PARAM_VL53_LEFT_INSET 0.0f
|
||
|
||
/** @brief [改进A][实测] 右侧 VL53L0X 传感器内缩距离 [m]
|
||
* 测量方法：同上，right_inset = 理论值 - 实测右侧前后平均读数
|
||
* ⚠ 标定后请填入实测值！当前为 0.0 表示未标定。
|
||
*/
|
||
#define PARAM_VL53_RIGHT_INSET 0.0f
|
||
|
||
/** @brief [调优] 侧向 VL53 近墙退化阈值 [m]
|
||
* 含义：当侧向 VL53 读数小于该值时，认为已进入最小量程附近，数据不再可信。
|
||
* 处理策略：预处理层直接将该侧观测判为无效，让上层退化为主要信另一侧。
|
||
* 典型值：0.05~0.06
|
||
*/
|
||
#define PARAM_VL53_SIDE_SAT_NEAR_M 0.03f
|
||
|
||
/* ------------------- 编码器参数 ------------------- */
|
||
/** @brief [实测] 编码器每转脉冲数 (CPR)
|
||
* 测量方法：查阅 F407 固件配置，或实车转动一圈数脉冲
|
||
* 典型值：500 (F407 默认固件)
|
||
*/
|
||
#define PARAM_ENCODER_CPR 3680
|
||
|
||
/* =========================================================
|
||
* 【P1-EXT】VL53 驱动切换
|
||
* ========================================================= */
|
||
|
||
/** @brief 侧向 ToF 驱动选择
|
||
* 0 = VL53L0X
|
||
* 1 = VL53L1X
|
||
*/
|
||
#ifndef PARAM_VL53_USE_L1X
|
||
#define PARAM_VL53_USE_L1X 1
|
||
#endif
|
||
|
||
/* =========================================================
|
||
* 【P1】里程计参数 (实车标定)
|
||
* ========================================================= */
|
||
|
||
/** @brief [调优] 里程计过程噪声 [m²/s²]
|
||
* 含义：对里程计信任程度，越小越信任
|
||
* 调优方法：实车直线跑 10 米，对比 EKF 输出的 s 与实际距离
|
||
* 典型值：0.01~0.1
|
||
* 过大：EKF 过度依赖里程计，轮滑时误差累积
|
||
* 过小：EKF 不信任里程计，沿程 s 估计不准
|
||
*/
|
||
#define PARAM_ODOM_PROCESS_NOISE 0.1f
|
||
|
||
/** @brief [调优] 里程计速度低通滤波系数
|
||
* 含义：alpha=0.8 表示新采样占 20%，历史值占 80%
|
||
* 典型值：0.7~0.9
|
||
*/
|
||
#define PARAM_ODOM_LOWPASS_ALPHA 0.8f
|
||
|
||
/* =========================================================
|
||
* 【P2】走廊 EKF 滤波器参数 (调优)
|
||
* ========================================================= */
|
||
|
||
/* --- EKF 过程噪声 Q (对模型信任度) --- */
|
||
/** @brief [调优] 横向偏差 e_y 过程噪声方差 [m²]
|
||
* 含义：横向状态的自然发散速度，越小表示横向越稳定
|
||
* 调优方法：从 0.001 开始，逐步增大直到走廊跟踪平滑
|
||
* 典型值：0.001~0.02
|
||
* 过大：EKF 认为横向很不稳定，过度依赖观测 -> 震荡
|
||
* 过小：EKF 不信任观测，响应慢
|
||
*/
|
||
#define PARAM_EKF_Q_EY 0.02f
|
||
|
||
/** @brief [调优] 航向偏差 e_th 过程噪声方差 [rad²]
|
||
* 含义：航向状态的自然发散速度
|
||
* 调优方法：IMU 短时漂移小，可以设很小
|
||
* 典型值：0.0001~0.005
|
||
* 过大：EKF 认为航向不稳定，IMU 作用被削弱
|
||
* 过小：EKF 过度信任 IMU，雷达观测不起作用
|
||
*/
|
||
#define PARAM_EKF_Q_ETH 0.002f
|
||
|
||
/** @brief [调优] 沿程进度 s 过程噪声方差 [m²]
|
||
* 含义：里程估计的不确定度
|
||
* 调优方法：地毯轮滑严重时调大
|
||
* 典型值：0.05~0.2
|
||
*/
|
||
#define PARAM_EKF_Q_S 0.1f
|
||
|
||
/** @brief [改进] 横向-航向耦合过程噪声 [m·rad]
|
||
* 含义：e_y 和 e_th 的动力学耦合强度
|
||
* 物理意义：横向偏差会导致航向修正，航向偏差会导致横向漂移
|
||
* 调优方法：从 0 开始，如果发现横向和航向震荡相关可适当增大
|
||
* 典型值：0~0.005
|
||
* 设为 0 则退化为对角 Q 矩阵
|
||
*/
|
||
#define PARAM_EKF_Q_EY_ETH 0.0f
|
||
|
||
/* --- EKF 观测噪声 R (对传感器信任度) --- */
|
||
/** @brief [调优] 横向观测噪声方差 [m²]
|
||
* 含义：VL53L0X 测距的可靠程度
|
||
* [改进B] 从 0.002 提高到 0.015，适配 VL53 可信度不高的实际表现
|
||
* 调优方法：VL53 噪声越大，此值越大
|
||
* 典型值：0.005~0.02
|
||
* 过大：EKF 不信任侧向雷达，横向响应慢
|
||
* 过小：EKF 过度信任雷达，对跳变敏感
|
||
*/
|
||
#define PARAM_EKF_R_EY 0.007f
|
||
|
||
/** @brief [调优] 航向观测噪声方差 [rad²]
|
||
* 含义：同侧前后雷达差分测角的可程靠度
|
||
* 调优方法：从 0.001 开始
|
||
* 典型值：0.0005~0.003
|
||
*/
|
||
#define PARAM_EKF_R_ETH 0.001f
|
||
|
||
/** @brief [调优] IMU 航向观测噪声方差 [rad²]
|
||
* 含义：IMU yaw 作为 EKF 航向观测时的噪声
|
||
* [改进C] 从 0.01 降低到 0.002，充分利用 IMU yaw 的高精度。
|
||
* IMU 是航向 e_th 的唯一观测源（侧墙航向已取消），R 值应小。
|
||
* 调优方法：IMU yaw 越稳定，此值可越小
|
||
* 典型值：0.001~0.01
|
||
* 过大：IMU 航向观测作用弱，e_th 靠 wz 积分漂移
|
||
* 过小：IMU 航向主导过强，短时抖动可能传入
|
||
*
|
||
* [修正] 如果场地本身是歪的，过度信任IMU会导致车持续擦墙
|
||
* 增加到0.01，让EKF更多依赖VL53传感器判断相对航向
|
||
*/
|
||
#define PARAM_EKF_R_ETH_IMU 0.01f
|
||
|
||
/* --- EKF 初始状态 --- */
|
||
/** @brief [调优] e_y 初始不确定度 [m²]
|
||
* 含义：开机时横向位置的先验不确定度
|
||
* 典型值：0.01~0.1 (对应 10~30cm 不确定)
|
||
*/
|
||
#define PARAM_EKF_P0_EY 0.1f
|
||
|
||
/** @brief [调优] e_th 初始不确定度 [rad²]
|
||
* 含义：开机时航向的先验不确定度
|
||
* 典型值：0.05~0.2 (对应 13°~26°不确定)
|
||
*/
|
||
#define PARAM_EKF_P0_ETH 0.1f
|
||
|
||
/* --- χ² 检验门限 (鲁棒拒绝) --- */
|
||
/** @brief [固定] 1 自由度 χ² 检验门限 (95% 置信度)
|
||
* 含义：单路观测的马氏距离超过此值则拒绝
|
||
* 理论值：3.84 (95%), 6.63 (99%)
|
||
* 建议：不要修改，除非传感器质量极差
|
||
*/
|
||
#define PARAM_CHI2_1DOF 3.84f
|
||
|
||
/** @brief [固定] 2 自由度 χ² 检验门限 (95% 置信度)
|
||
* 理论值：5.99
|
||
*/
|
||
#define PARAM_CHI2_2DOF 5.99f
|
||
|
||
/* =========================================================
|
||
* 【P3】走廊控制器参数 (实车调优)
|
||
* ========================================================= */
|
||
|
||
/** @brief [调优] 航向比例增益 kp_theta [1/s]
|
||
* 含义：航向偏差 1 rad 时产生的角速度
|
||
* 调优方法：从 0.5 开始逐步增加，直到走廊跟踪响应快但不震荡
|
||
* 典型值：1.0~3.0
|
||
* 过大：车头左右摆动 (震荡)
|
||
* 过小：纠偏太慢，容易撞墙
|
||
*/
|
||
#define PARAM_CTRL_KP_THETA 2.5f /* 适度增加，提高航向纠偏速度 */
|
||
|
||
/** @brief [调优] 航向微分增益 kd_theta [s]
|
||
* 含义：IMU 角速度阻尼项，抑制车头转动速度
|
||
* 调优方法：从 0.05 开始
|
||
* 典型值：0.05~0.2
|
||
* 过大：车头转动被过度抑制，响应迟钝
|
||
* 过小：阻尼不足，容易超调震荡
|
||
*/
|
||
#define PARAM_CTRL_KD_THETA 0.4f
|
||
|
||
/** @brief [调优] 横向比例增益 kp_y [1/(m·s)]
|
||
* 含义：横向偏差 1m 时产生的角速度修正
|
||
* 调优方法：从 1.0 开始
|
||
* 典型值：1.5~4.0
|
||
* 过大：横向纠偏过猛，引起震荡
|
||
* 过小：偏了拉不回来
|
||
*/
|
||
#define PARAM_CTRL_KP_Y 7.0f /* 提高横向回中速度，适配更高巡航速度 */
|
||
|
||
/** @brief [调优] 走廊巡航速度 [m/s]
|
||
* 含义：走廊内正常行驶速度
|
||
* 调优方法：从 0.1 m/s 开始逐步增加
|
||
* 典型值：0.15~0.4 m/s
|
||
* 注意：速度越高，对滤波器带宽和控制器增益要求越高
|
||
*/
|
||
#define PARAM_CTRL_V_CRUISE 0.15f
|
||
|
||
/** @brief [调优] 角速度输出限幅 [rad/s]
|
||
* 含义：最大允许转角速度
|
||
* 典型值：1.0~2.0 (约 57°/s ~ 115°/s)
|
||
*/
|
||
#define PARAM_CTRL_W_MAX 1.5f
|
||
|
||
/** @brief [调优] 线速度输出限幅 [m/s]
|
||
* 含义：最大允许线速度
|
||
* 典型值：0.3~0.5
|
||
*/
|
||
#define PARAM_CTRL_V_MAX 0.3f
|
||
|
||
/** @brief [调优] 弯道减速系数 [0~1]
|
||
* 含义：角速度大时线速度打折程度
|
||
* 公式：v = v_cruise * (1 - k * |w/w_max|)
|
||
* 典型值：0.3~0.5
|
||
*/
|
||
#define PARAM_CTRL_SPEED_REDUCTION 0.4f
|
||
|
||
/** @brief [调优] 弯道减速死区 [0~1]
|
||
* 含义：当 |w|/w_max 小于该比例时，不做减速。
|
||
* 目的：小幅修正时保持巡航，不要长期因为轻微纠偏而慢速运行。
|
||
*/
|
||
#define PARAM_CTRL_SPEED_RED_DB 0.35f
|
||
|
||
/** @brief [调优] 角速度变化率限幅 [rad/s^2]
|
||
* 含义：限制相邻控制周期之间角速度变化，防止一帧突然猛打方向。
|
||
*/
|
||
#define PARAM_CTRL_W_SLEW_RATE 6.0f
|
||
|
||
/** @brief [调优] 入沟软启动距离 [m]
|
||
* 含义：进入沟内后的前一小段距离，降低回中力度，避免一入沟就猛打一把。
|
||
*/
|
||
#define PARAM_CTRL_STARTUP_DIST 0.15f
|
||
|
||
/** @brief [调优] 入沟起始横向增益缩放 [0~1]
|
||
* 含义：state->s=0 时 kp_y 的缩放比例，随后随距离线性恢复到 1。
|
||
*/
|
||
#define PARAM_CTRL_STARTUP_KPY_SCALE 0.45f
|
||
|
||
/** @brief [调优] 入沟起始角速度限幅缩放 [0~1]
|
||
* 含义：state->s=0 时 w_max 的缩放比例，随后随距离线性恢复到 1。
|
||
*/
|
||
#define PARAM_CTRL_STARTUP_W_SCALE 0.45f
|
||
|
||
/** @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】安全阈值与状态机参数
|
||
* ========================================================= */
|
||
|
||
/* --- 到端检测 --- */
|
||
/** @brief [调优] 前向停车距离阈值 [m]
|
||
* 含义：前向雷达 (传感器到墙的距离) 低于此值强制停车
|
||
* 计算方法：PARAM_FRONT_LASER_OFFSET (传感器内缩) + 安全裕量 (2~5cm)
|
||
* 示例：传感器与车头齐平(offset=0) + 3cm安全裕量 = 0.03m
|
||
* 传感器内缩5cm(offset=0.05) + 3cm安全裕量 = 0.08m
|
||
* ⚠ 此阈值应 >= PARAM_FRONT_LASER_OFFSET，否则传感器到墙的距离
|
||
* 还没到阈值，车体前端就已经撞墙了！
|
||
* 过小：可能撞墙
|
||
* 过大：离墙很远就停车，走不完走廊
|
||
*/
|
||
#define PARAM_SAFE_D_FRONT_STOP 0.10f
|
||
|
||
/** @brief [调优] 前向减速预警距离 [m]
|
||
* 含义：前向雷达低于此值开始线性减速
|
||
* 典型值：0.20~0.40
|
||
*/
|
||
#define PARAM_SAFE_D_FRONT_APPROACH 0.25f
|
||
|
||
/** @brief [调优] 减速区最低速度 [m/s]
|
||
* 含义：到端前爬行速度
|
||
* 典型值：0.03~0.08
|
||
*/
|
||
#define PARAM_SAFE_APPROACH_MIN_V 0.05f
|
||
|
||
/* --- 置信度降级 --- */
|
||
/** @brief [调优] E-Stop 置信度阈值
|
||
* 含义：EKF 置信度低于此值触发紧急停车
|
||
* 典型值：0.05~0.15
|
||
*/
|
||
#define PARAM_SAFE_CONF_ESTOP 0.1f
|
||
|
||
/** @brief [调优] 单侧退化置信度
|
||
* 含义：单侧 VL53L0X 失效时，置信度降为多少
|
||
* 典型值：0.6~0.8
|
||
*/
|
||
#define PARAM_SAFE_CONF_DEGRADED 0.7f
|
||
|
||
/* --- 入口/退出 --- */
|
||
/** @brief [调优] 入口对准速度 [m/s]
|
||
* 含义：从启动区进入走廊的慢速对准速度
|
||
* 典型值：0.05~0.10
|
||
*/
|
||
#define PARAM_SCRIPT_ENTRY_V 0.08f
|
||
|
||
/** @brief [调优] 入口对准超时 [ms]
|
||
* 含义：超过此时间强制进入走廊模式 (保护)
|
||
* 典型值：20000~40000 (20~40 秒)
|
||
*/
|
||
#define PARAM_SCRIPT_ENTRY_TIMEOUT 30000U
|
||
|
||
/** @brief [调优] 原地转向角速度 [rad/s]
|
||
* 含义：到端 180 度转向的角速度
|
||
* 典型值：0.8~1.5
|
||
*/
|
||
#define PARAM_SCRIPT_TURN_OMEGA 1.0f
|
||
|
||
/** @brief [调优] 退出场地后冲距离 [m]
|
||
* 含义：检测到离开垄沟后再往前冲的距离
|
||
* 典型值：1.5~2.5
|
||
*/
|
||
#define PARAM_SCRIPT_EXIT_RUNOUT 2.0f
|
||
|
||
/** @brief [调优] 退出场地直线冲出速度 [m/s]
|
||
* 含义：检测到离开垄沟后直线冲出的速度
|
||
* 典型值：0.3~0.5
|
||
* 注意：与 turn_omega (转向角速度) 无关，两者独立
|
||
*/
|
||
#define PARAM_SCRIPT_EXIT_V 0.3f
|
||
|
||
/* =========================================================
|
||
* 【P5】传感器驱动参数
|
||
* ========================================================= */
|
||
|
||
/* --- VL53L0X --- */
|
||
/** @brief [实测] VL53L0X 测距预算 [μs]
|
||
* 含义：单次测距的时间预算，越大精度越高但频率越低
|
||
* 选项：20000(20ms, 高速), 33000(33ms), 100000(100ms), 200000(200ms, 高精度)
|
||
* 典型值：33000 (33ms，折中)
|
||
* 调优：本车实测稳定上限按 33ms 配置，若噪声偏大再逐步提高
|
||
*/
|
||
#define PARAM_VL53_TIMING_BUDGET 33000U
|
||
|
||
/** @brief [调试] 是否启用 VL53L0X 端EMA滤波
|
||
* 1: 使用滤波后的 range_mm_filtered
|
||
* 0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比
|
||
*/
|
||
#define PARAM_VL53_USE_EMA_FILTER 0
|
||
|
||
/** @brief [调优] VL53L0X EMA滤波平滑系数 alpha
|
||
* 含义：新测量值的权重 (0.0~1.0)
|
||
* - 越大响应越快，延迟越低，但抖动越大
|
||
* - 越小输出越平滑，但延迟越高
|
||
* 典型值：
|
||
* 0.3 - 平滑优先 (延迟约100ms)
|
||
* 0.4 - 折中 (延迟约60ms，推荐)
|
||
* 0.5 - 快速响应 (延迟约40ms)
|
||
* 0.6 - 极速响应 (延迟约25ms，抖动较大)
|
||
*/
|
||
#define PARAM_VL53_EMA_ALPHA 0.6f
|
||
|
||
/* --- IMU --- */
|
||
/** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad]
|
||
* 含义：IMU 零位与车头方向的偏差
|
||
* 测量：车头对准正北，读取 IMU 航向作为偏置
|
||
* 典型值：需要通过校准确定
|
||
*/
|
||
#define PARAM_IMU_YAW_OFFSET 0.0f
|
||
|
||
/* =========================================================
|
||
* 【P6】赛道级导航参数 (混合导航方案)
|
||
* ========================================================= */
|
||
|
||
/** @brief 编译开关：1=赛道模式(6沟S型遍历) 0=单沟测试模式(原nav_script) */
|
||
#define USE_GLOBAL_NAV 1
|
||
|
||
/** @brief 上电后导航启动延迟 [ms]
|
||
* 用于等待 IMU 清零、VL53 连续测距和前后激光任务稳定，
|
||
* 避免刚上电时用到脏数据导致错误入场或异常转向。
|
||
*/
|
||
#define PARAM_NAV_STARTUP_DELAY_MS 5000U
|
||
|
||
/** @brief 导航主循环周期 [ms]
|
||
* 保留为参数形式，当前使用 20ms，与 411 稳定版本一致。
|
||
*/
|
||
#define PARAM_NAV_TASK_PERIOD_MS 20U
|
||
|
||
/* --- 入场段 --- */
|
||
/* 启动区入口(Y=40)距第一条垄沟(Y=36~39)极近，入场距离仅约 10~40cm */
|
||
#define PARAM_GNAV_ENTRY_V 0.08f /* m/s — 入场速度 (沿左端纵向通道向北) */
|
||
#define PARAM_GNAV_ENTRY_DISTANCE 0.30f /* m — 入场里程上限 (启动区到C1入口约10~30cm) */
|
||
#define PARAM_GNAV_ENTRY_TIMEOUT 10000U /* ms — 入场超时 */
|
||
|
||
/* --- 转向 --- */
|
||
#define PARAM_GNAV_TURN_OMEGA 1.0f /* 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 — 单次转向超时 */
|
||
|
||
/* --- 重捕获 --- */
|
||
#define PARAM_GNAV_REACQUIRE_V 0.1f /* m/s — 重捕获入沟速度 */
|
||
#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.06f /* 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.38f /* 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.70f /* m — 连接段标称距离 (沟间距中心到中心) */
|
||
#define PARAM_GNAV_LINK_TIMEOUT 8000U /* ms — 连接段超时 */
|
||
#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 /* — 预留给其他通道状态，连接段本身不再使用融合 */
|
||
|
||
/* --- 出场 --- */
|
||
/* C6完成后在左端通道左转朝南，沿纵向通道南行回到入口，距离约390cm */
|
||
#define PARAM_GNAV_EXIT_V 0.15f /* m/s — 出场速度 (沿左端通道向南) */
|
||
#define PARAM_GNAV_EXIT_RUNOUT 1.50f /* m — 出场侧向丢失后冲刺距离 */
|
||
#define PARAM_GNAV_EXIT_MAX_DIST 4.50f /* m — 出场里程保护 (通道全长约3.9m) */
|
||
#define PARAM_GNAV_EXIT_TIMEOUT 30000U /* ms — 出场超时 (距离长，给足时间) */
|
||
|
||
/* --- 回停 --- */
|
||
#define PARAM_GNAV_DOCK_V 0.05f /* m/s — 回停速度 */
|
||
#define PARAM_GNAV_DOCK_DISTANCE 0.50f /* m — 回停推进距离 */
|
||
|
||
/* --- 航向保持 --- */
|
||
#define PARAM_GNAV_HEADING_KP 0.03f /* — — 航向保持P增益 (输入°，输出rad/s) */
|
||
|
||
/* =========================================================
|
||
* 宏工具函数
|
||
* ========================================================= */
|
||
#define PARAM_RAD2DEG(rad) ((rad) * 57.2957795131f)
|
||
#define PARAM_DEG2RAD(deg) ((deg) * 0.01745329252f)
|
||
|
||
/* =========================================================
|
||
* 快速参考表
|
||
* =========================================================
|
||
*
|
||
* 调参流程:
|
||
* 1. 实测几何参数 (P0 组) -> 卷尺测量
|
||
* 2. 里程计标定 (P1 组) -> 跑 10 米对比
|
||
* 3. 原地测试控制器 -> 只调 kp_theta, kd_theta
|
||
* 4. 走廊低速测试 -> 调 kp_y, v_cruise
|
||
* 5. 逐步提速 -> 微调各增益
|
||
* 6. 安全阈值 -> 根据实际场地
|
||
*
|
||
* 常见问题诊断:
|
||
* 车头左右摆 -> 减小 kp_theta 或增大 kd_theta
|
||
* 横向纠偏慢 -> 增大 kp_y
|
||
* 到端刹车不住 -> 减小 d_front_stop 或增大 approach 距离
|
||
* 出口冲不出 -> 减小 exit_runout 或增大 v_cruise
|
||
* EKF 发散 -> 增大 Q 或减小 R
|
||
*
|
||
* =========================================================
|
||
*/
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
#endif /* ROBOT_PARAMS_H */
|