覆盖式路径规划（CPP）要走遍一片区域而不是从 A 点到 B 点，评价标准也完全不同。本文按「区域怎么被切开」这条轴梳理 CPP 的算法体系，回顾四篇综述二十年的关注点迁移，并给出六个主流开源实现的实测对比。

一、问题定义

1.1 覆盖与点到点的区别

全局路径规划回答「怎么从 A 走到 B」，覆盖式路径规划（Coverage Path Planning, CPP）回答的是「怎么走遍这片区域的每一个可达位置」。扫地机器人、割草机、农业植保、消杀、巡检安防都属于后者。

这个差别不只是目标不同，评价体系是另一套：

转弯代价是最容易被低估的一项。 Bormann 等人在 550 多张真实户型图上做的对比里，神经网络法的覆盖率不差，但路径最长、累计转角最大、主观观感最差；而 Boustrophedon分解转弯最少。在实机上，几十次多余的原地旋转带来的时间损失，往往超过路径长度差异本身——因为非完整约束底盘的原地转向速度远低于直行。

1.2 复杂度

CPP 与覆盖旅行商问题和割草机问题同源，均为 NP-hard。这决定了：

• 精确最优解只在很小规模下可求
• 实用算法基本是「先用几何把问题拆小，再在小规模上做组合优化」
• 「完备性」（只要存在完全覆盖的路径就一定能找到）和「最优性」要分开谈，绝大多数工程算法只保证前者

Galceran 与 Carreras 在 2013 年的综述里总结了 CPP 的六条理想准则：完整性、高效性、连续性、避障、轨迹简单性、最优性。没有算法能同时满足全部六条，选型本质上是在这六条之间取舍。

二、算法体系：按「区域怎么被切开」看

CPP 的算法名目繁多，但真正的分野只有一个问题：要不要先把区域切开，以及怎么切。

flowchart TD    A["目标区域 + 障碍"] --> B{"要不要先切开？"}    B -->|"不切"| C["栅格遍历<br/>Wavefront / Spiral-STC / BSA"]    B -->|"几何切"| D["单元分解<br/>梯形 / Boustrophedon / Morse"]    B -->|"语义切"| E["房间分割<br/>Voronoi / 形态学"]    B -->|"不切，学策略"| F["DRL<br/>DQN / PPO / A3C"]    D --> G["cell 内弓字形往复"]    E --> G    G --> H{"cell 间顺序怎么定？"}    H -->|"邻接图穷举"| I["走一遍就完"]    H -->|"组合优化"| J["TSP / GTSP<br/>近年主流"]    C --> K["路径后处理<br/>Clothoid 平滑"]    I --> K    J --> K

2.1 不切：直接在栅格上走

把地图栅格化，直接设计一条遍历所有自由栅格的规则。

生成树覆盖（STC）系列的价值在于重复率：因为沿树走，理论上每个大单元只经过一次。代价是把地图按 2D×2D 粗化，分辨率损失在窄通道场景会致命。

2.2 几何切：单元分解

把自由空间切成若干互不重叠的简单子区域（cell），并集恰好等于自由空间，每个 cell 内部无障碍，可用简单的弓字形往复走完。

• 梯形分解：在每个障碍物顶点做垂直线，产生大量细碎 cell。实现最简单，但cell 多、路径长。
• Boustrophedon（牛耕式）分解：只在「连通性发生变化」的临界点处划分，cell 数量通常约为梯形分解的一半，覆盖路径显著更短。这是室内覆盖最成熟的方法。
• Morse 分解：Boustrophedon 的推广，基于 Morse 函数的临界点划分，可处理非多边形障碍，且临界点可以在线检测，理论更完备但实现复杂。

Boustrophedon 的失效场景要记住：在杂乱环境中，障碍物顶点密集会导致 cell 被切得极碎，每个 cell 只够走一两趟，cell 间转移的代价反而主导了总路径。这时候它退化得比栅格法还差，Bormann 等人建议与局部能量法互补使用。

2.3 切完之后：遍历顺序才是近年的主战场

分解完成后还剩一个问题：这些 cell 按什么顺序走。早期做法是在邻接图上找一条穷举遍历路径，走通即可，不追求最优。近年的主流是把它建模成组合优化问题：

• Grid-based TSP：把栅格中心当节点求解 TSP。路径质量高，但计算开销随区域急剧上升——Bormann 等人的实验里，优化类方法平均 41.6 秒一个房间，最坏超过 30 分钟，而启发式方法都在 4 秒以内。
• 广义旅行商问题（GTSP）：把 BCD 分解后的每个 cell 视为一个「点集」（该 cell有多个可能的进入/离开点），求解 GTSP 得到最优 cell 遍历序列。Bähnemann 等人的工作把这条路线做扎实了，是目前多边形覆盖里效果最好的组合优化方案。

一条负面结果值得记：Manzini 与 Murphy 尝试把 BCD 路径参数化成可微形式，用梯度下降优化扫描方向和间距，结论是搜索空间高度非凸、梯度方法效果有限。这条路暂时走不通，组合优化仍是当前的正解。

2.4 语义切：先分房间再覆盖

室内场景还有第三条路：不按几何临界点切，按语义切成房间，再逐个房间覆盖，最后用 TSP 优化房间访问顺序。Fraunhofer IPA 的工具链就是这个结构——房间分割、房间内覆盖、多房间访问顺序三个模块分离。

好处是分解结果符合人的直觉（一个房间一次走完，不会来回横跨），并且天然支持「跳过某些房间」这类业务需求。

2.5 不切，学一个策略出来

深度强化学习路线把覆盖建模成 MDP，直接学一个从状态到动作的策略：DQN 处理大状态空间、PPO 用于工业喷涂、A3C/DDPG 处理连续控制。卖点是不需要显式建模几何约束。

但工程落地要清醒：训练成本高、可解释性差、对地图变化的泛化未经充分验证。在静态已知地图这个前提下，几何方法的确定性和可调试性目前仍明显占优。DRL 真正有价值的场景是未知或动态环境——那里几何方法本身也不成立。

2.6 路径后处理：平滑

上面所有方法产出的都是折线路径，弓字形的直角转弯对非完整约束底盘尤其难跟踪（要减速、停车、原地旋转、再起步）。

Šelek 等人的做法是用 clothoid（回旋曲线） 替换转弯段。clothoid 的曲率随弧长线性变化，这正好匹配「方向盘匀速转动」或「差速底盘角速度线性变化」的物理过程，使机器人能以时间最优的方式通过转弯，不必在转弯点停车旋转。他们在 Pioneer 3DX上实测降低了覆盖时间、路径长度和重叠面积。

注意他们平滑的对象是生成树覆盖（RSTC）路径，不是弓字形路径——不过思路可迁移。

三、四篇综述，二十年的关注点迁移

共同底座：Boustrophedon、STC、栅格法在四篇综述中被反复引用，是 CPP 的基石。

另有一篇 2021 年 IEEE Access 的综述（A Comprehensive Review of Coverage PathPlanning in Robotics Using Classical and Heuristic Algorithms）被引已超过 340 次，按经典法与启发式法两分，可作为 2013 与 2025 之间的补充参照。

一个从引文数据里读出来的观察

顺着 Galceran 2013 的被引展开会发现一个明显的现象：2021 年以后引用它的高被引论文，绝大多数是无人机、农业和海事搜救——多机 UAV 覆盖、精准农业路径规划、水面搜救覆盖占据了前列，室内地面机器人的 CPP 几乎看不到高被引新作。

我的解读是：室内地面 CPP 在学术上被认为基本解决了。剩下的困难是工程性的（算力、跟踪精度、动态障碍、长期运行）。

四、开源实现

以下数据为 2026-08-29 通过 GitHub API 实测，非转述。

选型的几个维度

抛开具体项目，选型时真正会卡住的是这几点：

1. 输入形式：接受多边形，还是只接受 OccupancyGrid / costmap？如果业务是「用户圈一片区域」，前者省掉一整套地图裁剪逻辑。
2. 孔洞支持：区域内部有柱子、固定设备时，规划器能不能直接处理带孔多边形。不支持的话要么预处理拆分，要么靠代价地图兜底。
3. 许可证：见上。这一条决定能不能用，应该第一个查而不是最后一个查。
4. 输出形式：多数项目输出的是航点序列（PoseArray / Pose2D[]），需要自己写一个节点逐点下发给导航栈，这部分工作量在选型时容易被漏算。
5. ROS 版本：ROS 1 生态里可选项多但多数已停更；ROS 2 侧目前主要是Nav2 官方的 opennav_coverage 一条路。
6. 覆盖语义：是机体覆盖（清扫、消杀，要求实体走过）还是传感器视场覆盖（巡检、监控，扫到即可）。两者的行距计算完全不同，后者的行距由 FOV 决定，通常远大于机体宽度。

五、几个值得注意的方向

转弯最小化被单独立题了。 有工作专门研究非凸环境的最优分区以最小化转弯次数（RA-L 2022），而不是最小化路径长度。这与 1.1 节的观察一致——转弯才是实际耗时的大头。

巡检场景带来了新指标。 CPP 的传统目标是「走完」，而周期性巡检关心的是「多久回来一次」。Kachavarapu 等人的 FaRe-CPP 面向巡逻机器人，用长距离传感器信息生成覆盖航点并做随机搜索优化，报告路径长度减少 21%–40%、重访时间减少 33%–45%。这是一个和覆盖率正交的优化目标，传统 CPP 方法并不直接优化它。

多机从启发式转向了组合优化与学习。 近年有把大规模多机 CPP 做成局部搜索的工作（AAAI 2024），也有用混合整数规划求时间最优多机覆盖的（RA-L 2023）。学习类的代表是 MARVEL（ICRA 2025），用图注意力网络处理受限视场下的多机协同——注意它解决的是探索（exploration）问题而非覆盖（coverage），两者目标不同，不要混为一谈。

可重构机器人是个冷门但有趣的分支。 有一类工作研究能改变自身形状的瓦片式机器人如何覆盖——形状可变意味着覆盖宽度不再是常数，这让传统的等行距假设失效。

六、我的判断

如果场景是室内静态已知地图 + 多边形圈定区域，BCD 加上 GTSP 优化遍历顺序仍是当前最优实践，DRL 方法的工程成熟度还不足以替代它。这个结论在过去几年基本没变，也印证了第三节那个「室内地面 CPP 已经成熟」的观察。

但真正决定项目成败的往往不是算法选择，而是三件更琐碎的事：

1. 航点下发与跟踪——几乎所有开源项目都只到「输出一串航点」为止，从航点到实际平滑行驶之间的工程量，比算法本身大
2. 转弯的实际代价——纸面上的路径长度对比，可能和实机耗时排序完全不同

最值得投入的优化方向，我认为是路径后处理而非路径生成：用 clothoid 之类的曲率连续曲线替换直角转弯，收益直接体现在耗时和跟踪稳定性上，且不依赖于换算法，对现有系统是增量改进。

七、参考

综述

• Galceran, E., & Carreras, M. (2013). A survey on coverage path planning for robotics.Robotics and Autonomous Systems, 61(12), 1258–1276.DOI: 10.1016/j.robot.2013.09.004
• Bormann, R., Jordan, F., Hampp, J., & Haegele, M. (2018). Indoor coverage pathplanning: Survey, implementation, analysis. ICRA 2018.｜💻 ipa_coverage_planning
• Almadhoun, R., Taha, T., Seneviratne, L., & Zweiri, Y. (2019). A survey onmulti-robot coverage path planning for model reconstruction and mapping.SN Applied Sciences, 1(8), 847.DOI: 10.1007/s42452-019-0872-y
• Jayalakshmi, K. P., Nair, V. G., & Sathish, D. (2025). A comprehensive survey oncoverage path planning for mobile robots in dynamic environments. IEEE Access, 13.DOI: 10.1109/ACCESS.2025.3556446
• Tan, C. S., et al. (2021). A comprehensive review of coverage path planning inrobotics using classical and heuristic algorithms. IEEE Access, 9.DOI: 10.1109/ACCESS.2021.3108177

方法

• Choset, H., & Pignon, P. (1998). Coverage path planning: The boustrophedon cellulardecomposition. Field and Service Robotics.DOI: 10.1007/978-1-4471-1273-0_32
• Bähnemann, R., Lawrance, N., Chung, J. J., et al. (2021). Revisiting boustrophedoncoverage path planning as a generalized traveling salesman problem.Springer Proceedings in Advanced Robotics（FSR 2019）.DOI: 10.1007/978-981-15-9460-1_20｜💻 polygon_coverage_planning
• Manzini, T., & Murphy, R. (2023). Differentiable boustrophedon paths that enableoptimization via gradient descent. arXiv:2309.09882
• Šelek, A., Seder, M., Brezak, M., & Petrović, I. (2022). Smooth complete coveragetrajectory planning algorithm for a nonholonomic robot. Sensors, 22(23), 9269.DOI: 10.3390/s22239269
• Kachavarapu, S., Doernbach, T., & Gerndt, R. (2025). Fast-revisit coverage pathplanning for autonomous mobile patrol robots using long-range sensor information.arXiv:2501.07343
• Karakaya, S., & Konyar, M. Z. (2025). Hybrid boustrophedon and direction-biasedregion transitions for mobile robot coverage path planning.Applied Sciences, 15(23), 12666.DOI: 10.3390/app152312666
• Ni, J., Gu, Y., & Tang, G. (2024). Cooperative coverage path planning formulti-mobile robots based on improved K-means clustering and deep reinforcementlearning. Electronics, 13(5), 944.DOI: 10.3390/electronics13050944
• MARVEL: Multi-agent reinforcement learning for constrained field-of-view multi-robotexploration in large-scale environments. ICRA 2025.arXiv:2502.20217｜💻 marmotlab/MARVEL

开源实现（Star 数与许可证为 2026-08-29 实测）

轮式机器人（含轮式人形）在地面导航，本质是 2.5D 的全局路径规划问题。本文按地图表示组织文献，覆盖 2D 栅格、2.5D 高程图、3D 体素、稀疏图与表面表示五条路线，梳理各自的可通行性估计与规划方法、开源实现和适用边界；再顺着引文关系还原技术演进脉络,最后给出选型建议和四个可能的研究方向。

