两轮差速无人车避障实战：从Simulink模型到STM32实车部署

第一次把仿真里跑得稳稳当当的避障程序搬到实车上，我的小车在距离纸箱还有一米多远的地方突然猛打方向，然后一头扎进旁边的椅子腿里。那一刻我意识到，数字模型和实车部署之间隔着的根本不只是一层"移植代码"的工作，而是一整套关于现实世界摩擦、延迟、噪声和饱和的隐性问题。

这篇文章记录的就是这个项目——一辆两轮差速无人车从Simulink数字模型到STM32实车部署的全过程。核心链路包括两轮差速运动学建模、VFH+DWA避障算法选型、级联PID速度与航向控制，以及最终在实车上的反复联调。如果你正在做机器人竞赛、自动化课程设计，或者想把一套仿真验证过的自主避障算法真正放到轮子上跑起来，这篇内容应该能帮你省掉十几次来回返工的时间。

1. 先算清楚这笔账：数字模型解决的是时间与安全成本

1.1 仿真不是"省一台车"那么简单

很多朋友一接触无人车避障，第一反应就是"买套底盘，挂个雷达，写个PID就开跑"。这种路径不是不行，但它有一个很现实的矛盾：避障算法的绝大部分问题都发生在障碍物附近，而实车调试一旦撞上东西，轻则撞歪雷达支架，重则烧掉电机驱动板。我的第一块TB6612就是在这种状态下报废的——不是因为电路问题，而是程序在转向时给了一个过大的反向占空比，电机瞬间堵转，驱动芯片直接冒烟。

所以在把车造好之前，我决定先花时间把整个系统在数字模型里跑一遍。数字模型的价值首先是安全：算法可以在仿真里随便撞，撞一千次也只是损失几个小时的CPU时间。其次是时间：一个参数组合，在Simulink里改完跑一次仿真只要十几秒，在实车上改参数、充电、接电脑、跑路径，一轮下来至少五分钟起步。调PID的时候我算过一笔账，仿真阶段一天能迭代接近两百个参数组合，实车联调一天能试二十个已经算很快了。

再者，仿真真正解决了"问题复现"这个难题。实车避障失败往往是一闪而过的现象——车先震了一下，然后才偏出去，你要回放数据才知道是速度环超调还是雷达丢帧。而在数字模型里，每一帧的状态都是完整记录的，我可以在同一个障碍物场景下反复回放，找到一帧一帧的因果关系。没有这种能力，几乎不可能把问题定位到具体某个参数上。

1.2 仿真与实车之间的鸿沟，到底在哪

不过数字模型绝不是"万能替身"，我在构建模型时列过一张"仿真假设 vs 现实偏差"的清单，把每一项我在仿真里默认成立的条件都写下来。最后实车联调时遇到的所有惊险意外，几乎全部能从这张清单里找到源头，它后来也成了我排错时的第一检索表。

这张表不是劝大家别做实车，而是想说明：数字模型的目的是把所有"可控的理想逻辑"先验证正确，让实车联调时的变量尽量缩小到一个或两个。如果仿真里算法本身就存在逻辑问题，实车阶段只会更乱，因为你要同时面对算法问题和物理问题，根本没法定位。

2. 数字模型这样搭：运动学、传感器与控制器分开建模

数字模型我一直强调"分开建模、连起来仿真"。不要一上来就把所有东西堆进一个大模块，否则任何一个子模块出问题，你都无法判断是运动学算错了、传感器噪声加多了，还是控制器参数不对。

2.1 两轮差速运动学模型

我的平台是两轮差速结构：左右两个驱动轮独立控制，前面一个万向轮支撑。这种底盘的运动学是整个系统最简单、也最容易算错的地基。

两轮差速的核心关系只有两组公式：

v = (v_R + v_L) / 2 ω = (v_R - v_L) / d

v是车体线速度，ω是车体角速度，v_R和v_L是左右轮线速度，d是轮距（左右轮中心之间的距离）。反过来，给定期望的v和ω，左右轮的目标速度就是：

v_R = v + ω * d / 2 v_L = v - ω * d / 2

