机器人软件与具身智能 · 专栏四：定位、建图与导航 · 第 3 篇
前两篇分别讨论里程计漂移和已知地标下的状态估计，本篇继续处理地图也需要被估计的情况。从八界绕过家具后再次看见出发位置讲起，把观测关联、位姿约束、图优化和地图更新接成一条链。示例使用 13 个合成平面位姿，不包含图像前端，也不调用真实 SLAM 服务。
八界从客厅绕过一组家具，最后又看见出发时的区域。里程计认为终点离起点还有几十厘米，但眼前的墙角、柜子和门框都很熟悉。这次"再次看见"究竟能帮系统修正什么？为什么有时地图会变得更整齐，有时反而被拉歪？
理解 SLAM，需要把两个问题分开：哪些观测应当对应同一处真实结构，以及在这些对应关系成立时，怎样安排所有位姿和地图元素，让观测尽量自洽。前一个问题主要由前端和回环验证处理，后一个问题交给后端估计。错误关联一旦被当作强约束，优化器也可能非常努力地得到一张错误地图。
图 1｜重新观察同一区域，为此前的轨迹补充跨时间约束。
1. SLAM 同时估计的是轨迹与环境关系
假设第 k 次观测为 z_k，机器人位姿为 X_k，环境中的地标为 M_j。观测可以写成 z_kj≈h(X_k,M_j)：机器人在哪里、地标在哪里，会共同决定相机或激光传感器看见什么。
如果位姿已知，可以把传感器数据放进固定坐标系里建图；如果地图已知，可以用当前观测匹配地图来定位。SLAM 则在二者都不完全已知时，利用运动和重复观测逐步估计它们。
"同时"不要求所有模块在同一个线程、同一频率里计算。工程系统常让高频跟踪先给出局部运动，再让较低频的后端调整关键帧和地图。用户看见的连续定位、保存的长期地图和回环后的全局轨迹，可能来自不同更新节奏。
八界公开 SDK 提供 eco_autoMapBuild，ROS 2 开发文档列出了建图与重定位相关接口。12 这些入口告诉应用怎样请求能力，不能仅凭接口名反推出内部用了哪种特征、图优化库或回环算法。本篇使用通用模型解释这些能力背后的核心问题。
2. 前端真正交给后端的，不应该只是一串位姿
沿用专栏三第 1 篇的成像模型、第 3 篇的点云处理与第 5 篇的位姿估计知识：相邻帧中先找到候选对应，再用几何关系检查它们。若采用二维特征加深度，可以构造三维到二维的重投影残差；若采用点云匹配，可以构造点到点或点到面的几何残差。不同残差对应不同噪声和退化条件。
前端输出相对运动 Z_ij 时，还需要说明：它连接哪些帧 i、j，变换方向是什么，哪些观测支持这个结果，估计对哪些方向更敏感，以及数据来自哪一时刻。长直走廊可能对某些运动分量约束弱；特征集中在画面一小块，也未必能稳定约束全部自由度。
把"匹配点很多"直接翻译成"协方差很小"，会让后端过度相信一条几何上退化的边。把经过筛选的内点当成完全独立样本，也可能低估共享标定、时间或结构误差。
关键帧的作用，是保留对估计有代表性的观测与状态。它不等于每隔固定帧数无条件保存一张图；常见选择依据还包括视差、视野变化和跟踪质量。少留关键帧可降低计算量，但删掉的信息需要以合适的方式压缩或边缘化，不能假设成本下降时约束完全不变。
对八界而言，头部或腕部相机观察到的运动要先结合机构关系，才能形成基座运动约束。外参、关节状态和时间戳错误，会把本应固定的墙角变成"会动的地标"。后端可能仍能拟合这些数据，却无法知道哪一个上游假设出了问题。
3. 把一次回访写成位姿图
为方便演算，暂时只保留平面位姿 X_i=(t_i,θ_i)，其中 t_i=[x_i,y_i]ᵀ。边 Z_ij=(z_t,z_θ) 表示后一个位姿在前一个位姿坐标系中的相对关系。当前估计预测出的相对运动为：
text
t̂_ij = R(θ_i)ᵀ(t_j − t_i)
θ̂_ij = wrap(θ_j − θ_i)本篇定义残差：
text
e_ij = [t̂_ij − z_t,
wrap(θ̂_ij − z_θ)]ᵀ平移残差在位姿 i 的参考系表达，角度残差按周期归一。它是一个明确的平面残差模型，并非完整 Log(Z_ij⁻¹X_i⁻¹X_j) 的逐项实现。实际使用李群残差时，扰动方向、雅可比和协方差参考系都要与定义一致。
相邻关键帧连接成一条链，主要提供局部运动信息。回环则把相隔很久的两个关键帧连接起来：如果八界重新看见同一处结构，就有机会检查"沿整条链走过来"的相对关系是否与直接匹配一致。
图 2｜节点表示待估计位姿，边表示观测约束。连线是信息关系，不是机器人必须行驶的路径。
图优化框架如 g2o，提供的正是将非线性误差项组织成图并求解的能力。3 它不会凭空产生可靠的对应关系。把错误边交进去，图在数学上仍然可以被优化。
4. 最小二乘为什么会把一处修正传到整条轨迹
设边的误差协方差为 Σ_ij，信息矩阵为 Ω_ij=Σ_ij⁻¹，目标是：
text
min_X Σ_(i,j) e_ijᵀ Ω_ij e_ij可以把这个式子理解为：按各方向的可信尺度衡量误差，再寻找整体代价较小的位姿组合。位置用米、角度用弧度，直接不加权相加会隐含一个缺乏解释的单位选择。
在当前估计附近，一条边线性化为 e(X+δ)≈e+Jδ。把所有边叠起来，求解：
text
(Jᵀ W J) δ = −Jᵀ W e每条边只连接少量状态，所以大系统的矩阵通常有稀疏结构。本例只有 13 个位姿，为便于阅读使用 NumPy 稠密矩阵，不代表大型 SLAM 应采用同样实现。
以平移残差 R_iᵀ(t_j−t_i) 为例，对 t_i、t_j 的导数分别是 −R_iᵀ、R_iᵀ，对 θ_i 的导数为 [t̂_y,−t̂_x]ᵀ。因此调整某个节点的朝向，会同时改变与它连接的平移解释。回环修正并非"把终点拖回起点"这么简单，而是通过共同状态影响多条约束。
配套优化代码 保存每轮目标值，使用小阻尼与回溯步长，并以中心差分核对雅可比。正常回环实验的代价从约 472.821 降到 3.423714。目标下降说明优化过程沿当前模型改善，不单独证明模型和关联正确。
5. 为什么必须固定一个参考，而不是直接把矩阵求逆
只有相对约束时，把所有位姿一起平移、一起旋转，节点之间的关系不变。整个图没有办法自己决定"客厅原点到底在哪里"。这叫规范自由度，也常用英文 gauge freedom 表示。
本例有 13 个平面位姿，共 39 个参数。未固定参考时，堆叠雅可比的秩为 36，留下三个自由方向：全局 x 平移、全局 y 平移和全局旋转。把整条轨迹平移 (10,−4) m、再旋转 0.7 rad，代价差只有浮点误差量级。
因此程序固定第一个位姿，求解其他 12 个位姿的增量。这是在选择坐标参考，不是在凭空增加一条真实位置测量。也可以使用适当先验，但先验的意义和强度必须明确。
对单目系统还要关注尺度自由度；是否有额外自由度，取决于传感器和约束模型。本篇边已经使用米制相对位移，因此不包含自由尺度。
如果遇到奇异矩阵，盲目给对角线加一个很大的数，可能让程序暂时返回结果，却掩盖了未固定参考或约束不足。应该先检查图是否连通、哪些自由度被测量约束，再讨论数值正则化。
6. 终点闭合了，整张地图仍然可能有误差
程序构造一条闭合曲线上的 13 个真值位姿，给 12 条相邻运动加入固定偏差与随机噪声，再逐段积分得到初始轨迹。首尾实际位姿相同，但积分终点偏离约 0.358832 m。
加入一条正确的首尾回环后，结果为：
text
闭环端点位置误差：0.358832 m → 0.001159 m
全轨迹位置 RMSE：0.176111 m → 0.086706 m
图 3｜首尾闭合约束显著改善端点，但全轨迹仍有误差。RMSE 使用固定同一参考下的 13 个位姿计算；图中数值来自合成数据。
端点误差降到约 1.2 mm，并不意味着整条轨迹都达到毫米精度。回环只提供了有限约束，其他边仍然带噪声，内部形状也可能被错误拉伸或弯曲。评估地图时，需要同时看局部一致性、跨区域一致性、回环残差和独立参考，不能只看起终点是否重合。
八界能够返回收纳区，并不自动证明那次建图在每个角落都足够精确；导航成功和精细操作所需的空间精度，也不是同一项验收。对取放任务，最终接近仍需要有效的局部感知与机构坐标关系。
7. 错误回环会怎样拉坏图，鲁棒核又能做多少
现在再加入一条错误的跨时间约束：把第 3 个和第 9 个位姿之间的相对观测人为扰动，平移偏差为 (1.4,−1.0) m，角度偏差为 0.7 rad，而且给它较强权重。
普通最小二乘会努力满足这条边。实验中全轨迹位置 RMSE 从正常回环后的约 0.0867 m，恶化到约 0.5646 m。优化器没有"故意把地图弄坏"；它是在执行我们给定的目标，而错误信息被写进了目标本身。
一种缓解办法是使用鲁棒损失，让大残差的边降低影响。本例对回环边采用 Cauchy 损失，令白化后的残差平方为 q，尺度为 c：
text
ρ(q) = c² log(1 + q/c²)
迭代权重 w = 1 / (1 + q/c²)小残差附近近似平方损失，大残差时增长变缓。从正常回环结果初始化，取 c=3，该实验的 RMSE 约为 0.0853 m。
图 4｜一条虚假约束可使全局轨迹变差；本例鲁棒权重减弱了它的影响。结果依赖初值、约束分布与损失尺度，不是错误回环一定能被自动修复的保证。
鲁棒核不等于回环验证器。如果错误回环一开始残差很小、正确约束反而因漂移显得很大，或者许多错误边互相支持，鲁棒优化仍可能选错。回环候选需要继续经过几何验证、足够的空间覆盖和一致性检查；候选相似分数只能帮助检索，不能直接变成位姿真值。
家具外观重复的房间特别容易产生这类歧义。八界"看见一张相似的柜子"与"确认回到了同一张柜子前"，中间还需要几何与上下文证据。专栏三第 4 篇的实例分割与第 6 篇的语言指代，都强调了类别与实例身份的区别；这个区别在回环检测中同样重要。
8. 位姿变了，之前建进去的点和障碍怎么办
假设一个点在关键帧 k 的局部坐标为 p_k，它在地图中的坐标是：
text
p_map = R_k p_k + t_k后端优化修改了 R_k、t_k，这份地图点也应随之更新。实验选取第 6 个关键帧下的一个局部点，正确回环前后，它在地图中的位置变化约 0.147575 m。若只更新机器人轨迹，保留旧点云或旧障碍栅格，地图内部就会同时存在两个不一致的解释。
图 5｜整体换参考不会改变相对残差；局部关键帧经过优化后，其关联地图点需要按新位姿更新。右侧点位移来自本篇合成实验。
工程上可以保留子地图及其锚点，在优化后调整子地图关系；也可以在需要时重新投影或重新融合观测。具体策略取决于地图表示及实时预算。把稠密数据过早压成一张不可回溯的栅格，会让后续修正更困难。
导航消费者还要知道自己正在使用哪个地图版本。八界正在走的路径若来自旧地图，而当前位置已经切换到新优化结果，不能假设两者自动一致。地图修正、路径有效性和局部运动连续性，需要通过参考系、时间戳和更新策略接起来。
REP 105 中 map → odom → base_link 的分工，提供了一种保留局部连续性、同时接收全局修正的组织方式。4 它解决的是变换语义，不会自动替应用重建所有旧地图数据。
9. 运行后端实验，不把它误称为完整 SLAM
从仓库根目录执行：
bash
python3 -m venv .venv-nav
.venv-nav/bin/python -m pip install -r "04_定位建图与导航/21_SLAM的基本结构/examples/requirements.txt"
.venv-nav/bin/python "04_定位建图与导航/21_SLAM的基本结构/examples/21_pose_graph.py"最终输出 PASS: 8 experiments; synthetic pose graph, no SLAM service.。完整代码 展开了残差、雅可比、固定参考、迭代优化与鲁棒权重；运行说明 给出预期数值和边界。
这里的相对约束直接由真值加扰动生成，没有提取图像特征、估计点云匹配或检索回环。它验证的是"约束进入后端以后会发生什么"。真正的系统还需要评估前端观测质量、失效检测、重定位、长期地图维护和计算资源。
理解这条分工以后，排查地图问题就能更具体：是观测关联错了，几何退化被赋予了过高权重，参考没有固定，还是优化后关联地图没有同步更新。每一种都可能表现为"地图不对"，但不能用同一组参数解决。
位姿和观测关系确定以后，还要决定环境信息怎样保存、怎样供下游查询。本专栏第 4 篇《地图表示怎么选》会从占据、表面与净空这几种不同需求继续展开。
欢迎关注科沃斯八界 AtomGit，相关代码将逐步释放，实际开放内容与使用说明以仓库为准。5