NoeticMaze 在 STM32 上实现激光雷达 SLAM 与自主导航的工程拆解

声明： NoeticMaze 是一个开源项目，作者为 dokdo2013。本文是阅读该项目源码和文档后整理的学习笔记，用于理解嵌入式 SLAM 的工程实现方式。本文作者不是该项目的开发者，未参与该项目的任何代码贡献。 文中所有工程细节均来自对开源代码的分析，不代表本文作者的设计决策。

NoeticMaze 是一个基于 STM32F446RE 的室内自主导航项目。它把激光雷达建图、ICP 定位、A* 路径规划、路径跟踪、动态重规划和蓝牙遥测放进了一个资源受限的 MCU 工程。

这篇文章按照技术报告的完整度展开，不把项目压缩成一句“STM32 跑 SLAM”。真正有价值的部分在工程边界：地图怎么存，点云怎么传，ICP 怎么收敛，A* 怎么在小内存里搜索，FreeRTOS 任务怎么隔离实时控制和重计算。

核心结论

NoeticMaze 在 STM32F446RE 上实现了基于激光雷达的局部室内建图与自主导航闭环。系统不依赖 ROS，不依赖上位机做核心算法，也不使用预先导入的地图。

完整导航能力包括：

• 激光雷达 360 度扫描：UART DMA 接收，逐帧解析为极坐标点云。
• ICP 实时定位：点到线 ICP 匹配前后激光扫描，估计机器人位移。
• 占据栅格建图：2cm 分辨率栅格，Bresenham 光线追踪更新地图。
• A* 路径规划：在 10cm 降采样规划图上搜索低代价可行路径。
• 预瞄点航向跟踪：100Hz 运动控制差速底盘沿路径行驶。
• 动态重规划：目标变化、路径阻断或位姿偏移时重新规划。
• 蓝牙遥测：向上位机发送位姿、地图增量、路径和调试事件。

它的适用范围也很明确：固定 4m x 4m 局部地图、小型室内或迷宫场景、相对静态环境。它没有回环检测，不适合长距离、大范围、动态人群环境。

几个需要先厘清的技术边界

理解 NoeticMaze 前，需要先厘清几个技术边界。它们直接影响后面对定位、建图、路径规划和避障机制的判断。

第一，地图是在线构建的。机器人开机后从零开始扫描环境，通过 ICP 估计位姿，再用 Bresenham 光线追踪把扫描结果写入占据栅格。用户是在实时构建的地图上点选目标。

第二，ICP 采用帧到帧匹配。系统维护参考扫描帧，用当前扫描和参考帧做点到线匹配，估计相对运动，再把定位结果用于建图。

第三，A* 结果应描述为低代价可行路径。项目在 10cm 规划图上做 8 连通搜索，并叠加障碍膨胀、未知区策略、旧路径亲和度和路径平滑。

第四，当前雷达任务没有真正按车体运动状态做跳帧。代码中有运动学跳帧相关宏和设计意图，但实际数据流是每完成一圈 360 度扫描后投递给 SLAM；队列满时复用当前缓冲。

第五，路径规划结果通过全局路径快照和事件标志通知运动线程。系统里存在 RespQueueHandle，但最终路径发布机制是全局路径公告板。

硬件平台与资源约束

NoeticMaze 使用 NUCLEO-F446RE，核心 MCU 是 STM32F446RE。

外围传感器和通信接口如下。

底盘参数如下。

128KB SRAM 是最紧的资源。系统同时需要维护地图、雷达帧、点云、A* 队列、任务栈、蓝牙缓冲和各种状态快照。

关键内存压力包括：

• 2cm 分辨率的 200 x 200 占据栅格地图：2bit/cell，约 10KB。
• 10cm 分辨率的 40 x 40 规划地图：4bit/cell，800B/缓冲，乒乓双缓冲共 1600B。
• 3 个激光雷达帧缓冲：LidarMap_t 每个约 736B，共约 2.2KB。
• FreeRTOS 任务栈：多个任务合计约 20KB。
• ICP 使用的 360 点当前帧和参考帧点云。
• A* 优先队列：2500 个节点，每个 4B，共约 10KB。