一、调研概述

1.1 问题描述

轮式人形机器人上半身再复杂，底盘仍然是轮式移动机器人：它只能在连续的地面上运动，不能像无人机那样在自由三维空间里穿行。所以它的全局路径规划不是 3D 问题，而是「在一张带高度信息的地面上找路」—— 也就是 2.5D。

这个定位决定了后面所有的技术取舍。把它当自由 3D 问题做，会付出没必要的计算代价；当纯 2D 做，又会在坡道、门槛、台阶前失效。

调研目标：为轮式机器人的全局路径规划方案设计提供选型依据，回答三个问题 ——在什么地图表示上规划、可通行性怎么算、用什么规划器。

顺带一提，「轮式人形」这个关键词在 arXiv 上几乎搜不到导航规划的工作 —— 该方向当前的研究重心在上肢操作和全身控制，底盘导航被默认为已解决、直接复用轮式移动机器人方案。所以下文的调研对象是轮式机器人这个更大的类别。

1.2 背景知识

全局与局部的分工。 全局规划器在已知地图上算出从起点到终点的一条路径，关心的是连通性和总代价；局部规划器（或控制器）跟踪这条路径并处理未建图的动态障碍，关心的是运动学可行性和实时性。二者频率差一个数量级 —— 参考 2.5D 导航系统 的配置：建图与全局规划约 1 Hz，局部跟踪约 10 Hz。

可通行性（traversability） 是这个领域的核心概念：不是二值的「有没有障碍」，而是「这块地形对这个机器人有多难走」。它至少包含坡度、粗糙度、机器人在该处的姿态三个维度，进阶的还会包含不确定性。这个概念是 2.5D 及以上所有方法的公共基础。

为什么 3D 激光的点云不能直接规划。 SE(2) NavMesh 讲得清楚：点云缺乏结构组织，几何表面和连通性都要靠额外处理去推断，在大场景下计算开销很大且容易出错。所以从传感器到规划器之间，必须插一层地图表示 —— 选哪一层就是本文的主线。

二、文献调研

2.1 地图表示：五类路线

全局规划的第一个决策不是选算法，而是选在什么表示上规划。

flowchart TD    A["传感器数据<br/>3D 激光 / RGB-D"] --> B["2D 栅格<br/>占据/空闲"]    A --> C["2.5D 高程图<br/>每格一个高度"]    A --> D["3D 体素 / OctoMap<br/>完整三维占据"]    A --> E["表面表示<br/>网格 / NavMesh"]    A --> J["稀疏图<br/>TRG / 拓扑图"]    D -.多层投影.-> C    B --> F["图搜索<br/>A* / Theta* / D* Lite"]    C --> G["带代价的图搜索<br/>通行性作权重"]    D --> H["3D 图搜索 / 采样"]    E --> I["多边形寻路"]    J --> K["图优化 / A*"]

一个容易被忽略的事实：2.5D 不是 3D 的简化，而是一种针对地面运动的建模选择。对只能贴地运动的轮式底盘，「同一平面位置只有一个可站立高度」这个假设在绝大多数场景成立。

2.2 2D 栅格路线

主流实现

ROS2 的 Nav2 提供了一整套成熟的 2D 全局规划器：

对轮式人形这类差速或全向底盘，SmacPlannerLattice 通常比 Hybrid-A* 更合适 ——后者的 Dubins/Reeds-Shepp 模型是为转弯半径受限的类车平台设计的。两者都保证路径运动学可行。注意一个差异：Hybrid-A* 支持路径平滑，State Lattice 不支持（格点依赖代价地图分辨率）。

经典算法层面还有 Theta*（any-angle，路径不受栅格八方向约束）、D* Lite（增量式重规划，适合地图局部更新）和 JPS（Jump Point Search，在均匀代价栅格上跳过对称路径，大幅减少扩展节点数）。A-OctoMap 就是在 JPS 上做的改进。

失效边界

T-Hybrid A* 给了个有说服力的量化结果：火星地形数据集上 50 组随机起终点实验，2D 地图规划出的路径有一半无法被安全跟踪 —— 部分路径点的地形粗糙度或机器人投影姿态超出安全阈值。

室内失效更具体，2.5D 导航系统 列举得清楚：坡道、楼梯、路沿、粗糙地面、路上的杂物 —— 这些对机器人可能可通行，但在 2D 栅格上一律被标成障碍。

结论：如果场景是平整地面加静态障碍，2D 栅格 + Nav2 就是正确答案，不要过度设计。

2.3 2.5D 高程图路线（重点）

这是轮式机器人最主流的一路。链路分三段：建图 → 算可通行性 → 在其上规划。

2.3.1 建图

事实标准是 ETH / ANYbotics 这套，三篇论文对应三代实现：

• Fankhauser 等 2018「Probabilistic Terrain Mapping for Mobile RobotsWith Uncertain Localization」—— 这是 elevation_mapping 的论文。机器人中心的高程图，卡尔曼滤波融合位姿与距离传感器，再做基于邻域的概率融合，每格维护高度和方差。维护方差这点很关键，它是后续做不确定性感知的前提。
• Miki 等 2022「Elevation Mapping for Locomotion and Navigation usingGPU」—— 这是 elevation_mapping_cupy 的论文。GPU 加速之外，三个关键增强：高度漂移补偿（对抗状态估计漂移造成的伪影）、基于光线投射的可见性清理（去除虚假体素，并正确处理悬空障碍）、学习式可通行性滤波器。
• Wermelinger 等 2016「Navigation planning for legged robots inchallenging terrain」—— traversability_estimation 的方法论来源。

那条「可见性清理正确处理悬空障碍」值得记 —— 它部分缓解了 2.5D 表达不了悬空物的固有短板。

一个实机必踩的坑，2.5D 导航系统 专门提到：RGB-D 相机通常看不到机器人正下方和正前方，启动时机器人站在未建图的空洞里，跟踪器无法工作。解法是按机器人位姿和坡度主动填充足迹范围内的空格，初始化阶段用较大半径（0.4 m），完成后收到较小半径（0.25 m）补偶发空洞。

2.3.2 可通行性估计

这是 2.5D 的核心，也是各家差异最大的地方。四种思路，复杂度递增。

思路一：高度差分（最轻量）

2.5D 导航系统 的公式，沿横纵两向取间隔 $\Delta$ 的高度差估坡度，映射成概率：

$$S_h(i,j) = \frac{|z(i,j+\Delta) - z(i,j-\Delta)|}{2\Delta\varepsilon}, \quadS_v(i,j) = \frac{|z(i+\Delta,j) - z(i-\Delta,j)|}{2\Delta\varepsilon}$$

$$S = \frac{S_h + S_v}{2}, \quad p(i,j) = e^{-\lambda S(i,j)}$$

$p > 0.5$ 判可通行。只有 $\Delta$ 和 $\lambda$ 两个参数，标定简单，适合算力紧张的平台。

思路二：局部平面拟合（最主流）

足迹范围内最小二乘拟合平面 $ax + by + c = z$，坡度由法向量与竖直方向夹角得到，粗糙度取各点到平面距离的标准差：

$$S_{slope} = \arccos\left(\frac{(a, b, -1) \cdot (0,0,1)}{\sqrt{a^2+b^2+1}}\right), \quad\sigma = \sqrt{\frac{1}{|\mathcal{B}|}\sum_j \frac{(a x_j + b y_j + c - z_j)^2}{a^2+b^2+1}}$$

T-Hybrid A* 用 PCA 取最小特征向量作法向量，本质相同。它还额外算点到拟合平面的有符号距离极差 $h = \max_i(d_i) - \min_j(d_j)$ —— 有符号是关键，这样凹坑和凸起都能检出，绝对值会把两者混为一谈。

思路三：把机器人姿态算进去

前两种算的是「地形本身好不好走」，但真正决定安全的是机器人以某个朝向停在该处时的姿态。T-Hybrid A* 规划时实时把机器人投影到局部平面，解出真实 roll 和 pitch：

$$\tilde{\tau} = 1 - w_r \frac{r_{sum}}{r_{max}}- w_{\theta x} \frac{|\theta_x|}{\theta_{xmax}}- w_{\theta y} \max\left(\frac{\theta_y}{\theta_{ymin}}, \frac{\theta_y}{\theta_{ymax}}\right)$$

注意 pitch 用了两个不对称阈值 —— 上坡和下坡的安全裕度本来就不同。对重心高的平台，这个不对称只会更明显。

思路四：把不确定性和风险算进去

前三种都把可通行性当确定值。但地图本身有噪声，感知退化场景尤甚。STEP（DARPA地下挑战赛）给出另一套框架：不确定性感知的建图与可通行性评估，用条件风险价值（CVaR）做尾部风险评估 —— 关注的不是平均有多难走，而是最坏那部分有多糟。配合风险感知 MPC 与快速恢复行为，在洞穴、矿井、隧道做了轮式和腿式平台的实地验证。

对轮式人形这个视角值得重视：重心高的平台，一次翻倒的代价远高于多绕十米，用均值代价优化本身就是错配。

一个容易整体遗漏的类别：负障碍。 上面讨论的都是正障碍（凸起、坡、墙），但坑洞、台阶下沿、装卸口这类负障碍感知上更难 —— 激光打不到坑底就没有回波，表现为数据缺失而非障碍。Hines 等 用虚拟表面加姿态感知的规划与行为处理。

综述：针对地面车辆最对口的是 Borges 等 2022（方法、传感器与挑战），另有可通行性估计综述 和地面分割与可通行性估计综述。

2.3.3 在高程图上规划

路线 A：把通行性喂给现成的 2D 规划器。 2.5D 导航系统 最省事：把逐格通行性概率写进 Nav2 代价地图，直接用现成规划器（他们用 Theta*），局部跟踪用 MPPI。

他们对比两种 MPPI 代价函数的结论有意思：Slope-Roughness 路径更短但会贴着悬崖走；Slope-Traversability 离边缘更远更保守但会直接开过小坑。没有绝对更好，取决于你的失败代价是翻车还是颠簸。

路线 B：专门的地形感知规划器。 T-Hybrid A* 分层混合 —— 绝对不可通行区只用 2D 表示、规划时跳过；其余区域才建 2.5D 做完整姿态评估。点云分割用动态体素栅格，尺寸按机器人算：

$$G_l = G_w = \sqrt{R_l^2 + R_w^2}, \quad G_h = G_l \tan\rho_{max}$$

高度取 $G_h$ 后，树木桥梁这类悬空结构自然落在栅格外 —— 一个几何约束同时完成降采样和悬空物过滤。实机（0.7×0.5×0.5 m，Velodyne-16，最大坡度 20°）长坡场景 roll cost 从纯2.5D 的 537 降到 464。

2.3.4 2.5D 的硬边界

三条，都无法靠调参绕过：

1. 悬空结构——每格一个高度，桌子货架门框横梁表达不了；
2. 多层重叠——天桥、楼梯间、多层厂房，同一平面位置有多个可站立高度；
3. 矮小物体误判——纯几何下，线缆、地垫容易被当成地面起伏。

碰到前两条就该往 2.4 走。

2.4 3D 体素与多层结构

投影法：3D 建图，2.5D 规划

性价比最高的折中，室内不平整环境导航 是典型。流程：轮式里程计 + 2D 激光 +RGB-D 建 3D OctoMap → 投影出多层 2D 占据图 → 按层间高度差生成可通行图 →变步长 RRT 规划。

它的场景设定对室内轮式机器人特别贴切：现代室内环境大多按无障碍标准设计，因此轮式机器人可以走坡道、同时避开楼梯。这直接决定了可通行性判据 —— 不是「地面平不平」，而是「这个坡度轮式底盘能不能上」。

投影法的价值：建图阶段保留完整 3D 信息（悬空物和多层都在），规划阶段投影到 2.5D（开销可控）。

直接在多层结构上规划

多层是常态时，多层点云加权可通行图 更彻底：点云用多层跳表结构加瘦身算法组织；直接通过车体与点云交互分析取通行性指标，跳过曲面拟合；指标赋到多层连通图得到加权可通行图，改造 A* 搜索。

OctoMap 效率优化：A-OctoMap 自适应分辨率下采样，Parallel OctoMapping并行化。

2.5 稀疏图与表面表示

这一类的共同点是把稠密栅格压缩成稀疏结构，用空间换时间。

TRG 系列：风险图（对轮式平台最相关）

TRG-Planner（RA-L 2025）提出可通行风险图（TRG）：节点表示地形的稳定性与可达性，边表示相对通行风险加权的候选路径。用波前传播方式构建、分层管理,因此大场景也能实时规划。最后在 TRG 上做图优化，同时优先安全和短距离。四足实机验证,是 DreamSTEP 队在 ICRA 2023 Quadruped Robot Challenge 夺冠方案的全局规划器。

RB-TRG（2026-08）是本文调研到的最贴近轮式人形平台的工作，实机 Go2-W轮式四足。它指出原生 TRG 的代价是在圆形邻域和沿边地形区域上算的，而非机器人有朝向的机体足迹 —— 对轮式腿式平台会漏掉部分支撑丧失和机体与地形干涉，转弯时尤其明显。

做法：沿图的边和 yaw 扫掠采样有向矩形足迹，度量纵向支撑变化、侧向倾斜、地形干涉、以及对不可信地图区域的暴露；把这些量的均值与上尾特征并入转移代价，A* 在有序节点对状态上最小化。关键是它保留了 TRG 的构建过程和规划接口，是增强而非另起炉灶。

结果：端到端成功率 51.5% → 68.5%，平均路径长度增加 2.3%。这个交换比很说明问题 ——多走 2.3% 的路换 17 个百分点的成功率。拿了 IEEE ICRA 2026 Legged Robot Challenges最佳自主与最佳机动奖。

注意它和 STEP 的呼应：「对不可信地图区域的暴露」和「上尾特征」本质都是风险感知，不是平均代价优化。两条独立线索收敛到同一思路。

