机器人经典基础总结：把几何、估计、规划与控制接成闭环

发展时间线与架构图 · 完整目录 · 上一篇：KISS-ICP

机器人经典文献最有价值的部分，不是某个算法缩写，而是它们如何把一个过大的问题拆成可计算的模型，并明确代价。坐标表示让运动可描述，概率估计让噪声可处理，搜索和优化让动作可寻找，反馈让模型与现实之间的偏差可纠正。系统和基准则决定这些组件能否协作，以及比较是否可信。

读完一百篇论文，真正需要保留的是一组判断能力：知道某个结论依赖什么条件，知道改进发生在哪个环节，也知道何时需要把问题交给另一个模块。

六条贯穿经典文献的思想

表示会改变问题的难度

DH 用规则化坐标建立机械链；Brockett 将刚体运动与指数表达联系起来；构型空间 把整个机器人的碰撞问题转成配置点的可行性问题。它们没有让物理系统本身变简单，而是让关系变得可组织。

这种思想同样出现在估计里：保留更多历史位姿的 iSAM 可以利用稀疏关系；直接消掉变量可能制造耦合。变量少不等于计算便宜，矩阵稀疏也不等于其消元因子一定稀疏。

在 CHOMP 中，改变衡量轨迹更新大小的度量，可以使局部避障信号沿时间平滑传播。表示与度量不仅影响公式好不好看，还影响数值计算实际走向哪一个解。

几何准确、概率可信与任务成功是三种结果

ICP 可能降低对齐残差，却对错地方；Kalman 滤波 可能给出很小协方差，却使用了错误噪声模型；GMapping 的地图可能拓扑正确，却存在双墙。

因此，同一个“误差变小”必须带上对象：优化残差、相对真值误差、置信区间覆盖率，还是任务成功率。它们相关，但不能互相替代。单个漂亮地图也不能证明误差分布经过校准。

最优性总有一个明确的对象

A* 在所给图、代价与启发式条件下寻找最优路径；连续机器人运动还要考虑离散化和动力学。抓取质量指标 衡量指定接触及力矩尺度下的能力，不保证感知误差下不会滑落。SE-Sync 和 TEASER 的认证也针对明确定义的优化问题与条件。

“最优”不是对真实世界一切目标的总评。对一个模型精确求解，如果模型遗漏执行器限制，结果仍可能无法执行。

时间预算本身就是算法的一部分

D* Lite 复用旧搜索信息；AD* 在当前预算与路径质量之间持续调整；PTAM 把快速跟踪与较慢建图分开；iSAM 将增量更新与周期性整理结合。

它们没有消灭计算成本，而是决定什么时候做、重复哪些部分、何时返回一个足够好的答案。平均耗时、峰值延迟、吞吐量和截止期违反次数应该分开看。

反馈不是优化的附属品

混合力/位置控制、阻抗控制 与 操作空间控制 处理的是机器人怎样与任务和环境相互作用。再漂亮的开环路径，也可能因接触刚度、摩擦、负载误差而表现不同。

LIPM、ZMP preview、Capture Point 用不同的简化模型解释平衡。模型内可恢复、完整刚体系统可实现、实机在摩擦和力矩限制下可恢复，是逐级增加要求的命题。

工具和基准改变研究能够回答的问题

ROS 让模块、日志与观察工具更容易组合；OMPL 让规划器可以在共同接口下比较；TUM RGB-D、KITTI、EuRoC 提供了可复用的评价对象。

工具提高了比较的可操作性，但没有自动统一参数选择、硬件、失败处理与数据版本。基准的真值覆盖、对齐方式和难度分布同样需要进入结论。

用两个守恒与约束检查，把跨篇接口真正接上

速度与力必须给出同一个功率

原创可手算闭环片段。 两根1米连杆的平面机械臂位于 $q_1=\pi/2,q_2=-\pi/2$，位置雅可比为 $J=\begin{bmatrix}-1&0\\1&1\end{bmatrix}$。取关节速度 $\dot q=(0.1,0.2)^T$弧度/秒，得到末端速度 $v=J\dot q=(-0.1,0.3)^T$米/秒。若机器人向环境施加基座系力 $F=(2,3)^T$牛，则对应静态关节力矩 $\tau=J^TF=(1,3)^T$牛·米。