这些数字解释了项目为什么大量使用静态内存、打包存储、零拷贝队列和降采样规划图。桌面端常见的宽松数据结构，在这个平台上很快会把 RAM 用完。

总体数据流

系统可以按以下链路理解：

激光雷达 USART1 + DMA
  -> LidarRouteTask 解析并组装 360 度扫描
  -> LidarQueueHandle 投递 LidarMap_t 指针
  -> AlgorithmBrain 执行 ICP、建图、降采样、重规划判断
  -> ReqQueueHandle 向 PlannerTask 请求路径
  -> PlannerTask 在 10cm 地图上运行 A* 和路径平滑
  -> 全局路径公告板 + PathEventHandle 发布路径
  -> MotionTask 100Hz 跟踪路径并控制电机
  -> TaskPrint 通过蓝牙发送地图、路径、位姿和调试事件

高频运动控制和低频规划分开。A* 可以按需运行，MotionTask 必须稳定周期运行。雷达、SLAM、Planner 和蓝牙也通过队列和事件解耦，避免单个模块卡住整个控制链。

激光雷达数据采集与预处理

激光雷达数据通过 USART1 + DMA 循环接收。UART 空闲中断触发解析逻辑，LidarRouteTask 将原始帧组装为完整的 360 度扫描。

协议结构要点：

• 单帧 84 字节。
• 每帧包含 40 个测距点。
• 帧头高半字节匹配 0xA?、0x5?，低半字节组合为校验值。
• 相邻点角度间隔约 0.705 度。
• 单帧覆盖约 27.5 到 28.2 度。
• 完成 360 度扫描需要约 13 到 14 帧原始数据。

代码采用动态插值解算器。解析时根据相邻两帧起始角度差计算精确步长，补偿雷达电机转速波动。

距离过滤也分层处理。雷达模块有效距离宏为 30mm ~ 10000mm，SLAM 转换点云时使用 30mm ~ 8000mm 过滤，无效点标记为 0。

雷达坐标系还有方向适配问题。该雷达角度方向是顺时针为正，而常见 SLAM 坐标系按逆时针为正。极坐标转笛卡尔坐标时需要做角度方向转换，并叠加雷达安装角补偿。

零拷贝雷达缓冲池

系统维护 3 个静态 LidarMap_t 缓冲区，通过 FreeRTOS 队列实现借还机制。

空闲池 -> 雷达任务填充 -> 就绪队列 -> SLAM 任务处理 -> 归还空闲池

队列里传的是指针，不复制整帧数据。当前结构体每个约 736B，三块缓冲区可以接受；如果每帧复制，会浪费 CPU 和 SRAM。

运动学跳帧的当前状态

代码中定义了直行跳帧、转弯全投递、角速度阈值等宏，设计意图是转弯时全量处理，直行或静止时降低投递频率。

当前 StartLidarRouteTask() 没有读取角速度，也没有使用这些宏丢弃直行帧。实际行为是每完成一圈 360 度扫描后，队列可用就投递给 SLAM；队列满时复用当前缓冲。

因此，文章里不能把“直行时每 10 帧只处理 1 帧”写成已生效优化，只能写成预留设计。

IMU 与轮式里程计

MPU6500 使用 DMP 做姿态解算。DMP 在传感器内部输出四元数，STM32 通过 SPI2 读取结果，减少 MCU 上的姿态计算负担。

差速底盘的里程计来自左右轮编码器。系统根据左右轮位移计算线位移和角位移，给 ICP 提供初始猜测。轮式里程计会因为打滑、地面不平和积分误差逐步漂移，但短时间运动预测很有价值。