SE(2) NavMesh：朝向相关的多边形表示

SE(2) NavMesh（ETH RSL，2026-07）把可通行区域抽象成连通凸多边形。经典NavMesh 假设通行性与朝向无关，对非圆形机体失效。它用足迹掩膜评估通行性并按yaw 离散成多层，层内平移连通、层间原地旋转连通。

规划用三阶段 ASA（A* → String Pulling → A*）：yaw 分层图上搜可行路径与区域序列→ 丢弃 yaw 信息合并区域得多边形走廊、走廊内几何拉直 → 拉直位置上二次搜索优化朝向。

结果：比经典 NavMesh 多捕获 50% 以上可通行面积，ASA 再降 6.2% 路径长度。实机通过0.8 m 宽通道 —— 只比机体宽 0.27 m。

它自承的局限对轮式人形恰是机会：只考虑固定机体高度，「降低高度就能过」的区域被误判为不可通行。很多轮式人形能调整躯干高度，把高度作为额外离散维度加入是自然扩展。

同类还有 SCAN-Planner 的朝向相关双圆柱足迹与投影 A* 加 z 梯度抑制 ——避障主要在水平方向变形，保持斜坡楼梯上的垂直趋势。以及 ArtPlanner，traversability 引导的采样加 MPC，面向野外腿式机器人。

拓扑图：更高层的抽象

再往上还有一层本文未展开的表示：分层语义拓扑图。osmAG 用 OSM 格式表达分层的语义区域图（房间、走廊、连接关系），适合建筑尺度的粗粒度路径规划，再由下层几何规划器细化。这条路线和上面几种是互补关系而非竞争。

2.6 横切主题：动态环境下的重规划

上面绝大多数工作假设全局地图静态、规划一次然后跟踪。但轮式人形的作业场景（商场、办公、家庭）高度动态。

这个方向本文覆盖不足，只找到一条明确线索：De Luca 等 2023「AutonomousNavigation With Online Replanning and Recovery Behaviors for Wheeled-Legged RobotsUsing Behavior Trees」—— 用行为树组织在线重规划与恢复行为，平台正是轮式腿式机器人。行为树在这里的作用是把「什么时候该重规划、重规划失败怎么办」这类控制流从规划器里剥离出来显式建模。

结合 D* Lite 这类增量式重规划算法，是值得单独展开的方向。

2.7 最新进展（2026 年）

把 2026 年 4 月之后的工作单独拎出来看，会发现三条清晰的趋势线。

趋势一：把全局规划推到传感器视距之外

这是今年最集中的方向。核心矛盾是：车载传感器只能看到周边几十米，但安全高效的路线取决于视距之外的地形特征，纯靠局部感知的全局规划必然近视。

Viswanath 等 2026 的做法是从俯视数据（卫星影像、航空激光、矢量地图）学习连续的可通行性图，监督信号直接来自人类驾驶的 GPS 轨迹，再用激光的自监督几何先验做约束。他们同时公开了一个 299 场景、约 1244 km² 地形配 1130 km 人类驾驶数据的数据集（HuggingFace）。实车结果：路径长度在人类路径的 3.66% 以内，操作员干预次数比纯局部规划方案减少约 85%。

Sivaprakasam 等 2026（CMU）走的是蒸馏全局可通行性先验到图像 affordance预测，同样针对长时程下的近视行为。Singh 等 2026 则更直接：把低分辨率的全局DEM 与实时传感器反馈做混合，弥补「全局建图与实时感知脱节」的问题。

对室内轮式平台的启发：室内没有卫星影像，但建筑 BIM 图纸、楼层平面图是等价的俯视先验。这条路线的思想可以迁移，数据源要换。

趋势二：风险建模从「加权代价」走向「概率融合」

STEP 在 2021 年引入 CVaR 尾部风险，RB-TRG 在 2026 年用上尾特征，两条独立线索已经收敛。今年又出现了第三条：

Genua 等 2026 面向自主轮椅的分层风险图，把四类异质风险 —— 地形坡度、静态障碍、动态障碍、语义可通行性 —— 用 Noisy-OR 融合模型合成统一的概率代价面。这个选择值得注意：Noisy-OR 假设各风险源独立地「导致失败」，比线性加权更符合安全语义。配对蒙特卡洛评估中，碰撞率从 73% 以上降到 32% 以下，障碍物间距翻倍，并在商用电动轮椅上做了室内外三种任务剖面的验证。

电动轮椅在运动学和安全约束上与轮式人形高度相似，这篇的参考价值比多数越野工作更高。

另有 Arief 等 2026 从理论侧切入：在真实通行代价先验未知、驱动不完美的前提下，把 Dijkstra 当作 oracle 解在线随机最短路问题，并给出可证明的保证。这类工作填的是「代价函数本身不确定时该怎么规划」的空白。

趋势三：跨本体的可迁移性成为显式问题

Wang 等 2026 提出的问题很尖锐：对一个机器人语义上合理的路径，对另一个机器人可能物理上不可行。他们用 VLA 模型提供语义先验，再用轨迹残差适配特定本体的移动性约束。

这正好呼应本文反复提到的一点 —— 同一套可通行性公式，换个平台语义可能完全不同。过去这被当作调参问题，现在开始被当作建模问题处理。

三、开源项目

选型上的一条经验：推荐工具时把它背后的论文一起读，比只看 README 清楚得多 ——参数含义、适用假设、已知局限通常只在论文里。上表的「对应论文」一列就是为此。

系统级整体架构可参考 ETH 的轮式腿式机器人工作：自适应运动控制 + 移动性感知的局部导航 + 城市尺度的大范围路径规划，在苏黎世和塞维利亚做了公里级自主任务。

四、演进脉络

把这些工作按引用关系串起来，能看出比单篇阅读更清楚的东西。下面这张图是顺着引文关系整理的 —— 箭头表示后者引用并建立在前者之上。

flowchart TD    W16["Wermelinger 2016<br/>腿式机器人地形导航规划"] --> F18["Fankhauser 2018<br/>概率地形建图"]    K17["Krüsi 2017<br/>点云直接规划"] --> W16    K17 --> TH23["T-Hybrid A* 2023<br/>2D+2.5D 混合地图"]    F18 --> M22["Miki 2022<br/>GPU 高程图"]    F18 --> D22["2.5D 导航系统 2022<br/>Nav2 集成"]    W16 --> STEP21["STEP 2021<br/>CVaR 尾部风险"]    M22 --> TRG25["TRG-Planner 2025<br/>可通行风险图"]    STEP21 --> TRG25    TH23 --> RB26["RB-TRG 2026<br/>有向足迹风险图"]    TRG25 --> RB26    H20["Hines 2020<br/>负障碍"] --> RB26    M22 --> SE26["SE(2) NavMesh 2026<br/>朝向相关多边形"]    STEP21 --> LR26["分层风险图 2026<br/>Noisy-OR 融合"]

三条主线：

建图线（ETH 系）。 Krüsi 2017 证明了可以直接在点云上规划但代价高昂 → Wermelinger2016 转向高程图上的导航规划 → Fankhauser 2018 把建图做成概率框架并维护每格方差 →Miki 2022 上 GPU 并加入可见性清理。方差这条线索很关键 —— 2018 年埋下的这个设计,是后来所有不确定性感知方法的前提。

风险线。 STEP 2021 首次把 CVaR 尾部风险引入可通行性评估 → TRG-Planner 2025 把风险直接编码进图的边权 → RB-TRG 2026 进一步用有向足迹算风险的上尾特征 → 同年分层风险图用 Noisy-OR 做多源风险融合。四篇独立工作、不同团队，都在朝「用概率而非加权代价描述风险」收敛，这个信号比任何单篇的性能数字都重要。

表示线。 从 2D 栅格到 2.5D 高程图是为了表达地形；从高程图到 TRG / NavMesh 是为了压缩规模并显式编码朝向。注意 SE(2) NavMesh 和 RB-TRG 是 2026 年同期的两篇，一个走多边形、一个走稀疏图，但解决的是同一个问题 —— 非圆形机体的朝向相关通行性。这种「同期不同路径攻同一问题」的格局，通常意味着该问题刚刚成熟到值得攻。

一个观察：RB-TRG 同时引用了 T-Hybrid A*、TRG-Planner 和 Hines 的负障碍工作，它是本文所覆盖的三条线的汇合点。想快速切入这个领域，从它的参考文献读起效率最高。

五、调研结论与方案建议

以下是我基于上述文献的推断，不是论文结论，需自行验证。

5.1 选型决策

flowchart TD    A{"场景里有<br/>坡道 / 门槛 / 地面起伏吗"} -->|否| B["2D 栅格 + Nav2<br/>不要过度设计"]    A -->|是| C{"有悬空障碍<br/>或多层结构吗"}    C -->|否| D["2.5D 高程图<br/>elevation_mapping_cupy<br/>+ 通行性写入代价地图<br/>+ Nav2 规划器"]    C -->|是| E{"需要跨楼层吗"}    E -->|否| F["3D 建图 + 多层投影<br/>OctoMap → 2.5D<br/>规划仍在 2.5D 上"]    E -->|是| G["多层图表示<br/>多层可通行图 / TRG / NavMesh"]

5.2 具体判断

默认路线是「3D 建图 + 2.5D 规划」。 既然已有 3D 激光，建图阶段没理由丢掉三维信息（悬空物和多层都在里面），但规划投影到 2.5D 控制开销。

通行性判据必须按平台标定，不要照搬论文参数。 公式结构通用，但 $\rho_{max}$、$h_{max}$、roll/pitch 阈值必须按自己底盘实测能力定。

不对称的 pitch 阈值值得照搬。 上下坡安全裕度不同，对重心高的平台尤其重要。

朝向相关的通行性，取决于机体长宽比。 底盘接近方形或圆形时圆形膨胀够用；明显细长、或需要窄通道原地转身时，才考虑 RB-TRG / SE(2) NavMesh 这类方案。先量长宽比再决定，不要预先上复杂方案。

风险感知比平均代价更适合重心高的平台。 STEP 的 CVaR 和 RB-TRG 的上尾特征是同一思路的两种实现。轮式人形一次翻倒的代价远高于多绕路，值得优先考虑。

同一个公式在不同平台上语义可能相反。 T-Hybrid A* 的$G_h = G_l \tan\rho_{max}$ 是为了排除悬空物（室外的树和桥与底盘无关）；但对轮式人形，桌子货架本身就是障碍，这个高度应取机体高度。照搬公式不看语义会出事。

全局规划不需要高频。 建图与全局规划 1 Hz、跟踪 10 Hz 是合理配置，算力优先给跟踪和局部避障。

5.3 开放问题

动态环境下的全局重规划触发条件。 见 2.6 节 —— De Luca 2023 的行为树方案是目前找到的唯一明确线索，D* Lite 与通行性代价如何结合仍需自行验证。

机体高度可变时的表示扩展。 SE(2) NavMesh 处理 yaw 的分层方式能否推广到高度维度，尚无现成工作。

六、可能的研究方向与创新方案

以下是我基于上述脉络的推断，不是文献结论。 每条给出问题、依据和一个可落地的方案草图。

6.1 把机体高度加进 SE(2)，做 SE(2)×H 的通行性

问题。 SE(2) NavMesh 自己在 Limitations 里承认：它只考虑固定机体高度，因此「降低高度就能通过」的区域会被误判为不可通行，可通行面积被系统性低估。而轮式人形、可升降底盘、轮式腿式平台恰恰普遍能调整躯干高度。

依据。 SE(2) NavMesh 已经证明了「把一个额外自由度离散成分层图、层内平移连通、层间转移连通」这套机制是可行且高效的 —— 它对 yaw 就是这么做的。高度和 yaw 在结构上是同一类问题。

方案草图。 把机体高度离散成 $N_H$ 档（如 3 档：常态 / 半蹲 / 最低），对每档用对应的足迹掩膜和高度阈值独立评估通行性，生成 $N_\Psi \times N_H$ 层。层间转移分两类：同高度不同 yaw 的原地旋转、同 yaw 不同高度的升降。升降的代价按耗时加稳定性惩罚计 —— 降低重心通常更稳，但升降本身耗时且过程中重心轨迹有风险。ASA 的三阶段寻路可以直接复用，只是状态多了一维。

预期收益与风险。 收益是窄空间可通行面积的显著提升（对标 SE(2) NavMesh 相对经典NavMesh 的 50%）。风险是状态空间乘以 $N_H$，需要验证大场景下是否还能实时 —— 好在高度的档位数远少于 yaw，$N_H \le 4$ 应该可控。

6.2 用 BIM / 楼层平面图做室内版的「超视距先验」

问题。 趋势一那批工作（卫星影像、航空激光学可通行性）全部面向室外。室内轮式机器人有同样的近视问题 —— 传感器看不到隔壁房间，但全局路径的优劣恰恰取决于那里。

依据。 Viswanath 等 的关键设计不是「用卫星图」，而是用一种覆盖全域的俯视先验，配合稀疏的真实轨迹监督。室内的等价物是 BIM 模型或楼层平面图 ——覆盖全域、免费获取、且天然含语义（门、楼梯、电梯、房间功能）。osmAG 已经证明了建筑级拓扑图对粗粒度规划有用，缺的是「从图纸预测可通行性」这一步。

方案草图。 以 BIM / 平面图为输入，预测一张连续的先验可通行性图；监督信号用机器人自己历史运行的轨迹（等价于人类驾驶轨迹），几何先验用已建的高程图做自监督约束。在线运行时把先验图与实时高程图融合 —— 融合方式可以直接借 Genua 等 的 Noisy-OR。

这个方案的额外价值：图纸和实际不符的地方（临时堆放、改造）会表现为先验与实测的显著分歧，这本身就是有用的信号，可以驱动重新探索。

6.3 把重规划触发条件建模成风险变化，而不是几何变化

问题。 第 2.6 节留的开放问题 —— 动态环境下什么时候该重规划。现有做法要么定时触发，要么在检测到路径被阻断时触发，两者都不够。定时重规划浪费算力；等到阻断再规划往往已经晚了。