公式看着简单，但我在数字模型里第一次跑的时候就踩了个坑：把轮距d的单位写成了厘米，速度却用了m/s，结果仿真里小车一转向就像陀螺一样原地转圈。后来我规定所有单位统一用米、秒、弧度制，再把规则注释贴在模型文件头部，这个低级错误再也没有犯过。有人会问为什么不用Gazebo直接做带物理引擎的仿真，我的回答是：Gazebo确实更贴真实，但配置雷达模型、地面摩擦参数、电机模型的门槛也更高。在验证算法逻辑的阶段，Simulink轻量模型更快、更可控。

在Simulink里，我用两个积分器分别推演车体的x、y坐标和航向角：把ω积分得到航向，把v按照航向分解到x和y方向。模型本身不复杂，但它是后面所有传感器模型和控制器的坐标基准。坐标系这一关一定要在仿真里就过掉，否则实车上雷达的点云投影到车体坐标系时，你会被方向搞到崩溃。

2.2 传感器模型：测不到边界，就谈不上避障

避障算法依赖的输入只有一类：障碍物在哪里。我最初用的传感器是单线激光雷达，因为它相对于视觉方案更直接，点云转成距离信息也不需要太多算力。在数字模型里，我给它建了三个关键参数：探测半径、水平视场角（FOV）和更新频率。

我的雷达探测半径设为8米，FOV为360度。但我不会在仿真里模拟完整点云，那样太慢。我用的是射线求交的方式：在车体周围按1度间隔发射射线，计算射线与最近障碍物的距离，得到一帧极坐标距离数组。这个数组就是后面VFH算法的直接输入。很多人喜欢在Gazebo里用现成的雷达模型，出图漂亮，但参数的语义不一定能对应到算法上。我在Simulink里用射线求交建模型，虽然简陋，却清楚知道每个参数在算法里起什么作用。

传感器模型里必须加噪声和丢帧，否则实车数据会和仿真产生巨大落差。我给每根射线加的高斯噪声标准差是0.03米，另外让雷达以10Hz更新，而控制器跑50Hz，也就是说控制器有5个周期看到的都是同一帧数据。如果你在仿真里不给传感器加随机性，实车第一次跑就会觉得"这算法怎么这么神经质"。

2.3 控制器模型：从单PID到级联PID

控制器层面，很多入门教程会让你"先写个PID就能走直线"，但避障场景对动态响应要求更高。我用的是级联PID：外环是航向角PID，内环是角速度PID，此外左右轮各带一个速度PID，整体结构是这样的：

航向角误差 -> 外环PID -> 目标角速度 -> 内环PID -> 左右轮速度差 -> 轮速PID -> PWM占空比

外环输出的是角速度期望，内环负责把实际角速度稳定到这个期望值。这样做的好处是：如果只用一个PID直接从航向误差控到PWM，任何一点打滑或地面摩擦变化都会让转向响应变得不可控；而级联结构把"希望转多快"和"实际转多快"分成两个环节，内环就算因为打滑慢了一点，外环也会慢慢修正，整体鲁棒性明显更好。

控制周期选择直接影响数字模型的复杂度。我的实车主控是STM32F103，跑裸机PID循环没有问题，但为了留足余量，我把内环速度环周期定在5ms（200Hz），航向外环周期定在20ms（50Hz），雷达数据更新则只有10Hz（100ms）。在数字模型里，我也是按这几个周期分别给采样器，而不是让所有模块都跑同一个步长。只有仿真贴合实车的节奏，后面的避障算法调参才有参考价值。

3. 避障算法选型：为什么最终是VFH加DWA

确定运动学、传感器和控制器模型后，核心的避障决策逻辑就在眼前了——车在每一帧到底往哪走、走多快。我在这一步纠结了很久，因为最终选择会直接影响后续实车阶段的调参成本和处理链路。下面把选型过程完整讲一遍，如果你也在做类似项目，可以参考这个对比框架做决定。

3.1 候选方案怎么比较

我当时在四个方案之间犹豫：VFH（向量场直方图）、DWA（动态窗口法）、TEB（时间弹性带）和基于深度学习的端到端避障。把它们放一起对比，差别很直观：

最终我选了VFH+DWA组合，原因有三个。第一，可解释性：VFH输出"该往哪个方向走"，DWA输出"以什么速度走"，每一帧的行为都能用成本和轨迹解释，出了问题可以精确回溯是方向选错了还是速度窗口太窄。第二，实时性：两种算法加起来在STM32F103上完全跑得动，不需要上树莓派做辅助加速。第三，运动学约束：DWA天然在速度空间采样，不会因为避障转向太急导致小车在光滑地面上滑出去。