关节端功率为 $\tau^T\dot q=0.7$瓦，末端功率为 $F^Tv=0.7$瓦。两边相等才说明所用速度、力和雅可比约定相容；若环境施加的是 $-F$，反作用符号也要一起改。这里尚未加入加速、重力与损耗力矩，因此不是完整逆动力学实验。Modern Robotics第5.2节从同一功率关系推导转置映射。

这条检查连接运动学、逆动力学、操作空间与层级控制。后端优化器即使求解准确，若前端把空间雅可比与机体系力相乘，物理接口仍不相容。

位置可达，不代表瞬间可以朝那个方向运动

轮式机器人的简化模型为 $\dot x=v\cos\theta,\dot y=v\sin\theta,\dot\theta=\omega$，$\theta$是车头朝向，$v,\omega$是前进与转向速度。因而 $-\sin\theta\dot x+\cos\theta\dot y=0$。车头朝x正向时，瞬时横向速度必须为零；想移到侧面，需要先改变朝向再组合运动。

Modern Robotics第13.3.1节还区分独轮、差速和汽车的控制集合。将几何RRT-Connect的线段直接给汽车，不会自动满足转弯半径；DWA考虑局部可达速度，TOPP-RA则为合适的既定路径选时间。这个边界解释了为什么经典路线要相互组合，而不能只保留一个“最好规划器”。

继续到机器人学习：先看学到了闭环中的哪一层

机器人学习系列的DMP可以紧接本系列的阻抗与逆动力学阅读，但这里是教学联系与模块组合，不能把它们写成同一算法的连续版本。DMP把示范形状写进有吸引子的运动生成器；2002原文明确把策略限定为运动学层，并假设下层反馈/前馈能够跟踪，实机使用逆动力学控制器。原文第2节脚注与第3.1节

所以“参考最终收敛到目标”与“接触力、安全和执行器饱和都受控”仍是两个问题。带着这个接口问题往下读，才容易判断后续学习论文到底替换了轨迹生成、状态估计、动力学模型还是低层控制，以及哪些经典条件仍需保留。

把不同路线放在同一张比较表里

这张表不是给算法排名。一个实时机器人可能同时需要其中全部路线：视觉匹配产生几何约束，图优化估计状态，规划生成动作，反馈修正执行，日志记录全过程。

自构实验一：误差小的估计器，是否也知道自己错多少

这个实验是教学设计，尚未运行，没有实验结果。 它连接 Kalman、概率地图、粒子滤波与图优化，重点区分准确性和一致性。

场景与对照

建立二维移动机器人仿真，地图有可识别路标，机器人先沿开阔路径走一圈，再经过重复布局区域返回起点。仿真器保存完整状态真值；算法只能获得带噪里程计和距离/方位观测。

第一阶段采用已知路标地图，对比线性小例子的 Kalman 更新、非线性 EKF 与粒子滤波。第二阶段开放地图未知，比较 EKF-SLAM 与保留历史的平滑后端。先给真实关联，隔离数值估计；之后才增加歧义关联，避免把前端错误与后端线性化混成一个变量。

对于已固定规范且可观测的 $d$ 维状态误差 $e$，NEES 为

$$\epsilon=e^TP^{-1}e.$$

正确线性高斯条件下，其分布与自由度相关。非线性 SLAM 要先处理规范自由度，不能对奇异协方差直接求逆，再照搬自由度。粒子滤波若保持多峰，也不应只用一个拟合高斯掩盖其结构，可补充真值附近概率质量和模式保留率。

预期检验的假设，不是预写的结论

噪声设得过小可能让估计器过度自信；保留历史可能在回环后更好地修正线性化误差；错误关联可能同时破坏滤波和平滑。每个判断都要由对应曲线验证，不能仅因算法名字就预定胜负。

若某方法 ATE 低但覆盖率差，意味着它看起来准，却低估了风险。若提高粒子数后无改善，应检查正确模式是否早已灭绝、提议是否遗漏模式，而不是只继续增加粒子。若平滑的残差很低而真值误差大，应检查错误关联和模型偏差。

最小结果图

