龙芯2k0300 - 智能车走马观碑组控制算法介绍
本文对应 car_project 当前控制实现，用于单独记录控制链路、调试顺序和关键参数含义。
一、控制算法结构
当前控制器采用左右轮独立速度闭环。核心目标是让左右轮分别跟踪自己的目标速度，控制链路直接围绕左右轮目标 RPS、实际 RPS 和左右电机占空比展开。
控制链路分为视觉线程和控制线程：
| 线程 | 链路 | 说明 |
|---|---|---|
| 视觉线程 | 巡线误差/道路状态 -> 视觉外环和速度决策 -> ControlSetpoint |
只生成控制目标，不直接写电机 |
| 控制线程 | 停车门控 -> yaw rate 闭环 -> 差速运动学 -> 左右速度闭环 -> 左右电机占空比 |
门控放行后才计算电机输出 |
应用层停车门控优先级高于控制器。drive_enabled=false 或 obstacle_stop=true 时，控制线程直接输出左右电机 0%，并在进入停车门控的边沿重置控制器积分状态。控制器只负责门控放行后的目标速度、目标角速度和左右轮占空比计算。
重点代码和配置文件：
| 文件 | 作用 |
|---|---|
core/src/controller.cpp |
控制器主逻辑，包括视觉外环、角速度修正、差速目标和左右轮速度闭环 |
core/inc/controller.h |
控制器配置、目标和调试数据结构 |
config/system.yaml |
车辆几何参数和硬件设备路径 |
config/tuning.yaml |
速度、转向、滤波和道路状态相关调参项 |
车辆几何参数位于 config/system.yaml：
vehicle:
wheel_track_m: 0.155
encoder_gear_diameter_m: 0.018
其中：
wheel_track_m：左右轮中心距，用于把角速度换算成左右速度差；encoder_gear_diameter_m：编码器测速齿轮直径。当前编码器RPS是测速齿轮RPS，不是车轮RPS。
调试时重点观察 left_target_rps、right_target_rps、实际左右编码器 RPS 和最终左右电机占空比。
编码器驱动返回的是 rps_milli 固定点测速值，单位为 0.001 RPS。用户态 Encoder 会转换成小数 RPS 后再进入 DataFusion、控制器和上位机显示。因此当前调参应直接看小数 left_rps/right_rps，不要再按整数 RPS 估算速度环响应。
上位机关键字段：
| 类别 | 字段 | 作用 |
|---|---|---|
| 巡线 | tracking_error、tracking_raw_error |
判断视觉误差是否稳定，区分图像问题和控制问题 |
| 目标速度 | target_speed_mps、target_speed_rps |
观察道路状态和速度斜率限制后的中心目标速度 |
| 左右轮目标 | left_target_rps、right_target_rps |
差速运动学输出，速度环真正要跟踪的目标 |
| 左右轮实际 | left_rps、right_rps |
编码器反馈，判断左右速度环是否跟上目标 |
| 角速度 | target_yaw_rate、yaw_rate_command、actual_yaw_rate |
判断外环目标、闭环修正和实际转向能力 |
| 输出 | duty_left、duty_right |
最终电机占空比，判断是否饱和或左右差异异常 |
二、分阶段调试指南
本章节按实际调车顺序组织。学生调试时不要跳阶段：前一阶段没有通过，后一阶段看到的问题通常没有参考价值。
下面公式统一使用：
dt：本次控制周期，单位 s；clamp(x, min, max)：把x限制到[min, max]；I：积分项，代码中会做积分限幅和抗积分饱和；k：当前控制周期，k-1表示上一周期。
当前代码对周期做了夹紧，避免偶发调度抖动把微分和积分放大：
- 外环
dt限制在[0.001, 0.2]秒； - 内环
dt限制在[0.001, 0.1]秒。
2.0 调试前准备
先把安全和记录条件准备好，否则后面的参数判断容易失真。
| 准备项 | 要求 |
|---|---|
| 车辆安全 | 2.1、2.2 优先架空测试，落地测试时保证前方有足够空旷区域 |
| 电池状态 | 同一轮调参尽量使用同一块电池，低电压会让 speed_kp/yaw_kp 判断失真 |
| 上位机 | 能看到状态面板和曲线，必要时打开 CSV 记录 |
| 编码器精度 | 确认驱动和应用程序版本匹配，left_rps/right_rps 应显示小数 RPS |
| 停车门控 | 确认 drive_enabled=true，且没有 obstacle_stop，否则控制器输出会被应用层强制置 0 |
| 调参原则 | 一次只改一类参数，例如只改 speed_kp，不要同时改速度、角速度和视觉参数 |
推荐阶段顺序：
2.1开环检查电机和编码器。2.2调左右轮独立速度环。2.3检查差速运动学目标是否合理。2.4固定目标角速度，调角速度闭环。2.5打开视觉外环，调巡线误差到目标角速度的映射。2.6调道路速度、入弯提前减速和平滑过渡。2.7整车跑圈，按现象回到对应阶段修正。
2.1 让车能动（开环检查）
这个阶段不依赖闭环控制，先确认电机、方向和编码器读数正确。
2.1.1 检查步骤
| 检查项 | 方法 | 正常现象 |
|---|---|---|
| 电机方向 | 临时给左右电机固定小占空比 | 左右轮前进方向一致 |
| 编码器方向 | 单独转动左右轮并观察 left_rps/right_rps |
前进为正，反转为负 |
| 编码器量级 | 固定 PWM 后观察小数 RPS |
占空比越大，RPS 整体越高，低速不应只有整数跳变 |
| 电机死区 | 从较小占空比开始逐步增加 | 低于某个占空比时电机可能不转 |
2.1.2 通过标准
- 左右电机给正占空比时都向前转；
left_rps/right_rps前进为正，反转为负；- 固定占空比时
RPS不应长期为 0，也不应正负乱跳； - 低速采样时
RPS应带有小数变化。如果只看到整数，说明驱动和应用程序没有同步更新到rps_milli版本； - 左右轮同占空比下
RPS可以不同，但不能差异离谱。
如果前进时 RPS 为负，优先检查编码器方向 GPIO、驱动方向极性和数据融合中的方向符号，不能靠控制参数补偿。
2.2 左右轮速度环（让车能稳定跟踪目标速度）
速度环是基础。当前实现不是用平均速度做反馈，而是分别给左右轮生成目标编码器 RPS，再让左右轮各自闭环跟踪。
2.2.1 控制逻辑
单侧速度环是“模型前馈 + 小 PI 修正”。模型前馈负责给出维持目标速度的大致占空比，PI 只修正负载、电池电压、摩擦差异和左右电机差异造成的残余误差。
编码器测速齿轮周长：
\[C_e = \pi D_e
\]
单侧模型前馈：
\[u_{ff} = K_s\operatorname{sign}(n_t) + K_v n_t
\]
其中 \(K_s\) 和 \(K_v\) 由 tools/debug_motor 下的 motor_feedforward_calibrate 标定得到，左右轮分别配置。完整标定原理、运行流程和结果判断见 电机调试工具说明。
当前编码器 RPS 已经由驱动层 rps_milli 转换成小数值，模型拟合和 PI 误差计算都使用小数 RPS。这能减小低速量化误差，避免速度环在整数 RPS 台阶之间来回修正。
单侧速度误差：
\[e_v(k) = n_t(k) - n_a(k)
\]
候选积分：
\[I_c(k) = \operatorname{clamp}
\left(I(k-1) + e_v(k)\Delta t,\ I_{\min},\ I_{\max}\right)
\]
速度环比例项根据加速/减速方向选择不同增益。目标高于实际时使用 speed_kp，目标低于实际时使用 speed_decel_kp。这样减速段可以更强，不需要再叠加额外的反拖前馈补丁参数。
\[K_{pv}^{eff}=
\begin{cases}
K_{pv}^{dec}, & e_v(k)<0 \\
K_{pv}, & e_v(k)\ge 0
\end{cases}
\]
候选输出：
\[u_c(k) = u_{ff} + K_{pv}^{eff}e_v(k) + K_{iv}I_c(k)
\]
抗积分饱和逻辑：
\[I(k)=
\begin{cases}
I(k-1), & u_c(k)>100 \ \text{且}\ e_v(k)>0 \\
I(k-1), & u_c(k)<u_{\min} \ \text{且}\ e_v(k)<0 \\
I_c(k), & \text{其他情况}
\end{cases}
\]
最终单侧电机占空比：
\[u(k)=\operatorname{clamp}
\left(u_{ff}+K_{pv}^{eff}e_v(k)+K_{iv}I(k),\ u_{\min},\ 100\right)
\]
输出下限由目标编码器转速 \(n_t\) 决定：
\[u_{\min}=
\begin{cases}
-\operatorname{clamp}(\texttt{max\_reverse\_brake\_percent},0,100), & n_t \ge 0 \\
-100, & n_t < 0
\end{cases}
\]
当 \(n_t \ge 0\) 时，目标仍是正转或停车，负占空比只用于减速反拖，因此需要用 max_reverse_brake_percent 限制反拖力度，防止内侧轮在高速弯道中过度反转造成打滑。当 \(n_t < 0\) 时，目标本身就是反转，输出下限才放开到 -100%。
左右轮各自有独立积分项：
\[u_L(k)=\operatorname{PI}\left(n_{Lt}, n_{La}, I_L\right)
\]
\[u_R(k)=\operatorname{PI}\left(n_{Rt}, n_{Ra}, I_R\right)
\]
符号对应关系：
| 符号 | 代码变量 | 含义和单位 |
|---|---|---|
| \(D_e\) | encoder_gear_diameter_m |
编码器测速齿轮直径，单位 m |
| \(C_e\) | encoder_circumference_m |
编码器测速齿轮周长，\(C_e=\pi D_e\)，单位 m |
| \(v_0\) | normal_target_speed_mps |
普通直道车体中心目标线速度，单位 m/s |
| \(n_t\) | target_encoder_rps |
单侧目标编码器转速，单位 RPS，由差速运动学把目标线速度换算得到 |
| \(n_a\) | actual_encoder_rps |
单侧实际编码器转速，单位 RPS，来自编码器反馈并经过 DataFusion 滤波 |
| \(e_v\) | speed_error |
单侧速度误差，\(e_v=n_t-n_a\)，单位 RPS |
| \(K_s\) | left/right_speed_static_percent |
单侧静摩擦前馈，占空比单位 %，表示刚开始克服静摩擦所需的基础输出 |
| \(K_v\) | left/right_speed_kv_percent_per_rps |
单侧速度前馈斜率，单位 %/RPS，表示目标编码器转速每增加 1 RPS 需要增加的占空比 |
| \(u_{ff}\) | 速度前馈输出 | 模型前馈占空比，\(u_{ff}=K_s\operatorname{sign}(n_t)+K_vn_t\) |
| \(K_{pv}\) | speed_kp |
加速或同向修正时的比例增益，占空比单位 %/RPS |
| \(K_{pv}^{dec}\) | speed_decel_kp |
减速修正时的比例增益，占空比单位 %/RPS |
| \(K_{iv}\) | speed_ki |
积分增益，占空比单位 %/(RPS\cdot s) |
| \(I\) | 左右速度积分项 | 速度误差积分，单位 RPS*s，左右轮各自独立保存 |
| \(u_{\min}\) | max_reverse_brake_percent 决定的反拖下限 |
正转目标下允许输出的最小占空比，防止减速时反拖过强 |
这里的目标和实际速度都使用编码器测速齿轮 RPS，不是车轮 RPS。标定 \(K_s/K_v\) 时也要直接使用工具输出的编码器 RPS，不要再按车轮直径二次换算。
编码器不可用时，代码会把实际 RPS 临时视为目标 RPS，此时速度误差为 0，速度环只输出前馈占空比，并清零左右速度积分。这是硬件降级逻辑，只能让车辆以开环方式可控运行，不能用于闭环调参。
2.2.2 关键参数
下表直接对应 2.2.1 的公式。速度环调参时先区分“模型前馈”和“闭环修正”：\(K_s/K_v\) 负责基础输出，\(K_{pv}/K_{iv}\) 只负责修正残余误差。
| 参数 | 公式位置 | 代码作用 | 调试方向 |
|---|---|---|---|
normal_target_speed_mps |
\(v_0\) | 普通直道车体中心目标线速度，后续会通过差速运动学换算成左右 \(n_t\) | 先给低速目标，确认能稳定跟踪后再提高 |
left/right_speed_static_percent |
\(K_s\) | 左右轮静摩擦前馈，参与 \(u_{ff}=K_s\operatorname{sign}(n_t)+K_vn_t\) | 由 motor_feedforward_calibrate 标定，不建议手感猜 |
left/right_speed_kv_percent_per_rps |
\(K_v\) | 左右轮速度前馈斜率，决定目标 \(n_t\) 增大时前馈占空比如何增大 | 由 motor_feedforward_calibrate 标定，速度提高后优先检查它 |
speed_kp |
\(K_{pv}\) | \(e_v=n_t-n_a\) 的加速段比例修正 | speed_ki=0 时逐步增加，直到实际 \(n_a\) 能跟上目标 \(n_t\) |
speed_decel_kp |
\(K_{pv}^{dec}\) | \(e_v<0\) 时使用的减速段比例修正 | 减速慢时调整，通常大于 speed_kp |
speed_ki |
\(K_{iv}\) | 速度误差积分 \(I\) 的修正增益，用于消除长期静差 | 有长期静差再小幅增加 |
max_reverse_brake_percent |
\(u_{\min}\) | 正转目标下允许的最大反拖占空比，决定 \(u(k)\) 的下限 | 内侧轮容易反转打滑时降低，减速不够时再谨慎增大 |
speed_integral_min/max |
\(I_{\min}/I_{\max}\) | 速度环积分限幅，约束候选积分 \(I_c(k)\) | 防止长时间饱和后恢复过冲 |
rps_filter_alpha |
\(n_a\) 的输入滤波 | 编码器实际转速进入速度环前的低通系数，不在公式中单独展开 | 改变后速度环参数需要重调 |
2.2.3 调试步骤
速度闭环调试的核心是：给定一个明确的目标速度，观察左右实际 RPS 能否快速、稳定地跟上左右目标 RPS。
- 架空车轮，关闭转向影响，让
target_yaw_rate = 0，此时left_target_rps和right_target_rps应基本相等。最简单的临时做法是把outer_kp=0、outer_kd=0、max_yaw_rate_correction=0，或在compute_outer_target()中临时强制target_yaw_rate=0。 - 设置较低的
normal_target_speed_mps，上位机应能看到稳定的target_speed_mps、target_speed_rps、left_target_rps和right_target_rps。 - 先按照 电机调试工具说明 运行
motor_feedforward_calibrate，写入左右轮Ks/Kv。 - 关闭积分，设置
speed_ki = 0，先只调speed_kp。 - 给目标速度做阶跃测试，例如从低速目标切到稍高目标，观察
left_rps/right_rps跟随left_target_rps/right_target_rps的过程。 - 如果实际
RPS上升太慢或长期低于目标，逐步增大speed_kp；如果实际RPS围绕目标来回振荡，降低speed_kp。 - 如果只有单侧偏差明显，优先重新标定该侧
Ks/Kv，再检查编码器方向、机械阻力和轮胎摩擦。 - 减速慢时先调
speed_decel_kp；如果输出已经被max_reverse_brake_percent限住，再谨慎增大max_reverse_brake_percent。 - 当
speed_kp能让实际RPS接近目标但仍存在稳定静差时，再小幅增加speed_ki。 - 如果加入
speed_ki后出现慢速来回摆动、输出长时间饱和或松手后恢复很慢，说明积分过强，应降低speed_ki或收紧speed_integral_min/max。
2.2.4 通过标准和失败处理
| 现象 | 判断 | 调整方向 |
|---|---|---|
实际 RPS 长期低于目标，输出未饱和 |
比例修正不足或前馈偏低 | 增大 speed_kp 或重新标定 Ks/Kv |
实际 RPS 快速超过目标后反复振荡 |
速度环过强 | 降低 speed_kp，必要时降低 rps_filter_alpha |
实际 RPS 接近目标但有稳定静差 |
缺少积分补偿 | 小幅增加 speed_ki |
目标下降时实际 RPS 下降慢 |
减速段比例不足或反拖限幅过小 | 增大 speed_decel_kp，必要时再调整 max_reverse_brake_percent |
| 弯中内侧轮明显反转打滑 | 反拖下限太宽或目标角速度过大 | 降低 max_reverse_brake_percent，再检查 max_target_yaw_rate |
输出长期接近 100% 仍跟不上 |
目标速度超过当前电机能力 | 降低 normal_target_speed_mps 或检查电池/机械阻力 |
| 左右轮响应差异很大 | 单侧硬件或编码器问题 | 优先检查方向、接线、摩擦和编码器读数 |
left_rps/right_rps 一直为 0 或不变化 |
编码器没有有效反馈 | 先修编码器，不要继续调 speed_kp/speed_ki |
本阶段通过后，左右轮应能分别跟随自己的目标 RPS，并且输出不长期顶到 100% 或 -100%。
2.3 差速运动学（检查左右轮目标是否合理）
这一阶段不调 PID，只检查“车体速度 + 目标角速度”换算成左右轮目标 RPS 是否符合预期。它是 2.4 角速度闭环的前置检查。
2.3.1 基础模型
设：
- \(v\)：车体中心线速度，对应代码里的
target_speed_mps； - \(\omega\)：车体目标角速度，对应代码里的
yaw_rate_command； - \(L\)：左右轮中心距，对应配置里的
wheel_track_m； - \(v_L\)：左轮目标线速度；
- \(v_R\)：右轮目标线速度。
差速模型的两个基础关系是：
\[v = \frac{v_L + v_R}{2}
\]
\[\omega = \frac{v_R - v_L}{L}
\]
反解得到：
\[v_L = v - \frac{\omega L}{2}
\]
\[v_R = v + \frac{\omega L}{2}
\]
按当前代码的符号约定：
\[\texttt{left\_speed\_mps}
= \texttt{target\_speed\_mps}
- \frac{\texttt{yaw\_rate\_command}\cdot\texttt{wheel\_track\_m}}{2}
\]
\[\texttt{right\_speed\_mps}
= \texttt{target\_speed\_mps}
+ \frac{\texttt{yaw\_rate\_command}\cdot\texttt{wheel\_track\_m}}{2}
\]
2.3.2 编码器 RPS 换算
当前编码器测的是测速齿轮 RPS，不是车轮 RPS。因此用编码器测速齿轮周长把左右轮线速度换算成编码器目标 RPS：
\[C_e = \pi D_e
\]
\[n_L = \frac{v_L}{C_e}
\]
\[n_R = \frac{v_R}{C_e}
\]
其中 \(D_e\) 对应 encoder_gear_diameter_m。
2.3.3 调试检查
- 先确认
2.2左右速度环已经稳定，否则左右目标再合理也无法执行。 - 给定固定
target_yaw_rate，并临时设置max_yaw_rate_correction=0，让yaw_rate_command直接等于target_yaw_rate，便于单独检查差速换算。 - 当
yaw_rate_command = 0时，left_target_rps应等于right_target_rps。 abs(yaw_rate_command)越大，左右轮目标速度差应越大。- 如果普通弯道里内侧目标频繁接近 0 或反转，通常说明
max_target_yaw_rate过大，或者目标速度太低。
本阶段通过后，固定 target_yaw_rate 下左右目标 RPS 应按差速模型变化，并且左右实际 RPS 能分别跟随目标。
2.4 角速度闭环（让实际角速度跟上固定目标）
角速度闭环根据 IMU yaw rate 修正目标角速度。这个阶段先不要依赖视觉外环，应该用固定 target_yaw_rate 验证车辆能不能按指定角速度转起来。
2.4.1 控制逻辑
角速度闭环是 PI。它不直接控制电机占空比，只把 target_yaw_rate 修正成 yaw_rate_command，后续再由差速运动学转换成左右轮目标速度。
实际角速度：
\[\omega_a(k)=\texttt{state.yaw\_rate\_rad\_s}
\]
角速度误差：
\[e_\omega(k)=\omega_t(k)-\omega_a(k)
\]
候选积分：
\[I_c(k)=\operatorname{clamp}
\left(I(k-1)+e_\omega(k)\Delta t,\ I_{\omega\min},\ I_{\omega\max}\right)
\]
当前实现对角速度环增加了速度增益调度。yaw_kp 和 yaw_ki 仍然是基准参数，但控制器会根据本周期目标线速度计算有效参数：
\[S_p(v)=\operatorname{clamp}
\left(1+G_p\max(0,\ v-v_b),\ S_{p\min},\ S_{p\max}\right)
\]
\[K_{p\omega}^{eff}=K_{p\omega}S_p(v)
\]
高速段积分缩放为：
\[S_i(v)=
\begin{cases}
0, & v_{off}>0 \ \text{且}\ v\ge v_{off} \\
1, & v\le v_b \\
S_{ihigh}, & v>v_b
\end{cases}
\]
\[K_{i\omega}^{eff}=K_{i\omega}S_i(v)
\]
这里速度越高，比例修正可以更强；积分在高速时降低或关闭，避免出弯后积分继续推动车头转向。
候选修正量：
\[\Delta\omega_c(k)=K_{p\omega}^{eff}e_\omega(k)+K_{i\omega}^{eff}I_c(k)
\]
符号对应关系：
| 符号 | 代码变量 |
|---|---|
| \(\omega_a\) | actual_yaw_rate |
| \(\omega_t\) | target_yaw_rate |
| \(\omega_c\) | yaw_rate_command |
| \(e_\omega\) | yaw_error |
| \(K_{p\omega}\) | yaw_kp |
| \(K_{i\omega}\) | yaw_ki |
| \(v_b\) | yaw_schedule_base_speed_mps |
| \(G_p\) | yaw_kp_speed_gain |
| \(S_{p\min}\) | yaw_kp_min_scale |
| \(S_{p\max}\) | yaw_kp_max_scale |
| \(S_{ihigh}\) | yaw_ki_high_speed_scale |
| \(v_{off}\) | yaw_ki_disable_speed_mps |
| \(\Delta\omega_{\max}\) | max_yaw_rate_correction |
| \(\omega_{\max}\) | max_target_yaw_rate |
若 max_yaw_rate_correction <= 0 或 IMU 不可用，控制器只使用 target_yaw_rate，不具备角速度闭环修正能力。
最终角速度修正量：
\[\Delta\omega(k)=\operatorname{clamp}
\left(K_{p\omega}^{eff}e_\omega(k)+K_{i\omega}^{eff}I(k),\
-\Delta\omega_{\max},\ +\Delta\omega_{\max}\right)
\]
其中积分项 I(k) 使用和速度环类似的饱和保持策略：如果候选输出已经超过 max_yaw_rate_correction，且误差还在继续把输出往饱和方向推，则保持上一周期积分，避免积分继续累积。
最终角速度命令：
\[\omega_c(k)=\operatorname{clamp}
\left(\omega_t(k)+\Delta\omega(k),\ -\omega_{\max},\ +\omega_{\max}\right)
\]
2.4.2 关键参数
| 参数 | 作用 | 调试建议 |
|---|---|---|
yaw_kp |
实际角速度滞后时的比例修正 | 先从小值开始，避免车头高频抖动 |
yaw_ki |
角速度长期静差修正 | 初期关闭，确认比例项稳定后再加 |
yaw_schedule_base_speed_mps |
角速度环增益调度基准速度 | 通常设为当前已稳定速度，例如 20RPS 对应线速度 |
yaw_kp_speed_gain |
高速段比例增强斜率 | 24RPS 实际角速度跟不上时小幅增大 |
yaw_kp_min/max_scale |
yaw_kp 调度缩放范围 |
限制高速增强幅度，避免目标左右轮 RPS 大幅跳变 |
yaw_ki_high_speed_scale |
高速段积分缩放 | 高速建议小于 1.0，降低出弯拖尾 |
yaw_ki_disable_speed_mps |
高速关闭积分阈值 | 0 表示不关闭；高速拖尾严重时再启用 |
yaw_integral_min/max |
角速度积分限幅 | 检查积分贡献是否过大 |
max_yaw_rate_correction |
最大角速度修正量 | 太大会导致左右目标 RPS 跳变 |
yaw_rate_filter_alpha |
yaw rate 低通系数 |
改变后 yaw_kp/yaw_ki 需要重调 |
2.4.3 调试步骤
- 确认
2.2和2.3已通过，左右实际RPS能跟随左右目标RPS。 - 低速落地，临时给定固定
target_yaw_rate，例如先从较小角速度开始。可在compute_outer_target()中临时把target.target_yaw_rate赋为固定值，调完后必须恢复视觉外环计算。 - 先设置
yaw_ki = 0，只调yaw_kp，并给较小的max_yaw_rate_correction。 - 观察
target_yaw_rate、yaw_rate_command、actual_yaw_rate和left_target_rps/right_target_rps。 - 如果
yaw_rate_command已经明显大于target_yaw_rate，但actual_yaw_rate仍跟不上，说明执行侧角速度响应不足，可小幅增加yaw_kp或适当放宽max_yaw_rate_correction。 - 如果左右目标
RPS高频跳变，或者车头开始高频摆动，优先降低yaw_kp或max_yaw_rate_correction。 - 只有在
yaw_kp已经稳定、但yaw_error长时间存在固定偏差时，才小幅增加yaw_ki。 - 当
20RPS已稳定但24RPS角速度明显滞后时，保持yaw_kp/yaw_ki不变，优先增加yaw_kp_speed_gain或yaw_kp_max_scale。 - 如果
24RPS出弯后车头继续转，降低yaw_ki_high_speed_scale；仍明显拖尾时再设置yaw_ki_disable_speed_mps。
角速度环没有像速度环那样的自动前馈标定工具。原因是 yaw rate 响应依赖轮胎抓地、地面摩擦、车体姿态和左右轮速度闭环，只能落地测试。推荐用固定 target_yaw_rate 的短距离阶跃测试，通过上位机 CSV 观察 target_yaw_rate/yaw_rate_command/actual_yaw_rate 三条曲线来整定。
2.4.4 通过标准和失败处理
| 现象 | 判断 | 调整方向 |
|---|---|---|
actual_yaw_rate 慢慢跟随，但长期偏小 |
比例修正不足或物理抓地不足 | 先小幅增大 yaw_kp，再检查轮胎和速度 |
20RPS 正常，24RPS 明显滞后 |
高速段角速度修正不足 | 增大 yaw_kp_speed_gain 或 yaw_kp_max_scale |
| 高速出弯后仍继续转 | 积分拖尾或轮胎滑移 | 降低 yaw_ki_high_speed_scale，必要时设置 yaw_ki_disable_speed_mps |
yaw_error 正负来回跳，车头高频摆 |
角速度闭环过强或滤波太跟手 | 降低 yaw_kp，必要时降低 yaw_rate_filter_alpha |
左右目标 RPS 突然大幅分离 |
修正量过大 | 降低 max_yaw_rate_correction |
| 输出占空比长期饱和 | 目标速度或目标角速度超过车辆能力 | 降低 normal_target_speed_mps 或 max_target_yaw_rate |
本阶段通过后，固定 target_yaw_rate 下 actual_yaw_rate 应能跟随变化趋势，且车头没有持续高频抖动。
2.5 视觉外环（让赛道误差生成目标角速度）
视觉外环只负责把巡线横向误差转换成 target_yaw_rate，不直接输出电机占空比。它必须在 2.4 固定目标角速度验证通过后再调。
2.5.1 控制逻辑
视觉外环是 PD，不含积分项。积分不适合直接加在图像横向误差上，因为赛道识别、透视畸变和曝光变化会造成误差漂移，积分容易把视觉噪声累计成转向偏置。
横向误差微分：
\[\dot e_x(k)=\frac{e_x(k)-e_x(k-1)}{\Delta t}
\]
目标角速度：
\[S_x(v)=\operatorname{clamp}
\left(1+G_x\max(0,\ v-v_{xb}),\ S_{x\min},\ S_{x\max}\right)
\]
\[K_{px}^{eff}=K_{px}S_x(v)
\]
\[S_d(v)=\operatorname{clamp}
\left(1+G_d\max(0,\ v-v_{xb}),\ 1,\ S_{d\max}\right)
\]
\[K_{dx}^{eff}=K_{dx}S_d(v)
\]
\[\omega_t(k)=K_{px}^{eff}e_x(k)+K_{dx}^{eff}\dot e_x(k)
\]
默认只增强比例项 outer_kp，outer_kd_speed_gain=0 时微分项不随速度放大。原因是 S 弯、圆环、十字和远端丢线会让微分项对噪声更敏感；只有在误差曲线稳定、比例项已经调好后，再小幅打开 outer_kd 调度。
符号对应关系：
| 符号 | 代码变量 |
|---|---|
| \(e_x\) | tracking_error |
| \(\dot e_x\) | 视觉误差微分 |
| \(K_{px}\) | outer_kp |
| \(K_{dx}\) | outer_kd |
| \(v_{xb}\) | outer_schedule_base_speed_mps |
| \(G_x\) | outer_kp_speed_gain |
| \(G_d\) | outer_kd_speed_gain |
| \(S_{x\min}\) | outer_kp_min_scale |
| \(S_{x\max}\) | outer_kp_max_scale |
| \(S_{d\max}\) | outer_kd_max_scale |
| \(s\) | straight_turn_scale |
| \(e_{\text{straight}}\) | straight_error_threshold |
| \(\omega_{\max}\) | max_target_yaw_rate |
直线小误差抑制：
\[\omega_t(k)=
\begin{cases}
\omega_t(k)\cdot \operatorname{clamp}(s,0,1), & |e_x(k)|<e_{\text{straight}} \\
\omega_t(k), & |e_x(k)|\ge e_{\text{straight}}
\end{cases}
\]
目标角速度限幅：
\[\omega_t(k)=\operatorname{clamp}
\left(\omega_t(k),\ -\omega_{\max},\ +\omega_{\max}\right)
\]
2.5.2 关键参数
下表直接对应 2.5.1 的公式。调参时先看“公式位置”，再看“调试方向”，避免把比例、微分、限幅和滤波混在一起调。
| 参数 | 公式位置 | 代码作用 | 调试方向 |
|---|---|---|---|
outer_kp |
\(K_{px}\) | 巡线误差 \(e_x\) 到目标角速度 \(\omega_t\) 的基础比例增益 | 入弯晚且 target_yaw_rate 偏小时增大；直道摆动或过度转向时减小 |
outer_kd |
\(K_{dx}\) | 巡线误差微分 \(\dot e_x\) 的基础增益，用于提前响应误差变化 | 初期设为 0；误差曲线稳定但入弯目标仍滞后时小幅增加 |
outer_schedule_base_speed_mps |
\(v_{xb}\) | 位置环速度调度基准速度，低于该速度时调度增益基本不增加 | 通常设为当前已经稳定跑圈的速度，例如 20RPS 对应线速度 |
outer_kp_speed_gain |
\(G_x\) | 目标速度高于 \(v_{xb}\) 后，按速度差增加 \(S_x(v)\)，进而放大 outer_kp |
高速入弯晚且 target_yaw_rate 偏小时小幅增大；直道摆动时减小 |
outer_kp_min_scale |
\(S_{x\min}\) | outer_kp 调度缩放下限 |
一般保持接近 1.0；低速过敏时可小幅降低 |
outer_kp_max_scale |
\(S_{x\max}\) | outer_kp 调度缩放上限 |
限制高速比例放大；高速仍入弯晚可增大，直道摆动则减小 |
outer_kd_speed_gain |
\(G_d\) | 目标速度高于 \(v_{xb}\) 后，按速度差增加 \(S_d(v)\)，进而放大 outer_kd |
默认 0；只有误差稳定且比例项已调好后再小幅打开 |
outer_kd_max_scale |
\(S_{d\max}\) | outer_kd 调度缩放上限 |
限制高速微分放大；微分噪声导致抖动时保持 1.0 或降低 |
straight_error_threshold |
\(e_{\text{straight}}\) | 小误差直线区判定阈值，满足后进入直线抑制公式 | 直道小误差频繁修正时适当增大；弯前响应变慢时不要盲目增大 |
straight_turn_scale |
\(s\) | 直线小误差区对 \(\omega_t\) 的缩放系数 | 直道摆动大时降低；直道修正不足时提高 |
max_target_yaw_rate |
\(\omega_{\max}\) | 对视觉外环输出的目标角速度做最终限幅 | 低速调车先限制；高速小半径弯目标不够时再逐步放宽 |
tracking_error_filter_alpha |
\(e_x\) 的输入滤波 | 进入控制器前对巡线误差做低通，不在 2.5.1 公式中单独出现 | 越大越跟手，越小越平滑；误差抖动先降它，不要先加 outer_kd |
2.5.3 调试步骤
- 低速落地运行，把
outer_kd设为 0，只调outer_kp。 - 同时观察
tracking_raw_error和tracking_error。如果原始误差本身跳变很大，先处理二值化、边线、镜头畸变矫正和前瞻点，不要先加控制增益。 - 观察
target_yaw_rate是否随tracking_error平滑变化。若误差很小但目标角速度频繁正负跳变，说明外环过敏或滤波不足。 - 直道先让
straight_turn_scale小一些，例如0.5~0.7，确认小误差不会被放大成车头左右摆。 - 入弯晚时小幅增大
outer_kp，同时检查track_aim、error_lookahead_offset和tracking_curve_ahead。 - 如果
target_yaw_rate已经足够大，但actual_yaw_rate跟不上，问题不在视觉外环，应回到2.4检查角速度闭环、左右轮目标和抓地。 - 只有在误差稳定且比例项已经合适后，再小幅增加
outer_kd。 - 当
20RPS已稳定但24RPS入弯晚，并且target_yaw_rate明显偏小，优先增大outer_kp_speed_gain或outer_kp_max_scale。 - 如果高速时误差变化已经平滑，但入弯目标角速度仍滞后，可小幅增大
outer_kd_speed_gain，并用outer_kd_max_scale限制放大幅度。 - 如果
24RPS直道开始摆动，降低outer_kp_max_scale、outer_kd_speed_gain或straight_turn_scale，不要先动角速度环。
2.5.4 通过标准和失败处理
| 现象 | 判断 | 调整方向 |
|---|---|---|
| 直道左右摆 | 小误差被外环放大 | 降低 outer_kp 或 straight_turn_scale |
入弯晚且 target_yaw_rate 偏小 |
外环给的目标角速度不足 | 小幅增大 outer_kp，检查 track_aim/error_lookahead_offset |
20RPS 正常，24RPS 入弯晚且目标角速度偏小 |
高速段位置环不够积极 | 增大 outer_kp_speed_gain 或 outer_kp_max_scale |
| 高速误差变化平滑但目标角速度仍滞后 | 微分提前量不足 | 小幅增大 outer_kd_speed_gain，限制 outer_kd_max_scale |
入弯晚但 target_yaw_rate 已很大 |
执行层跟不上 | 回到 2.4 检查角速度闭环和差速目标 |
| 入弯后过度转向 | 外环目标过大或限幅过宽 | 降低 outer_kp 或 max_target_yaw_rate |
| 误差抖动明显 | 视觉输入不稳定 | 降低 tracking_error_filter_alpha，检查二值化和边线稳定性 |
2.6 道路速度和平滑过渡
道路状态只决定原始目标速度比例，最终速度会经过斜率限制，不会从直道速度瞬间跳到弯道速度。注意：obstacle_stop 的真正停车由应用层门控处理；速度决策公式只描述门控放行时控制器如何生成目标速度。
2.6.1 控制逻辑
原始速度决策：
\[s_v=\operatorname{clamp}(\texttt{speed\_scale},0.1,1.5)
\]
\[v_0=\max(0,\texttt{normal\_target\_speed\_mps})
\]
\[v_{\text{raw}}=
\begin{cases}
v_0\cdot s_{\text{slow}}\cdot s_v, & \texttt{obstacle\_slow}=true \\
0, & \texttt{obstacle\_stop}=true \\
v_0\cdot s_{\text{ring}}\cdot s_v, & \texttt{road}=LeftRing \text{ 或 } RightRing \\
v_0\cdot s_{\text{cross}}\cdot s_v, & \texttt{road}=Cross \\
v_0\cdot s_{\text{barrier}}\cdot s_v, & \texttt{road}=Barrier \\
v_0\cdot s_{\text{bridge}}\cdot s_v, & \texttt{road}=Bridge \\
v_0\cdot s_{\text{curve}}\cdot s_v, & \texttt{road}=Normal \text{ 且满足普通弯道条件} \\
v_0\cdot s_v, & \text{其他情况}
\end{cases}
\]
应用层停车门控会在 obstacle_stop=true 时立即输出 0% 并跳过控制器。上面的 obstacle_stop 分支只是控制器内部的兜底逻辑，正常实车停车不依赖该分支。
普通弯道条件：
\[c_{\text{err}} = |e_x| \ge e_{\text{curve}}
\]
\[c_{\text{ahead}} =
\left(e_{\text{ahead\_th}}>0\right)
\land
\left(|e_{\text{ahead}}|\ge e_{\text{ahead\_th}}\right)
\]
\[c_{\text{curve}}=c_{\text{err}}\lor c_{\text{ahead}}
\]
速度斜率限制：
\[\Delta v = v_{\text{raw}}(k)-v_t(k-1)
\]
\[a_{\text{limit}}=
\begin{cases}
a_{\text{up}}, & \Delta v \ge 0 \\
a_{\text{down}}, & \Delta v < 0
\end{cases}
\]
当 \(a_{\text{limit}}\le 0\) 或 \(\Delta t\le 0\) 时：
\[v_t(k)=v_{\text{raw}}(k)
\]
否则：
\[\Delta v_{\max}=a_{\text{limit}}\Delta t
\]
\[v_t(k)=v_t(k-1)+
\operatorname{clamp}\left(\Delta v,\ -\Delta v_{\max},\ +\Delta v_{\max}\right)
\]
第一次生成目标速度时，m_prev_target_speed_mps < 0，代码会直接用当前原始目标速度初始化，避免上电后从 0 慢慢爬升。之后从直道切到弯道时，目标线速度不会从直道值瞬间跳到弯道值，而是按 target_speed_ramp_down_mps2 每个周期逐步下降。出弯恢复速度时，则按 target_speed_ramp_up_mps2 逐步上升。
2.6.2 关键参数
| 参数 | 作用 | 调试建议 |
|---|---|---|
normal_target_speed_mps |
普通直道基准速度 | 先保守，确认能稳定跑圈后再提高 |
curve_target_scale |
普通弯道速度比例 | 入弯过快时降低 |
curve_ahead_threshold |
远端弯道提前降速阈值 | 入弯前来不及降速时适当降低 |
target_speed_ramp_down_mps2 |
目标速度每秒最大下降量 | 速度切换太硬时降低 |
target_speed_ramp_up_mps2 |
目标速度每秒最大上升量 | 出弯恢复太慢时提高 |
ring_target_scale/cross_target_scale |
圆环、十字速度比例 | 只用于对应道路状态，不要拿来修普通弯道 |
2.6.3 调试步骤
- 先确认
2.5能低速跑完基本赛道，不要一开始就追求高速。 - 设置较保守的
normal_target_speed_mps，保证车能稳定跑圈。 - 若入弯前速度过快，先降低
curve_target_scale或调小curve_ahead_threshold，让远端弯道更早触发降速。 - 若从直道到弯道速度变化仍然太硬，降低
target_speed_ramp_down_mps2。 - 若出弯后恢复速度太慢，提高
target_speed_ramp_up_mps2。 - 圆环、十字、坡道、障碍只调整对应的
ring_target_scale/cross_target_scale/bridge_target_scale/barrier_target_scale，不要用这些参数修普通弯道问题。
2.6.4 通过标准和失败处理
| 现象 | 判断 | 调整方向 |
|---|---|---|
| 直道刚进弯就冲出去 | 入弯前没有提前降速或外环目标不足 | 检查 tracking_curve_ahead、curve_ahead_threshold、outer_kp |
| 速度从直道到弯道变化很突兀 | 斜率限制太宽或关闭 | 降低 target_speed_ramp_down_mps2 |
| 出弯后很久才恢复速度 | 加速斜率太小 | 提高 target_speed_ramp_up_mps2 |
| 圆环里速度明显过快 | 圆环速度比例偏高 | 降低 ring_target_scale |
| 十字里速度过慢或过快 | 十字速度比例不合适 | 调整 cross_target_scale |
2.7 整车跑圈和问题回退
整车跑圈时不要看到现象就直接改最后一层参数，应按问题来源回退到对应阶段。
推荐最终调试顺序：
- 开环确认电机方向、编码器方向和
RPS量级。 - 固定较低的
normal_target_speed_mps，确认左右目标RPS稳定且左右实际RPS能随目标变化。 - 悬空调左右独立速度环，先
speed_ki=0，只调speed_kp，让实际RPS能跟上目标RPS。 - 若前馈明显偏低或偏高，重新运行
motor_feedforward_calibrate，不要用 PI 长期补偿模型误差。 - 小幅加入
speed_ki，消除稳定静差，并确认左右轮不会长期饱和。 - 检查差速目标，确认固定
target_yaw_rate下左右轮目标一高一低且实际能跟随。 - 固定
target_yaw_rate调角速度闭环，先yaw_ki=0，再调yaw_kp/max_yaw_rate_correction。 - 打开视觉外环，低速调
outer_kp和straight_turn_scale。 - 观察弯道目标速度是否提前平滑下降，再调
curve_ahead_threshold和速度斜率。 - 最后根据整车跑圈表现微调
outer_kd、yaw_ki和各道路速度比例。
常见问题定位：
| 现象 | 优先回退阶段 | 优先检查 |
|---|---|---|
上位机目标正常但实际 RPS 不动 |
2.1 |
编码器、电机驱动、停车门控状态 |
左右实际 RPS 跟不上左右目标 |
2.2 |
speed_kp/speed_ki、电池、电机和机械阻力 |
| 固定角速度都转不好 | 2.3/2.4 |
差速目标、yaw_kp、max_yaw_rate_correction、抓地 |
| 直道左右摆 | 2.5 |
tracking_error_filter_alpha、outer_kp、straight_turn_scale |
| 入弯晚 | 2.5/2.6 |
track_aim、error_lookahead_offset、curve_ahead_threshold、outer_kp |
| 弯中内侧轮停转或反转 | 2.3/2.4 |
max_target_yaw_rate、max_yaw_rate_correction、wheel_track_m、目标速度 |
| 实际角速度明显滞后 | 2.4 |
IMU 安装方向、yaw_rate_filter_alpha、左右轮抓地 |
| 输出长期饱和 | 2.2/2.6 |
目标速度过高、电池电压不足、Ks/Kv 标定偏低 |
drive_enabled=false 或障碍停车时仍在看控制参数 |
2.0 |
应用层门控已强制输出 0，先解除停车条件 |
参考文章
浙公网安备 33010602011771号