深度学习方案当时也认真考虑过，但它的主要问题是数据依赖和不可解释。对一个课程设计和竞赛项目来说，我手里没有几万条带标注的避障轨迹去训练，而且一旦实车撞了，我连"为什么撞"都无法回答。在排错阶段，不可解释是致命的，所以我最终没有选择这条路。

3.2 VFH加DWA的工作逻辑

VFH的核心思路是先把传感器得到的极坐标距离数据映射成一张极坐标直方图：每个角度扇区根据障碍物距离计算障碍密度，距离越近密度越高。然后用一个阈值把直方图切成"可通行"和"不可通行"两类区域。在可通行区域里，根据三个成本选择最优方向：与目标方向的偏差、与前一个选择方向的角度差、离障碍物的距离余量。三者加权和最小的方向，就是VFH输出的目标方向。

DWA做的事情则结合运动学。它在速度空间(v, ω)里采样：线速度v从0到最大速度分成若干档，角速度ω从负最大到正最大分成若干档，形成候选速度组合。对每个组合，根据车体运动学向前推演一段短时间（典型1-2秒），生成预测轨迹。然后排除会碰撞障碍物的轨迹、排除超过加速度限制的轨迹，剩下的轨迹用目标函数打分。我的目标函数包含三部分：轨迹终点与VFH目标方向的贴合程度、轨迹距离障碍物的最近距离、速度本身的大小。打分最高的轨迹对应的(v, ω)，就是这一周期的控制指令。

这个组合的分工很清晰：VFH负责方向，DWA负责速度。如果只有VFH没有DWA，那车在方向改变时会使用固定角速度，低速还行，速度一快就会因为转弯半径过大而撞上障碍。如果只有DWA没有VFH，目标方向的来源就成问题——DWA不知道该往哪边走，总是会倾向于走一条能绕开局部障碍但不一定朝向目标的轨迹。

3.3 仿真阶段最容易翻车的两个地方

第一是VFH的阈值设置。阈值太松，直方图会把很多带障碍物的方向判成可通行，车会直冲冲朝障碍物方向走；阈值太紧，车在稍微复杂的环境里找不到任何可行方向，直接原地罚站。我的做法是先在静态环境下反复扫描，看不同阈值下直方图的输出差异，再在动态环境里测"避障成功率"和"无路可走发生率"两个指标，找到一个折中点。这个参数到了实车阶段，还需要再往保守方向收一点，因为实车传感器噪声更大。

第二是DWA的加速度窗口。刚开始我把加速度约束设得很理想，仿真里没问题，但实车电机响应有延迟，实际加速度达不到设定上限。表现就是仿真里轨迹规划得很漂亮，实车转向跟不上，实际路径和DWA预测轨迹差了一大截。问题本质是执行器模型没建准。我后来在数字模型里给电机加了一个一阶惯性环节，模拟真实电机的响应延迟，DWA预测的轨迹才准了许多。

提示：仿真里一切参数先记下来，实车阶段基本都要再调一遍，尤其是阈值、PID、加速度上限这三类。

4. 实车部署：从坐标系到PWM的完整翻译

仿真阶段所有模块跑通之后，真正的硬仗才开始。接下来要做的不是简单地把代码从Simulink搬到STM32，而是把仿真里的每一个理想化假设翻译成实车世界里能落地的物理参数。这个"翻译"过程涉及坐标系、标定、驱动链路、传感器安装等多个层面，每一步做不好都会在后面联调时暴露出来。我把它拆成四个环节逐个解决。

4.1 底盘控制链路：编码器、PID与PWM

我的实车主控是STM32F103，底层控制链路是这样：编码器AB相脉冲经过定时器计数得到单位时间脉冲增量，换算成轮子的实际线速度；速度误差送入每个轮子的速度PID；PID输出映射为PWM占空比；PWM送给TB6612驱动直流减速电机。

