/** * @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 #include #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.140f /** @brief [实测] 同侧前后 VL53L0X 雷达纵向间距 [m] * 测量方法：卷尺测量同侧前后两个 VL53L0X 的中心距离 * 典型值：根据机械安装，约 0.10~0.15m * 影响：航向观测精度 z_eth = atan2((d_back-d_front), L_s) */ #define PARAM_SENSOR_BASE_LENGTH 0.120f /** @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.0f /** @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.0f /* ------------------- 编码器参数 ------------------- */ /** @brief [实测] 编码器每转脉冲数 (CPR) * 测量方法：查阅 F407 固件配置，或实车转动一圈数脉冲 * 典型值：500 (F407 默认固件) */ #define PARAM_ENCODER_CPR 500 /* ========================================================= * 【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.01f /** @brief [调优] 航向偏差 e_th 过程噪声方差 [rad²] * 含义：航向状态的自然发散速度 * 调优方法：IMU 短时漂移小，可以设很小 * 典型值：0.0001~0.005 * 过大：EKF 认为航向不稳定，IMU 作用被削弱 * 过小：EKF 过度信任 IMU，雷达观测不起作用 */ #define PARAM_EKF_Q_ETH 0.001f /** @brief [调优] 沿程进度 s 过程噪声方差 [m²] * 含义：里程估计的不确定度 * 调优方法：地毯轮滑严重时调大 * 典型值：0.05~0.2 */ #define PARAM_EKF_Q_S 0.1f /* --- EKF 观测噪声 R (对传感器信任度) --- */ /** @brief [调优] 横向观测噪声方差 [m²] * 含义：VL53L0X 测距的可靠程度 * 调优方法：从 0.001 开始，根据走廊跟踪平滑度调整 * 典型值：0.001~0.005 * 过大：EKF 不信任侧向雷达，横向响应慢 * 过小：EKF 过度信任雷达，对跳变敏感 */ #define PARAM_EKF_R_EY 0.002f /** @brief [调优] 航向观测噪声方差 [rad²] * 含义：同侧前后雷达差分测角的可程靠度 * 调优方法：从 0.001 开始 * 典型值：0.0005~0.003 */ #define PARAM_EKF_R_ETH 0.001f /** @brief [调优] IMU 航向观测噪声方差 [rad²] * 含义：IMU yaw 作为 EKF 航向观测时的噪声 * 设计意图：IMU yaw 用于长时航向约束 (防止 wz 积分漂移)， * R 值应明显大于侧墙航向观测 R_ETH，避免干扰侧墙短时精确修正 * 调优方法：从 0.01 开始，如果 IMU yaw 非常稳定可减小 * 典型值：0.005~0.05 * 过大：IMU 航向观测作用弱，等于没接入 * 过小：IMU 航向主导，侧墙观测被压制 */ #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.0f /** @brief [调优] 航向微分增益 kd_theta [s] * 含义：IMU 角速度阻尼项，抑制车头转动速度 * 调优方法：从 0.05 开始 * 典型值：0.05~0.2 * 过大：车头转动被过度抑制，响应迟钝 * 过小：阻尼不足，容易超调震荡 */ #define PARAM_CTRL_KD_THETA 0.1f /** @brief [调优] 横向比例增益 kp_y [1/(m·s)] * 含义：横向偏差 1m 时产生的角速度修正 * 调优方法：从 1.0 开始 * 典型值：1.5~4.0 * 过大：横向纠偏过猛，引起震荡 * 过小：偏了拉不回来 */ #define PARAM_CTRL_KP_Y 3.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 /* ========================================================= * 【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.08f /** @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, 高精度) * 典型值：100000 (100ms，折中) * 调优：走廊跟踪时用 100ms，原地转向时可切 200ms 提高精度 */ #define PARAM_VL53_TIMING_BUDGET 100000U /** @brief [调优] VL53L0X 卡尔曼滤波参数 Q * 含义：测距过程噪声 * 典型值：5.0~20.0 */ #define PARAM_VL53_KALMAN_Q 10.0f /** @brief [调优] VL53L0X 卡尔曼滤波参数 R * 含义：测距观测噪声 * 典型值：10.0~30.0 */ #define PARAM_VL53_KALMAN_R 14.1f /* --- IMU --- */ /** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad] * 含义：IMU 零位与车头方向的偏差 * 测量：车头对准正北，读取 IMU 航向作为偏置 * 典型值：需要通过校准确定 */ #define PARAM_IMU_YAW_OFFSET 0.0f /* ========================================================= * 宏工具函数 * ========================================================= */ #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 */