依据。 De Luca 等 用行为树把重规划的控制流显式建模，但触发条件本身仍是几何判据。而趋势二那批工作已经把可通行性做成了概率量。既然代价是概率，代价的变化量就是可度量的。

方案草图。 维护当前全局路径沿线的累积风险 $R(t)$。局部地图每次更新后重算$R(t)$，当风险增量超过阈值或当前路径风险超过次优路径的估计风险时触发重规划。前者应对突发障碍，后者应对「原路径还能走但已经不划算」的渐变情形 —— 后一种是纯几何判据完全捕捉不到的。配合 D* Lite 做增量式重算，避免每次全量重规划。

验证方式。 在同一动态场景下对比定时触发、几何触发、风险触发三种策略的干预次数和总耗时。

6.4 轮式人形专属：把上半身状态纳入底盘通行性

问题。 本文所有工作都把机器人当成一个刚性足迹。但轮式人形的上半身是可动的 ——手臂展开时的投影、搬运物体时的重心偏移、俯身时的高度变化，都会改变底盘的通行性。

依据。 Wang 等 已经把「本体特定的移动性约束」提为显式问题；门洞导航那篇 也开始协调底盘与机械臂的运动。但都停留在局部规划层面，全局规划仍然假设机体形状固定。

方案草图。 把上半身构型抽象成少数几个任务态（空载 / 持物 / 手臂收拢 /手臂展开），每个任务态对应一组足迹掩膜和重心高度参数。全局规划时，任务序列是已知的（先去 A 取物、再去 B 放下），因此可以分段用不同的通行性图规划，在任务态切换点做拼接。这比在线维护一个连续的构型相关通行性要现实得多。

这条我认为是轮式人形相对普通 AGV 最本质的差异，也是现有文献覆盖最薄的地方 ——本次调研没有找到任何一篇正面处理它。

七、参考文献

说明：💻 = 有公开代码，📊 = 有公开数据集，🔗 = 仅有项目页。本节逐条核对过各论文正文中的链接，未标注的即为未找到公开实现。

核心论文

• T-Hybrid A* — Hybrid Map-Based Path Planning for Robot Navigation inUnstructured Environments, arXiv:2303.05304 ｜ 💻 代码
• 2.5D 导航系统 — 2.5D Mapping, Pathfinding and Path Following,arXiv:2209.07252 ｜ 无独立仓库，基于 elevation_mapping 与 Nav2 实现
• SE(2) NavMesh — SE(2) Navigation Mesh, arXiv:2607.01454 ｜ 🔗 项目页（代码待发布）
• SCAN-Planner — arXiv:2606.19555 ｜ 💻 代码（待发布）

图表示与风险感知

• TRG-Planner — Traversal Risk Graph-Based Path Planning, arXiv:2501.01806｜ 🔗 项目页（代码未公开）
• RB-TRG — Robot-Body-Aware TRG Planning for Wheeled-Legged Robots,arXiv:2608.16433 ｜ 💻 代码
• STEP — Stochastic Traversability Evaluation and Planning, arXiv:2303.01614｜ 未开源
• ArtPlanner — Robust Legged Robot Navigation in the Field, arXiv:2303.01420
• osmAG — Hierarchical Semantic Topometric Area Graph Maps, arXiv:2309.04791

建图与可通行性

• Fankhauser 等 2018 — Probabilistic Terrain Mapping for Mobile Robots WithUncertain Localization, RA-L ｜ 💻 elevation_mapping
• Miki 等 2022 — Elevation Mapping for Locomotion and Navigation using GPU,arXiv:2204.12876 ｜ 💻 elevation_mapping_cupy
• Wermelinger 等 2016 — Navigation planning for legged robots in challengingterrain, IROS ｜ 💻 traversability_estimation
• Krüsi 等 2017 — Driving on Point Clouds, JFR
• Hines 等 2020 — Negative Obstacle Navigation, arXiv:2010.16018

其他地图表示

• 室内不平整环境导航 — arXiv:1710.10523 ｜ 未开源
• 多层点云加权可通行图 — arXiv:2504.21622 ｜ 未开源
• A-OctoMap — arXiv:2406.13910（改进 JPS，未开源）／Parallel OctoMapping — arXiv:2603.22508

2026 年最新进展

• Viswanath 等 2026 — Learning Traversability-Aware Global Planners for LongHorizon Off-Road Navigation, arXiv:2607.23743 ｜ 📊 数据集（299 场景 / 1244 km²）
• Sivaprakasam 等 2026 — Distilling Global Traversability Priors, arXiv:2607.17984
• Singh 等 2026 — Terrain-Aware Local Path Planning with Global DEM Integration,arXiv:2608.17038
• Genua 等 2026 — Layered Risk Mapping for Autonomous Patient Transport,arXiv:2607.13497
• Arief 等 2026 — Dijkstra as an Oracle for Online Stochastic Shortest Path,arXiv:2608.17703
• Wang 等 2026 — CrossTracer: Cross-Embodiment Navigation, arXiv:2608.06688
• Sinaei 等 2026 — Motion Planning for Mobile Manipulators Navigating Doorways,arXiv:2608.00206

系统与综述

• 轮式腿式机器人导航与运动 — arXiv:2405.01792
• De Luca 等 2023 — Online Replanning and Recovery Behaviors Using BehaviorTrees, RA-L
• Borges 等 2022 — A Survey on Terrain Traversability Analysis for AutonomousGround Vehicles, Field Robotics
• 可通行性估计综述 — arXiv:2204.10883 ／地面分割与可通行性估计综述 — arXiv:2312.16839
• MRPB 1.0 — 局部规划统一 benchmark, arXiv:2011.00491

移动机器人开发中反复用到的命令，按类别整理成速查表。涵盖系统排查、Docker、Git/SVN、ROS 基础命令、rosbag 录制过滤，以及 TF 与可视化调试。

平时调机器人，命令散落在各处笔记里，用的时候总要翻半天。这里按使用场景归了类，方便直接检索。环境基于 Ubuntu 20.04 + ROS Noetic。

一、系统与硬件信息

1
2
3
4
5
6
7
8
9
10

uname -a                # 内核 / 操作系统 / CPU 信息
cat /etc/issue          # 操作系统版本
cat /proc/version       # 含 GCC 版本信息
lsb_release -a          # 发行版信息
hostname                # 计算机名

lspci -tv               # 所有 PCI 设备
lsusb -tv               # 所有 USB 设备
lsmod                   # 已加载的内核模块
env                     # 环境变量

CPU 相关的几条值得单独记，排查算力瓶颈时常用：

1
2
3
4
5
6

cat /proc/cpuinfo | grep "physical id" | sort | uniq    # 几颗物理 CPU
cat /proc/cpuinfo | grep "cpu cores" | uniq             # 每颗物理 CPU 的核心数
cat /proc/cpuinfo | grep "processor" | wc -l            # 逻辑 CPU 总数

# 是否启用超线程：比较 cpu cores 与 siblings，后者是前者两倍即为开启
cat /proc/cpuinfo | grep -e "cpu cores" -e "siblings" | sort | uniq

二、资源占用与网络排查

1
2
3
4
5
6
7
8

free -m                 # 内存与交换区使用量
df -h                   # 各分区使用情况
du -sh <目录名>          # 指定目录大小
uptime                  # 运行时间、用户数、负载
cat /proc/loadavg       # 系统负载
mount | column -t       # 挂载分区状态
fdisk -l                # 所有分区
swapon -s               # 所有交换分区

1
2
3
4
5

ifconfig                # 所有网络接口属性
route -n                # 路由表
netstat -lntp           # 所有监听端口
netstat -antp           # 所有已建立的连接
iptables -L             # 防火墙设置

排查端口被占用时 netstat -lntp 最直接，能同时看到端口和占用它的进程。多机ROS 通信连不上，先看这里。

三、用户、进程与计划任务

1
2
3
4
5
6
7
8
9
10

w                       # 活动用户
id <用户名>              # 指定用户信息
last                    # 用户登录日志
cut -d: -f1 /etc/passwd # 系统所有用户
cut -d: -f1 /etc/group  # 系统所有组
crontab -l              # 当前用户的计划任务

ps -ef                  # 所有进程
top                     # 实时进程状态
htop                    # top 的增强版，需单独安装

四、系统配置与环境

新机器初始化时的几件事。改主机名：

1
2

sudo vim /etc/hostname
sudo vim /etc/hosts     # 两个文件都要改，否则 sudo 会变慢

改用户名（涉及多个文件，改错会登不进系统，操作前先确认有 root 可用）：

1
2
3
4
5
6
7
8
9
10
11
12
13

# 进入命令行模式，避免图形界面占用该用户
Ctrl+Alt+F1

sudo vim /etc/passwd
sudo vim /etc/shadow
sudo vim /etc/group
sudo vim /etc/sudoers
mv /home/olduser /home/newuser

# vim 内批量替换
:%s/olduser/newuser/g

shutdown -r now

基础环境：

1
2
3
4
5
6
7
8
9
10
11

# ROS 一键安装
wget http://fishros.com/install -O fishros && . fishros

# 常用工具
sudo apt-get install terminator htop net-tools ssh

# 系统时间同步（机器人长期离线后时间会漂，影响 tf 时间戳）
sudo apt-get install ntpdate
sudo ntpdate time.nist.gov
sudo date --set="2026-08-26 11:07:00"    # 无网络时手动设置
sudo hwclock --systohc                   # 写入硬件时钟

多版本 OpenCV 共存，编译时指定独立安装路径，避免污染系统默认版本：

1
2
3
4
5

cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local/opencv346 ..
make -j8
sudo make install
sudo /bin/bash -c 'echo "/usr/local/opencv346/lib" > /etc/ld.so.conf.d/opencv.conf'
sudo ldconfig

conda 环境：

1
2

conda create --name myenv python==3.8
pip install -r requirements.txt -i https://mirror.baidu.com/pypi/simple

打包与开机自启：

1
2
3
4

tar -cvf workspace_20260826.tar my_ws    # 仅打包不压缩
tar -xvf workspace_20260826.tar          # 解包

gnome-session-properties                 # 图形化配置开机自启

五、Git 与 SVN

1
2

git branch -a           # 查看全部分支（含远程）
git remote -v           # 当前分支的远端仓库地址

六、Docker

服务管理：

1
2
3
4
5
6
7
8
9

systemctl start docker
systemctl stop docker
systemctl restart docker
systemctl status docker
systemctl enable docker     # 开机启动

docker info                 # 概要信息
docker --help               # 总体帮助
docker <具体命令> --help     # 单条命令帮助

镜像：

1
2
3
4

docker images                       # 本地所有镜像
docker rmi -f <镜像ID>               # 删除单个
docker rmi -f <ID1> <ID2>           # 删除多个
docker rmi -f $(docker images -qa)  # 删除全部

docker images 输出的四列含义：REPOSITORY 仓库源、TAG 版本号、IMAGE ID 镜像 ID、SIZE 大小。

七、ROS 基础命令

工作空间与编译：

1
2
3
4
5

catkin_create_pkg my_pkg roscpp rosmsg rospy std_msgs   # 创建功能包

catkin_make                                             # 编译全部
catkin_make -DCATKIN_WHITELIST_PACKAGES="pkg1"          # 只编译指定包
catkin_make -DCATKIN_WHITELIST_PACKAGES=""              # 恢复编译全部

只编译单个包这条在大工作空间里很省时间，但用完记得清空白名单 —— 忘了会导致后面改其他包怎么编都不生效，这个坑很容易踩。

节点与话题：

1
2
3
4
5
6
7
8
9
10
11
12

roscore                     # 启动 ROS master
rosrun <pkg> <node>         # 运行单个节点
roslaunch <pkg> <file>      # 启动一组节点

rosnode list                # 活动节点
rosnode info <node>         # 节点信息
rosnode ping <node>         # 测试节点连通性

rostopic list               # 所有活跃话题
rostopic echo <topic>       # 打印话题消息
rostopic pub <topic> ...    # 发布消息到话题
rostopic hz <topic>         # 话题发布频率

rostopic hz 是排查传感器问题的第一站 —— 雷达或相机掉帧、IMU 频率不稳，都能在这里最快发现。

服务、消息与参数：

1
2
3
4
5
6
7
8
9
10
11
12

rosservice list             # 可用服务
rosservice call <srv> ...   # 调用服务
rosservice type <srv>       # 服务类型
rosservice args <srv>       # 服务参数

rosmsg list                 # 消息类型列表
rosmsg show <type>          # 消息结构
rossrv show <type>          # 服务结构

rosparam list               # 参数服务器上的所有参数
rosparam get <param>        # 获取参数值
rosparam set <param> <val>  # 设置参数值

命令行调用带复杂结构的服务时，YAML 要按层级写：

1
2
3
4
5
6

rosservice call /my_service "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
flag: 1
id: 47"

八、rosbag 录制、过滤与回放

录制指定话题：

1

rosbag record -O output.bag /scan /imu/data_raw /odom /tf /tf_static

过滤是我用得最多的一条。原始包动辄几个 G，回放和分析都慢，先按话题裁一遍：

1
2
3

rosbag filter input.bag output_filtered.bag \
  "topic == '/imu/data_raw' or topic == '/scan' or topic == '/odom' \
   or topic == '/tf_static'"

过滤 SLAM 数据时有个实用技巧 —— 排除算法自己发布的 tf，只保留传感器原始数据，这样重新跑建图时不会被上一次的结果干扰：

1
2
3
4

rosbag filter input.bag output_filtered.bag \
  "topic == '/imu/data_raw' or topic == '/scan' or topic == '/odom' \
   or topic == '/tf_static' \
   or (topic == '/tf' and not m.transforms[0].header.frame_id in ['carto_map', 'map'])"

m 是当前消息对象，可以直接访问其字段，所以过滤条件能写得很细。

回放与查看：

1
2

rqt_bag output.bag                  # 图形化浏览包内容
rosrun plotjuggler plotjuggler      # 曲线分析，拖拽话题即可绘图

