无人车自主避障这件事真正上手做过的人都知道最难的从来不是把算法跑通而是让它在仿真里表现完美的同一套逻辑到了实车上还能稳住。我前后做过三轮从数字模型到实车部署的完整链路第一轮在仿真里跑得漂漂亮亮上车第一天就撞了三次第二轮把感知延迟补进去之后勉强能跑但一遇到动态障碍就发飘第三轮才算是把整套东西理顺。这篇就把这三轮踩过的坑、补过的课、最后沉淀下来的方案完整讲一遍从建模、控制器选型、避障策略设计一直到实车调试的细节尽量说透。1. 先想清楚为什么数字模型和实车之间隔着一道鸿沟1.1 仿真里完美的假象是怎么来的大部分人做无人车避障第一步都是在仿真环境里搭一个差速底盘模型给它一个理想的位置反馈然后跑A*或者DWA规划出一条路径再用一个PID或者纯跟踪控制器去追这条路径。仿真里跑出来的轨迹圆滑、误差曲线漂亮横向误差常年稳定在几厘米以内。这时候人很容易产生一种错觉算法已经搞定了剩下的只是部署。但仿真环境默认了几个在现实中根本不成立的前提。第一位置反馈是零延迟、零噪声的仿真里你读到的位姿就是真实位姿第二执行器是理想响应你给轮子发一个速度指令它立刻就是那个速度第三环境是静态且已知的障碍物不会突然从盲区里冒出来第四时间步是严格对齐的控制周期不会抖动。这四个假设里任何一个在实车上被打破都会让原本完美的控制器性能断崖式下跌。我第一轮部署时最直观的感受就是仿真里横向误差0.03米实车上直接变成0.3米而且是一顿一顿地修正像喝多了走路。1.2 实车上的四类真实扰动把仿真和实车的差距拆开看主要来自四个方向我习惯把它们叫做四类扰动因为它们对控制器的影响机制完全不同处理方式也不一样。第一类是感知延迟与噪声。激光雷达或者深度相机给出障碍物距离中间要经过驱动、点云处理、聚类、坐标变换整条链路下来几十到上百毫秒是常态。定位模块无论是轮式里程计融合IMU还是其他定位方式输出的位姿同样有延迟和漂移。控制器拿到的永远是过去的世界而车已经在往前走了。第二类是执行器非线性与延迟。电机有死区低速时尤其明显你发一个很小的速度指令轮子可能根本不动发大一点它又猛地窜出去。轮胎和地面之间的摩擦也不是线性的转向时存在侧滑。这些在仿真里通常被简化成一个一阶惯性环节但真实情况要复杂得多。第三类是通信与控制周期抖动。上位机算完控制量通过串口或者CAN总线发给下位机中间有传输延迟下位机的执行周期受中断、任务调度影响也不是严格等间隔。控制周期一抖离散控制器的实际表现就和理论设计偏离。第四类是环境动态性。仿真里的障碍物是静止的实车上行人、其他车辆、突然打开的门都是动态的。静态避障和动态避障在算法设计上是两回事前者可以离线规划后者必须在线滚动优化。理解了这四类扰动后面所有的设计选择就都有了依据。控制器选型、避障策略、调试方法本质上都是在和这四类扰动做对抗。1.3 一个判断标准你的模型够不够用很多人纠结要不要建一个高保真的车辆动力学模型。我的经验是看你的运行工况。如果车速低于1米每秒、路面平整、转向不急那么一个带延迟补偿的运动学模型差速或阿克曼就够用了没必要上复杂的动力学。但如果车速上到2米每秒以上或者需要频繁急转那轮胎侧偏、载荷转移这些效应就必须考虑否则模型预测控制MPC预测出来的轨迹和实际走的轨迹会差得很远。判断方法很简单让车以目标车速跑一段直线然后给一个阶跃转向指令记录实际轨迹和你模型预测的轨迹对比。如果偏差在可接受范围内比如横向偏差小于车宽的十分之一模型就够用如果偏差很大说明模型缺了关键动态得补。2. 控制器选型PID、LQR、MPC、ADRC到底怎么选2.1 从需求反推控制器而不是从算法热度反推网上关于控制算法的讨论特别多LQR、MPC、ADRC、模糊PID每个都有人说好。但我踩过的最大坑就是追热度选算法——看到MPC能处理约束、能滚动优化就直接上MPC结果调参调到怀疑人生实时性还跟不上。正确的做法是从需求反推。先问自己三个问题第一我的控制周期能做到多快如果下位机只能跑50赫兹那MPC的预测时域和求解时间就要仔细权衡。第二我的模型准不准MPC和LQR都依赖模型模型不准它们还不如PID。第三我的工况对约束处理的要求高不高如果只是简单避障DWA加底层PID就够了不一定需要MPC。2.2 四种控制器的适用边界我把这四种控制器在无人车避障场景下的适用边界整理成了一张表这是我实际用过之后的总结不是教科书上的理论对比。控制器适用场景优势主要短板我的实测感受PID/模糊PID低速、路径平缓、模型不准实现简单、鲁棒、无需精确模型无法处理约束、大延迟下易振荡底层速度环首选横向控制勉强够用LQR中低速、模型较准、需要最优性有理论最优解、计算量小无法显式处理约束、对模型误差敏感横向跟踪很稳但需要仔细调Q/RMPC中高速、需要处理约束、动态避障能处理约束、能预测、适合动态环境计算量大、依赖模型、调参复杂动态避障效果好但实时性是门槛ADRC模型不准、扰动大、执行器非线性强抗扰强、不依赖精确模型参数多、理论门槛高低速抖动场景表现突出这张表不是绝对的实际项目里经常是组合使用。我最后落地的方案是底层速度环用增量式PID带死区补偿横向控制用LQR上层避障用MPC做滚动规划。这样既保证了实时性又兼顾了动态避障能力。2.3 为什么我最终选了LQR加MPC的组合单独用MPC做横向控制计算量太大50赫兹的控制周期下每次求解都要几十毫秒留给避障规划的时间就不够了。单独用LQR做避障又处理不了障碍物约束。所以我把两者分层MPC负责在预测时域内规划一条满足避障约束的局部轨迹LQR负责高频跟踪这条轨迹。这个分层的关键在于接口设计。MPC输出的是一条带时间戳的轨迹点序列LQR以固定周期读取最近的轨迹点作为参考同时用当前位姿做反馈。两层之间的时间对齐很重要如果MPC输出的轨迹时间戳和LQR的当前时间对不上跟踪就会滞后。LQR的调参我走了不少弯路。Q矩阵代表状态误差的权重R矩阵代表控制量的权重。一开始我把Q调得很大想让跟踪更紧结果控制量抖得厉害车在路上画龙。后来把R适当加大控制量平滑了但跟踪又变松。最终的平衡点是位置误差权重和航向误差权重之比大约3比1控制量权重取位置误差权重的十分之一左右。这个比例不是理论推导出来的是我在实车上一点点试出来的供参考。2.4 ADRC在什么情况下值得上ADRC自抗扰控制这两年被提得很多它的核心思想是把所有不确定扰动都当成一个总扰动用一个扩张状态观测器去估计并补偿掉。这个思路在无人车低速、执行器非线性强的场景下确实有效。我试过在一个低速巡检车上用ADRC做速度环效果比PID好尤其是在过减速带、地面摩擦突变的时候ADRC的恢复速度明显更快。但ADRC的参数太多扩张状态观测器的带宽、控制器的带宽、非线性函数的参数每一个都要调而且参数之间的耦合关系不直观。如果团队里没有对ADRC比较熟的人我建议还是先用PID加前馈把基础打牢。3. 避障策略从静态地图到动态滚动规划3.1 静态避障和动态避障是两套逻辑静态避障的本质是路径规划问题给定一张已知地图和起点终点找一条不碰障碍物的路径。A*、Dijkstra、RRT这些算法都能做做完之后用纯跟踪或者LQR去追。这套逻辑在结构化、静态的环境里很好用比如仓库、固定路线的园区。但动态避障不一样。障碍物在动你规划的路径可能下一秒就失效了。这时候必须用滚动时域的思路每个控制周期都基于当前感知到的环境重新规划一小段轨迹只执行最近的一小段下一个周期再重新规划。这就是MPC的核心思想也是动态避障的主流方案。我第一轮部署时犯的错就是拿静态规划的路径去跑动态环境结果遇到一个横穿的行人规划器还在追那条旧路径车直接冲过去了。后来改成滚动规划每个周期重新算才解决。3.2 障碍物表示栅格、多边形还是距离场避障算法怎么表示障碍物直接影响计算效率和避障效果。常见的有三种表示方式。栅格地图是最直观的把环境离散成一个个小格子有障碍物的格子标记为占用。优点是简单、通用A*和DWA都直接支持。缺点是分辨率受限于内存和计算格子太大避障不精细格子太小计算量爆炸。多边形表示是用凸多边形或者多边形集合来描述障碍物轮廓适合激光雷达聚类之后的障碍物。优点是紧凑、精确适合做几何约束。缺点是需要做凸分解非凸障碍物处理起来麻烦。距离场是预先计算每个点到最近障碍物的距离形成一个场。优点是查询快适合做基于梯度的优化。缺点是需要预计算动态障碍物更新成本高。我在MPC里用的是多边形表示加距离约束。每个障碍物用一个凸多边形近似MPC的约束就是车辆轮廓多边形和障碍物多边形之间的距离大于安全阈值。这个约束是非线性的实际求解时做了线性化近似在障碍物距离较远时精度够用距离很近时需要缩小控制周期或者用更精确的约束。3.3 安全阈值的设定不能拍脑袋安全阈值设多少直接关系到避障的安全性和通行效率。设太大车在窄通道里过不去设太小遇到突发情况刹不住。我的做法是分两层设定。第一层是硬约束也就是绝对不能碰的距离这个值等于车辆外廓到障碍物的最小几何距离通常取车宽的一半再加5到10厘米。第二层是软约束也就是开始减速或者转向的预警距离这个值和车速有关用制动距离公式估算制动距离等于车速平方除以两倍减速度。假设最大减速度是2米每平方秒车速1米每秒时制动距离0.25米车速2米每秒时制动距离1米。软约束距离取制动距离再加一个安全余量。这个计算看起来简单但实际调试时我发现感知延迟会让有效制动距离变长。如果感知延迟是100毫秒车速1米每秒那么这100毫秒里车已经走了0.1米这部分必须加到软约束距离里。所以最终的软约束距离等于制动距离加感知延迟距离加安全余量。3.4 动态障碍物的预测匀速模型够不够动态避障里预测障碍物未来位置是关键。最简单的预测是匀速模型假设障碍物保持当前速度直线运动。这个模型在障碍物运动规律时够用但遇到突然转向或者加减速就失效。我试过匀速模型和匀加速模型实测下来在园区场景里行人、自行车为主匀速模型加一个不确定性膨胀把预测位置周围扩一个随时间增大的圆效果就不错。匀加速模型对参数估计更敏感噪声大的时候反而更差。如果场景里有车辆可以考虑用交互式多模型IMM做预测但这套东西实现复杂调参也麻烦除非场景确实需要否则不建议一上来就上。4. 从仿真到实车的部署链路4.1 仿真环境要尽量脏仿真环境如果太干净部署到实车必然翻车。我的经验是在仿真里就要主动注入四类扰动给感知加高斯噪声和固定延迟给执行器加死区和一阶惯性给控制周期加随机抖动给环境加动态障碍物。具体做法是在仿真里加一个扰动层所有传感器数据经过这个层再给控制器所有控制指令经过这个层再给车辆模型。这个层的参数根据实车实测数据来标定比如感知延迟实测是80毫秒仿真里就注入80毫秒延迟。这样做的好处是控制器在仿真里就已经见过这些扰动部署到实车时不会因为扰动而性能骤降。我第二轮部署之所以比第一轮顺利就是因为加了这一层。4.2 实车调试的分阶段策略实车调试不能一上来就跑完整避障要分阶段。我的分阶段策略是这样的。第一阶段静态标定。车不动标定传感器外参、轮速计尺度因子、IMU零偏。这一步做不好后面全是错的。我见过有人跳过这一步结果定位漂移得离谱还以为是算法问题。第二阶段低速直线跟踪。让车以0.3米每秒跑直线调底层速度环PID确保速度跟踪误差小于5%。然后跑定曲率圆弧调横向控制器确保横向误差小于10厘米。第三阶段静态避障。在路径上放静止障碍物测试规划器能不能绕开。这个阶段主要验证规划逻辑和约束处理。第四阶段动态避障。加入移动障碍物从慢速、可预测的运动开始逐步增加速度和不确定性。第五阶段长距离连续运行。让车连续跑几十分钟观察累积误差和偶发问题。每个阶段都要有明确的通过标准不达标不进入下一阶段。我第一轮就是跳过了第二阶段直接上避障结果底层速度环都没调好避障逻辑再对也没用。4.3 日志系统出问题时你唯一的朋友实车调试最怕的是偶发问题跑十次出一次复现不了。这时候唯一的依靠就是日志。我的日志系统记录三类数据第一类是原始传感器数据包括激光雷达点云、IMU、轮速计用于事后回放第二类是中间计算结果包括定位位姿、规划轨迹、控制量用于分析算法行为第三类是系统状态包括各模块耗时、CPU占用、通信延迟用于排查性能问题。日志要带精确时间戳最好用统一时钟源。我一开始用各模块自己的时钟结果分析时对不齐浪费了很多时间。后来改成所有模块用同一个单调时钟问题就好定位多了。另外日志要能回放。我写了一个回放工具把记录的传感器数据按时间戳重新喂给算法这样偶发问题可以在离线环境里反复复现和调试。这个工具帮我解决了好几个实车上根本没法调试的问题。4.4 实时性保障别让算法饿死执行器实车上控制周期抖动是常态。如果算法偶尔算得慢超过了控制周期执行器就会饿死车的行为就会异常。保障实时性有几个手段。第一给控制算法设一个时间预算比如控制周期是20毫秒算法预算就是15毫秒留5毫秒余量。第二MPC求解设最大迭代次数或者最大时间超时就返回当前最优解不能无限等。第三把耗时长的任务比如全局规划、地图更新放到低优先级线程不要阻塞控制线程。第四监控各模块耗时超过阈值就报警。我用的是实时操作系统加优先级调度控制线程优先级最高规划次之感知和日志最低。这样即使感知偶尔卡顿控制也不会受影响。5. 那些让我熬夜的坑和最后的解法5.1 定位漂移导致的幽灵障碍物有一段时间车在空旷地方跑得好好的突然就急刹日志显示前方有障碍物但实际什么都没有。排查了很久最后发现是定位漂移导致障碍物坐标变换错误把远处的墙搬到了车前。根因是定位模块在长时间运行后累积误差变大而障碍物坐标变换依赖定位位姿位姿一错障碍物位置就错。解法有两个一是提高定位精度融合更多传感器二是在避障逻辑里加一个合理性检查如果障碍物出现在物理上不可能的位置比如突然出现在车正前方极近距离就暂时忽略同时报警。我两个都做了。定位方面加了轮速计和IMU的紧耦合避障方面加了合理性检查。之后这类误刹就基本没有了。5.2 控制周期抖动引发的低频振荡车在直线行驶时偶尔会出现低频的左右摆动频率大概1到2赫兹。查了很久发现是控制周期抖动导致的。当控制周期偶尔变长时离散控制器的等效增益发生变化系统相位裕度下降就出现了振荡。解法是让控制器对周期变化不敏感。具体做法是用实际时间间隔做积分和微分而不是假设固定周期。另外在控制量输出上加一个低通滤波滤掉高频抖动。这两个改动之后低频振荡就消失了。5.3 动态障碍物预测失效导致的急停有一次测试一个行人从侧面突然加速横穿车的预测模型还按匀速预测结果预测位置和实际位置差了很多等发现时已经来不及绕只能急停。虽然没撞上但急停本身也是危险动作。后来我改进了预测逻辑当障碍物的运动状态发生突变加速度超过阈值时不再依赖预测而是直接按当前观测位置做保守避障同时降低车速。这个逻辑牺牲了一点通行效率但安全性提升明显。5.4 参数在不同场景下不通用在A场景调好的参数到B场景就不行了。比如在平整水泥地上调好的速度环参数到略有坡度的草地上就抖得厉害。这是因为地面摩擦特性变了执行器负载变了。解法是加参数自适应或者增益调度。简单做法是根据车速和转向角查表不同工况用不同参数。复杂做法是在线辨识地面摩擦实时调整参数。我用的是查表加简单自适应在几个典型场景下分别标定参数运行时根据工况切换。这套东西不优雅但实用。6. 给准备上实车的朋友几句实在话如果你正准备把避障算法从仿真搬到实车我有几个建议都是花钱花时间换来的。第一别急着上复杂算法。PID加LQR能解决大部分低速场景的问题先把这套跑通再考虑MPC和ADRC。我见过太多人一上来就搞MPC结果连底层速度环都没调好最后项目延期。第二仿真里一定要加扰动。干净的仿真环境会给你虚假的信心部署时摔得更惨。扰动层的参数要从实车实测来不能拍脑袋。第三日志和回放工具要早做。实车调试的时间成本很高有了回放工具很多问题可以在办公室解决不用反复跑车。第四分阶段验证每阶段有明确标准。不要跳阶段跳过的阶段迟早要补回来而且补的时候代价更大。第五安全永远第一。实车测试一定要有急停手段最好有物理急停按钮不要只依赖软件。测试场地要封闭不要在有人的地方测试。这套东西我前后做了大半年从最开始仿真里跑得漂亮、上车就撞到后来能稳定连续运行中间踩的坑基本都写在这里了。控制算法本身没有那么神秘难的是把算法和真实的物理世界对齐。对齐的过程没有捷径就是理解扰动、补偿扰动、验证补偿。希望这些经验能帮你少走点弯路。