ICP 如果从零初值开始匹配，容易收敛慢或落到错误局部最优。里程计和 IMU 预测让 ICP 从一个接近真实运动的位置开始迭代，减少计算量，也提高匹配稳定性。

ICP 定位：帧到帧点到线匹配

室内机器人需要持续回答“我在哪里”。编码器可以推算位移，但误差会积累；室内没有 GPS；磁力计在电机和结构件附近也不可靠。因此 NoeticMaze 用激光雷达扫描之间的几何关系修正位姿。

ICP 的直观含义是：当前扫描得到一圈环境轮廓，参考帧保存上一关键帧的环境轮廓。将当前轮廓平移、旋转，直到它和参考轮廓尽量重合。最终平移和旋转量就是机器人相对参考帧的运动。

NoeticMaze 使用帧到帧模式。

地图由定位结果驱动建出来。ICP 负责估计机器人移动了多少，建图模块用这个位姿把激光扫描写入栅格。

参考帧维护

ICP 需要参考帧。系统将第一次有效激光扫描作为初始参考帧，之后根据运动阈值更新。

• 初始时：第一次扫描直接成为参考帧。
• 正常运行：机器人平移超过 10cm 或旋转超过 5.7 度时，当前帧替换参考帧。
• 静止时：如果编码器检测位移小于 2mm 且旋转小于 0.14 度，触发 ZUPT 死区，跳过 ICP。

参考帧存在全局数组 ref_scan[360] 中，同时预计算每个点附近的表面法向量 ref_normals[360]，供点到线误差计算使用。

ICP 完整流程

一帧扫描到来后，定位流程可以拆成以下步骤。

1. 极坐标转笛卡尔坐标
   雷达每个角度槽有一个距离值。系统将距离和角度转换为 XY 点，同时处理顺时针角度方向和安装角补偿。

2. 运动畸变补偿
   一圈扫描跨越一段时间。机器人扫描过程中仍在运动，因此不同角度点处于不同采样时刻。系统用线速度和角速度把每个点补偿到扫描结束时刻。

3. 里程计初始猜测
   编码器和 IMU 给出本帧位姿预测，作为 ICP 初始估计。

4. 迭代匹配
   对当前帧每个有效点，按当前估计位姿变换到参考坐标系，在参考帧角度窗口内搜索最近线段，并计算点到线误差。

5. 构造最小二乘问题
   每个匹配点贡献一行方程，未知量是 dx、dy、dtheta。

6. 求解 3 x 3 线性方程组
   使用高斯消元、部分主元和岭回归，得到本轮位姿增量。

7. 更新位姿并判断收敛
   若增量足够小则退出，否则继续迭代，最多迭代 10 次。

点到线误差的数学形式

点到线 ICP 每次迭代求解的是最小二乘问题。对第 i 个匹配点：

其中：

• $p_i$ 是当前帧点。
• $p'_i$ 是当前估计位姿变换后的点。
• $\text{ref}_i$ 是参考帧中匹配的点。
• $n_i$ 是参考点附近表面的法向量。
• $e_i$ 是当前点到参考线的垂直距离。

线性化后得到关于 $dx$、$dy$、$d\theta$ 的超定方程组。有效匹配对通常有数百个，但未知量只有 3 个。

岭回归项用于防止矩阵病态或接近奇异。

为什么使用点到线 ICP

激光雷达扫到的是离散点，但室内墙面、走廊和家具边缘常常形成连续线段。点到点 ICP 只关心离散最近点，当前扫描和参考扫描角度稍有错位时，最近点未必是真正对应点。

点到线 ICP 把相邻参考点形成的局部表面当作线段，计算当前点到表面的垂直距离。对墙面和走廊这类结构，误差方向更稳定。

法向量通过相邻点计算：

环形边界用模运算处理：0 度点的左邻居是 359 度，359 度点的右邻居是 0 度。

法向量计算还包含保护条件：