九、可视化与 TF 调试

1
2
3
4
5
6
7

rqt                                 # 插件容器，下面几个也可从中打开
rqt_graph                           # 节点与话题的连接关系
rqt_console                         # 节点日志的显示与过滤
rqt_plot                            # 话题数据实时曲线

rosrun tf view_frames               # 生成 TF 树 PDF
rosrun rqt_tf_tree rqt_tf_tree      # 交互式查看 TF 树

坐标系报错时先 rqt_tf_tree，能一眼看出树是断的还是有多个根。rqt_graph 则用来确认节点是不是真的连上了 —— 话题名拼错导致"发了但没人收"，在图里非常明显。

十、CUDA 与显卡环境核查

装完驱动和 CUDA 后的验证顺序：

1
2
3
4
5
6

lspci | grep -i nvidia          # 确认识别到显卡
nvidia-smi                      # 驱动版本与显存占用
nvcc -V                         # CUDA 版本

# cuDNN 版本（8.x 起版本号移到了 cudnn_version.h）
cat /usr/local/cuda/include/cudnn_version.h | grep CUDNN_MAJOR -A 2

三者版本必须匹配：驱动版本要不低于 CUDA 要求的最低版本，cuDNN 要对应 CUDA 大版本。nvidia-smi 显示的 CUDA 版本是驱动支持的最高版本，不是实际安装的版本 —— 实际版本以 nvcc -V 为准，这两个不一致是正常的。

十一、相机标定

1
2
3
4
5
6
7

sudo apt install ros-noetic-camera-calibration       # 安装标定包

roslaunch <camera_pkg> <camera>.launch               # 启动相机驱动

rosrun camera_calibration cameracalibrator.py \
  --size 8x6 --square 0.024 \
  image:=/camera/color/image_raw camera:=/camera

--size 是棋盘格内角点数量（不是方格数），--square 是单个方格边长，单位米。这两个参数填错标定结果会整体偏掉，且不容易看出来。

一场关于大模型优化与智能体应用的技术讲座整理。内容包括 DeepSeek 的五个核心部件拆解、智能体与人工智能三大主义的关系、多智能体协作架构，以及问答环节中关于端侧算力、垂域微调策略和自动化科研的讨论。

以下是听讲后按自己的理解重新组织的笔记，不是讲稿原文。为便于阅读做了大量压缩和重排，讲者的个别口语化举例予以保留。

一、AI 是怎么"学以致用"的

讲者用一个通俗的四步框架描述机器学习的过程，作为后面技术内容的铺垫。

第一步，数字化感知。 人眼看到猫就知道是猫，AI 看到的则是一串数字，每个数字代表亮度、色彩、纹理、边缘特征，本质是 0/1 串的组合。这些数字难以用结构化数据描述。

第二步，抽丝剥茧。 神经网络第一层可能只看到边缘特征，仅凭这个无法区分猫和猫头鹰；第二层能看出耳朵、鼻子、毛发等部位特征；经过不同层次的抽象，最终得出结论。这些属性就是 features。

第三步，熟能生巧。 讲者这里的说法比较直白：识别过程本质上是"乱猜"。第一次问是不是猫，答错了给它反馈；第二次再问，可能还是错；反复迭代千次万次，直到猜对。迭代过程中学到的东西就是模型里的权重——开源大模型最终开源的也正是这些神经网络权重。AI 并非单纯计算，而是基于概率不断逼近正确预测。

第四步，举一反三。 训练过猫、狗、狐狸之后，模型能够泛化；再进一步可以做生成式输出，比如根据"宇宙空间中看书的一只狗"生成对应图片。

大模型"大"在哪里

到目前为止大模型没有严格定义，"大"指的是海量的参数量，也就是学习反馈过程中存储的权重。其优势可归为三点：泛化理解能力（任务间甚至模态间迁移）、复杂推理和生成能力（一个模型处理多个问题）、调用外部工具和自主拓展的能力——最后一点与智能体强相关。

预训练加微调的两阶段范式，好处同样是三点：预训练用海量数据和算力换取对通用知识的理解；通用能力强则迁移起点高，有些任务甚至无需额外训练；前期投入摊薄之后，适配单个任务所需资源极少且可复用。

二、当前大模型研究的四个前沿方向

这部分是我认为整场讲座信息密度最高的地方。

第一，长推理。 决定大模型能力上限的指标，目标是增强复杂推理和问题求解能力。

第二，基于可验证奖励的强化学习。 大模型判断好坏必须经得起推敲——过程可见、结果可解释。

第三，测试时计算（test-time compute）。 不改变模型本身，而是在下游任务中更好地适配，核心是用计算预算换取更强能力。讲者的类比是：不再考前复习，而是做完题后用不同方法反复验证答案。没有额外训练开销，成本可控。

第四，智能体框架（agent harness）。 让模型只负责推理，其余交给 harness——循环调度、上下文的推测与预测、容错机制。讲者认为这是目前国产大模型核心缺少的东西，也是很多国产模型在结合智能体时难以公开推广的原因。

这第四点值得单独记一笔。它把"模型能力"和"系统工程能力"明确切开了，而后者恰恰是从 demo 走到可交付产品之间的主要差距。

三、DeepSeek 的技术拆解

训练流程

以 V3 为基础，经过大规模强化学习得到 R1-Zero。这里的强化学习方案关注两点奖励：准确度，以及输出格式是否规范。

flowchart TD    A["V3 基座"] --> B["大规模强化学习<br/>奖励: 准确度 + 格式规范"]    B --> C["R1-Zero"]    C --> D["能力涌现 + 冷启动数据<br/>提高可读性"]    D --> E["checkpoint 1"]    E --> F["GRPO 强化<br/>准确性 + 语言一致性"]    F --> G["checkpoint 2"]    G --> H["高质量数据集采样<br/>场景微调"]    H --> I["checkpoint 3"]    I --> J["全场景强化学习"]    J --> K["满血版"]    K --> L["蒸馏出 1.5B / 8B / 14B / 70B<br/>及多模态模型"]

中间的"能力涌现"讲者称为 aha moment，即知识涌现时的顿悟过程，可进一步提升推理能力。

部件一：GRPO（群体相对策略优化）

与传统 PPO 相比，GRPO 删掉了价值评估模型（critic），计算量和通讯量因此节省约三分之一。代价是需要额外开销做同一轮次的评估。两个关键作用：

1. 无 critic 的优化，显著降低算力消耗；
2. 群体相对评估：在一次响应的采样组里，超过平均值给奖励，低于平均值给惩罚，保证每一批生成内容质量都较高。

规则奖励长什么样

讲者用一道小学题说明规则奖励的具体形态：「小明有五个苹果，又买了三袋，每袋四个，一共多少个？」

第一阶段（R1-Zero 上做准确性和格式约束）：答案要准确、步骤要准确；输出必须分步而不能直接给结果，还要用 LaTeX 标准化排版并加粗最终结果。类比是考试大题只写答案不给分——因为可能是抄的。满足格式强约束的生成内容给高奖励。

第二阶段（得到 checkpoint 2）：在准确的基础上，还要用自然语言连贯地解释计算过程，像人与人之间的自然交流。

惩罚的情况有两类：一是结果正确但没按步骤、没用标准化公式；二是语言不一致导致无关假设，比如盲目猜想"袋子是否在回家路上漏了"——这就是幻觉。

关于幻觉的成因，讲者的解释我觉得很有意思：训练数据集中可能混有脑筋急转弯类内容（“树上十只鸟，打掉一只还剩几只”——“都吓跑了”）。解数学题的思维与脑筋急转弯的思维不同，但数据里两者并存，模型因此可能无端假设。幻觉在相当程度上是数据本身造成的。

训练成本为什么能压下来

因为每一轮回答都附带思考，走一步看一步，而不是走到最后才反馈，这保证了每轮迭代都呈向上趋势，训练过程不断接近收敛。

讲者提到一个背景：DeepSeek 公布算力开销时，外界曾质疑其只公布了单次训练成本不合理，因为对比方的训练过程是十次、百次，每次耗资上千万美金。而 DeepSeek 未做反驳，是因为附加思考的过程可以保证一次收敛。

对比 GPU 小时数，讲者给出的量级是对比方的二十五分之一到十分之一。

部件二：细粒度混合专家（MoE）

DeepSeek 的做法是把专家细粒度划分并引入共享专家隔离：

1. 细粒度专家划分：参数总量不变的前提下从更多专家中选择激活。讲者的例子很直观——传统请 16 组专家评判，细粒度划分后拆成 64 组、从中选 8 组，仍然只用了四分之一的专家库，但组合可能性大大提高，成本不变。
2. 共享专家机制：不同任务下由共享专家判断哪些专家参与本轮任务。
3. 无辅助损失的负载均衡：根据任务复杂程度动态选择模型。

部件三：多头潜在注意力（MLA）

通过低秩联合压缩减少计算消耗：把 KV 值投射到更低维空间，需要时再恢复。此外在旋转位置编码上做了引入，把位置信息融入高效推理过程。

部件四：多 token 预测（MTP）

训练推理时不只看一个 token 而是多个，保证更长的注意力。三点创新：可预测后续多个 token，为每个输入位置产生多个监督信号；缓解误差累积；提高推理速度。

顺带一提讲者关于 token 译名的说法：从去年底到今年初它被统一译为「词元」——美国用美元、香港用港元，数字经济时代衡量流通价值的单位就叫词元。

部件五：FP8 混合精度训练

用 FP8 代替传统的 16 位和 32 位浮点数，节省约 75% 的计算开销。代价是需要人工调整和反馈，在推理任务层面额外调参，在计算开销和精度之间找平衡。配套还有双管道机制，在前向、后向过程中内置重叠的计算和通信模块。

可以看出，这几个部件的着力点高度一致：都在压计算消耗。

关于蒸馏

基于满血版按场景蒸馏出 1.5B、8B、14B、70B 等模型。之所以能蒸馏，是因为特定任务下不需要额外的通用能力——做医学模型就不必保留体育知识的相关参数。

讲者的比喻：原始模型是从大海里钓鱼，蒸馏后的模型是从鱼护里钓鱼——先从海量数据挖掘知识，再从有用知识里凝练知识。他也提到，不排除学生网络比老师网络更强的情况。

四、智能体与三大主义

讲者在开场埋了一个问题：为什么时至今日还要谈人工智能发展之初的符号主义、连接主义、行为主义？答案要结合智能体的演进来看。

结论是三大主义共同催生了当代智能体：连接主义作基座（神经网络构建技术底座）、符号主义作校验约束（规则、领域知识、专家经验）、行为主义作交互强化。没有哪个更好或更快，它们在不同层面共同推进。

由此引出一个反直觉的问题：大模型出现得早，还是智能体出现得早？答案是智能体更早。智能体的每个阶段都需要一个"AI 大脑"，这个大脑不断演进，时至今日大模型可以担任这个角色。大模型只是智能体的一个部分。

五、智能体的工作原理

从任务起点到输出，智能体要做四步：

1. 理解目标——任务是什么，有哪些约束，评判标准是什么；
2. 目标规划——任务拆解、步骤排序、动态重规划；
3. 工具调用——搜索、代码生成、数据库、办公软件，甚至其他智能体；
4. 反馈评价——结果检查、信息补充；执行失败则重走上述流程分析原因，最终输出可验证的结果。

"可验证"这一环由谁把握？讲者的回答是人。他对"AI 取代人"的表述做了一个修正，我觉得是全场最值得记下来的一句：

会使用 AI 的人，取代不会使用 AI 的人所承担的工作。

以医疗智能体为例，它看病好不好，不能让 AI 从业者评判，也不能用静态标准评判，而需要医学专家做动态评判。这就是 AI+X 与 X+AI 的关系：X 在最后一步把关。

系统支撑层面的四项工作

1. 知识库：RAG、传统数据库、实时信息获取；
2. 记忆库：长程记忆、长短时记忆、上下文记忆；
3. 工具调用：搜索、代码执行、可调用 API，决定了任务规划执行的好坏；
4. 安全与权限：权限控制、关键阶段的人工确认、全过程日志留存。

多智能体协作

结构上分输入端、输出端和中间的协作中枢。中枢由主管智能体负责规划、目标理解、任务拆分分发，以及结果收集汇总。典型分工包括检索智能体（数据采集整理、确认来源）、数据智能体（清洗计算分析）、代码智能体（代码生成和目标实现）、审查智能体（发现漏洞、关键阶段检查）。

最终输出需要是可核验、可验证、可约束的报告，形成集中调度 → 决策分工 → 并行执行 → 评审优化 → 闭环执行的工作流。

治理

可交付不等于可放心用，还需要治理：任务结果的正确性与时效性、质量评估的成功率与成本、工具调用的合规性、执行日志可追溯、关键阶段的权限划分和人工可接入。

六、关于科研智能体

讲者介绍了自己实验室部署的科研智能体，流程大致是：读取多个关键网站的最新论文并总结 → 梳理每篇与既往相似论文的关系（通过参考文献和被引用关系找线索）→ 调用服务器验证论文结果是否可复现 → 提出该领域接下来可能的研究方向及具体创新方案 →在人工确认后运行这些方案，看效果是否优于被复现的论文。

他强调每一步之后都要追问"这够了吗"，逐层加深，而不是停在第一步的总结报告上。

同时也给了一个冷静的判断：正因为智能体太好用，学术圈才频繁出现"论文工厂"现象。一般科研从业者在发掘和思考上很难与之相比，因为它见多识广，读文献的速度和分析能力可能强过大多数人。

对应的要求是科研评价也在加码：不仅查重复率、查创新性，还要查 AI 率。

七、问答环节的几个要点

问答部分的信息密度不低于正式内容，摘录四个我认为有价值的。

端侧算力与智能体能力的矛盾

问：机器人等端侧设备算力只能支持 30B 以下模型，但要达到较好的智能化水平和工具链能力需要 100B 以上，这个矛盾怎么解？