画误差随时间、NEES/覆盖率随噪声失配、回环前后误差、每步延迟分布四张图。报告失败样本，不能只把收敛运行纳入均值。保存原始噪声种子、关联、估计状态和协方差，才能追查“哪里开始错”。

自构实验二：规划得更快，是否真的更早到达

这个实验是教学设计，尚未运行。 它连接 A*、D* 系列、RRT 系列与 CHOMP/STOMP/TrajOpt，区分图可行、几何可行与可执行。

两层任务，避免不公平比较

第一层用二维差速机器人，在开放区域、窄通道和动态揭示障碍的栅格地图中比较 A*、D* Lite 和 AD*。第二层用机械臂，在相同碰撞模型下比较 RRT-Connect 找路、RRT* 持续改进、轨迹优化从直线或相同搜索初值出发。

两类任务不直接混排一张速度榜。前者研究搜索复用和在线更新；后者研究连续几何和轨迹质量。对所有输出，再用共同的轨迹参数化与控制器执行，记录最终任务表现。

为什么必须同时保存失败信息

方法 A 很快找到一条锯齿路径，方法 B 较慢找到平滑路径。如果 A 还需长时间参数化或执行速度很低，“首解快”可能不等于“到达早”。反过来，优化器耗尽预算仍未脱离碰撞，低总代价也不能算有效答案。

建议同时报告条件均值与无条件结果：成功运行的路径质量可以单独展示，但总成功率、超时和无效输出必须保留。比较“平均运行时间”时，如果某方法在失败时提前退出，数值反而可能更小，需要解释退出语义。

动态地图组还要明确障碍什么时候对算法可见。提前给全图的方法不能与仅在局部感知后重规划的方法混称同等信息。AD* 的次优界针对所用图与代价假设；执行器造成的额外时间不是它原本保证的对象。

最小结果图

画“可行解代价—墙钟时间”的 anytime 曲线、成功率—通道宽度曲线，以及规划时间和执行时间堆叠图。另用案例展示离散碰撞检查通过但插值段失败的轨迹。后者可以解释为什么几何检查模型比单看优化器输出更重要。

自构实验三：模型里稳定的接触，遇到刚度误差后怎样

这个实验是教学设计，尚未运行。 它连接逆动力学、力/位置控制、阻抗、操作空间与现代约束控制，重点检验“名义稳定”与实际接触的差距。

从一维开始，再扩展到机械臂

第一阶段构造一维质量块与弹性阻尼墙，比较高增益位置控制、阻抗控制与带力目标的控制。第二阶段采用平面二连杆，让末端沿墙滑动，同时约束法向接触力。初期用无摩擦墙确认机制，再加入摩擦、负载失配和通信延迟。

名义环境接触模型可写为

$$F_e=k_e\max(0,x-x_w)+b_e\dot x,$$

其中阻尼项只在接触状态按适当单边约束处理；不能允许模型无意产生拉住机器人的粘附力。名义阻抗可写为

$$M_d\ddot e+D_d\dot e+K_de=F_{\mathrm{ext}}.$$

这些是教学模型，不是任何一篇论文的完整复现。先用它们明确功率、能量和符号方向，再进入多自由度实验。

解释可能出现的反直觉现象

位置误差最小的控制器可能产生最大接触力；柔顺控制可能容许位置偏差却更好地管理受力。比较时要先声明任务目标，否则“更好”没有确定含义。

名义动力学补偿可能减少跟踪误差，但负载估错时又会注入错误补偿。零空间运动也未必在错误惯量模型下完全不影响任务。若接触力振荡，应该区分环境刚度、离散延迟、摩擦切换和饱和，而非一律归为“增益没调好”。

数值仿真还有一层误差：不同接触求解器、步长和正则化可能表现出不同的穿透与耗散。先在简单已知解或能量收支上校验仿真器，再把复杂结果解释为控制器性能，避免把求解器伪影当作物理发现。

最小结果图

画力与位置时间曲线、峰值力—墙刚度图、稳定区域—延迟图，以及能量收支图。每条曲线注明是否发生饱和。只有在仿真和限幅条件明确之后，才适合讨论向实机转移；这里不预设任何实机成功结果。

三处尚需进一步澄清的具体疑点

RRT*：原证明缺口与算法失效不是同一个命题