• 当前点无效则跳过。
• 邻居点无效则跳过。
• 邻居点距离太远则跳过，避免墙角或不同物体误连。
• 切线向量太短则跳过，避免法向方向不稳定。

ICP 参数表

运动畸变补偿

激光雷达完成一圈扫描需要时间。机器人在这一段时间内可能前进或转弯，导致早期采集的点和后期采集的点不在同一车体坐标系。

补偿思路是把每个点变换到扫描结束时刻。

for each point i in scan:
    dt = i * (scan_time / 360) - scan_time
    dtheta = omega * dt

    if (fabs(omega) > 1e-4) {
        dx = (v / omega) * sin(dtheta);
        dy = (v / omega) * (1 - cos(dtheta));
    } else {
        dx = v * dt;
        dy = 0;
    }

    point[i] = rotate(point[i], dtheta) + [dx, dy];

dt 为负值是合理的：早期点距离扫描结束时刻更远，需要补偿更多运动。代码将 scan_time / 360 提到循环外，用乘法替代重复除法，减少 360 次循环中的开销。

map-odom 变换

ICP 输出的是相对更可信的地图坐标位姿，里程计输出的是高频但会漂移的 odom 位姿。系统维护 map <- odom 变换，将二者连接起来。

tf_theta = icp_theta - odom_theta;
tf_x = icp_x - (cos(tf_theta) * odom_x - sin(tf_theta) * odom_y);
tf_y = icp_y - (sin(tf_theta) * odom_x + cos(tf_theta) * odom_y);

global_x = tf_x + cos(tf_theta) * odom_x - sin(tf_theta) * odom_y;
global_y = tf_y + sin(tf_theta) * odom_x + cos(tf_theta) * odom_y;
global_theta = tf_theta + odom_theta;

这样 100Hz 运动线程可以使用高频里程计，同时通过 ICP 结果修正长期漂移。

占据栅格建图

定位回答“机器人在哪”，建图回答“环境里有什么”。NoeticMaze 使用占据栅格表示局部地图。

SLAM 高精图参数：

每次有激光点命中障碍，系统从机器人所在栅格到命中点栅格做 Bresenham 光线追踪：

for each lidar hit point:
    ray = bresenham_line(robot_pos, hit_point);
    for each cell in ray excluding endpoint:
        mark cell as FREE;
    mark endpoint as OCCUPIED;

Bresenham 只需要整数加减和比较，很适合 MCU。沿途格子标为空闲，终点格子标为占据，这正是占据栅格建图的核心。

2bit 打包存储

每个格子只需要 2 bit 表示 3 种状态：

00 = 未知
01 = 空闲
10 = 占据

一个字节存 4 个格子：

bit[7:6] = cell3
bit[5:4] = cell2
bit[3:2] = cell1
bit[1:0] = cell0

40000 个格子如果用 1 字节表示需要 40KB；2bit 打包后约 10KB。对 128KB SRAM 来说，这个优化非常关键。

地图写保护和增量传输

项目通过 map_core_write.h 的宏定义控制写权限，确保只有 SLAM 线程写地图，降低并发写导致数据损坏的风险。

蓝牙发送地图时只发送变化的格子增量。报告中提到最多记录 512 个 diff，这能显著降低蓝牙带宽压力。

双地图结构：2cm 建图，10cm 规划

SLAM 高精图适合建图和显示，但不适合直接跑 A*。200 x 200 网格有 40000 个节点，搜索空间过大。

系统将 2cm 图降采样为 10cm 规划图。

一个 10cm 规划格对应 5 x 5 个 2cm 细格。降采样采用保守规则：

• 25 个细格中只要有一个占据，规划格标为占据。
• 否则只要有一个空闲，规划格标为空闲。
• 全部未知则保持未知。

这个规则可能牺牲一点通行空间，但能降低漏检障碍的风险。

A* 路径规划

PlannerTask 在 10cm、40 x 40 的规划图上运行 A*。使用 8 连通网格，允许上下左右和 4 个对角方向移动。

