第一次把 LD14 激光雷达的串口接进 STM32再从 USB 口把雷达数据拉到电脑上最后看到屏幕里房间轮廓被一条条描出来的时候我才真正理解“数据采集”这四个字的分量。很多人一提到 SLAM 就想到 Cartographer、想到层层递进的数学公式实际上用 LD14 这种入门级 360° 雷达加一块 STM32完全可以把二维建图的整条链路跑通。我写这篇文章不打算灌水从硬件接线讲到位姿估计覆盖数据采集、协议解析、STM32 转发、上位机建图的全流程适合手里有雷达和开发板、想自己把 SLAM 拼出来的朋友。1. 先画清楚链路图一套简易建图系统到底由哪几块拼成1.1 雷达侧LD14 提供了什么LD14 是一颗典型的低成本单线 360° 激光雷达常用于入门级移动机器人。它能输出以雷达自身为原点的极坐标数据每个采样点包含两个信息一个是相对于雷达正前方的角度一个是障碍物到雷达的距离。把这些点画在极坐标纸上就是一张“一圈环境轮廓”的原始图。很多新手容易忽略一个事实雷达本身不产生地图它只产生“当前这一圈测距结果”。你要的地图是把雷达在不同位置的多次测量结果“拼”起来之后形成的栅格地图。这个“拼”的过程才是 SLAM。LD14 的常见参数大致是这样参数典型值说明测量原理三角测距近距离精度尚可远距离噪声较大测量范围0.1m ~ 12m以实物手册为准扫描角度360°单线平面扫描扫描频率7 ~ 14Hz 可配置每圈一圈点云数据接口UART TTL直接接单片机串口工作电压5V注意电机启动瞬间电流所谓“单线”就是只测一个平面上的距离。二维 SLAM 只需要这个平面的信息所以 LD14 这类雷达是学习 2D 建图成本最低的入口。如果后续想上三维或者更复杂的感知再考虑换多线雷达那时候的算力需求和算法复杂度也会大幅上升。1.2 STM32 的角色数据搬运与轮子控制而不是大脑这块是很多新手最大的误区。看到标题里“LD14 STM32 实现 SLAM”第一反应是让 STM32 直接把地图算出来。但实际上常规 STM32比如 F103 系列只有几十 KB 内存主频几十到几百 MHz跑一个完整的图优化 SLAM 后端是非常吃力的。Cartographer 这类算法在 PC 上运行时会占用数百 MB 内存还有大量矩阵运算和回环检测把这套东西塞进单片机已经不是优化问题而是方向问题。那 STM32 在这套方案里做什么我的答案是它做“神经”做数据链路的搬运工。用 UART 接收 LD14 发来的雷达数据帧解析出角度、距离、信号质量读编码器里程计推算车轮转速和底盘位姿通过 USB 虚拟串口或第二路 UART把雷达数据和里程计数据打包发给上位机同时还可以跑电机 PID 控制让小车按指令运动。上位机才是“大脑”负责跑 Cartographer、gmapping或者你自己写的一个简单建图脚本。人眼、神经、大脑三者分工不同这套系统也是同样逻辑。很多成品机器人方案里单片机做底层实时控制树莓派或者工控机做 SLAM 和导航就是这个道理。1.3 整条数据流用文字就能画清楚从物理接线到最终地图数据流是这样走的LD14 上电后电机开始旋转内部测距模块按固定角度间隔输出距离点每积累一小段角度范围的数据雷达就通过串口发出一帧数据包STM32 的 UART DMA 把数据包完整收进内存解析出“一帧包含哪些角度、哪些距离”STM32 把这些信息再打包通过 USB 或串口发给上位机上位机拿到当前帧二维点云后结合里程计或帧间匹配估计雷达的新位置把新一帧点云投影到世界坐标系更新栅格地图。理解这条链路的核心价值在于以后无论换雷达、换单片机、换算法你都知道问题出在哪一环。数据采不稳后面再高级的算法也是白搭。我在给很多新人做项目评审时第一件事就是让他们把这条链路图讲清楚讲不清楚的往往后面都会在各种奇怪的问题上卡很久。2. 解析 LD14 雷达协议从串口原始字节到角度-距离序列2.1 接线、电平与串口参数LD14 按常见接口逻辑来说需要接四根线5V 电源、GND、雷达 TX数据输出、雷达 RX配置输入不是所有版本都有。我这里拿到的版本数据输出是 TTL 电平波特率 2304008 位数据、无校验、1 位停止位也就是常说 230400 8N1。和 STM32 连接时雷达的 TX 接 STM32 的 RX雷达 GND 和 STM32 GND 必须共地。供电不要直接从 STM32 的 3.3V 引脚取雷达电机启动瞬间会有电流尖峰容易把单片机电平拉垮。最稳妥的做法是单独 5V 电源给雷达供电同时和 STM32 共地。如果雷达输出的是 5V TTL而你的单片机引脚不支持 5V 容忍中间要加电平转换模块否则长时间跑有烧引脚的风险。2.2 帧结构找头、拆字段、验校验我手头这颗 LD14 出厂固件的协议是典型的低成本 360° 雷达协议和市面上不少雷达的解析思路一致但具体字节位置可能略有差异请一定以你自己的雷达手册为准。一帧数据的结构大致如下第 1 字节帧头固定 0x54第 2 字节包类型正常数据包固定值第 3 字节采样点数 LSN表示这一帧里有几个测距点第 4~5 字节起始角 FSAuint16 小端表示单位是 0.01°第 6~7 字节结束角 LSAuint16 小端表示单位是 0.01°第 8~9 字节转速 Speed之后是 LSN 组数据每组 3 字节距离值占 2 字节小端单位毫米信号质量占 1 字节最后一字节是校验和。注意两个坑一是角度值用了“小端”存储和平时读十六进制直觉相反二是角度会以 0.01° 为单位有些跨 360° 的情况处理不当角度范围会突然跳变。解析的关键就是把起始角、结束角和每个采样点对应起来。2.3 先用 Python 把协议验证一遍拿到任何雷达我建议先不要动 STM32先在电脑上用串口助手或者 Python 把协议解析通了再往下走。这样可以快速确认雷达本身工作是否正常。下面是我调试时用过的最小解析思路import serial ser serial.Serial(/dev/ttyUSB0, 230400, timeout0.1) def to_u16(lo, hi): return lo | (hi 8) frame bytearray() while True: data ser.read(1) if not data: continue if len(frame) 0 and data[0] ! 0x54: continue frame.append(data[0]) if len(frame) 3: lsn frame[2] total_len 10 lsn * 3 # 帧头1 类型1点数1起止角4转速2数据3*lsn校验1 if len(frame) total_len: fsa to_u16(frame[3], frame[4]) lsa to_u16(frame[5], frame[6]) cs frame[-1] if sum(frame[:-1]) 0xFF ! cs: frame bytearray() continue start_angle fsa / 100.0 end_angle lsa / 100.0 if end_angle start_angle: end_angle 360.0 step (end_angle - start_angle) / (lsn - 1) points [] for i in range(lsn): idx 9 i * 3 dist_mm to_u16(frame[idx], frame[idx 1]) quality frame[idx 2] angle start_angle step * i if angle 360.0: angle - 360.0 points.append((angle, dist_mm, quality)) print(points) frame bytearray()这个脚本每次读到一帧完整数据就打印全部点。跑起来你可以看到角度从 0 到 360 覆盖距离值和现场尺子量出来的大致对得上。校验和这里的算法是“之前所有字节累加取低 8 位”如果你的固件不是这个规则把校验改成手册里的公式就行解析框架不变。2.4 脏数据和帧同步的常见处理思路雷达在启动瞬间电机没转稳串口可能吐出一堆乱码。如果程序按固定偏移去解析很容易全盘崩溃。因此健壮的做法是状态机式找帧头每收一个字节先判断当前是不是帧头 0x54收到帧头后继续收第 2、3 字节得到 LSN根据 LSN 算出这一帧的总长度收满后再校验校验不过整帧丢弃重新回到找帧头状态。另一个坑是距离值为 0 的情况。很多雷达协议里0 表示“没有有效回波”或“距离太近”不是真正的 0 距离。解析之后要做一层过滤把距离为 0 或者质量很低的点剔掉否则建图时会出现杂点。3. STM32 端采集代码DMA 空闲中断把数据稳稳接住3.1 为什么不用一个字节一个中断地收雷达 230400 波特率按 10 bit 一个字节算每秒大约 23040 字节。如果每收一个字节触发一次串口中断对 72MHz 主频的 STM32 来说不是不能处理但会让系统非常难受。因为你的主循环还在跑电机控制、编码器读取、和上位机通信频繁打断会造成线程抖动严重时数据收着收着就丢。推荐方案是 DMA 空闲中断DMA 负责把串口数据自动搬运到内存缓冲区不占用 CPU空闲中断表示“串口线上太平了没有新数据进来了”这时说明一包数据已经接收完毕CPU 再去处理缓冲区。3.2 CubeMX 配置要点我习惯用 STM32CubeMX 生成工程再手动改回调。核心配置就这么几项USART2 选 Asynchronous波特率 230400DMA Settings 添加 USART2_RX方向 Peripheral To Memory模式 NormalNVIC 使能 USART2 global interrupt在代码中调用HAL_UARTEx_ReceiveToIdle_DMA启动一次接收。注意 DMA 模式这里我用 Normal 而不是 Circular因为空闲中断加 Normal 模式的理解成本最低。每帧处理完再重新启动 DMA。如果数据量大到一帧缓冲区装不下就要加大缓冲区或者改成 Circular 模式但入门阶段 Normal 够用。3.3 HAL 库回调里的接收处理逻辑直接在中断回调里做很多解析操作不是好习惯正确思路是回调里置标志位主循环里再解析。如果回调里直接就解析解析时间稍长下一帧数据可能开始覆盖当前缓冲区。uint8_t rx_buf[512]; volatile uint16_t rx_len 0; volatile uint8_t rx_flag 0; void start_rx(void) { HAL_UARTEx_ReceiveToIdle_DMA(huart2, rx_buf, sizeof(rx_buf)); } void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) { if (huart huart2) { rx_len Size; rx_flag 1; } } int main(void) { // HAL_Init、SystemClock_Config、MX_GPIO_Init 等... start_rx(); while (1) { if (rx_flag) { rx_flag 0; parse_radar_frame(rx_buf, rx_len); start_rx(); } // 其他任务电机控制、编码器读取等 } }parse_radar_frame函数内部就做我上面 Python 脚本同样的事情找帧头 0x54提取 LSN计算长度校验然后解析成角度和距离数组。解析出来的数据可以直接存到一个结构体里供后续转发使用。3.4 转发上位机透传原始帧还是重新打包STM32 解析出雷达数据后有两种转发策略。第一种STM32 只做透传把雷达发来的原始字节原封不动通过 USB 或者第二路串口发给上位机。优点是简单、实时性高、逻辑最少上位机重新走一遍协议解析即可。缺点是如果雷达中间有坏帧单片机完全不知道上位机要自己处理。第二种STM32 在解析后重新打包把角度、距离、质量整理成更紧凑的格式甚至可以顺手过滤掉无效点。优点是上位机更省事缺点是 STM32 逻辑变多可能引入额外延时。我的建议是第一步先用透传保证链路通第二步再做重新打包。很多人在最初调试时又想解析又想过滤又想转发一出问题根本分不清是雷达问题还是代码问题。链路里每一层尽量简单排错才容易。3.5 数据量和丢帧估算这里有个简单计算假设 LD14 一圈有 200 个采样点一帧数据长度约为10 3 * 200 610字节。按 10Hz 扫描频率算每秒数据量约 6100 字节。230400 波特率理论上每秒能传约 23040 字节串口占用率只有 26% 左右余量很充足。如果你发现上位机收到的帧率明显低于雷达理论扫描频率优先检查三件事供电是否稳定、DMA 缓冲区是否太小、主循环是否做得太重。4. 上位机把数据流变成地图ROS 与 Python 两条线路4.1 线路一ROS Cartographer标准且省心如果只是想快速看到一张漂亮地图推荐直接上 ROS 加 Cartographer。这套方案需要一台 Linux 上位机笔记本或者树莓派都行。流程是写一个串口节点读取 STM32 转发上来的雷达数据解析成sensor_msgs/LaserScan消息发布再用 Cartographer 订阅/scan和里程计信息完成建图。LaserScan 消息的关键字段包括angle_min、angle_max、angle_increment、range_min、range_max、ranges数组。假设一圈 200 点角度从 -π 到 πangle_increment 就是 2π/200。把解析出来的距离填进ranges就行。Cartographer 的 Lua 配置里几个核心选项是options { map_builder MAP_BUILDER, tracking_frame base_link, published_frame odom, odom_frame odom, provide_odom_frame true, use_odometry true, num_laser_scans 1, }tracking_frame是激光雷达所在的坐标系published_frame是里程计坐标系。如果没有里程计可以把provide_odom_frame设为 false让 Cartographer 纯靠激光帧间匹配估计位姿但效果会差一些。建图完成后保存地图rosrun map_server map_saver -f ~/carto_map会生成carto_map.pgm和carto_map.yaml这张就是标准的栅格地图文件后面做导航也能直接复用。4.2 线路二Python 手写简易栅格建图适合理解本质ROS 路线虽然省心但如果你想知道 SLAM 到底发生了什么建议再花点时间手写一个简化版。基本假设是STM32 不只发雷达数据还通过编码器里程计把当前底盘位姿(x, y, θ)一起发给上位机。上位机每收到一帧雷达数据就把每个激光点从雷达坐标系变换到世界坐标系更新一张栅格地图。变换公式很简单wx x r * cos(θ alpha) wy y r * sin(θ alpha)其中r是激光点距离alpha是这个点相对于雷达正前方的角度θ是机器人当前朝向。落到代码里用一张二维数组作为栅格地图每个格子存一个 log-odds 值import numpy as np from math import cos, sin logodds np.zeros((600, 600)) # 栅格地图 def bresenham(x0, y0, x1, y1): 返回线段经过的所有栅格坐标 points [] dx abs(x1 - x0) dy -abs(y1 - y0) sx 1 if x0 x1 else -1 sy 1 if y0 y1 else -1 err dx dy while True: points.append((x0, y0)) if x0 x1 and y0 y1: break e2 2 * err if e2 dy: err dy x0 sx if e2 dx: err dx y0 sy return points def update_map(pose, scan): for alpha, r, quality in scan: if r 0.1 or r 12.0 or quality 20: continue wx pose[0] r * cos(pose[2] alpha) wy pose[1] r * sin(pose[2] alpha) # 简化更新命中点增加占据概率 grid_x int(wx * 10) 300 grid_y int(wy * 10) 300 logodds[grid_x, grid_y] 0.5这里的更新逻辑只演示了“命中点”实际还要把激光束经过的空白栅格也更新为“空闲”不然地图会有很多空洞。一个完整但不复杂的做法是用 Bresenham 算法把从传感器位置到命中点之间经过的所有栅格都标记为 free命中点那一个栅格标记为 occupied。栅格概率更新公式为new_logodds old_logodds l_occ - l_free简单解释log-odds 越大表示该栅格越可能是障碍物越小表示越可能是空白。这样迭代很多帧后地图会越来越清晰。这个简易版最大的局限是没有回环检测也没有复杂扫描匹配走一圈大范围后位姿会漂移地图角落会重叠。但作为理解 SLAM 的第一课价值极高。4.3 两条路线的选择建议如果你是要做毕业设计、做产品原型直接选 ROS Cartographer能省下大量时间。如果你是想理解 SLAM 原理或者面试被问到“栅格地图怎么更新”心里发虚那就把手写版跑通。两条路线不冲突我自己的建议是先手写一遍简易版再折腾 ROS这样你看到 Cartographer 的每个参数时不会觉得是在调魔法。5. 建图效果调优我实测中踩过的坑和解决记录5.1 供电压降导致串口乱码第一次把整套系统放上车我发现雷达数据偶尔出现半个多小时才丢一帧但一跑起来就疯狂乱码。排查到最后发现问题不是协议解析而是雷达和电机共用一个电源模块电机一转雷达供电被拉低雷达内部测距就出杂波。后来把雷达单独接一路 5V 2A 电源再和 STM32 共地数据立刻稳定下来。遇到莫名其妙丢帧先量一下雷达供电电压别先怀疑代码。5.2 黑色踢脚线和反光物体的特殊表现LD14 这种三角测距雷达对深色、吸光材质物体并不友好。家里黑色的踢脚线、黑色的椅子腿经常在雷达数据里表现为“一段距离突然变成 0”或者“距离忽大忽小”。建图时这些位置会产生噪点。我的处理是在上位机解析阶段把质量值偏低的点过滤掉同时在栅格更新时对单点命中且置信度很低的位置不强制增加占据概率。5.3 地图重影和漂移优先查里程计标定如果你已经接了编码器里程计却发现地图左右重影、转角处错开大概率不是雷达的问题而是轮子直径标定不准。比如原本直径 65mm 的轮子实际有效直径因为轮胎压扁略小一点走 10 米就偏出不少。一个小技巧是让小车走一段已知距离的直线例如 2 米看里程计输出是多少反过来修正轮径系数。我实测这个修正对建图质量提升非常明显比调任何算法参数都管用。5.4 手动推车建图的节奏很多建图失败其实不是设备问题是人推得太快、转得太急。激光 SLAM 需要相邻两帧有足够的重叠区域才能匹配如果你推着车快速甩尾雷达还没来得及扫完整个环境位姿已经大幅变化匹配自然崩溃。我的经验是直线速度控制在 0.2m/s 以内转弯尽量原地小半径转房间至少绕两圈特别是回到起点附近时慢一点让算法有机会修正累积漂移。5.5 Cartographer 参数未必需要疯狂调很多人一觉得建图效果不好就想着调 scan_matcher 搜索窗口、调 loop closure 距离阈值。我的建议是在硬件数据没稳定之前一律用默认参数。默认值在室内小场景里已经相当好。真正需要动参数的场景是把地图边界明显错开或者回环闭合失败那时候再去把loop_closure_rotation_weight相关权重适当加大一次只改一个参数保存一次地图对比。5.6 日志记录比肉眼调试更可靠还有一个很容易被忽略的小技巧建图过程中把每一帧的时间戳、机器人的位姿、雷达点数、帧率全部记录到日志文件。出问题时直接回放日志而不是靠肉眼盯屏幕。另外去看雷达数据可视化时优先看原始点云而不是看建图结果因为原始点云一旦就有问题地图无论如何都不会好。写在最后一个帮我节省大量时间的小习惯我现在做任何一套雷达建图项目第一步永远不是接 STM32也不是写 ROS 节点而是先把雷达接到串口转接板上在电脑上打印原始帧确认角度范围、点数、距离值都正常再继续往下走。这个习惯看着很笨但能避免把“雷达固件配置错误”误诊成“单片机代码问题”。从串口原始字节到屏幕上的二维地图每一步都是可以单独验证的哪怕方案再简易只要链路清晰排错就快。希望这篇分享能帮你少走点弯路早日跑出第一张自己拼出来的地图。