答：方向是大小模型协同。边缘端算力受限，但边缘端任务通常也不通用，是很专的处理——那就聚焦场景、聚焦任务本身，蒸馏出专用模型，不需要额外的通用泛化能力。但判断它做得好不好、效率高不高，仍然需要一个大脑来控制和总体协调，这时又需要大模型。于是形成大模型作大脑负责调度、分发、收集，小模型聚焦边缘端处理具体任务，反馈结果由大脑统一调度交互的架构。

补充一点：这里的"小模型"不应狭隘理解为传统小模型，它也可以是从基础大模型蒸馏出来的。二者如何协同以达到场景要求，是需要解决的工程化问题。

垂域优化：多任务联合训练还是每任务一个 adapter

问：垂域场景下从基座模型用场景数据调优，希望模型能处理多个任务，是多任务数据联合训练一个模型好，还是每个任务独立训练一个 adapter 好？

答：视情况而定，判断依据是任务之间的差异度。医学模型迁移到教育领域，有共性但差异也大；历史学模型迁移到体育领域，差异会更大。细分任务之间的差异决定了该多任务同时训练还是单任务分别训练。

第二个考虑是任务之间是否存在阶段关联。工程化应用中不同任务所处阶段往往有关联关系，训练数据所隐含的知识在不同任务下呈现的内容也不一样。这时需要结合实际场景的专家经验——比如把阶段 A 的历史积累经验融入模型训练。做垂域训练时要考虑的不只是本地化数据，还包括多任务场景下每个任务的专业知识如何融入。

什么时候该停止工程优化、开始训练模型

问：端侧模型在大部分情况下表现很好，但工具调用能力不足或上下文过长时开始胡说八道。如何判断当前瓶颈应该继续做工程化优化（上下文、知识库），还是必须开始训练专用模型？

答：分三个层面看。第一，如果算力有限而任务复杂，那一定需要额外注入专业知识来提升性能，这时工程手段已经不够。第二，如果不是离线场景，可以把结果或数据反馈到大脑层面做通讯，此时要优化的是边缘端与云端的任务分配，以及传输和交互内容本身。第三，算力不够就用算法来凑，边缘端智能是一个方向——比如路上的监测摄像头未来可直接做决策，即便算力有限也要具备执行监测任务的能力，这样通讯和及时反馈的成本会很低。

讲者也提到一个相关概念：遥感和部分特殊领域正在提的"天数天算"，即把航天采集的数据直接在轨计算，算完再反馈给地面，形成天空地协同。

学生该如何看待自动化科研 Agent

这个问题来自一位在读硕士：GitHub 上已有很多自动化科研 agent，从检索文献、阅读文献到自己找创新点、做实验、分析结果全流程覆盖。几乎不用自己想创新点和分析结果了，那如何平衡能力提升与工具使用？

讲者的回答分三层：

第一，回到科研是什么。 科研问题来自哪里？一定是从应用问题中找到问题，把它科学化、广泛化，进而解决。 用 agent 辅助科研，不等于完全代替人做科研。

第二，科研的目标不只是发论文。 他转述了一个相当尖锐的追问：如果把你发表的论文全部下架，对你所在的研究领域会不会造成影响？如果还有点影响，说明这个问题是有意义的；如果没有影响，那科研目标就不是为了发论文——否则没有意义。

第三，未来分两类人：让 AI 变得更好的人，和把 AI 用得更好的人。 一个是研究，一个是用。无论哪类，都要能回答"你这项工作具体能解决什么、能改变什么、意义在哪里"。带着任务和问题去使用 AI 作为辅助科研工具，才更有价值。

关于"导师不让用 AI"，讲者在正文里也给过一个类比：可以问老师画图是否用电脑，绘图时还要不要像古人一样磨墨。计算机是工具，未来 AI 也是工具，人手多个智能体是方向。要用好它，也不要被它完全替代，要保留独立思考的能力。

八、面向未来的三个问题

第一，计算资源与成本。 这是训练和推理中不可回避的核心问题，未来更侧重降低算力消耗——更短的训练周期、更省的数据量。回看前面的技术拆解，几乎每个部件都在回答这个问题。

第二，数据质量与安全风险。 "数据投毒"这个词出现得越来越多。如果 AI 工具老是给出你不想看到的内容，就要考虑推荐内容是否经过投毒，最简单的形式就是投广告。

讲者举了一个我觉得很具体的例子：医生反映现在看病更难了，因为患者看病前先问 AI，问完之后说"你说的不对"。医生无法直接否定 AI 总结的内容，但要判断是否对症，需要花更多时间与非专业人士探讨专业知识——沟通成本反而更高了。

第三，可靠性与可解释性。 如果医生告诉患者"这是 AI 给出的诊断"，患者不会相信。可解释性、可溯源始终是绕不开的话题。

结合这三点，讲者给出的方向判断是：性价比优势、更易推广至垂直领域、更精细化与专业化、以开源策略激发创新；同时结合智能体概念，利用强化学习实现智能体的自主进化——终身学习、长期学习、环境交互与主动学习。

九、我的几点记录

几条对自己方向有参考价值的：

agent harness 那一点值得重视。 把模型的推理能力和系统的调度、容错、上下文管理能力切开看，这个视角对做机器人系统的人尤其有用——机器人本来就是典型的"模型

• harness"结构，规划、执行、恢复的循环调度早就存在，只是过去没有用这套语言描述。

大小模型协同的架构判断，和端侧机器人的现实高度吻合。 边缘算力受限但任务专用，这个前提在移动机器人上一直成立。问答里"聚焦场景、聚焦任务，蒸馏专用模型，由大脑统一调度"的结论，基本可以直接套用到导航和感知的分层设计上。

"什么时候该停止工程优化"那个问题问得很好，但答案偏原则。 实际项目里更需要的是可量化的判据。这一块我认为还需要自己在具体任务上摸索。

关于自动化科研 agent 的态度，我认同"回到科研问题本身"的判断。 工具能加速文献消化和方案验证，但"这个问题值不值得做"仍然需要自己回答。

以上整理如有理解偏差，责任在我而非讲者。

Navigation框架在移动机器人中是比较常用的框架，在使用过程中发现框架本身存在了一些问题，包括组件高耦合、单体类职责过多、硬编码状态机、反馈信息贫乏、恢复机制不灵活、接口陈旧等问题，对Navigation框架进行分析和总结。

Navigation (Noetic版本) 整体功能包

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

navigation (metapackage)
├─ amcl                      # 定位
├─ base_local_planner        # 基础局部规划器
├─ carrot_planner            # 简单全局规划器
├─ clear_costmap_recovery    # 恢复行为
├─ costmap_2d                # 代价地图
├─ dwa_local_planner         # DWA 局部规划器
├─ fake_localization         # 假定位
├─ global_planner            # 全局规划器
├─ map_server                # 地图服务器
├─ move_base                 # 核心协调器
├─ move_base_msgs            # 消息定义
├─ move_slow_and_clear       # 恢复行为
├─ navfn                     # Dijkstra 规划器
├─ nav_core                  # 接口定义
├─ rotate_recovery           # 恢复行为
└─ voxel_grid                # 体素网格

一、组件间耦合度高

内部耦合度高，难以替换单个组件。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

高耦合组件：
move_base
├─→ costmap_2d (强耦合)
├─→ nav_core (接口)
├─→ base_local_planner (强制)
├─→ navfn (强制)
├─→ clear_costmap_recovery (强制)
└─→ rotate_recovery (强制)

base_local_planner
├─→ costmap_2d (强耦合)
├─→ nav_core (接口)
└─→ voxel_grid (强耦合)

dwa_local_planner
├─→ base_local_planner (继承耦合)
├─→ costmap_2d (强耦合)
└─→ nav_core (接口)

navfn
├─→ costmap_2d (强耦合)
└─→ nav_core (接口)

global_planner
├─→ costmap_2d (强耦合)
├─→ navfn (依赖)
└─→ nav_core (接口)


move_base 依赖图简化：
        move_base
           ↓
    ┌──────┴──────┐
    ↓             ↓
costmap_2d    nav_core
    ↓             ↓
voxel_grid   planners
                  ↓
              costmap_2d (循环)

二、单体类职责过多

无分层设计，职责过多；成员变量过多，难以理解和维护。

move_base 类，进行状态机管理、线程管理、规划器管理、代价地图管理、恢复行为管理、Action 服务器管理。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

class MoveBase {
private:
  // 状态机
  MoveBaseState state_;  // PLANNING, CONTROLLING, CLEARING
  RecoveryTrigger recovery_trigger_;
  
  // 规划器（直接持有）
  boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
  boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
  
  // 代价地图（直接持有）
  costmap_2d::Costmap2DROS* planner_costmap_ros_;
  costmap_2d::Costmap2DROS* controller_costmap_ros_;
  
  // 恢复行为（直接持有）
  std::vector<boost::shared_ptr<nav_core::RecoveryBehavior>> recovery_behaviors_;
  
  // 线程管理
  boost::thread* planner_thread_;
  boost::recursive_mutex planner_mutex_;
  boost::condition_variable_any planner_cond_;
  
  // 路径缓冲
  std::vector<geometry_msgs::PoseStamped>* planner_plan_;
  std::vector<geometry_msgs::PoseStamped>* latest_plan_;
  std::vector<geometry_msgs::PoseStamped>* controller_plan_;
  
  // ... 50+ 个成员变量
};

**costmap_2d **

costmap_2d 混合了多种职责，除了数据结构、消息定义和TF，还有传感器数据处理还有laser_geometry,、sensor_msgs、message_filters等。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

<depend>dynamic_reconfigure</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>laser_geometry</depend>
<depend>map_msgs</depend>
<depend>message_filters</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>rostest</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<depend>voxel_grid</depend>

三、多线程混乱导致的同步问题

move_base 的线程模型非常复杂且难以控制，规划器需要频繁加锁访问代价地图，代价地图更新时持有锁，阻塞规划器，可能导致规划器使用过时的代价地图数据，频繁的锁竞争，可能导致碰撞。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78

导航流程：Global Costmap -> Planner -> Local Costmap -> Planner -> Command Velocities
一旦全局规划线程（Global Planner）计算出路径，它会立即、自动地将路径交换给局部规划器（Local Planner）所在的控制线程。
    
[主线程 / Control Thread]                      [规划线程 / Planner Thread]
               |                                              |
      1. 收到目标 (executeCb)                                  | 挂起等待 (wait)
               |                                              |
      2. 唤醒规划线程 (notify) ------------------------------>  | 醒来
               |                                              |
      3. 进入控制循环 (executeCycle)                            | 4. 锁定全局地图
         (20Hz)                                               |    运行 makePlan
               |                                              |    
               |                                              |
               |                                              | 5. 规划完成
               |                                              |    锁定 planner_mutex_
               |                                              |    交换指针: planner_plan -> latest_plan
               | <----------(new_global_plan_ = true)-------- |    解锁
               |                                              |    继续挂起或再次规划
               |
      6. 发现新路径标志
         锁定 planner_mutex_
         交换指针: latest_plan -> controller_plan_
         解锁
               |
      7. 锁定局部地图
         运行 computeVelocityCommands
         发布 cmd_vel


// move_base/src/move_base.cpp
void MoveBase::executeCb(...) {
  // 主线程
  ros::Rate r(controller_frequency_);
  
  while(n.ok()) {
    // 访问代价地图（需要加锁）
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(
      *(planner_costmap_ros_->getCostmap()->getMutex())
    );
    
    // 全局规划（可能需要几百毫秒）
    planner_->makePlan(start, goal, global_plan_);
    
    lock.unlock();  // 释放锁
    
    // 局部规划（又需要加锁）
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock2(
      *(controller_costmap_ros_->getCostmap()->getMutex())
    );
    
    controller_->computeVelocityCommands(cmd_vel);
  }
}

// 代价地图更新线程
void Costmap2DROS::mapUpdateLoop(double frequency) {
  ros::Rate r(frequency);  // 例如 5Hz
  
  while (ros::ok() && !stop_updates_) {
    // 获取锁
    boost::unique_lock<boost::recursive_mutex> lock(*(layered_costmap_->getCostmap()->getMutex()));
    
    // 更新代价地图（可能需要几十毫秒）
    layered_costmap_->updateMap(x, y, yaw);
    
    lock.unlock();
    
    r.sleep();
  }
}


//多线程协调有时出现7秒左右的延迟（github仓库提交的问题）
时间轴：
T0: 传感器检测到障碍物
T1: 代价地图更新线程处理数据（可能延迟）
T2: 局部规划器读取代价地图（可能读到旧数据）
T3: 机器人执行速度命令（可能已经撞上障碍物）

四、move_base 硬编码状态机

在标准 move_base 中，导航逻辑被硬编码为一个固定的有限状态机（FSM）：PLANNING -> CONTROLLING -> CLEARING（恢复），限制了机器人的复杂行为。

无法插入自定义逻辑，比如"规划前先旋转"、"失败后等待一段时间"等。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

// move_base/src/move_base.cpp
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal) {
  // 状态机完全硬编码
  ros::Rate r(controller_frequency_);
  
  while(n.ok()) {
    if(as_->isPreemptRequested()) {
      // 处理抢占
      as_->setPreempted();
      return;
    }

    // 固定流程：全局规划 → 局部规划 → 恢复行为
    if(runPlanner_) {
      boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
      
      if(!planner_->makePlan(start, goal, global_plan_)) {
        // 规划失败 → 执行恢复行为
        state_ = CLEARING;
      }
    }

    if(state_ == CONTROLLING) {
      if(!controller_->computeVelocityCommands(cmd_vel)) {
        // 控制失败 → 执行恢复行为
        state_ = CLEARING;
      }
    }

    if(state_ == CLEARING) {
      // 固定的恢复行为序列
      if(recovery_index_ < recovery_behaviors_.size()) {
        recovery_behaviors_[recovery_index_]->runBehavior();
        recovery_index_++;
      } else {
        // 所有恢复行为都失败
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan");
        return;
      }
    }
  }
}

五、move_base 反馈贫乏

