1. 先搞清楚 VL53L0X V2 到底怎么测距的再调 Bug 也不迟我在不少群里看到新入坑的爱好者拿到 VL53L0X V2 模块后第一件事就是接上单片机读寄存器然后发现读回来的距离要么跳变要么直接卡在一个诡异的固定值。有人甚至怀疑是模块坏了连换好几个还是一样。其实大部分问题不是硬件坏了而是没有真正理解这颗芯片的工作方式。VL53L0X 是 ST 推出的一款基于 ToFTime of Flight飞行时间原理的激光测距传感器。它内部集成了发射器和接收器发射的是 940nm 波长的红外激光接收端通过 SPAD单光子雪崩二极管阵列来捕捉反射回来的光子。关键点在于它测量的不是光强而是光子从发射到返回所经历的微小时间差再乘以光速除以 2换算成距离。这就是它和超声波、普通红外测距模块最本质的区别。这也就解释了为什么 VL53L0X 在近距离测量上能做到毫米级精度而且响应速度极快。但是它的量程并不像很多人想象的那么夸张。官方标称在普通环境下有效测距范围大约在 30mm 到 2000mm 之间实际要看你设置的测距模式、目标物体的反射率以及环境光照情况。我在测试中发现一个很容易被忽略的细节VL53L0X 内部为了保证测量精度会连续发射上万次激光脉冲然后通过统计平均值和直方图分析来计算飞行时间。这并不是一次激光反射就能完成的。所以如果你要求它做单次测量Single Measurement它实际上也要花几十毫秒来采集和计算数据而不是像读普通 I2C 寄存器那样瞬间返回。很多人在代码里没有等待测量完成的标志位就直接读结果读到的自然是上一次的数据或者默认值——这是新手最常见的一个坑。另一个需要注意的概念是测量模式的差异。VL53L0X 支持 Single单次、Continuous连续和软件触发的单次测量。在高频率连续测距时芯片功耗会上升测距误差也会因为温度漂移而逐渐显现。后面我会重点讲温度对测距精度的影响以及如何校准。2. 从模块原理聊到真实使用边界为什么它能测 2 米却又经常测不到 2 米很多淘宝详情页会放一张大表格告诉你 VL53L0X 测距范围是 20mm 到 2000mm。这个数字没错但有一个前提目标物体需要有足够的反射率而且环境不能有太强的红外干扰。实际用下来我总结了几条经验供参考。先看反射率的影响。VL53L0X 的接收端 SPAD 阵列对反射光子的强度非常敏感。如果目标是一面白墙反射率可能在 85% 以上测 1 米到 2 米都很稳定。但如果目标是一块黑色哑光布料反射率可能只有 20% 左右测距距离会明显缩短甚至在 50cm 以外就开始报错。粗糙的深色表面还会造成漫反射有效信号更弱。还有一个极限情况是目标物体表面与激光束呈大角度倾斜比如你拿它去测一堵斜着的墙反射回 SPAD 的光子数量大幅下降测距结果要么跳变要么返回一个远大于实际距离的值。更麻烦的是环境中的阳光干扰。940nm 波段的红外光虽然肉眼看不见但是在阳光下、或者室内有强白炽灯/卤素灯的情况下环境光中的红外成分会在 SPAD 上产生大量噪声光子。VL53L0X 内部有环境光抑制算法但这是有上限的。实测在正午阳光直射下许多模块的最大有效测距距离会从 2 米缩水到 1 米以内甚至出现无法锁定信号的情况。所以如果你的项目要求户外长距离稳定测距并且目标物体是低反射率的深色材料那我建议你不要指望 VL53L0X 单独搞定这超出它的设计定位了。它更擅长的是室内近距离的精准测距、手势识别、机器人避障、低速无人机定高这类场景。理解它的使用边界能省去后面一大半的调 bug 时间。3. 硬件接线和 I2C 通信里的细节别把 VL53L0X 的 GPIO1 和 XSHUT 当摆设接线的部分看起来简单VL53L0X 模块一般只有 6 个引脚VIN有的模块标 VCC、GND、SDA、SCL、GPIO1中断输出、XSHUT关断控制。但细节不算少。首先是电压匹配。VL53L0X 芯片本体支持 2.6V 到 3.5V 供电市面上大多数 breakout 板子会带稳压和电平转换电路。如果你用的是 3.3V 单片机比如 STM32F103 的 3.3V 逻辑或者 Raspberry Pi 的 3.3V GPIO那直接接线没问题。但如果你用的是 5V 逻辑的 Arduino UnoAVR 芯片哪怕模块焊盘上印着 VIN 可以接 5VSDA 和 SCL 的上拉电阻和电平匹配来讲大部分 5V Arduino 直接连也能跑起来因为板子上的电平转换或者内部上拉会帮你兜底。不过严格从可靠性角度讲我还是建议在 SDA 和 SCL 之间串联 1kΩ 到 4.7kΩ 的限流电阻或者在通信线上加电平转换模块长期运行的设备更需要这样做。然后是 GPIO1。这个引脚是中断输出不是必需接的但它非常好用。VL53L0X 在完成一次测量后如果开启了中断模式GPIO1 会输出一个低电平脉冲具体极性要配置。你可以把 GPIO1 接到 MCU 的外部中断引脚然后等中断来了再去读 I2C 数据这样就不需要轮询寄存器等待测量完成能大幅降低 CPU 占用也能避免读到无效数据。在连续测距模式下GPIO1 中断尤其有用因为测距频率可能高达 50Hz 甚至更高。XSHUT 引脚也值得重视。这个引脚是硬件复位/关断控制拉低会让 VL53L0X 进入完全关断状态拉高后重新上电复位。这个特性带来的最大好处是你可以把多个 VL53L0X 模块挂在同一组 I2C 总线上通过分别控制每个模块的 XSHUT 引脚来实现 I2C 地址修改和区分。每个 VL53L0X 芯片默认的 I2C 地址是 0x29如果不改地址两个模块接同一总线必冲突。改地址的办法就是先只拉高其中一颗的 XSHUT让它在总线上独享 0x29然后通过 I2C 写入新地址再拉高下一颗的 XSHUT。如果你的项目只是单模块应用XSHUT 可以不接但如果你做的是多模块阵列或者需要低功耗模式就必须把它用好。注意XSHUT 引脚同样不应该直接接 5V最好是 MCU 的 3.3V GPIO 控制。4. 初始化流程与寄存器操作的实战顺序跑通看似简单的 I2C 只是第一步VL53L0X 是一个相当复杂的传感器内部寄存器非常多寄存器地址也从 0x00 到 0xFF 遍布。直接用裸寄存器操作会把绝大多数人劝退。好在 ST 官方提供了一套 API把底层初始化和校准封装得比较完整普遍的做法是直接移植 STSW-IMG005 驱动包。我先说一套最简单的基于官方 API 的初始化流程这是所有应用的地基。首先是确保 I2C 总线上能扫到设备。在树莓派上你可以用 i2cdetect -y 1 命令在 ESP32 的 Arduino 环境中你也可以扫描总线。正常扫描到的地址是 0x29。然后是初始化顺序将 XSHUT 拉高等待至少 1ms让芯片完成上电复位。通过 I2C 发送 0x00 寄存器读取确认芯片响应Device ID 寄存器通常是 0x0B 或类似的值需要查具体驱动。调用 API 的 VL53L0X_DataInit() 完成数据初始化。进行静态校准读取或执行完整校准。设置测距模式单次或连续。设置测距精度与定时预算。启动测距等待测量完成标志读取距离数据。如果你用的是 Arduino已经有非常成熟的 Adafruit_VL53L0X 库或者 Pololu 的 VL53L0X 库接口封装得更好。例如 Pololu 库的典型用法如下#include Wire.h #include VL53L0X.h VL53L0X sensor; void setup() { Wire.begin(); sensor.init(); sensor.setTimeout(500); sensor.startContinuous(); } void loop() { Serial.print(sensor.readContinuous()); Serial.println( mm); }代码看起来简单到令人发指但要注意几个隐藏点Sensor.init()内部其实调用了很多寄存器配置步骤如果 I2C 通信不稳定初始化过程会静默失败。这时你需要监测返回的布尔值不要盲目往下执行。实测不少用户卡在这里是因为 SDA/SCL 线太长或者电平不匹配导致数据偶然出错。另一个容易忽略的是测距模式的选择。Pololu 库里面readContinuous()读到的单位是毫米但如果有些示例代码里用的是readRangeSingleMillimeters()你需要按需选择。连续模式下如果两次读取间隔过短可能会读到同一帧数据或者跳帧。通常建议连续模式的读取频率不要超过传感器设定的测量速率。在树莓派上直接部署的时候Python 库如VL53L0X_raspbian_python或社区维护的vl53l1x_python移植版本也很常用不过很多旧库对 VL53L0X 的校准支持不完整。后面我会专门说校准的问题。5. 定量测量做不精确十有八九是忽略了这几个关键参数我刚拿到 VL53L0X 模块的时候也是直接初始化、直接读距离然后发现数据在 600mm 到 650mm 之间波动。一开始我以为传感器有问题后来才意识到这是它在正常工作因为我没有做校准也没有针对具体环境去调配置。先说温度漂移。VL53L0X 的测量原理决定了它对温度比较敏感。激光二极管的发光效率、SPAD 的暗计数都会随温度变化。芯片内部有温度补偿机制但补偿效果有限。实测在环境温度从 20°C 升到 45°C 的过程中同一个固定距离的读数可能会漂移几十毫米。尤其是长时间运行后芯片自身发热也会带来明显的读数偏移。如果你做的是测量精度要求高的项目建议在设备启动后空跑几分钟等芯片温度稳定后再进行数据采集。再说校准。VL53L0X 出厂时会在模块内部写入一部分校准数据NVM包含 SPAD 数组设置、反射率和温度系数等等。正常使用官方 API 初始化时会加载这些 NVM 数据。但是如果你买的是非常廉价的模块或者二手工装芯片NVM 数据可能不完整或者不准确。这时就需要执行所谓的“交叉校准”cross-talk calibration和“偏移校准”offset calibration。交叉校准是针对目标物体位于视场角落或近距离高反射表面时发生的测量距离偏短问题。做交叉校准需要在一个已知距离位置放置一个已知反射率的目标通常是白纸板或者专用靶标然后调用 API 的参数算出一组交叉修正值。很多模块出厂时预设了默认交叉值但如果你要测量的是近距离白色物体强烈建议重新校准。偏移校准是用来修正内部信号处理延迟导致的固定距离偏移。做法是将传感器对着一个固定距离的平面比如距离刚好是 100mm读取测量值然后把测量值和真实值之间的差值作为偏移量写入校准结构体。这里注意VL53L0X 有一个 1.2mm 左右的已知偏移量芯片内部默认认为传感器到目标之间的距离等于从芯片顶部算起但实际上测量的是从 SPAD 阵列到目标的距离所以这个固定 offset 必须要处理。如果你的应用是测距差值、障碍物检测、手势识别这类相对测量场景不做严格校准也可以因为误差是固定或缓慢变化的不影响判断。但如果是测量液位、定位、料位计这类需要绝对精度的场景不校准基本没法用。6. 从原始数据到稳定读数滤波与多帧融合的实际经验光有校准还不够实测中你会碰到单个读数的随机抖动。比如对着同一个固定目标连续读 100 次数据有时会在 ±50mm 范围内跳。原因可能是环境光噪声、目标表面微小的振动、SPAD 增益波动等等。这时不能直接拿单帧数据去驱动电机或者决策逻辑一定要做滤波。我用的方案分三档按需求复杂度递增第一档是简单滑动平均/中值滤波。取最近 N 次测距结果取平均或取中位数。中值滤波对去除突发离群值效果最好我常取 5 到 7 帧。代价是每次输出都会滞后N-1/2 帧的时间动态响应变差。如果应用是低速避障或者存在检测这个滞后可以接受。第二档是带阈值的滤波。只有当连续 M 帧测量值的差值都在某个阈值以内时才认为测量稳定输出有效结果。这适合高可靠性的存在检测、安全围栏。在实物中这个阈值一般设置成 20mm 到 30mm具体看反射率差异。第三档是限幅平均滤波。将每次新数据与上次有效数据比较如果差异超过一定阈值就丢弃或显著降低权重同时保留滑动窗口内的平均值。这个方法在目标快速移动和静止之间取得了平衡。如果你做的是高精度测量项目还可以考虑二次校准补偿。简单来说就是用一组已知标准距离比如 100mm、200mm、500mm、1000mm去测记录测量值和真实值之间的误差曲线然后做线性插值修正。实际上单位距离内的误差不是完全线性但在 100mm 到 1200mm 之间线性拟合效果已经不错了。7. 温度、光照、反射率之外还有三个我踩过的硬件坑做软核方案的时候最容易忽略硬件设计我踩过几个坑这里单独列出来。第一个坑是镜头污染。VL53L0X 模块表面有两个开孔一个是激光发射窗口一个是接收窗口。这两个窗口不能脏不能有贴纸遮挡也不能用不透光的胶水或漆料覆盖。很多人打样外壳时为了美观给模块全部封死结果测距直接废掉。激光可以通过部分透明塑料比如特定牌号的聚碳酸酯透射但透过率会大打折扣需要实际测试。还有一点是窗口材料不能是深色或磨砂的会严重影响光路。第二个坑是供电纹波。VL53L0X 内部的 SPAD 需要较高的偏置电压所以模块内部一般有 DC-DC 升压电路。如果供电电压纹波很大会直接影响升压电路的稳定性进而导致测距噪声明显增大。实测在 5V 供电的 Arduino 上如果电机同时也在转测距数据会漂得很厉害。解决办法是给模块单独用 LDO 供电或者至少在 VIN 引脚附近增加 10uF 和 0.1uF 的去耦电容。第三个坑是 I2C 总线上还挂了其他设备。VL53L0X 的 I2C 地址是 0x29如果总线上还有别的传感器也用这个地址就会冲突。有些模块厂商提供的 I2C 地址可配置只在某些寄存器中有效如果你在初始化后直接调用底层写地址函数可能会覆盖掉驱动中已经配置好的内部状态导致后续测距数据异常。我建议严格按照官方 API 的修改地址流程来不要自己乱写寄存器。8. 实测数据复盘不同材质和模式下的表现以及怎样设置才最稳我这里给出一组我自己实测的参考数据环境是室内常温模块距目标 500mm供电 3.3V LDO。目标材质单次测量模式典型误差连续测量模式典型误差备注白色 A4 纸±2mm±3mm反射率高最稳定浅色木质桌面±5mm±6mm略低于纸张黑色哑光塑料±20mm±30mm部分帧可能出现离群值深色粗糙布面±35mm±50mm反射率极低不稳定镜面/高光表面时好时坏不可靠强镜面反射造成信号饱和或丢失可以看到材质差异对精度的影响远大于模式差异。如果你的目标物体是低反射率建议把测量模式调整为更长距离模式但这会牺牲速度。VL53L0X 的 API 里有几个预设模板比如VL53L0X_PRESETMODE_LONG_RANGE、VL53L0X_PRESETMODE_HIGH_SPEED、VL53L0X_PRESETMODE_HIGH_ACCURACY。默认情况下很多库用的是LONG_RANGE它牺牲了一部分精度换取了更远的测距能力。如果你在 300mm 内做高精度测量切换成HIGH_ACCURACY反而效果更好。下表是我在几个预设模式下的典型表现预设模式典型最大测距单次测量时长600mm 处稳定性HIGH_SPEED约 1.2m20ms一般±12mmLONG_RANGE约 2m33ms较好±5mmHIGH_ACCURACY约 1.2m200ms最佳±2mm如果你做的是机械臂抓取、近距离测距这种需要高精度的场景不要偷懒用默认模式一定要明确设定HIGH_ACCURACY并做好校准。如果你做的是无人机定高、避障追求速度那就用HIGH_SPEED并适当放宽误差容忍度。9. 常见故障排查速查表从无法识别到数据恒定的完整定位链路调试 VL53L0X 时遇到问题不要瞎猜按照下面的链路逐步排查基本能定位 90% 的故障。I2C 扫不到设备地址 0x29确认 VIN 电压用万用表量模块上的 VCC 是否正常有些模块需要用 3.3V。确认 SDA/SCL 接线是否接反这两个引脚接反很常见。确认 SDA/SCL 上是否有上拉电阻。虽然很多模块板载了上拉但如果你接了很长的杜邦线需要再外接 4.7kΩ 上拉到 3.3V。确认 XSHUT 没有被拉低。如果还是扫不到把模块从面包板上拔下来直接焊接杜邦线再试。面包板接触不良也是高频问题。能扫到设备但读距离永远是最大值/固定值先确认目标距离是否在量程内以及目标反射率是否足够低。如果是 2 米开外的黑色物体读回最大值不奇怪。确认初始化是否失败有没有设置测距模式。初始化失败后寄存器里会有很多默认值读到的数据没有意义。检查是否在单次模式下连续读结果。每次单次测量都需要先触发测量等待完成标志再读取结果。连续读上一次的缓存就会出现固定值。读到的距离比真实值小很多最常见原因是交叉校准值不适用于当前场景。尤其当你把模块从出厂默认环境挪到一个近距离高反射率的环境交叉校准值偏差会导致 0 到 50mm 的误差。检查光学窗口是否有异物或者是否有透明塑料片贴在窗口上这会产生主光线散射缩短有效测量距离。检查供电纹波尤其在电机工作状态下。部分帧出现严重离群值目标表面是否有强反射点、镜面或玻璃。如果必须测这些材质建议加装遮光罩或者更改安装角度避免垂直正对镜面。环境光变化剧烈比如有人走过时的阴影变化也会影响测量。开启多帧中值滤波直接过滤离群点。10. 实现场景再扩展从单点测距到 ToF 雷达思路的迁移最近聊到 ToF 雷达很多爱好者会自然而然地想到把 VL53L0X 和旋转机构结合做成一个 360° 扫描的测距雷达。这个思路完全可行但要注意 VL53L0X 本身的视场角和更新率限制。VL53L0X 的视场角FOV大概在 25° 左右做旋转扫描时角度分辨率取决于旋转速度、采样频率和视场角重叠。假如你的雷达 1 秒钟转一圈VL53L0X 连续测距模式算上传感器内部处理和 I2C 读取最高能跑到 50Hz 左右那么一圈最多采 50 个点也就是 7.2° 一个点。这要覆盖 360° 肯定不够细。所以我建议把转速降下来或者使用多颗传感器交错安装。ToF 雷达的真正价值在于它对目标材质和颜色的适应能力比超声波强得多。超声波在斜角入射时会产生镜面反射导致测不到回波而激光漫反射相对好一点测一段粗糙墙体基本没有问题。另外激光测距雷达的体积也可以做得很小这对于空间受限的机器人来说是一大优势。但也要坦诚一点VL53L0X 毕竟是一个近距离模块和真正的工业级激光雷达比如镭神、速腾这类产品在量程、精度、多回波处理上差距巨大。做机器人避障、扫地机规划这类近距辅助可以用它做自动驾驶或者大面积建模就算了吧。不是一回事。11. 我踩过的一个最长调试时间的问题和最终解法之前做一个小型机器人避障项目用了两个 VL53L0X分别装在机器人的左前方和右前方。刚上电时两个模块都正常但运行一两分钟后其中一颗模块的数据开始随机跳变跳得还非常厉害从正常距离跳到几百毫米外的异常值。我排查了接线排查了地址冲突排查了供电都没什么问题。后来用示波器抓信号才看到那颗模块在运行一段时间后SDA 线上的波形沿变缓有时甚至出现毛刺导致通信偶尔出错。原因是我用了很长的杜邦线把模块连接到主控板而模块内部虽然有电平转换但还没有规整到能对抗这条线上等效容性负载和电磁干扰的程度。最终解决方法是把线长缩短到 15cm 以内并且在模块的 VIN 和 GND 之间加了一个 470uF 的电解电容和 0.1uF 的陶瓷电容。改完之后连续跑了几个小时数据没有再跳变过。还有一个印象深刻的坑是用 Arduino 的Wire.h库时读取 VL53L0X 的 16 位数据寄存器时必须注意字节顺序VL53L0X 返回数据是大端模式。如果你先把两个字节拼成 int再用默认的小端解释得到的就是一个完全错误甚至负数的大数。这类问题看起来像是传感器坏了实际上就是字节序没有转过来。所以这里也要提醒一句拿到一个新模块不要直接上项目先在桌子上用固定目标物搭配串口打印和示波器做一次完整的数据体检确认各项指标符合预期再集成到系统里。否则后面所有调度和滤波算法的调优都会建立在一个不稳的地基上。12. 关于 VL53L0X V2 芯片版本差异以及模块选购的几个建议市面上叫 “VL53L0X V2” 的模块很多但芯片本身在不同批次、不同封装下其实没有本质差别最大的差异来自模块厂商用的周边电路设计和光学开孔工艺。有的模块为了保证量产良率会在 SPAD 窗口位置贴一层保护膜这种做法对测距影响不大但如果膜贴歪了或者用的胶水遮住了一部分窗口接收光强就会下降。选购模块时第一看有没有板载 LDO 和电平转换芯片这决定了你能否直接接 5V 系统第二看 I2C 上拉电阻是不是焊好了。有些极简模块为了省钱省掉了上拉电阻导致很多人接上后 I2C 不稳定。第三看光学窗口有没有做回流焊后清洁有的模块窗口会残留助焊剂测出来数据诡异就在这里。另外“V2” 和原版 VL53L0X 在实际使用中基本无差别主要是一些驱动库的版本兼容问题。某些老旧开源库在 V2 模块上初始化后会意外写入一些保留寄存器导致测量异常。遇到这种情况更换一个维护活跃的库比自己去查寄存器要快得多。我用过的组合里Pololu 的 VL53L0X 载板和树莓派 Python 库搭配比较舒适Adafruit 的板子质量稳定库也全淘宝上十几块的裸模块性价比高但一定要预算出调试时间。13. 把 VL53L0X 用在电池供电设备上的低功耗注意点很多便携设备会用到 VL53L0X 做距离检测但低功耗设计不能只看芯片手册上的待机电流。VL53L0X 在测量时峰值电流能到 20mA 到 30mA换算到电池供电的系统里影响不算小。如果做门铃触发、智能灯这种只在特定时刻唤醒的设备建议让模块平时处于 XSHUT 拉低的断电状态需要测量时再拉高并重新初始化。注意重新初始化需要时间从拉高 XSHUT 到第一次测距有效大约需要 20ms 到 50ms取决于驱动实现的效率。如果你不想每次重新初始化也可以保留 I2C 通信但让模块进入软件待机standby mode这时的功耗在微安级别。但从软件待机恢复后建议重新设置测距模式因为某些寄存器内容可能会被清空。当然低功耗下要格外注意第一次测距的稳定性。我实测过多次刚唤醒时的前几帧数据经常是异常值。稳妥的做法是软件里丢弃唤醒后的前 3 帧测量数据再进行滤波和决策。14. 结合实际应用从避障、定高到手势识别不同项目怎么调参面向不同场景参数的调整思路完全不同。做机器人避障时目标物体位移速度快需要在测量速度和精度之间取平衡。我建议使用连续测距模式测量预算设到 33msLONG_RANGE传感器内部可以自动调高灵敏度但外部要做滑动平均滤波。避障距离的阈值要留 30% 左右的余量因为低反射率物体和斜表面会让实际测距偏大。做低速无人机定高时地面的反射率相对稳定但机身震动会导致读数噪声变大。建议固定安装传感器指向正下方增加减震泡棉同时用限幅平均滤波。姿态变化超过一定角度时测距结果是偏大的这时要结合 IMU 数据做姿态补偿。做手势识别时测距频率要尽可能高最好在 30Hz 到 50Hz 左右。这时不适合用高精度的 200ms 测量预算而是要用 HIGH_SPEED 模式。但 HIGH_SPEED 模式下噪声变大需要配合平滑处理和差分算法检测手是靠近还是远离。手部肤色和角度千差万别识别阈值不固定实际中要用动态参考记录一段基线距离相对变化量超过阈值才触发手势判断这样能避免肤色差异带来的影响。15. 一些值得保留下来的经验短语帮你在下一次调试时少走弯路回想这几年用 VL53L0X 的经验真正有价值的东西是任何测距传感器的第一前提是光学孔径干净、供电干净、通信稳定。不要拿动态数据去硬测先静态校准。校准是环境相关、应用相关的不是一次校准保终身。数据滤波永远是在理解了噪声来源之后的优化手段而不是第一方案。模块的“官方标称”和“实际表现”之间差的就是你的项目里目标物体的材质、反射率和环境光。有时候接到一个新的场景需求比如测一个半透明塑料瓶的液位高度我会先花一天时间把传感器对着不同水位拍 1000 帧数据画出误差曲线再做软件补偿。这一步看起来麻烦但能让后续的系统设计省很多事。关于 VL53L0X 的实战内容基本就是这些。如果你正在做相关项目手头有模块建议先把这些章节里的排查链路挨个过一遍再开始写上层逻辑。毕竟再好看的避障算法喂进去的数据是错的跑得再快也是白搭。