低速时编码器测速是最容易出问题的一环。在0.2m/s时，一个500线的编码器经过4倍频后是2000个脉冲/圈，如果轮子直径是80mm，转一圈大约0.25米，那么每秒只有800个脉冲，折算到5ms控制周期只有4个脉冲。4个脉冲的计数波动反映在速度上就是很大的百分比误差。解决思路有两个：一是M法和T法结合测速，速度高用M法数脉冲，速度低用T法测脉冲间隔；二是把速度环周期从5ms放宽到10ms，让一个周期内多积累几个脉冲再算速度。我最终用了M/T法加10ms速度环周期，低速稳定性明显好转。

还有个不可避免的坑是电机死区。直流减速电机在PWM占空比低于某个值时，驱动力矩小于摩擦力矩，轮子根本不动。我用开环标定测过，电机在12V供电下占空比要到7%左右才开始转动。如果不做死区补偿，PID在小误差时输出一个位于死区内的占空比，轮子不动，积分项就会不断累积，等误差攒大了又突然冲出死区，车体一抖一抖。我的处理方式是：PID计算出的占空比如果在死区范围且目标速度不为零，就直接抬到死区边界；如果目标速度是零，则保持上一个输出且让积分项清零。这个细节让低速运行和停车动作的质感提升非常明显。

4.2 仿真参数到实车参数的换算标定

仿真里的控制量是m/s和rad/s，实车要的是PWM占空比，中间必须做标定。我做了两步标定。第一步，在空地上给左右轮分别施加不同固定占空比，用编码器测出轮子稳定后的线速度，做成一张占空比-速度表：

然后拟合出一条直线，作为开环的占空比前馈。PID输出叠加在前馈上，这样PID只需负责校正误差，不必从头建立稳态指令，收敛速度也快很多。

第二步是角速度标定，这一步要更小心。轮速达到稳态需要时间，直接给一个PWM差动会让转向响应滞后。我的做法是在数字模型里记录不同(v, ω)组合下的实际角速度稳态值，实车标定时按同样组合跑一遍，比较编码器推算的航向变化率与目标值的偏差。标定完之后你会发现，仿真里设置的v=0.5m/s、ω=1.5rad/s，在实车上对应的占空比并不是简单线性映射，因为地面摩擦、电池电压、轮胎形变都会影响，这些偏差只能靠闭环校正吸收。

电池电压变化也是实车特有的大问题。锂电池从满电到接近没电，电压会掉1-2V，同样占空比的电机转速差别不小。我建议在底层做一个简单的电压补偿：根据当前电池电压，把占空比乘一个修正系数。没有这个补偿，你会发现同一个程序早晨跑得好好的，下午电池电压一掉，车要么变慢要么突然失控。

4.3 传感器装车与数据接入的坑

雷达装车位置很讲究。我把单线激光雷达安装在车体正中偏前的位置，高度离地约30cm。这个高度能扫到大部分障碍物，但注意它扫不到低矮的砖块，也扫不到高于雷达视场面的障碍物边缘。如果你在仿真里假设"车体周围一圈都能探测到"，实车必然会因为安装位置产生盲区。

雷达的零方向标定也不能省。雷达扫描的0度通常指向安装箭头方向，但机械安装总会有几度偏差，不标定的话VFH输出的方向会整体偏转几度，避障时车会持续往一侧倾斜绕行。我第一版实测时就吃了这个亏，车能绕开纸箱，但绕完之后一直往某个方向偏，最后贴墙运行。用标定板重新校准雷达方向后，车才比较正地回到目标路线上。

我另外加了两个红外TOF传感器作为近距补充，安装在车头左右两侧，覆盖雷达近距离盲区。雷达近距离点会因反射面角度问题变得稀疏，而两个TOF可以在车头正前方和左右侧提供粗粒度判断：要不要紧急减速。融合逻辑很简单，TOF探测距离低于安全阈值时直接覆盖DWA指令进入紧急减速状态，否则以雷达数据为准。

调试最坑的一次是金属网围栏旁边试车，雷达把铁丝网反射成一片散点，VFH直方图几乎全扇区都显示有障碍，车站在那儿罚站。后来才意识到雷达对细密网状结构的反射极不稳定。我给数据加了一帧滤波：同一角度上的距离如果连续三帧跳动超过阈值，就认为不可靠，直接用上一帧可信值代替。处理之后环境鲁棒性好很多。