move_base 的 Action 接口反馈非常有限，通常只返回当前的位置（base position），无法知道进度、当前状态详情、剩余距离。

1
2
3
4
5
6
7

# move_base_msgs/MoveBase.action

geometry_msgs/PoseStamped target_pose
---
---
geometry_msgs/PoseStamped base_position

六、恢复行为机制不灵活

• 无论何种失败都执行相同序列 → 清除、旋转、清除、旋转
• 无法根据失败类型定制
• 可能执行无效的恢复动作

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

void MoveBase::loadDefaultRecoveryBehaviors() {
  recovery_behaviors_.clear();
  try {
    // 1. 保守清除代价地图
    boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(
      recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
    cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
    recovery_behaviors_.push_back(cons_clear);
    
    // 2. 原地旋转
    boost::shared_ptr<nav_core::RecoveryBehavior> rotate(
      recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
    if(clearing_rotation_allowed_) {
      rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behaviors_.push_back(rotate);
    }
    
    // 3. 激进清除代价地图
    boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(
      recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
    ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
    recovery_behaviors_.push_back(ags_clear);
    
    // 4. 再次原地旋转
    if(clearing_rotation_allowed_)
      recovery_behaviors_.push_back(rotate);
  } catch(...) { ... }
}

七、nav_core 接口设计陈旧

nav_core 近十年无更新，nav_core 定义的接口（makePlan, computeVelocityCommands）返回值通常只是简单的 bool值（成功/失败），返回 false 时，无法知道失败原因，是没有路径？是检测到障碍物？是速度限制？还是算法内部错误？

通过引用参数返回路径，不符合现代 C++ 设计。

1
2
3
4
5
6

// nav_core::BaseGlobalPlanner
bool makePlan(
    const PoseStamped& start,
    const PoseStamped& goal,
    std::vector<PoseStamped>& plan  // 输出参数
);

八、nav_core魔法数字

COST_NEUTRAL：值为 50，但为什么是 50？注释缺失

COST_FACTOR：代价缩放因子，计算逻辑不透明

COST_DIAGONAL：对角线移动代价，硬编码为 sqrt(2)

难以理解的魔法数字，难以阅读和修改，另外在某些情况下会产生不安全的路径。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

// navfn/src/navfn.cpp
bool NavFn::calcNavFnDijkstra(bool atStart) {
  setupNavFn(true);

  int cycle = 0;
  int startCell = getIndex(start[0], start[1]);
  
  for (int i = 0; i < ns; i++) {
    costarr[i] = COST_NEUTRAL + COST_FACTOR * costmap[i];  // 魔法公式
  }

  // 硬编码的代价计算
  float cost = costarr[c] + COST_NEUTRAL;  // 为什么是 COST_NEUTRAL？
  float dist = (abs(dx) > abs(dy)) ? 
    (float)abs(dx) + COST_DIAGONAL * (float)abs(dy) :  // 魔法常数
    (float)abs(dy) + COST_DIAGONAL * (float)abs(dx);
  
  float dnew = pc + cost + dist;  // 组合公式难以理解
}

九、强制使用 Layers

costmap_2d 强制使用“层（Layers）”架构，但有时用户只需要一个简单的网格。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

// costmap_2d/costmap_2d_ros.cpp
class Costmap2DROS {
public:
  Costmap2DROS(std::string name, tf::TransformListener& tf) {
    // 强制创建 LayeredCostmap
    layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window_, track_unknown_space_);
    
    // 必须通过插件系统加载 layers
    if (!loadPlugins()) {
      ROS_ERROR("Failed to load costmap plugins");
    }
  }

private:
  LayeredCostmap* layered_costmap_;  // 不能为空
  std::vector<boost::shared_ptr<Layer>> plugins_;  // 必须有至少一个 layer
};

十、OccupancyGrid 数据丢失问题

256 个唯一值被压缩到 101 个值（-1 到 100），中间的 252 个值（1-252）被映射到 98 个值（1-98），更丰富的信息（如不同的障碍物类型或膨胀层级），但在传输过程中精度被丢失了。

地图数据死板地绑定为 unsigned char 数组，并强依赖于 ROS 消息格式，只能存 0-255 的整数，无法直接存储浮点数（如距离场、概率值）或自定义结构体。

1
2
3
4

原始代价值: [0, 1, 2, ..., 252, 253, 254, 255]
            ↓ 转换
发布值:     [0, 1, 1, ..., 98,  99,  100, -1]
            ↑ 多个原始值映射到同一个发布值

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

// costmap_2d/src/costmap_2d_publisher.cpp
char* Costmap2DPublisher::cost_translation_table_ = NULL;

void Costmap2DPublisher::prepareGrid() {
  // 初始化转换表
  if (cost_translation_table_ == NULL) {
    cost_translation_table_ = new char[256];
    
    // 特殊值映射
    cost_translation_table_[0] = 0;      // FREE_SPACE
    cost_translation_table_[253] = 99;   // INSCRIBED_INFLATED_OBSTACLE
    cost_translation_table_[254] = 100;  // LETHAL_OBSTACLE
    cost_translation_table_[255] = -1;   // NO_INFORMATION
    
    // 中间值线性映射到 [1, 98]
    for (int i = 1; i < 253; i++) {
      cost_translation_table_[i] = char(1 + (97 * (i - 1)) / 251);
    }
  }

  // 转换代价地图数据
  unsigned int size = costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY();
  grid_.data.resize(size);
  unsigned char* data = costmap_->getCharMap();
  for (unsigned int i = 0; i < size; i++) {
    grid_.data[i] = cost_translation_table_[data[i]];
  }
}

十一、6DOF 位姿冗余

强制使用 3D 数据结构，标准导航栈强制使用 geometry_msgs/PoseStamped，即使是 2D 地面机器人也必须携带 Z 轴坐标和四元数（Quaternion）。

简单的角度计算需要反复进行“欧拉角 <-> 四元数”转换，不仅繁琐而且消耗算力。（在 10Hz 控制频率下，1000 次姿态操作累计需 23 毫秒）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

// geometry_msgs/PoseStamped
struct Pose {
    Point position;    // x, y, z  <- z 冗余
    Quaternion orientation;  // x, y, z, w  <- 只需要 yaw
};

// 即使是 2D 导航也用 3D 数据
geometry_msgs::PoseStamped goal;
goal.pose.position.x = 5.0;
goal.pose.position.y = 3.0;
goal.pose.position.z = 0.0;  // 永远是 0，但必须存在
goal.pose.orientation = tf::createQuaternionMsgFromYaw(1.57);  // 四元数转换开销

// 额外计算：每次访问都需要四元数转换
double yaw = tf::getYaw(pose.pose.orientation); 

十二、局部规划器的调试困难

参数较多，30+ 个参数，很难调整其评分标准（Critics）或理解其为何选择某条轨迹，调参困难。

1
2
3
4
5
6
7
8
9

base_local_planner 参数示例：
├─ max_vel_x, min_vel_x
├─ max_vel_theta, min_vel_theta
├─ acc_lim_x, acc_lim_theta
├─ xy_goal_tolerance, yaw_goal_tolerance
├─ sim_time, sim_granularity
├─ path_distance_bias, goal_distance_bias
├─ occdist_scale
└─ ... 

参考资料

http://wiki.ros.org/navigation

https://github.com/ros-planning/navigation

基于45°布局的四轮全向轮（Omni Wheel）底盘运动学建模与ROS实现。

一、底盘结构

全向轮的辐条上分布着一圈与轮轴成一定角度的小滚子，正是这些滚子让轮子在被动方向上可以自由滑动，从而实现全向运动。

1.1 轮子布局

轮子安装方向（以x轴为起点，逆时针为正）：

• 轮1（左前）：45°
• 轮2（左后）：135°
• 轮3（右后）：-135°（225°）
• 轮4（右前）：-45°（315°）

1
2
3
4
5
6
7
8
9

     前进x轴方向 ↑ 
     
  轮1 ↗      ↖ 轮4
     ┌────────┐
     │        │
←────┤   ●    │ y轴
     │        │
     └────────┘
  轮2 ↘      ↙ 轮3

1.2 关键参数

二、运动学模型

2.1 逆向运动学（速度 → 轮速）

输入：底盘目标速度 $(v_x, v_y, \omega_z)$
输出：四个轮子的线速度 $(V_1, V_2, V_3, V_4)$

矩阵形式：

$$\begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix} = \frac{1}{\sqrt{2}}\begin{bmatrix} 1 & 1 & \sqrt{2}L \\ -1 & 1 & \sqrt{2}L \\ -1 & -1 & \sqrt{2}L \\ 1 & -1 & \sqrt{2}L \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega_z \end{bmatrix}$$

展开形式：

$$\begin{cases}V_1 = \frac{1}{\sqrt{2}}(v_x + v_y) + L\omega_z \\V_2 = \frac{1}{\sqrt{2}}(-v_x + v_y) + L\omega_z \\V_3 = \frac{1}{\sqrt{2}}(-v_x - v_y) + L\omega_z \\V_4 = \frac{1}{\sqrt{2}}(v_x - v_y) + L\omega_z\end{cases}$$

2.2 前向运动学（轮速 → 速度）

输入：四个轮子的实际速度 $(V_1, V_2, V_3, V_4)$
输出：底盘实际速度 $(v_x, v_y, \omega_z)$

矩阵形式：

$$\begin{bmatrix} v_x \\ v_y \\ \omega_z \end{bmatrix} = \frac{1}{4}\begin{bmatrix} \sqrt{2} & -\sqrt{2} & -\sqrt{2} & \sqrt{2} \\ \sqrt{2} & \sqrt{2} & -\sqrt{2} & -\sqrt{2} \\ \frac{1}{L} & \frac{1}{L} & \frac{1}{L} & \frac{1}{L} \end{bmatrix} \begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix}$$

展开形式：

$$\begin{cases}v_x = \frac{1}{4\sqrt{2}}(V_1 - V_2 - V_3 + V_4) \\v_y = \frac{1}{4\sqrt{2}}(V_1 + V_2 - V_3 - V_4) \\\omega_z = \frac{1}{4L}(V_1 + V_2 + V_3 + V_4)\end{cases}$$

2.3 轮速与电机转速转换

轮速转电机转速：

$$\omega_{motor,i} = \frac{V_i \cdot N}{R}$$

电机转速转轮速：

$$V_i = \frac{\omega_{motor,i} \cdot R}{N}$$

从编码器增量计算轮速：

$$V_i = \frac{2\pi R \cdot \Delta encoder_i}{PPR \cdot N \cdot \Delta t}$$

其中：

• $R$：轮子半径
• $N$：减速比
• $PPR$：编码器每转脉冲数
• $\Delta t$：采样周期（秒）

三、典型运动模式

3.1 前进运动

目标：$v_x = v$，$v_y = 0$，$\omega_z = 0$

$$\begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix} = \frac{v}{\sqrt{2}} \begin{bmatrix} 1 \\ -1 \\ -1 \\ 1 \end{bmatrix}$$

3.2 侧向平移

目标：$v_x = 0$，$v_y = v$，$\omega_z = 0$

$$\begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix} = \frac{v}{\sqrt{2}} \begin{bmatrix} 1 \\ 1 \\ -1 \\ -1 \end{bmatrix}$$

3.3 原地旋转

目标：$v_x = 0$，$v_y = 0$，$\omega_z = \omega$

$$\begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix} = L\omega \begin{bmatrix} 1 \\ 1 \\ 1 \\ 1 \end{bmatrix}$$

四、速度约束

4.1 单轮速度限制

最大轮速：$V_{max}$

最大前进速度：

$$v_{x,max} = \sqrt{2} \cdot V_{max}$$

最大旋转角速度：

$$\omega_{z,max} = \frac{V_{max}}{L}$$

4.2 速度缩放

当计算出的轮速超限时：

$$k = \frac{V_{max}}{\max(|V_1|, |V_2|, |V_3|, |V_4|)}$$

$$\begin{bmatrix} v_x' \\ v_y' \\ \omega_z' \end{bmatrix} = k \begin{bmatrix} v_x \\ v_y \\ \omega_z \end{bmatrix}$$

五、C++代码实现

5.1 运动学类实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

#include <cmath>
#include <algorithm>

struct ChassisVelocity {
    double vx;  // m/s
    double vy;  // m/s
    double wz;  // rad/s
};

struct WheelSpeeds {
    double v1, v2, v3, v4;  // m/s
};

struct Pose {
    double x;      // m
    double y;      // m
    double theta;  // rad
};

class OmniKinematics {
private:
    const double L;              // 轮心到中心距离 (m)
    const double R;              // 轮子半径 (m)
    const double N;              // 减速比
    const double V_MAX;          // 最大轮速 (m/s)
    const double SQRT2_INV;      // 1/√2
    const double RAD_TO_RPM;     // 60/(2π)

public:
    OmniKinematics(double wheel_base, double wheel_radius, 
                   double gear_ratio, double max_speed)
        : L(wheel_base), R(wheel_radius), N(gear_ratio), 
          V_MAX(max_speed), 
          SQRT2_INV(1.0 / std::sqrt(2.0)),
          RAD_TO_RPM(60.0 / (2.0 * M_PI)) {}
    
    // 逆向运动学：底盘速度 → 轮速
    WheelSpeeds inverseKinematics(const ChassisVelocity& cmd, bool limit = true) {
        WheelSpeeds wheels;
        
        wheels.v1 = SQRT2_INV * ( cmd.vx + cmd.vy) + L * cmd.wz;
        wheels.v2 = SQRT2_INV * (-cmd.vx + cmd.vy) + L * cmd.wz;
        wheels.v3 = SQRT2_INV * (-cmd.vx - cmd.vy) + L * cmd.wz;
        wheels.v4 = SQRT2_INV * ( cmd.vx - cmd.vy) + L * cmd.wz;
        
        // 速度限幅
        if (limit) {
            double max_speed = std::max({std::abs(wheels.v1), std::abs(wheels.v2),
                                          std::abs(wheels.v3), std::abs(wheels.v4)});
            if (max_speed > V_MAX) {
                double scale = V_MAX / max_speed;
                wheels.v1 *= scale;
                wheels.v2 *= scale;
                wheels.v3 *= scale;
                wheels.v4 *= scale;
            }
        }
        
        return wheels;
    }
    