RRT* 篇区分原始渐近最优性论述与 Solovey 等人的后续工作。后者指出原证明中的逻辑缺口，并给出考虑采样到达顺序的另一组充分条件，涉及 $1/(d+1)$ 的连接尺度指数。

现有阅读范围不足以断言该理论问题全部后续研究的最终状态。待核对的是原证明具体使用了什么独立性或时序关系、替代证明改变哪些条件，以及不同版本的结论边界。不能把“一个证明不完整”缩写成“算法不渐近最优”，也不能把实践中有效反过来当作证明。

IMU 预积分：同一室外实验的终点误差出现两种数字

预积分篇的 RSS 2015 版本中，约 300 m 路线的结果在图 9 图注为 1.0 m，正文为 1.5 m；Tango 参照均为 2.2 m。两个本方法数值不能任选较好者计算提升。

读图：这里只展示原论文轨迹图；图注与正文关于本方法终点误差的1.0米和1.5米差异仍需同时保留，不能从图形外观替作者消除矛盾。

待澄清的是图注与正文是否对应不同运行、不同统计或排版错误。原始轨迹、后续更正或作者说明才可能闭合这个问题。无论最后确认哪个数，终点误差也不能代替全程轨迹误差，且两系统传感器不同的比较边界仍然存在。

LIO-SAM：变换符号与运动统计需要分别核对

LIO-SAM 篇保留两类具体问题：式（12）以转置表示相对变换，但齐次刚体变换的相对关系按对应坐标约定应使用逆；另外 Rotation 与 Walking 的最大角速度在表 I 和正文中互换。

前者需要结合符号约定和官方实现确认，不能从印刷表达直接断言程序算错；后者需要原始数据统计或作者更正来确认归属。两个问题都不应通过自行改写一个看起来合理的数值或符号后，假装原文从未有过歧义。

阅读记录：哪些结论来自什么范围

每篇开头的来源和范围优先于任何概括。部分较早文献可以核对完整正文，另一些只能取得可访问部分或相关原始资料；这些文章已经保留限制，不把无法确认的实验细节写成“作者未报告”。

Newton–Euler 正式勘误原页副本是已确认的版本记录，与上面尚待澄清的三处问题不同。其存在本身不会推翻递归逆动力学思想，却提醒读者：伪代码、数学思想、原始排版和实际实现不是完全同一层材料。

这组文章没有实际重跑一百篇原始实验，也没有重训练任何模型。各篇教学例子和伪代码用于理解机制；实测数值仅来自注明位置的论文。上面的三个实验是可执行的研究计划，不能引用为已验证结论。

接下来怎样把这些基础带进机器人学习

机器人学习仍然要回答同样的问题，只是某些组件由数据学习得到。策略输出的动作究竟在哪个坐标系、是否满足执行器限制；世界模型是否刻画接触和不确定性；VLA 的指令成功是否依赖精确定位、控制频率和恢复机制，都可以用本系列的框架拆解。

评价学习方法时，不妨沿信息链检查：传感器提供什么，状态估计丢失了什么，规划或策略优化什么，执行器真实完成什么，最终指标又测量了什么。能说清这条链，比仅背出某个模型名更接近读懂论文。

综合自测与参考答案

1．系统同时声称“精确优化、实时运行、轨迹安全”，你会分别查什么？

答： 查精确针对的是线性子问题、局部非线性解还是有证书的全局目标；查实时是平均吞吐、单步延迟还是最坏截止期；查安全基于采样点、连续几何、动力学限制还是带模型误差的闭环条件。三个说法不能互相补足缺失的前提。

2．新方法在同一日志上 ATE 更小，但真实机器人任务成功率更低，矛盾吗？

答： 不矛盾。日志估计误差可能更小，但延迟更大、偶发失效更严重或不确定性更不可信；闭环动作还会改变未来观测。要把估计准确性、时序、失败恢复和控制表现分开测量，再定位哪一环破坏了系统目标。

3．为什么理解经典方法的限制，对学习型机器人仍有价值？

答： 学习不会自动取消几何可观测性、接触单边约束、执行器饱和或计算预算。经典方法把这些约束显式写出来，提供可诊断的基线和分解方式。学习组件表现更好时，可以追问它改善了哪一项；失败时，也能区分数据不足、模型偏差和任务本身的不可辨识性。