4.4 调试与监控：日志、遥控接管、蓝牙APP

从仿真到实车，一个特别容易被忽视的环节是调试手段。你要能回答三个问题：刚才那一瞬间算法输出了什么？底层电机响应了什么？传感器看到了什么？没有实时数据，就只能靠眼睛猜。

我的做法是在STM32上开一个串口，以固定频率输出带时间戳的调试帧，格式类似：

t=1023.45ms v=0.21m/s w=0.83rad/s ref_dir=12.5deg obs_min=0.68m pwm_L=18% pwm_R=12%

实测时把这帧日志通过蓝牙模块转发到手机或电脑，就能实时看到DWA选了哪个速度、VFH选了哪个方向、雷达最近障碍在哪。很多问题的定位，就是靠这种逐帧日志而不是肉眼观察。

调试过程中我还发现，纯自动模式跑避障时，如果算法突然给出错误指令，最需要的是一套可靠的遥控接管手段。我直接用了一块ESP32加蓝牙模块，手机APP端有两个功能：一键急停和手动遥控切换。急停指令通过串口直接给到底层驱动，优先级高于所有算法输出；手动遥控则在自动模式出问题时快速接管，把车挪到安全位置。网上关于"蓝牙APP控制ESP32"的开源方案很多，但要注意一点：遥控指令一定要在底层做优先级仲裁，不能和自主避障指令混在一起判断，否则一旦遥控和算法抢方向盘，结果会很难看。

注意：遥控接管和急停必须在底层驱动做最高优先级仲裁，别放到上层算法里判，否则程序卡死时一样停不下来。

5. 避障实测：从0.2m/s到稳定不撞的过程

所有准备工作做完，真正有收获的部分是实测。我特意把实测流程分成四个阶段，从最低速的闭环跑通开始，再到提速、边界情况、最终指标。每个阶段都撞过车、卡过壳，也都留下了对应的调参记录。下面按时间顺序把过程写出来，你可以直接对照自己的项目。

5.1 先把基本避障闭环跑通

我一开始把目标速度压到0.2m/s，场地是十平方米左右的空地，放了三个纸箱作为静态障碍。这个速度下，电机低速死区、编码器噪声、雷达延迟都不算致命，目的是验证算法的整体闭环是否成立。

第一版跑通的结果很滑稽：车能绕开纸箱，但绕完会一直往某一侧偏，最后贴墙。排查之后发现就是雷达零方向标定不准，VFH目标方向整体偏了几度。重新校准之后，车就能比较正地回到目标路线上了。这个阶段的核心指标只有一个：在静态障碍物之间稳定走完一圈不碰撞。我把纸箱摆成一条直线通道，让车反复通过，观察VFH直方图和DWA轨迹输出是否稳定，同时把PID参数粗调到位，保证转向时航向角误差能收敛、不震荡。

5.2 提速之后的震荡与过冲

闭环跑通之后，我把目标速度提到0.5m/s，问题立刻暴露。最典型的现象是S形摆动：车发现右边有障碍，猛往左打方向，绕过之后想回目标方向，又打得太快冲过了头，接着又往右修，整台车像蛇一样扭动。

这个问题的根因有两层。第一层是DWA的角速度采样分辨率不够，转向指令离散化严重，速度一快就一格格地跳。我把角速度档位从9档增加到19档，轨迹平滑度立刻上来了。第二层是外环航向PID参数没跟着提速调整，比例增益过大，方向误差一大就给出接近极限的角速度指令。我把外环P从0.9降到0.4左右，又给角速度加限幅，S形摆动才压下去。

提速后的另一个典型情况是过冲：车检测到前方障碍时已经太晚，DWA最大减速度不够，刹不住直接撞上去。问题本质是感知距离和行驶速度不匹配。我改了两处，一是把DWA前向预测时间从1.2秒加长到1.8秒，让算法更早评估碰撞风险；二是在底层加了一条基于最近障碍物距离的减速曲线——离障碍物越近，允许的最大速度越低。这个约束放在DWA采样之前，雷达最近距离低于阈值时，DWA速度窗口上限就强制下调。

5.3 边界情况：窄通道、动态障碍与盲区