    // 前向运动学：轮速 → 底盘速度
    ChassisVelocity forwardKinematics(const WheelSpeeds& wheels) {
        ChassisVelocity vel;
        
        double coeff = SQRT2_INV / 4.0;
        
        vel.vx = coeff * ( wheels.v1 - wheels.v2 - wheels.v3 + wheels.v4);
        vel.vy = coeff * ( wheels.v1 + wheels.v2 - wheels.v3 - wheels.v4);
        vel.wz = (wheels.v1 + wheels.v2 + wheels.v3 + wheels.v4) / (4.0 * L);
        
        return vel;
    }
    
    // 轮速 ↔ 电机转速 (rad/s)
    void wheelToMotorSpeed(const WheelSpeeds& wheels, double motor_speeds[4]) {
        motor_speeds[0] = wheels.v1 * N / R;
        motor_speeds[1] = wheels.v2 * N / R;
        motor_speeds[2] = wheels.v3 * N / R;
        motor_speeds[3] = wheels.v4 * N / R;
    }
    
    WheelSpeeds motorToWheelSpeed(const double motor_speeds[4]) {
        WheelSpeeds wheels;
        wheels.v1 = motor_speeds[0] * R / N;
        wheels.v2 = motor_speeds[1] * R / N;
        wheels.v3 = motor_speeds[2] * R / N;
        wheels.v4 = motor_speeds[3] * R / N;
        return wheels;
    }
    
    // 轮速 ↔ 电机转速 (RPM)
    void wheelToMotorRPM(const WheelSpeeds& wheels, double motor_rpm[4]) {
        motor_rpm[0] = wheels.v1 * N * RAD_TO_RPM / R;
        motor_rpm[1] = wheels.v2 * N * RAD_TO_RPM / R;
        motor_rpm[2] = wheels.v3 * N * RAD_TO_RPM / R;
        motor_rpm[3] = wheels.v4 * N * RAD_TO_RPM / R;
    }
    
    WheelSpeeds motorRPMToWheel(const double motor_rpm[4]) {
        WheelSpeeds wheels;
        wheels.v1 = motor_rpm[0] * R / (N * RAD_TO_RPM);
        wheels.v2 = motor_rpm[1] * R / (N * RAD_TO_RPM);
        wheels.v3 = motor_rpm[2] * R / (N * RAD_TO_RPM);
        wheels.v4 = motor_rpm[3] * R / (N * RAD_TO_RPM);
        return wheels;
    }
    
    // 编码器增量 → 轮速
    WheelSpeeds encoderToWheelSpeed(const int encoder_delta[4], double dt, int ppr) {
        WheelSpeeds wheels;
        double coeff = (2.0 * M_PI * R) / (ppr * N * dt);
        
        wheels.v1 = encoder_delta[0] * coeff;
        wheels.v2 = encoder_delta[1] * coeff;
        wheels.v3 = encoder_delta[2] * coeff;
        wheels.v4 = encoder_delta[3] * coeff;
        
        return wheels;
    }
};

5.2 里程计类实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

class Odometry {
private:
    Pose pose;
    OmniKinematics* kinematics;
    
    // 角度归一化到 [-π, π]
    void normalizeAngle(double& angle) {
        while (angle > M_PI) angle -= 2.0 * M_PI;
        while (angle < -M_PI) angle += 2.0 * M_PI;
    }
    
public:
    Odometry(OmniKinematics* kin) 
        : pose{0.0, 0.0, 0.0}, kinematics(kin) {}
    
    // 更新位姿（从编码器）
    void update(const int encoder_delta[4], double dt, int ppr) {
        // 编码器 → 轮速 → 底盘速度
        WheelSpeeds wheels = kinematics->encoderToWheelSpeed(encoder_delta, dt, ppr);
        ChassisVelocity vel = kinematics->forwardKinematics(wheels);
        
        updatePose(vel, dt);
    }
    
    // 更新位姿（从底盘速度）
    void updatePose(const ChassisVelocity& vel, double dt) {
        // 转换到全局坐标系
        double cos_theta = std::cos(pose.theta);
        double sin_theta = std::sin(pose.theta);
        
        double vx_global = vel.vx * cos_theta - vel.vy * sin_theta;
        double vy_global = vel.vx * sin_theta + vel.vy * cos_theta;
        
        // 位姿积分
        pose.x += vx_global * dt;
        pose.y += vy_global * dt;
        pose.theta += vel.wz * dt;
        
        normalizeAngle(pose.theta);
    }
    
    Pose getPose() const { return pose; }
    
    void resetPose(double x = 0.0, double y = 0.0, double theta = 0.0) {
        pose.x = x;
        pose.y = y;
        pose.theta = theta;
    }
};

5.3 使用示例

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

#include <cstdio>

int main() {
    // 初始化参数
    double L = 0.3575;      // 轮心到中心距离 (m)
    double R = 0.0855;      // 轮子半径 (m)
    double N = 19.0;        // 减速比
    double V_max = 1.8;     // 最大轮速 (m/s)
    int PPR = 1024;         // 编码器分辨率
    
    OmniKinematics kinematics(L, R, N, V_max);
    Odometry odom(&kinematics);
    
    printf("=== 逆向运动学示例 ===\n");
    ChassisVelocity cmd = {0.5, 0.3, 0.2};  // vx, vy, wz
    
    WheelSpeeds wheels = kinematics.inverseKinematics(cmd);
    printf("底盘速度: vx=%.2f vy=%.2f wz=%.2f\n", cmd.vx, cmd.vy, cmd.wz);
    printf("轮速: V1=%.3f V2=%.3f V3=%.3f V4=%.3f m/s\n\n",
           wheels.v1, wheels.v2, wheels.v3, wheels.v4);
    
    double motor_rpm[4];
    kinematics.wheelToMotorRPM(wheels, motor_rpm);
    printf("电机转速: M1=%.1f M2=%.1f M3=%.1f M4=%.1f RPM\n\n",
           motor_rpm[0], motor_rpm[1], motor_rpm[2], motor_rpm[3]);
    
    printf("=== 前向运动学示例 ===\n");
    int encoder_delta[4] = {100, 120, 110, 90};
    double dt = 0.02;  // 20ms
    
    odom.update(encoder_delta, dt, PPR);
    Pose pose = odom.getPose();
    printf("编码器增量: E1=%d E2=%d E3=%d E4=%d\n",
           encoder_delta[0], encoder_delta[1], encoder_delta[2], encoder_delta[3]);
    printf("当前位姿: x=%.3f y=%.3f theta=%.2f°\n\n",
           pose.x, pose.y, pose.theta * 180.0 / M_PI);
    
    printf("=== 运动模式测试 ===\n");
    // 前进
    ChassisVelocity forward = {1.0, 0.0, 0.0};
    wheels = kinematics.inverseKinematics(forward);
    printf("前进: V1=%.3f V2=%.3f V3=%.3f V4=%.3f\n",
           wheels.v1, wheels.v2, wheels.v3, wheels.v4);
    
    // 侧移
    ChassisVelocity lateral = {0.0, 1.0, 0.0};
    wheels = kinematics.inverseKinematics(lateral);
    printf("侧移: V1=%.3f V2=%.3f V3=%.3f V4=%.3f\n",
           wheels.v1, wheels.v2, wheels.v3, wheels.v4);
    
    // 旋转
    ChassisVelocity rotation = {0.0, 0.0, 1.0};
    wheels = kinematics.inverseKinematics(rotation);
    printf("旋转: V1=%.3f V2=%.3f V3=%.3f V4=%.3f\n",
           wheels.v1, wheels.v2, wheels.v3, wheels.v4);
    
    return 0;
}

六、总结

6.1 核心公式

逆向运动学：

$$\begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix} = \frac{1}{\sqrt{2}}\begin{bmatrix} 1 & 1 & \sqrt{2}L \\ -1 & 1 & \sqrt{2}L \\ -1 & -1 & \sqrt{2}L \\ 1 & -1 & \sqrt{2}L \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega_z \end{bmatrix}$$

前向运动学：

$$\begin{bmatrix} v_x \\ v_y \\ \omega_z \end{bmatrix} = \frac{1}{4}\begin{bmatrix} \sqrt{2} & -\sqrt{2} & -\sqrt{2} & \sqrt{2} \\ \sqrt{2} & \sqrt{2} & -\sqrt{2} & -\sqrt{2} \\ \frac{1}{L} & \frac{1}{L} & \frac{1}{L} & \frac{1}{L} \end{bmatrix} \begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix}$$

6.2 性能参数

介绍电机驱动轮子移动的机器人，即轮式机器人的运动学模型，重点分析两轮差速、四轮差速、阿克曼、全向运动4个运动模型的基本原理和数学模型。

一、两轮差速模型（Two-Wheel Differential Drive）

1.1 基本原理

结构特点：

• 两个主动驱动轮分别位于底盘左右两侧
• 前方/后方配有1-2个万向轮作为支撑
• 通过控制左右轮的速度差实现转向

运动特性：

• ✅ 可原地旋转（左右轮反向等速）
• ✅ 可走任意曲线轨迹
• ❌ 不能侧向平移
• ❌ 速度向量约束：$v_y = 0$

物理基础：

• 驱动力来源：地面对轮子的滚动摩擦力
• 转向原理：左右轮线速度差产生角速度

1.2 数学模型

1.2.1 坐标系定义

• 全局坐标系：$\{X, Y\}$
• 机器人坐标系：$\{x, y\}$，x轴指向前进方向
• 状态变量：$\xi = [x, y, \theta]^T$

1.2.2 运动学约束

无侧向滑动约束：

$$\dot{x} \sin\theta - \dot{y} \cos\theta = 0$$

速度分解：

$$\begin{cases}\dot{x} = v_x \cos\theta \\\dot{y} = v_x \sin\theta \\\dot{\theta} = \omega_z\end{cases}$$

1.2.3 前向运动学（Forward Kinematics）

输入： 左右轮线速度 $V_L, V_R$
输出： 底盘整体速度 $v_x, \omega_z$

几何关系推导：

设左右轮在时间 $\Delta t$ 内走过的路程分别为 $s_L, s_R$：

$$\begin{cases}s_R = V_R \cdot \Delta t = \theta \cdot (r + \frac{d}{2}) \\s_L = V_L \cdot \Delta t = \theta \cdot (r - \frac{d}{2})\end{cases}$$

其中：

• $d$：左右轮轴距
• $r$：旋转中心到底盘中心的距离
• $\theta$：旋转角度

上式相减得：

$$V_R - V_L = \frac{\theta}{\Delta t} \cdot d = \omega_z \cdot d$$

因此：

$$\omega_z = \frac{V_R - V_L}{d}$$

上式相加得：

$$V_R + V_L = \frac{\theta}{\Delta t} \cdot 2r = \omega_z \cdot 2r$$

底盘中心线速度：

$$v_x = \omega_z \cdot r = \frac{V_R + V_L}{2}$$

矩阵形式：

$$\begin{bmatrix} v_x \\ \omega_z \end{bmatrix} = \begin{bmatrix} \frac{1}{2} & \frac{1}{2} \\ -\frac{1}{d} & \frac{1}{d} \end{bmatrix} \begin{bmatrix} V_L \\ V_R \end{bmatrix}$$

1.2.4 逆向运动学（Inverse Kinematics）

输入： 底盘目标速度 $v_x, \omega_z$
输出： 左右轮目标速度 $V_L, V_R$

对前向运动学矩阵求逆：

$$\begin{bmatrix} V_L \\ V_R \end{bmatrix} = \begin{bmatrix} 1 & -\frac{d}{2} \\ 1 & \frac{d}{2} \end{bmatrix} \begin{bmatrix} v_x \\ \omega_z \end{bmatrix}$$

展开形式：

$$\begin{cases}V_L = v_x - \frac{d}{2} \omega_z \\V_R = v_x + \frac{d}{2} \omega_z\end{cases}$$

1.2.5 轮子线速度计算

编码器反馈计算：

$$V = \frac{2\pi R}{N \cdot P} \cdot \frac{M}{\Delta t}$$

其中：

• $M$：采样周期内编码器计数值
• $P$：编码器线数（一圈总脉冲数）
• $N$：电机减速比
• $R$：轮子半径
• $\Delta t$：采样周期

二、四轮差速模型（Four-Wheel Differential Drive）

2.1 基本原理

结构类型：

1. 履带式：左右两侧各用履带连接前后轮
2. 机械连杆式：通过链条/连杆同步前后轮
3. 四电机独立驱动：每个轮子独立电机控制

运动特性：

• ✅ 载重能力强
• ✅ 稳定性好（四点支撑）
• ❌ 存在侧向滑动
• ❌ 里程计精度低于两轮差速

2.2 数学模型

2.2.1 理想情况（无侧滑）

当 左右轮距 >> 前后轮距 时，可近似为两轮差速：

$$\begin{bmatrix} v_x \\ \omega_z \end{bmatrix} = \begin{bmatrix} \frac{1}{4} & \frac{1}{4} & \frac{1}{4} & \frac{1}{4} \\ -\frac{1}{d} & \frac{1}{d} & -\frac{1}{d} & \frac{1}{d} \end{bmatrix} \begin{bmatrix} V_1 \\ V_2 \\ V_3 \\ V_4 \end{bmatrix}$$

其中 $V_1, V_2, V_3, V_4$ 分别为左前、右前、右后、左后轮速度。

2.2.2 侧向滑动分析

质心（COM）与几何中心（COG）不重合时：

设质心偏移量为 $d_{cx}$（沿x轴），转弯时质心处的侧向速度：

$$v_{cy} = \omega_z \cdot d_{cx}$$

结论：

• 当 $\omega_z \neq 0$ 且 $d_{cx} \neq 0$ 时，必然产生侧向滑动
• 侧滑导致里程计累积误差增大