1. 前馈速度控制到底在解决什么问题1.1 从“看到弯道再减速”说起如果你接触过自动驾驶的纵向控制大概率是从PID调参开始的。给定一个目标速度用当前速度和目标速度的差值去算加速度这就是最朴素的反馈控制。但实际跑起来你会发现一个问题车总是“慢半拍”。入弯的时候明明规划模块已经给出了前方弯道的曲率信息但速度控制还在等误差出现才动作结果就是入弯速度偏高要么触发ESP要么乘坐体验极差。前馈速度控制要解决的就是这个“慢半拍”的问题。它的核心思想非常直白不等误差出现提前根据已知的路径信息算出需要的控制量。就像你开车看到前方有个上坡你会提前踩油门而不是等车速掉下来才补油。前馈控制就是把这个“老司机的预判”变成数学表达。在自动驾驶的纵向控制架构里前馈通常和反馈是配合使用的。反馈负责消除稳态误差和抵抗扰动前馈负责提供“基准控制量”让反馈只需要处理小偏差。两者叠加才能既快又稳。1.2 前馈控制在速度环中的位置一个典型的纵向控制链路是这样的规划模块输出一条带速度规划的轨迹控制模块拿到轨迹后先做前馈计算得到基准加速度再叠加反馈修正量最后输出给执行器。前馈在这里的角色是“主力”反馈是“补刀”。为什么这么说因为前馈是基于模型的计算它知道当前路径的曲率、坡度、目标速度可以直接算出理论上需要的加速度。而反馈是基于误差的它不知道前方有什么只能等误差出现。如果前馈算得准反馈的负担就很小整个系统的响应速度和稳定性都会好很多。我见过不少团队一开始只调PID调到最后发现响应和超调始终无法兼顾。后来加了前馈PID参数几乎不用大改效果立竿见影。这不是PID不行而是PID的定位就是处理误差预判的事应该交给前馈。1.3 适合谁来看这篇内容这篇内容适合正在做自动驾驶纵向控制、速度规划、或者底盘控制的朋友。如果你正在被“入弯速度控制不准”“坡道速度波动大”“启停不平顺”这些问题困扰前馈速度控制大概率能帮到你。如果你是完全的新手建议先了解一下PID控制和车辆纵向动力学的基础再来看前馈会更容易上手。我会从原理推导讲到代码实现再讲到实车调试中踩过的坑。所有参数和公式都会给出计算过程你可以直接照着复现。文中涉及的项目和场景均为虚构代称但技术细节都是真实可用的。2. 前馈速度控制的核心原理拆解2.1 纵向动力学模型前馈的数学基础前馈控制不是拍脑袋给一个加速度它需要基于车辆纵向动力学模型来计算。最基础的模型是牛顿第二定律m * a F_drive - F_brake - F_roll - F_air - F_grade其中m是整车质量a是纵向加速度F_drive是驱动力F_brake是制动力F_roll是滚动阻力F_air是空气阻力F_grade是坡道阻力。前馈控制的目标就是给定目标加速度a_des反算出需要的驱动力或制动力。但这里有个问题——我们通常不是直接控制力而是控制油门开度或刹车压力。所以还需要一个“力到执行器”的映射这个映射通常用标定表来实现。在实际工程中我们不会把上面那个公式全部展开算因为空气阻力和滚动阻力的参数很难实时精确获取。常见的做法是简化a_ff k1 * a_des k2 * v k3 * grade k4 * curvature * v^2其中k1到k4是标定系数v是当前车速grade是坡度curvature是路径曲率。这个简化公式的每一项都有物理意义k1a_des是目标加速度的直接映射k2v是速度相关的阻力补偿k3grade是坡道补偿k4curvature*v^2是弯道阻力补偿。我试过用完整模型和简化模型做对比在低速场景下两者差异不大但高速时简化模型的弯道补偿项非常关键。因为高速过弯时轮胎的侧偏特性会显著增加纵向阻力这个阻力如果不补偿入弯速度会明显偏低。2.2 为什么前馈比反馈更适合处理已知扰动反馈控制的本质是“误差驱动”没有误差就没有输出。这意味着对于已知的、可预测的扰动反馈总是滞后一个周期。前馈控制则不同它可以在扰动发生之前就给出补偿量。举个例子车辆即将进入一个半径100米的弯道目标速度从80km/h降到60km/h。如果只用反馈速度控制会在检测到速度偏差后才开始减速但此时车辆已经入弯减速距离可能不够。而前馈控制可以在车辆距离弯道还有50米时就根据曲率信息算出需要的减速度提前开始减速。这个“提前量”的计算需要结合规划模块给出的轨迹信息。通常规划模块会输出一条带时间戳的轨迹每个轨迹点包含位置、速度、加速度、曲率等信息。前馈模块只需要取当前时刻对应的轨迹点就能算出基准加速度。注意前馈的提前量不是越大越好。如果提前太多车辆会在不需要减速的地方就开始减速影响通行效率。通常提前量取1到2秒的预瞄距离比较合适具体值需要根据车速和制动响应时间标定。2.3 前馈与反馈的耦合方式前馈和反馈的耦合方式直接决定了最终的控制效果。常见的耦合方式有两种并联式和串联式。并联式是前馈和反馈各自独立计算然后相加a_cmd a_ff a_fb串联式是反馈的输出作为前馈的修正a_cmd a_ff * (1 a_fb)我实测下来并联式更适合大多数场景。因为前馈是基于模型的计算反馈是基于误差的修正两者物理意义不同直接相加更直观。串联式在模型不准的时候容易产生耦合振荡调试起来更麻烦。但并联式也有一个问题如果前馈算得不准反馈需要承担很大的修正量这时候如果反馈增益太高容易振荡增益太低又修正不过来。所以前馈的精度直接决定了反馈参数能调多“激进”。2.4 前馈控制的关键输入信号前馈控制需要几个关键输入目标加速度、当前车速、道路坡度、路径曲率。其中目标加速度和曲率来自规划模块车速来自车辆状态估计坡度来自惯导或估计模块。这几个信号的精度直接影响前馈效果。我遇到过最坑的问题是坡度估计不准导致前馈在坡道上要么补偿过度要么补偿不足。后来加了一个坡度滤波和限幅情况才好一些。曲率信号也有类似问题规划模块输出的曲率有时候会跳变如果不做平滑前馈输出会跟着跳乘坐体验很差。实操心得所有前馈输入信号都要做低通滤波截止频率根据信号特性选择。车速信号滤波截止频率可以高一些比如10Hz坡度和曲率信号滤波截止频率要低一些比如2Hz。滤波带来的相位滞后可以通过预瞄来补偿。3. 前馈速度控制的实操实现3.1 从规划轨迹中提取前馈所需信息规划模块输出的轨迹通常是一个数组每个元素包含时间戳、位置、速度、加速度、曲率、坡度等信息。前馈模块需要做的是根据当前车辆位置在轨迹数组中找到对应的点然后取出目标加速度和曲率。这里有个细节车辆当前位置和轨迹点位置通常不是精确对齐的需要做插值。我一般用线性插值因为轨迹点足够密通常10ms一个点线性插值的精度已经够了。如果轨迹点比较稀疏可以用样条插值但计算量会大一些。import numpy as np def find_trajectory_point(trajectory, current_s): 在轨迹中找到距离当前里程最近的点的索引 trajectory: 轨迹数组每个元素包含 s, v, a, curvature, grade current_s: 当前车辆沿轨迹的里程 s_array np.array([p[s] for p in trajectory]) idx np.searchsorted(s_array, current_s) idx np.clip(idx, 1, len(trajectory) - 1) return idx def interpolate_trajectory(trajectory, current_s): 线性插值获取当前里程处的轨迹信息 idx find_trajectory_point(trajectory, current_s) p0 trajectory[idx - 1] p1 trajectory[idx] if p1[s] p0[s]: ratio 0.0 else: ratio (current_s - p0[s]) / (p1[s] - p0[s]) result {} for key in [v, a, curvature, grade]: result[key] p0[key] ratio * (p1[key] - p0[key]) return result这段代码的核心是searchsorted和线性插值。searchsorted找到当前里程在轨迹数组中的位置然后取前后两个点做插值。注意idx要限制在[1, len-1]范围内防止越界。3.2 前馈加速度的计算与标定拿到目标加速度、车速、曲率和坡度后就可以计算前馈加速度了。我用的公式是a_ff k1 * a_des k2 * v k3 * grade k4 * curvature * v^2其中k1到k4是标定系数。k1通常取1.0左右因为目标加速度本身就是前馈的主要部分。k2是速度阻力系数和车辆的风阻系数、迎风面积、传动效率有关。k3是坡度补偿系数通常取9.8 * sin(grade)的近似值。k4是弯道阻力系数和轮胎特性、悬挂参数有关。标定这几个系数的方法先让k2、k3、k4为0只保留k1在平直道路上跑不同加速度看实际加速度和目标加速度的偏差调整k1。然后上坡道标k3跑弯道标k4最后跑高速标k2。我标定的时候发现k4对乘坐体验影响最大。k4太小入弯速度偏高乘客会感觉被甩k4太大入弯速度偏低乘客会感觉车在“拖”。通常k4在0.02到0.05之间比较合适具体值取决于车型。def compute_feedforward_accel(target_a, v, grade, curvature, k1, k2, k3, k4): 计算前馈加速度 target_a: 目标加速度 (m/s^2) v: 当前车速 (m/s) grade: 道路坡度 (rad) curvature: 路径曲率 (1/m) k1-k4: 标定系数 a_ff k1 * target_a k2 * v k3 * grade k4 * curvature * v * v return a_ff3.3 前馈输出的限幅与平滑前馈输出不能直接给执行器需要做限幅和平滑。限幅是因为车辆的执行器能力有限比如最大驱动力和最大制动力都有上限。平滑是因为前馈输入信号可能有噪声如果不平滑输出会抖动。限幅的策略根据当前车速查表得到最大允许加速度和最小允许加速度最大减速度。这个表通常来自车辆标定低速时允许的加速度大一些高速时小一些。平滑的策略用一阶低通滤波截止频率根据执行器响应速度选择。我一般用5Hz到10Hz的截止频率既能滤掉高频噪声又不会引入太大的相位滞后。class FeedforwardController: def __init__(self, k1, k2, k3, k4, dt): self.k1 k1 self.k2 k2 self.k3 k3 self.k4 k4 self.dt dt self.prev_output 0.0 self.filter_alpha 0.2 # 低通滤波系数 def compute(self, target_a, v, grade, curvature): a_ff (self.k1 * target_a self.k2 * v self.k3 * grade self.k4 * curvature * v * v) # 限幅 a_max self.get_max_accel(v) a_min self.get_min_accel(v) a_ff max(a_min, min(a_max, a_ff)) # 低通滤波 a_ff_filtered (self.filter_alpha * a_ff (1 - self.filter_alpha) * self.prev_output) self.prev_output a_ff_filtered return a_ff_filtered def get_max_accel(self, v): # 根据车速查表得到最大加速度 # 这里用简化公式代替标定表 return max(0.5, 2.0 - 0.02 * v) def get_min_accel(self, v): # 根据车速查表得到最大减速度 return min(-0.5, -3.0 0.05 * v)3.4 前馈与反馈的叠加及执行器映射前馈算完之后要和反馈叠加。反馈通常用PID输入是速度误差输出是加速度修正量。叠加后的总加速度需要映射到油门开度和刹车压力。映射的方法先判断总加速度是正还是负。正的话走驱动映射负的话走制动映射。驱动映射通常是一个二维表输入是加速度和车速输出是油门开度。制动映射类似输入是减速度和车速输出是刹车压力。这个映射表需要实车标定。标定的方法是固定车速给不同的加速度指令记录实际加速度和油门开度/刹车压力的关系。标定的时候要注意加速度不能跳变要斜坡增加否则标定出来的表不准。注意事项前馈和反馈叠加后一定要再做一次限幅。因为前馈和反馈可能同时往一个方向出力叠加后可能超过执行器能力。我遇到过前馈算了个大减速度反馈又加了个大减速度结果刹车压力直接拉满乘客体验极差。4. 实车调试中的常见问题与排查4.1 入弯速度偏高或偏低的排查思路入弯速度不准是最常见的问题。如果入弯速度偏高说明前馈的弯道补偿不够需要增大k4。如果入弯速度偏低说明k4太大需要减小。但有时候调k4没用因为问题可能出在曲率信号上。规划模块输出的曲率可能有延迟或者跳变导致前馈计算用的曲率和实际曲率不匹配。这时候需要检查曲率信号的滤波和预瞄。我遇到过一次入弯速度总是偏高调k4调到很大也没用。后来发现是规划模块的曲率输出比实际路径滞后了200ms前馈用的曲率是200ms之前的当然不准。后来在曲率信号上加了一个预瞄补偿问题就解决了。排查步骤可以总结成这个表现象可能原因排查方法解决措施入弯速度偏高k4太小增大k4看是否改善标定k4入弯速度偏高曲率信号滞后对比规划曲率和实际路径加预瞄补偿入弯速度偏低k4太大减小k4看是否改善标定k4入弯速度偏低曲率信号超前对比规划曲率和实际路径加延迟补偿入弯速度波动曲率信号跳变观察曲率信号波形加低通滤波4.2 坡道速度波动的解决方法坡道上前馈补偿不准会导致速度波动。上坡时如果补偿不够车速会掉下坡时如果补偿过度车速会冲。这个问题通常是因为坡度估计不准或者坡度信号延迟。坡度估计通常来自惯导或者车辆动力学估计。惯导的坡度信号比较准但可能有延迟动力学估计的坡度信号响应快但噪声大。我一般用惯导的坡度做前馈用动力学估计的坡度做反馈修正。如果坡度信号延迟严重可以在前馈里加一个坡度变化率的补偿项。坡度变化率可以通过对坡度信号做差分得到然后乘以一个系数加到前馈输出上。这个系数需要标定通常在0.5到1.5之间。实操心得坡道场景下前馈的限幅要放宽一些。因为坡道阻力可能很大如果限幅太紧前馈输出被截断反馈需要承担很大的修正量容易振荡。我一般把坡道场景下的加速度限幅放宽20%左右。4.3 前馈引起的抖动与振荡排查前馈本身不会引起振荡因为它是开环的。但如果前馈输出和反馈输出耦合不当或者前馈输出跳变就会引起抖动。前馈输出跳变的常见原因是输入信号跳变。比如曲率信号从0.01突然跳到0.02前馈输出就会跟着跳。解决方法是给所有前馈输入加低通滤波滤波截止频率根据信号特性选择。另一个原因是前馈和反馈的叠加方式。如果前馈输出变化太快反馈会试图去“追”前馈导致振荡。这时候可以给前馈输出加一个速率限制让前馈输出缓慢变化。def rate_limit(current, target, max_rate, dt): 速率限制 current: 当前值 target: 目标值 max_rate: 最大变化率 (单位/秒) dt: 时间步长 max_delta max_rate * dt delta target - current if delta max_delta: delta max_delta elif delta -max_delta: delta -max_delta return current delta这个速率限制函数很实用我一般把前馈输出的最大变化率限制在2.0 m/s^3左右。具体值需要根据执行器响应速度标定。4.4 常见问题速查表问题现象排查方向解决方案入弯速度不准入弯速度偏高/偏低检查k4和曲率信号标定k4加曲率预瞄坡道速度波动上坡掉速/下坡冲速检查坡度信号加坡度变化率补偿前馈输出抖动加速度指令抖动检查输入信号加低通滤波和速率限制前馈反馈耦合振荡速度振荡检查叠加方式降低反馈增益加前馈速率限制启停不平顺停车时点头检查前馈限幅低速时减小前馈增益高速时加速无力高速加速慢检查k2标定k2增大高速补偿5. 前馈速度控制的调参经验与进阶技巧5.1 分场景标定策略前馈的标定系数不是一组值走天下不同场景需要不同的标定。我通常把场景分成低速、中速、高速三类每类单独标定k1到k4。低速场景0到30km/h主要关注启停平顺性k1可以取小一点避免加速太猛。中速场景30到60km/h主要关注弯道和坡道k4和k3要标定准确。高速场景60km/h以上主要关注风阻补偿k2要标定准确。分场景标定的好处是每个场景都能调到最优坏处是场景切换时可能有跳变。解决方法是做插值根据车速在两组标定值之间线性插值。5.2 前馈与规划模块的协同前馈的效果很大程度上取决于规划模块输出的轨迹质量。如果规划模块输出的加速度本身就不平滑前馈再怎么调也没用。所以做前馈之前一定要先确认规划模块的轨迹是平滑的。我一般会检查规划轨迹的几个指标加速度变化率是否连续、曲率是否连续、速度是否连续。如果发现跳变先让规划模块做平滑再调前馈。另外规划模块的预瞄距离也会影响前馈效果。预瞄太短前馈来不及响应预瞄太长前馈会过度补偿。我一般用1.5秒的预瞄时间具体值根据车速调整。5.3 前馈控制的边界与局限性前馈控制不是万能的。它的局限性主要有三个一是依赖模型精度模型不准前馈就不准二是依赖输入信号质量信号有噪声或延迟前馈效果就打折扣三是无法处理未知扰动比如突然的侧风或者路面变化。所以前馈必须和反馈配合使用。前馈负责已知的、可预测的部分反馈负责未知的、随机的部分。两者缺一不可。我在实际项目中的体会是前馈能把控制效果从“能用”提升到“好用”但前提是反馈已经调到了一个不错的水平。如果反馈本身都没调好加前馈只会让问题更复杂。所以我的建议是先把反馈调稳再加前馈提性能。最后分享一个小技巧前馈的标定系数可以用最小二乘法自动拟合。采集一段实车数据记录目标加速度、车速、坡度、曲率和实际加速度然后用最小二乘拟合k1到k4。这样比手动标定快很多而且精度更高。拟合的时候要注意数据要覆盖各种场景否则拟合出来的系数只在特定场景下有效。
