做无人驾驶算法这条路上很多人是从感知入门的跑通了目标检测、分割、跟踪觉得离“让车自己走”还很近。但其实真正让车动起来的关键一步是控制。感知负责看见世界定位负责知道我在哪规划负责决定往哪走而控制模块要把这些决策最终落到执行器上方向盘转多少、油门踩多深、刹车何时介入。说白了控制就是无人驾驶的“手脚”是整套系统里最直接接触车辆物理特性的环节。这个系列前面讲了感知和规划这一篇专门聊机器人控制。内容主要面向想从零手写控制算法的开发者以及正在做路径跟踪、轨迹跟踪项目的同学。我会从最基础的车辆模型讲起把横向控制里常用的 Pure Pursuit、Stanley、LQR、MPC 四种方法逐个拆开再讲纵向速度控制怎么和横向配合最后分享仿真验证和实车调试中容易踩的坑。所有算法都强调可复现核心公式和边界条件尽量讲透但不堆砌数学推导。1. 控制模块的定位与整体设计思路1.1 整个算法链路里控制做的是什么在完整的无人驾驶软件栈里控制模块是执行层这一点经常被初学者忽略。我见过不少朋友把控制想简单了觉得“不就是调个 PID 嘛三个参数的事”。在玩具车上确实可以但到了真实车辆上难度直接跃升一个数量级车是非线性系统轮胎有侧偏特性转向有延迟油门刹车响应不对称再加上高速工况下的动力学变化一套固定参数的简单反馈往往是低速还行、高速就发散。这也是为什么工程上会从几何法一路演进到 MPC。自己写整套算法时我建议先把控制模块的输入输出边界定义清楚。输入通常包含三块参考轨迹位置、航向、曲率、期望速度、车辆状态反馈位置、速度、横摆角速度、加速度、执行器状态当前转角、踏板位置。输出是转向指令和纵向加速度指令。接口定义得干净后面换算法甚至换车辆平台都方便。这个边界设计决定了整个模块的可扩展性不要图省事把感知、定位、规划的原始数据直接塞给控制那样后续每改一个上游模块控制代码就得跟着动。1.2 横向控制与纵向控制的分工控制任务可以拆成两路横向控制负责方向盘转角目标是让车辆的横向误差收敛、航向贴合参考轨迹纵向控制负责油门和刹车目标是让车速跟随规划模块给出的期望速度曲线。这两个方向并不是完全解耦的。横向控制里的曲率半径会限制可用的最大速度纵向速度又会影响横向控制器的增益和稳定性。所以实际工程里两者要做耦合常见的做法是横向控制器在计算转角时把当前速度作为参数纵向控制器把参考轨迹的曲率作为前馈参考。我自己实验时习惯先把纵向稳住再调横向。因为如果速度忽快忽慢横向控制器的参数很难准确评估同样的转角在 30 km/h 和 80 km/h 下产生的横摆响应完全不同。先把速度稳定下来横向的问题才容易定位。1.3 方案选型从几何法到最优控制横向控制算法有一条清晰的演进路径选型时要看你的场景和算力。算法模型依赖算力需求适用场景Pure Pursuit运动学极低低速、泊车、简单路径Stanley运动学极低低速到中速、平滑道路LQR线性动力学低高速巡航、结构化道路MPC动力学/运动学高复杂约束、极限工况我的建议是如果是从学习角度自己写先把 Pure Pursuit 和 Stanley 吃透因为它们直观、调试简单能让你快速建立起“控制器到底在干什么”的物理直觉。然后上 LQR体会一下状态反馈和最优控制的思路。最后再碰 MPC因为 MPC 能系统性处理约束转向角度限制、转向速率限制、加速度限制这是前几种算法很难优雅处理的。顺序走下来每个算法的优势劣势你会理解得特别深而不是囫囵吞枣地“调库跑通”。2. 车辆运动学与动力学模型一切控制的基石2.1 运动学自行车模型几乎所有几何法横向控制都建立在同一个模型上——自行车模型Bicycle Model。它把车辆的四轮简化成前轮和后轮两个轮子忽略轮胎侧偏认为车辆只做横摆运动、没有侧滑。在这个模型下车辆状态通常用一组四元组表示位置 (x, y)、横摆角 ψ、车速 v、前轮转角 δ。运动学方程写出来是这样x_dot v * cos(ψ) y_dot v * sin(ψ) ψ_dot v * tan(δ) / L其中 L 是轴距。这个式子的物理含义很直观前轮转角越大车转弯越急速度越快同样的转角产生的横摆角速度越大。注意 tan(δ) 这个非线性项当 δ 接近 ±90° 时转角趋向无穷大——不过真实车辆的前轮转角最大也就 30 到 40 度所以实际使用中这个极限不会触发但理解这一项能帮你明白为什么高速下小转角就够用了。为什么叫自行车模型因为简化之后车辆的转向行为可以等价为一个前轮转向的自行车后轮沿车身方向前进前轮相对车身转过一个角度。这个模型在低速、轮胎侧偏不明显的情况下精度足够是 Pure Pursuit、Stanley 等几何算法共同的基础。2.2 动力学模型什么时候必须上运动学模型有个前提假设轮胎不打滑。但车辆在高速过弯、紧急变道或者路面湿滑时轮胎会产生侧偏角前轮的实际速度方向和车身朝向并不一致。这时候运动学模型就失真了用失真模型算出来的控制量自然不准。动力学模型把轮胎的侧偏力引入进来典型的实现是二自由度单轨模型仍然是自行车形态但带轮胎力m * v * (β_dot ψ_dot) F_yf F_yr I_z * ψ_ddot a * F_yf - b * F_yr其中 β 是质心侧偏角a、b 分别是质心到前后轴的距离F_yf、F_yr 是前后轮侧向力。侧向力在轮胎线性区可以近似为 F_y -C_α * αC_α 是侧偏刚度α 是轮胎侧偏角。什么时候必须用动力学模型我的经验是车速超过 60 km/h 且曲率变化较大的场景比如高速匝道、紧急避障。这时运动学模型算出来的控制量会偏保守或者震荡动力学模型能把横摆特性和侧偏补偿考虑进去。当然动力学模型的参数获取也麻烦侧偏刚度需要标定不同路面差异很大。工程上有个折中低速用运动学高速用带侧偏补偿的简化动力学既保证精度又控制复杂度和标定成本。2.3 模型参数与离散化处理不管用哪种模型有几个参数是绕不开的轴距 L、整车质量 m、横摆转动惯量 I_z、质心位置a、b、轮胎侧偏刚度 C_α。轴距最容易拿到直接量或者查车辆规格书。质量和惯量可以通过称重和试验获得但对自研项目来说用估算值也能跑通关键是把控制器的鲁棒性调出来让参数偏差不至于让系统发散。模型最终要落到代码里运行离散化这步不能省。常用的一阶欧拉离散x(k1) x(k) T * f(x(k), u(k))T 是采样周期。采样周期的选择有讲究太快会放大噪声太慢会丢失动力学高频信息。我的经验是控制环采样 50 到 100 毫秒比较合适和大部分执行器的响应时间能对上。比这更快的部分交给底层伺服环处理算法层不需要也无须重复。3. 横向控制算法实现从几何法到最优控制3.1 Pure Pursuit 纯跟踪理解“预瞄”思想的最好入口Pure Pursuit 的出发点非常朴素公交车司机开车的直觉。老司机盯的不是车头正前方而是前方一段距离外的路面点打方向让车沿一条弧线滑过去。算法把这个过程模型化想象车辆要追踪前方一定距离处的一个目标点控制方向盘让车辆沿着一条圆弧驶向那个点。这个“前方一定距离”就是预瞄距离lookahead distance是整个算法最核心的参数。算法流程分三步。第一步在参考轨迹上找到离车辆当前位置最近的点。第二步从最近点出发沿轨迹向前找距离等于预瞄距离的目标点。第三步根据目标点相对车身的方位角 α计算前轮转角 δ。核心公式δ arctan(2 * L * sin(α) / ld)其中 ld 是预瞄距离。这个公式推导起来不复杂本质是让车辆沿一条经过目标点的圆弧运动圆弧半径 R 与转角的关系近似为 δ arctan(L / R)几何关系给出 R ld / (2 * sin(α))代进去就是上面这个标准形式。代码实现很简洁我用 Python 写过一版import math def pure_pursuit_steer(state, path, lookahead_distance, max_steer): nearest_idx find_nearest_index(state, path) target_idx find_target_index(path, nearest_idx, lookahead_distance) target path[target_idx] alpha math.atan2(target.y - state.y, target.x - state.x) - state.yaw delta math.atan2(2.0 * state.wheelbase * math.sin(alpha), lookahead_distance) return max(-max_steer, min(max_steer, delta))预瞄距离怎么调太小车辆会高频蛇形摆动太大车辆会切弯过急弯时直接刮内侧。常见做法是让预瞄距离随速度线性增长ld k_v * v ld_min。我常用的起点是 ld_min 2 米k_v 取 0.5 到 1 秒之间然后根据仿真效果微调。调试时先固定速度在不同曲率的轨迹上观察跟随效果再逐步提速看预瞄距离是否跟得上。3.2 Stanley 方法让前轴中心对齐轨迹相比 Pure Pursuit 的“预瞄后再追点”Stanley 方法的思路是“当前就修正”。它以车辆前轴中心为参考点控制律包含两部分航向误差项和横向误差项。δ ψ_error arctan(k * e / v)其中 e 是前轴中心到参考轨迹最近点的横向距离ψ_error 是车辆航向和轨迹切向方向的夹角k 是增益系数v 是车速。这个公式的含义可以拆开理解。航向误差项负责让车头方向和轨迹方向保持一致方向不对就立即打方向修正。横向误差项负责消除偏移车离轨迹远了就朝轨迹方向额外打方向距离越远修正越强。分母上的 v 是关键速度越高同样的横向误差产生的转角越小避免高速时修正过度。Stanley 的一个突出优势是能精确消除稳态横向误差而纯跟踪本质上是在追踪一个前方目标点轨迹内侧的误差不会被完全消掉。所以 Stanley 在平滑道路上的跟踪精度通常高于 Pure Pursuit。代价是对轨迹的连续曲率比较敏感轨迹有折角或曲率跳变时转角输出会突然变化乘客体感明显。我自己测试下来的感受是城市低速场景和封闭园区道路Stanley 的跟踪精度比 Pure Pursuit 高一个档次。但它不适合原地转向和倒车工况那些场景还是得靠纯跟踪或者专门的泊车策略。3.3 LQR把误差动力学当作最优控制问题LQRLinear Quadratic Regulator线性二次型调节器是进入现代控制理论的起点。它不再靠直觉加增益而是把误差系统建模成线性状态空间形式然后设计一个状态反馈控制器使某个二次型代价最小化。横向控制的误差状态通常选取为横向误差 e、横向误差变化率 ė、航向误差 Δψ、横摆角速度误差 Δψ_dot。把车辆模型在参考点处线性化得到x_dot A * x B * u其中 x 是误差状态向量u 是前轮转角增量。然后设计代价函数J ∫ (x^T Q x u^T R u) dtQ 矩阵衡量对各个状态误差的重视程度R 矩阵约束控制量的代价。解 Riccati 方程得到最优反馈增益 K控制律就是 u -K * x。求解 Riccati 方程现在已经有很多现成库Python 里可以用 scipy.linalg.solve_continuous_are实现难度主要在设计状态空间模型和调权重上。整定 LQR 的核心在 Q 和 R 的选择。我的经验是Q 里的横向误差项权重给大一些因为横向偏差是乘客最容易感知的R 给适中的值太大车反应迟钝太小转向频繁。通常先固定 R调 Q 的相对比例观察仿真里的误差曲线和转向曲线找到一个“不抖、误差小”的平衡点。道路曲率前馈可以额外加一项抵消稳态航向偏差。LQR 的优点是把多状态耦合系统化地考虑进去了比单变量 PID 更严谨。缺点是它是线性控制器在大转角、非线性区域效果退化而且本身没有约束处理能力可能算出超过物理极限的转角所以输出必须接一个限幅器。3.4 MPC带约束的滚动时域优化MPCModel Predictive Control模型预测控制是当前工程里能落地的横向控制算法中能力最强的一种。它的核心思想是每个控制周期用车辆模型预测未来 N 个步长的车辆状态在约束条件下求解一个优化问题最小化跟踪误差和控制量然后只执行第一步的控制量下一周期滚动重算。MPC 的优化问题一般写成min Σ (y_ref - y_pred)^T Q_y (y_ref - y_pred) u^T R_u u s.t. δ_min ≤ δ ≤ δ_max δ_dot_min ≤ δ_dot ≤ δ_dot_max a_min ≤ a ≤ a_max约束可以覆盖转向角度、转向速率、加速度甚至轮胎滑移率这是几何法和 LQR 做不到的。代价是计算量大需要在线求解二次规划QP问题对嵌入式算力有要求。我见过一些算力受限的方案用查表 MPC 或者显式 MPC 来规避在线求解把最优控制的解面离线算好在线只做查表。MPC 调试最大的坑是预测时域和控制时域的设置。预测时域太短控制器看不到前方的弯道入弯就晚了太长计算量上升而且模型误差累积导致预测失真。一般先固定控制时域比如 5 步再调预测时域20 到 50 步同时要特别注意模型失配问题——如果仿真模型和实车差异大MPC 预测得再准也是错的。3.5 四种算法横向对比与选型建议把这四种方法放在一张表里对比结论就很清楚指标Pure PursuitStanleyLQRMPC实现难度极低低中高跟踪精度中中高高很高约束处理无无无有算力需求极低极低低高抗模型误差强中中中选型经验给几条参考做学习 Demo 和快速原型Pure Pursuit 起步最快做封闭场地低速物流车Stanley 性价比最高做结构化道路高速巡航LQR 值得认真实现一版如果目标是接近量产工程水平MPC 是绕不开的方向。但我的建议是哪怕最终方案是 MPC也先把前面的几何法和 LQR 都亲手实现一遍。控制的核心感觉——什么样的误差对应什么样的转向修正——就是在由浅入深的实现过程中建立的。直接跳到 MPC遇到问题都不知道该往哪个环节查。4. 纵向速度控制实现4.1 速度环 PID油门刹车是两张不同的表纵向控制的执行器和横向完全不同它有两个指令通道油门和刹车而这两者的响应特性差异很大。踩油门时发动机响应有延迟刹车更直接但容易过度减速。所以纵向控制不能用一个 PID 输出一个“踏板值”然后既当油门又当刹车。常见做法是做一个油门刹车切换逻辑期望加速度为正时输出油门为负时输出刹车中间设一个死区避免频繁切换。死区大小很讲究太小会让油门刹车来回切换车一窜一窜的太大则速度误差变大。我一般用 ±0.2 m/s² 左右的加速度死区再配合滞后切换hysteresis——油门转刹车和刹车转油门用不同的切换阈值避免临界点抖动。基础速度 PID 的结构可以写成这样class SpeedPID: def __init__(self, kp, ki, kd, max_accel): self.kp kp self.ki ki self.kd kd self.max_accel max_accel self.integral 0.0 self.last_error 0.0 def compute(self, target_speed, current_speed, dt): error target_speed - current_speed self.integral error * dt derivative (error - self.last_error) / dt self.last_error error output (self.kp * error self.ki * self.integral self.kd * derivative) return max(-self.max_accel, min(self.max_accel, output))这里的 output 是期望加速度后续经过油门刹车切换逻辑映射成执行器指令。积分项抗静差很重要——比如持续上坡时如果没有积分补偿速度会一直偏低。4.2 前馈查表与反馈修正结合纯 PID 的毛病是响应慢、超调大因为反馈控制总是等误差出现才动作。更好的做法是加一个前馈feedforward通道直接根据期望速度和工况查表得到基准油门或刹车值PID 只负责修正偏差。前馈表怎么来最直接的办法是做标定实验在水平路面让车辆以不同稳定速度匀速行驶记录稳态油门踏板值就得到一张“速度→油门”的查表刹车侧类似记录不同减速度对应的刹车踏板值。这张表有平台差异性换一辆车就要重新标但一旦做好纵向控制的体验提升非常明显——起步跟脚、巡航稳定PID 只需要做小幅修正。我在实验车上调纵向时对前馈表的价值感受很深。只靠 PID速度从 30 升到 50 需要三四秒且有超调加上前馈表两秒内平稳到位超调几乎为零。前馈和反馈不是二选一而是互相配合前馈负责大趋势反馈负责消除小误差。4.3 舒适性jerk 才是乘客感知的关键纵向控制里乘客是否满意往往不看速度误差大小而看加速度的变化率也就是加加速度jerk。一脚深一脚浅的冲击感就是 jerk 太大。所以纵向控制输出端要加一个 jerk 限制器把加速度指令的变化斜率限制住。def limit_jerk(accel_cmd, last_accel_cmd, max_jerk, dt): delta_a accel_cmd - last_accel_cmd max_delta max_jerk * dt if delta_a max_delta: return last_accel_cmd max_delta elif delta_a -max_delta: return last_accel_cmd - max_delta return accel_cmd这个限制器的物理含义很直接加速度不能瞬间跳变要有过渡。我常用 max_jerk 在 1.5 到 2.5 m/s³ 之间追求乘坐舒适就取低一点需要快速响应的工况比如紧急制动前的预减速就取高一点。实现的时候注意限制器的输入输出要存上一次值跨周期持续生效否则每帧重启限制就没有意义了。5. 仿真环境搭建与参数整定实操5.1 最小闭环仿真怎么搭验证控制算法不需要一上来就上一个重型仿真器。我推荐先搭一个最小闭环系统一个车辆模型运动学或动力学加你的控制器再加一条参考轨迹和一个简单可视化。用 Python 加 NumPy 加 Matplotlib 就能搞定跑一轮几十毫秒改参数、看曲线的迭代速度极快。仿真的主循环长这样for step in range(total_steps): # 1. 根据当前状态计算控制量 steer_cmd lateral_controller(state, path) accel_cmd longitudinal_controller(speed_ref, state.speed) # 2. 模拟执行器延迟 steer_actual apply_delay(steer_cmd, delay_steps) # 3. 用车辆模型更新状态 state vehicle_model_update(state, steer_actual, accel_cmd, dt) # 4. 记录日志 log(state, steer_cmd, accel_cmd, step, t)这里有两个细节值得强调。一是执行器延迟真实车辆的方向盘和油门都有延迟仿真里一定要模拟进去否则整定出来的参数实车上很快就露馅。二是传感器噪声位置和航向反馈有噪声建议在仿真里给状态加一点高斯噪声看看控制器稳不稳。一个在无噪声环境下调出来的控制器加上噪声后很可能直接振荡提前暴露这个问题能省很多实车时间。5.2 三个核心参数整定的顺序参数整定有固定顺序乱调会越调越乱。我的经验是这样走第一步把纵向 PID 调稳。让车辆能稳定跟随一条恒定速度曲线用阶跃响应看超调量和稳定时间。第二步调横向几何法。用一条中等曲率的圆形轨迹或双移线轨迹从低速开始跑逐步提速观察误差变化。第三步再调高级控制器参数比如 LQR 的权重矩阵、MPC 的时域长度。在几何法已经能稳定跟住轨迹的基础上再去优化精度和舒适性。每个阶段只看一个目标指标。调横向时同时看两条曲线横向误差随时间的变化、前轮转角随时间的变化。误差大就加大增益转角抖就降低增益或增大预瞄距离。一次只改一个参数改完看趋势再决定下一步。这是控制调试的铁律违反它你永远不知道是哪个参数把系统搞成这样的。5.3 从仿真到实车模型误差、执行器延迟、安全限幅仿真跑得再漂亮也不代表实车一次通过。我拿自己的经验说几个最现实的差距。模型参数偏差方面仿真里的轴距、质量、轮胎刚度都是理想值实车一定会有差异。所以控制器增益不能压着稳定性边界调要留 30% 以上的余量。执行器非线性方面真实方向盘有摩擦和转向柱间隙油门响应有非线性区间仿真里的理想执行器会掩盖这些问题。通讯延迟方面控制指令通过总线到执行器再反馈状态回来一般有几十到上百毫秒的延迟仿真里不模拟实车就会震荡。所以从仿真转到实车时我的第一个动作不是调参数而是把安全限幅完整检查一遍最大转角、最大加速度、急停逻辑是否正常。控制算法出 bug 顶多是跟不住轨迹安全逻辑出问题才是真正的大事故。这个顺序不能反一次都不能省。6. 常见问题与排查技巧实录6.1 车辆蛇形或振荡现象是车辆沿直线行驶但方向左右摆动幅度时大时小。这是横向控制最经典的问题原因一般有三个增益太高导致反馈过强预瞄距离太短导致对误差过度敏感或者反馈信号噪声过大。排查顺序我建议这样走先看状态反馈曲线有没有高频噪声有就先加滤波然后把增益直接砍一半看振荡是否明显减弱最后逐步加大预瞄距离找到一个“再大就切弯”的临界值回退 20% 作为工作点。注意一次只改一个变量不然没法定位。6.2 弯道内侧切弯或外侧超调入弯时切内侧通常是预瞄距离太大在曲率大的弯道里纯跟踪看不到近处的弯点。解法是按曲率自适应调整预瞄距离曲率大时减小预瞄曲率小时增大预瞄。弯道出口外侧超调则通常是出弯时速度还高、横向增益不足优先降速或者增加出弯段的阻尼项。把这类问题的排查经验总结成一句话切弯先调预瞄甩尾先降速度切忌一上来就猛加横向增益。很多新手遇到误差大就加增益结果蛇形越来越严重方向完全搞反。6.3 纵向速度静差或来回闯动速度无法收敛到目标值多半是积分不够或前馈表不准。先看稳定状态下误差是恒定值还是周期性波动恒定值说明积分补偿不足加积分增益周期性波动大概率是油门刹车切换太频繁把死区调大或加滞后切换。还有一种隐蔽的情况期望速度曲线本身有高频抖动下游 PID 在拼命追一个本来就不平滑的目标。这属于规划模块的锅应该回头把速度曲线做平滑限速处理而不是让控制去硬扛。6.4 控制量饱和之后恢复不了转角到达极限后误差仍然很大控制器持续输出最大转角弯过完之后还顶着限幅车就冲出去了。这是积分饱和的变体。解法是加抗饱和逻辑如果上一周期控制量已经处于饱和区这一周期就不再累积积分项。对 MPC 来说控制量约束本身包含了这个处理所以这是 MPC 相对 LQR 加限幅方案的一个结构性优势。我把排查中经常用的一组检查项整理成速查表方便现场对照现象先查什么再改什么直线蛇形传感器噪声、增益滤波、减增益、加预瞄入弯切内预瞄距离按曲率自适应预瞄出弯甩尾出弯速度降速、加阻尼速度静差前馈表、积分项标定前馈、加积分油门刹车闯动切换死区加死区、加滞后饱和后失控抗饱和逻辑积分钳位、约束处理高速发散是否用了动力学模型换动力学模型、降增益6.5 从日志里快速定位问题的习惯最后分享一个长期养成的习惯控制调试全程记录日志至少记四类数据——指令值、反馈值、参考值、中间计算量比如预瞄目标点、横向误差、积分项、前馈值。很多问题光看最终输出曲线看不出来把中间量拉出来对比基本上一眼就能定位是哪个环节出了岔子。我自己习惯用 CSV 格式落盘每个控制周期一行字段包含时间戳、期望速度、实际速度、期望转角、实际转角、横向误差、航向误差、积分项、前馈值。实车测试后直接把曲线拉出来横纵对比排查效率比现场裸眼看现象高太多。有时候问题不是控制器本身而是上游的轨迹突变或者定位跳变有了日志才能快速甩锅还是背锅。这个模块做久了我的体会是控制不像感知那样有明确的精度指标可以反复刷榜也不像规划那样能展示各种漂亮的轨迹它更像是那种“做对了没人夸、做错了全车人骂”的苦活。但恰恰是这份苦活决定了整套无人驾驶系统的下限。理论算法再漂亮控制跟不住车上坐的人就一句话这车不行。最后再分享一个小技巧如果刚开始学这一块别急着上真车。把同一个算法在仿真里换成不同形状的轨迹、不同的速度、不同偏差的车辆参数反复压测你对算法边界的理解会指数级上升。控制算法的感觉不是看论文看出来的是一组一组参数亲手调出来的。