A* 的目标是在当前代价模型下找可行且代价较低的路径。代价模型中包含障碍膨胀、未知区策略和路径亲和度。

障碍膨胀

机器人有物理尺寸，路径不能贴着障碍物走。规划图对障碍周围区域设置多级惩罚。

物理碰撞半径约 0.10m。膨胀半径让路径在可通行区域中留出安全余量。

路径亲和度

重规划时，如果新路径和旧路径完全无关，机器人可能在局部代价变化中来回跳。NoeticMaze 对旧路径附近区域降低代价，让新路径倾向于延续旧路径。

报告中整理的策略是：旧路径附近约 0.3m 半径内代价获得折扣。这样能降低重规划振荡，尤其在未知区域和 U 型结构附近更有用。

路径平滑

A* 输出的是栅格折线，直接跟踪会造成频繁转向。系统做两步平滑：

1. 双向拉线
   从路径首尾尝试用直线连接更远路径点，用 Bresenham 射线检测是否穿墙。若直线安全，就替代中间折线。

2. 安全 Bezier 圆角
   在转折处用 Bezier 曲线做圆角。生成圆角时持续做碰撞检测，若碰撞则缩小圆角半径。

平滑后的路径更适合预瞄点跟踪，也能减少差速底盘频繁急转。

路径跟踪与运动控制

MotionTask 以 100Hz 运行。它读取全局路径公告板中的路径，选择前方预瞄点，生成线速度和角速度，再换算为左右轮速度。

核心思想如下：

lookahead_point = find_lookahead(path, robot_pos, L = 0.18m);
heading_error = angle_to(lookahead_point) - robot_yaw;

yaw_rate_cmd = Kp * heading_error;

if (fabs(heading_error) > 10deg) {
    linear_speed *= speed_scale;
}
if (fabs(heading_error) > 60deg) {
    linear_speed = 0;
}

v_left  = linear_speed - yaw_rate_cmd * wheel_track / 2;
v_right = linear_speed + yaw_rate_cmd * wheel_track / 2;

这套方法使用前方预瞄点，但没有使用经典 Pure Pursuit 的曲率公式。因此更准确的描述是“预瞄点航向控制”。

底层电机控制采用轮速 PID。

到达目标附近后系统减速停车。若启用自动返航，到达目标后会将初始位置 (1.05, 3.15) 设为新目标并规划返航路径。

动态避障与重规划

NoeticMaze 的避障机制是持续建图和重规划。系统检测到新障碍后先写入地图，再根据当前地图重新规划路径。

重规划触发条件如下。

避障链路如下：

激光雷达持续扫描
  -> SLAM 更新占据栅格
  -> 检查当前路径是否穿过新障碍
  -> 触发重规划请求
  -> PlannerTask 基于新地图运行 A*
  -> MotionTask 切换到新路径

这套机制适合静态或缓慢变化的室内环境。快速移动的人或物体可能被写入地图，导致临时虚假障碍或重规划抖动。

FreeRTOS 任务拓扑

NoeticMaze 由多个 FreeRTOS 任务协同运行。任务频率和优先级体现了实时系统设计：运动控制优先，规划按需，蓝牙遥测低优先级。

任务间通信机制如下。

系统数据流可以简化为：

LidarRouteTask
  -> LidarQueueHandle
  -> AlgorithmBrain
     -> ICP 位姿
     -> 2cm 栅格更新
     -> 10cm 规划图更新
     -> ReqQueueHandle
  -> PlannerTask
     -> A* 路径
     -> 平滑路径
     -> 全局路径公告板
     -> PathEventHandle
  -> MotionTask
     -> 预瞄点跟踪
     -> 左右轮速度
     -> PWM

这个结构避免了把所有逻辑塞进一个大循环。高频控制不等待 A*，SLAM 不等待蓝牙，上位机遥测也不能阻塞运动控制。