速度稳定之后，我开始测试各种难缠的场景。窄通道最容易暴露问题。我把两个纸箱之间的距离收窄到比车体宽度多20cm左右，车在通道入口还能找到可通行方向，但走到中间时，雷达射线打到两侧纸箱上，直方图每个扇区都有一定障碍密度，阈值一卡，所有方向都被判不可通行，车直接停在通道里罚站。这个问题不是算法逻辑错了，而是参数在该场景下过于保守。我调整了VFH障碍密度函数的距离权重，让较远障碍对直方图的贡献变小，通道中间区域的障碍密度降到阈值以下，车就可以缓慢通过。

动态障碍物测试时，我用的是推着人形纸箱来回移动的方式。主要问题是DWA的轨迹预测跟不上运动目标：算法规划出一条当前不撞的轨迹，但目标移动后下一帧要重新规划，看起来车总在追着障碍物绕。我的处理是降低VFH方向切换的敏感度，给方向选择加滞回区间：新方向相对当前方向的变化小于15度时不切换。这样能避免动态场景里频繁换向，但对快速接近的障碍会反应慢一些。鱼和熊掌不可兼得，我选择了静态环境更稳定、慢速动态目标更平滑的方案。

盲区问题也在实测中反复出现。雷达离地30cm能扫到纸箱，却扫不到低矮砖块。有次测试轮子压到砖块，车身弹跳了一下，把雷达支架震歪了。我的解决方案是加固雷达支架，同时在感知层面接受一个事实：单线激光雷达在复杂地面环境里天然有盲区。如果要做更可靠的项目，建议考虑Intel RealSense这类深度相机补足低矮障碍感知，但需要额外算力，成本和复杂度都会上升。

5.4 最终参数与性能记录

经过大约两个星期的联调，我得到了一组在静态障碍避障、窄通道通行、慢速动态障碍回避三个场景下都能稳定工作的参数，列在这里供参考：

最终性能：在十平米场地内摆8个纸箱的随机静态场景，从起点到目标点不碰撞成功率在96%左右（连续跑50次），窄通道通行成功率81%，慢速动态障碍成功率88%。撞车和卡死的场景都有日志记录，事后分析基本都是传感器丢帧或者地面打滑导致的，控制器和避障算法本身没有暴露逻辑漏洞。

这里必须坦白，这个数据放到专业机器人比赛里并不算惊艳，甚至在动态障碍和窄通道场景里还谈不上优秀。但对我这个项目来说，最重要的不是单项指标多亮眼，而是整个过程具备完整的可追溯性：每次碰撞、每次卡死，都能从日志里精准定位到是传感器丢帧、地面打滑还是参数边界问题。这个可追溯性，就是"从数字模型到实车部署"这条流程带给我的最大收益——仿真阶段越扎实，实车阶段的异常就越容易快速归因。

6. 如果重新做一次，我会改这些

如果让我重新做一次，第一件事是把自适应频率控制纳入设计，而不是固定所有控制周期。实车低速和高速时电机动态特性差异很大，固定周期只能取一个折中。如果控制周期能根据工况自适应——低速时拉长测速窗口、高速时缩短响应时间——低速抖动和高速过冲应该能同时得到改善。这也是我在搜索相关控制资料时重点关注的方向。

第二件事电机方案的选择。直流减速电机加编码器胜在便宜好调，但带宽和响应速度确实有限。如果预算允许，用轮毂电机加FOC控制会是另一个量级的体验，尤其是起步和堵转表现。控制算法上限很大程度上被执行器本身锁住了，算法再聪明也救不了滞后的电机。

第三件事是感知方案别只盯着单一传感器。这次项目里我已经体会到单线雷达的盲区局限，下一次我会在设计初期就做多传感器融合，比如激光加深度相机、激光加超声波，给每个传感器分配一个"可信区间"，避障决策的鲁棒性会有一个质的提升。另外，如果底盘换成麦克纳姆轮，运动学控制会更灵活，避障转向不需要绕大弯，但运动学解算和电机配合的复杂度也会明显上升。

做这个项目的最大体会是：数字模型不是把实车问题挡在外面，而是把所有问题搬到了一个能快速回放、能精确定位、能反复试验的地方。从数字模型到实车部署，本质上是一趟把假设变成现实约束的旅行，每一条在仿真里忽略的现实约束，最终都会在实车的某个角落等着你。