用户交互流程

从用户角度看，流程分为 4 个阶段。

阶段 1：开机初始化

• IMU 初始化并等待 DMP 输出稳定。
• 雷达启动，USART1 DMA 开始接收。
• 编码器和里程计归零。
• 初始位置设置在固定地图坐标范围内，例如 (1.05, 3.15)。
• FreeRTOS 任务开始调度。

阶段 2：自主探索建图

• 雷达持续输出 360 度扫描。
• ICP 做帧到帧匹配并估计位姿。
• Bresenham 光线追踪更新 2cm 占据栅格。
• 蓝牙上位机接收机器人位姿、地图增量和调试事件。
• 用户看到实时构建的局部地图。

阶段 3：用户下发目标点

用户在上位机地图上点击目标位置，上位机通过蓝牙发送目标点。报告中整理的目标点包是 GoalPacket (0x07)，包含 goal_x 和 goal_y。

STM32 通过 USART3 DMA 接收并解析包，调用目标点设置逻辑。SLAM 线程检测到目标变化后，向 PlannerTask 发起规划请求。

阶段 4：规划和自动行驶

• SLAM 将 2cm 地图降采样为 10cm 规划图。
• PlannerTask 使用 A* 搜索路径。
• 路径通过全局公告板发布。
• MotionTask 使用预瞄点航向控制跟踪路径。
• 若路径被新障碍阻断，系统触发重规划。
• 到达目标后停车，必要时自动返航。

这里的用户输入是目标点，系统自行完成定位、建图、路径搜索和轨迹跟踪。

项目亮点

NoeticMaze 的亮点集中在工程实现。

第一，完整链路在 MCU 端闭环。激光雷达、IMU、编码器、SLAM、A*、路径跟踪和蓝牙遥测都在 STM32 上运行。

第二，内存设计有明确约束。2bit 地图压缩、静态缓冲池、零拷贝队列、规划图降采样，都是为 128KB SRAM 服务的。

第三，算法选型贴合场景。室内墙面环境适合点到线 ICP，局部地图适合占据栅格，固定小范围导航适合 40 x 40 规划图。

第四，实时调度边界清晰。高频控制和低频规划分开，重计算不会直接压在运动控制周期上。

第五，通用库依赖少。项目没有使用 Cartographer、GMapping 这类 ROS SLAM 库，ICP、地图、规划和调度逻辑都针对 MCU 资源约束自研。

已知边界

这套系统的边界同样需要写清楚。

没有回环检测。机器人回到之前到过的位置时，系统不会识别并全局校正地图。长时间运行后，位姿和地图会累积漂移。

地图范围固定为 4m x 4m。它适合迷宫、小房间、教学和竞赛场景，不适合整层楼或大范围导航。

环境假设偏静态。移动的人或物体可能被写入地图，造成临时障碍和重规划抖动。

定位精度依赖硬件标定。轮径、轮距、编码器、IMU 零偏、雷达安装角和底盘机械误差都会影响结果。

A* 在未知区域和 U 型结构附近可能振荡。路径亲和度、最小重规划间隔和路径穿障连续帧判断可以缓解，但不能从根本上消除所有局部规划问题。

结论

NoeticMaze 展示了一种典型的嵌入式导航解法：放弃通用框架，把问题限制在明确范围内，并在这个范围内完成闭环。

在 128KB RAM 的 STM32F446RE 上，完整导航系统能跑起来，依赖一组连续的工程取舍：雷达零拷贝、点到线 ICP、2bit 占据栅格、2cm 到 10cm 的双地图、带代价约束的 A*、预瞄点航向控制，以及 FreeRTOS 下的任务隔离。

这也是 NoeticMaze 最值得学习的地方。它是一个约束明确、链路完整、适合学习嵌入式 SLAM 和室内导航的工程样本。

本文由 AI 辅助生成，可能存在错误或遗漏，请以实际资料和官方文档为准。