Day 0｜机器人经典基础阅读入门：让坐标、估计、规划与控制接起来

系列首页 · 下一站：发展时间线 · 中英概念与符号速查

建议用时：15–20 分钟，包含文末自测。 机器人经典论文看起来跨越矩阵、滤波、搜索和控制，实际上经常围绕同一条链路：机器人怎样描述世界，怎样知道自己在哪里，怎样选择可行运动，又怎样把它执行出来。本篇先建立这条链路，公式只保留理解接口所需的部分。

一、从机械臂拿起桌上的杯子开始

设想相机看到一个杯子，我们希望桌边机械臂把它拿起来。先不讨论复杂学习模型，只拆开必须解决的问题：

• 几何与坐标（geometry and coordinates）：杯子相对于相机在哪里，换到机器人基座坐标系又在哪里？
• 状态估计（state estimation）：有噪声的图像和关节测量支持怎样的位置、速度估计？
• 运动学（kinematics）：关节角与夹爪位姿如何对应？
• 规划（planning）：怎样到达抓取点，同时绕开桌沿与障碍？
• 动力学与控制（dynamics and control）：需要怎样的力矩，受到偏差后怎样修正？

这就是第一张概念地图：传感器提供测量，估计形成当前状态，规划产生目标运动，控制产生执行输入，物理系统产生新状态。 几何约定贯穿整条链路，动力学描述其中的运动与受力关系。真实系统会让多个模块反复交互，而不只是顺序跑一遍。

以下杯子与两连杆机械臂例子都是教学构造；它们用于说明概念，不是对后续论文实验的转述。

二、一个位置数字必须有参照

坐标系决定数字怎样解释

坐标系（coordinate frame）规定原点和坐标轴。杯子位置“$(0.3,0.1,0.2)$”只有在坐标系和单位已知时才有意义。相机、机械臂基座、末端工具都可以有自己的坐标系。坐标系速查

同一只杯子没有移动，在不同坐标系中可以有不同坐标。“相机向前”“机器人向前”“图像向下”也未必对应同一根轴。常见机器人系统会选择右手系，但具体轴朝向仍需查约定；不能把某个平台的 x、y、z 方向默认推广到所有相机和机器人。

图像像素坐标也不是三维米制坐标。单个二维像素通常只限定一条视线；得到三维位置还需要深度、已知几何、多视图或其他信息。相机内参帮助描述投影，外参描述相机与其他坐标系的关系。相机标定与 手眼标定分别解决相关接口问题。

位姿同时包含位置和朝向

位置（position）回答在哪里；姿态／朝向（orientation）回答朝哪边；位姿（pose）把二者合起来。夹爪到达杯子中心附近却朝向不对，仍可能无法抓取。位姿速查

三维刚体位姿有六个自由度，但表示它不一定只用六个数字。旋转矩阵用九个数加约束表示旋转，单位四元数用四个数加单位约束表示旋转。表示维数与物理自由度不是同一件事，欧拉角还需要说明转轴顺序。

三、先固定变换方向，再做矩阵乘法

本文采用明确约定：$^{A}T_B$ 把 B 系表达的点坐标转换成 A 系表达。对应关系为：

$${}^{A}p={}^A R_B\,{}^B p+{}^A p_B, \qquad {}^A T_B= \begin{bmatrix} {}^A R_B & {}^A p_B\\ 0 & 1 \end{bmatrix}.$$

$^{A}R_B$ 是旋转矩阵，$^{A}p_B$ 是 B 系原点在 A 系中的位置。齐次变换把旋转和平移放进一个 4×4 矩阵，便于组合。公式中的点坐标是三维向量；使用 4×4 矩阵时，要给它补上最后一维 1。

在杯子例子里，如果已知相机系中的杯子位置，以及把相机坐标映到基座坐标的变换，就能把杯子位置转到基座系。连续变换遵循：

$${}^A T_C={}^A T_B\,{}^B T_C.$$

中间的 B 对应起来，是检查乘法方向的一种实用方式。逆变换把 A 系坐标转回 B 系。旋转和平移通常不能交换顺序；先沿工具自身的 x 轴移动，再旋转工具，和先旋转再沿同一数值方向移动可能产生不同结果。

最简单的自检是拿一个已知点做变换，再乘逆变换，看是否回到原坐标。还应检查旋转块满足 $R^\top R=I$、$\det R=1$。这些代数检验能找出一部分错误，但不能证明标定值符合真实硬件。

注意：不同论文对上下标、主动旋转与坐标变换有不同约定。读新文章时先重建它的定义，再使用“下标消去”直觉，不能只因为矩阵也叫 T 就直接相乘。刚体变换的教材说明

四、运动学把关节空间与任务空间连接起来

构型（configuration）用一组变量描述机器人几何形状，例如关节变量 $q=(q_1,\ldots,q_n)$。所有允许构型组成构型空间（configuration space）。杯子在工作空间中，而机械臂避障往往要考虑整个身体在构型空间中的可行性。构型空间

正向运动学（forward kinematics）从关节变量求末端位姿。逆运动学（inverse kinematics，IK）从期望末端位姿找相应关节变量。两者都先讨论几何，不直接计算为此需要多大力矩。正向运动学 · 逆运动学

将机械臂简化成平面两连杆，长度分别为 $l_1,l_2$，两个关节角是 $q_1,q_2$。忽略工具偏移，末端位置为：

$$x=l_1\cos q_1+l_2\cos(q_1+q_2),\qquad y=l_1\sin q_1+l_2\sin(q_1+q_2).$$

第二根连杆的绝对方向用角度之和，因为 $q_2$ 是相对上一根连杆的转角。取两根长度均为 1 米，$q_1=\pi/2$、$q_2=-\pi/2$，末端在 $(1,1)$ 米。此处只约束平面位置，可能有不同的肘部构型到达同一点；附加朝向或关节限制后，可行解又会改变。

雅可比矩阵（Jacobian）描述当前位置附近，关节小变化如何影响末端小变化。对这里的二维位置，有：

$$\dot p=J(q)\dot q.$$

它是局部速度关系，不能不加条件地当成全局位置映射。某些构型下，J 丢失秩，机器人暂时失去某个方向的独立运动能力，这叫运动学奇异（kinematic singularity）。接近奇异处，即使只要求很小的末端速度，也可能需要很大的关节速度。雅可比速查 · 阻尼逆运动学

D–H 记号是系统组织运动链几何的一种方式。标准 D–H 与改进 D–H 的坐标选择、参数与乘法顺序不同；一份参数表不能随意塞进另一种约定的程序。

五、动力学把运动与受力连接起来

同一条几何路径，空手慢慢走和拿着重杯子快速走，需要的力矩不同。动力学（dynamics）研究质量、惯量、力与运动的关系。动力学速查

对一个简化的固定基座刚体机械臂，可写成：

$$M(q)\ddot q+C(q,\dot q)\dot q+g(q)=\tau.$$

$M$ 是惯性矩阵，$C\dot q$ 表示一类速度相关项，$g$ 是重力项，$\tau$ 是关节力矩。这里暂不写摩擦与外部接触项；真实系统可能需要补充它们。不同文献对速度项的记法和具体矩阵分解也会不同。

正动力学（forward dynamics）给定状态与力，求加速度等运动结果；逆动力学（inverse dynamics）给定期望运动，求所需力或力矩。它们与正／逆运动学不是同一组问题。Newton–Euler和 Featherstone关注动力学计算。

抓起杯子后负载变了，控制器的重力补偿可能需要调整。接触桌面时，接触力与约束也参与运动。单纯知道末端几何位置，并不足以解释它为什么压得太重或拿不稳。

六、估计、规划和控制怎样分工

估计回答现在可能处在什么状态

状态估计（state estimation）结合测量与模型推断状态，并尽可能表示不确定性。相机说杯子在这里，另一次测量略有偏差，估计器需要权衡信息；它并没有直接得到无误差的真相。状态估计速查

Kalman 滤波在相应线性、高斯等假设下组织预测与测量更新。协方差矩阵 P 描述所建模误差的不确定性；数值小不必然代表真实误差小，模型或噪声假设错误时可能过于自信。

定位（localization）通常在已有参照或地图下估计自身位姿；SLAM同时处理定位与建图。杯子抓取不一定需要完整 SLAM，但换成移动机械臂时，地图、机体位姿与相机位姿的关系就可能进入同一问题。GraphSLAM是后续入口之一。

规划回答怎样去，并明确要满足什么约束

运动规划（motion planning）寻找从当前构型到目标构型的可行运动。路径（path）描述经过哪些几何位置；轨迹（trajectory）还说明何时到达，因此包含时间信息。运动规划速查

一条避开桌沿的路径，可以执行得慢，也可以执行得快。加入速度、加速度、力矩或接触限制后，可行轨迹的条件会更严格。没有碰撞，只解决了其中一部分问题。

A*在图上搜索，RRT通过采样探索构型空间，CHOMP采用轨迹优化思路。比较它们时，需要明确地图表示、约束、最优性条件和计算预算，不能把不同问题设置下的一次运行时间当作统一排名。

控制回答怎样持续跟上目标

反馈控制（feedback control）根据期望与实测之间的误差调整输入。杯子位置稍有变化或关节受到扰动时，控制器不断修正。开环（open-loop）执行不利用相应反馈修正；闭环（closed-loop）执行则把反馈接回决策过程。反馈控制速查

控制不只追求位置误差小。接触任务还关心力与柔顺性；阻抗控制通过设计运动与力的动态关系，让机器人表现出适当的弹簧、阻尼等特性。高刚度不等于所有接触任务都更安全。

模型预测控制（model predictive control，MPC）反复利用模型优化有限未来时域的动作，通常执行前一部分，再根据新状态重新求解。它让规划与控制的边界相互交叠，因此上述分工是理解工具，并非不可跨越的分类墙。MPC 速查 · Cheetah MPC

从一个像素误差走到一次避障判断

下面把几个模块真正接起来算一遍，而不是只记名词。采用针孔模型，忽略畸变，相机点的横坐标为 $X=(u-c_x)Z/f_x$：$u$是像素横坐标，$c_x$是主点横坐标，$f_x$是像素单位焦距，$Z$是沿光轴的米制深度。原创教学设置取 $u-c_x=20$像素、$f_x=500$像素、$Z=1$米，得到 $X=0.04$米。

把误差作一阶展开，固定内参时有

$$\delta X\approx\frac{Z}{f_x}\delta u+\frac{u-c_x}{f_x}\delta Z.$$

若像素偏差界为1像素、深度偏差界为0.02米，单纯横向误差的保守一阶界为 $0.002+0.0008=0.0028$米。它是误差界相加，不能冒充独立高斯标准差；若求方差，还需协方差与相关项。标定误差、畸变误差和时间误差尚未包含在这里。

再设机器人以0.4米/秒平移，图像与位姿错开0.05秒，就额外错位约0.02米，比上面的像素与深度误差更大。一条名义净空只有0.015米的路径，即使规划器对名义地图完全无碰撞，也没有足够依据宣布执行安全。这正是从标定到状态估计、再到规划验证必须明确接口的原因。

最后把相机点变换到基座系时，应使用同一采集时刻的 $^{B}T_C$，而不是无条件用处理时刻的最新变换。Modern Robotics的变换讲解区分描述位姿、换坐标和主动位移；本例只做换坐标。这个小链条已能解释：某模块误差下降10%，为何完整机器人可能没有改善。

七、单位和方向往往比复杂公式更先出错

读实现或实验设置时，至少核对以下项目：

1. 长度、角度与时间。 米还是毫米，弧度还是度，秒还是毫秒；转动关节的 $q$ 与移动关节的 $q$ 单位不同。
2. 坐标表达。 点、速度、力分别在哪个坐标系里表达；变换到底从哪个系映到哪个系。
3. 向量顺序。 六维速度可能写成“角速度在前、线速度在后”，也可能相反；力与力矩的顺序同样要匹配。
4. 误差方向。 期望减实测还是实测减期望；控制律符号要与误差定义一致。
5. 时间对齐。 相机时间戳与关节测量是否对应同一时刻，控制周期是否与模型离散化一致。

举一个量纲自检：若位置误差单位是米，希望比例控制输出力，则相应增益应具有牛顿每米的单位。给它一个无说明的数值“100”，无法判断它与另一篇论文是否相同。坐标正确、单位一致、时间对齐，是模块能够连接的前提。

八、常用符号先查局部定义

不需要建立一套全领域唯一符号表。正确做法是在每篇文章中确认定义、维度、单位和坐标系，再对照共享概念与符号速查。

九、下一步怎样进入系列

先读发展时间线，然后用三条短路线建立基础：

1. 几何与运动：D–H → Resolved-rate → Newton–Euler → 阻抗控制。
2. 感知与估计：Kalman → RANSAC → ICP → GraphSLAM。
3. 搜索与规划：A* → 构型空间 → PRM → RRT → RRT*。

每篇先标出它负责系统中的哪一步，再问输入假设、保证成立的条件与失败情形。证明渐近最优不代表有限时间必然找到最佳路径；理想模型下稳定不代表任意延迟和饱和下都稳定；某组数据集上的低误差也不等于所有场景都可用。

十、三分钟自测

1. 杯子相机坐标与基座坐标不同，是否说明两次测量矛盾？

不一定。同一物理点在不同坐标系中的数值本来就可能不同。先统一参照与单位，再讨论测量是否冲突。

2. 逆运动学找到一组关节角，是否说明机器人可以按任意速度安全到达？

没有。还需检查路径碰撞、关节限制、速度与加速度、动力学和控制能力。几何可达只是其中一步。

3. 规划器得到一条无碰撞路径，控制器跟踪时为何仍可能失败？

实际状态估计、模型、时延、执行器限制与接触都可能偏离规划假设。还需要合适的时间参数化、反馈和约束处理，不能只验证路径几何。

资料与阅读边界

坐标与变换约定可对照 Modern Robotics 的齐次变换讲解；本篇固定自己的映射方向，便于检查教学推导。各篇历史论文的可得资料不同，例如 D–H 篇已说明原版全文解析的限制。这篇导读不替代原文，也不把现代教科书记号追认为早期论文逐字采用的形式；既有证据缺口与未复现说明继续有效。

继续阅读发展时间线 · 打开中英概念与符号速查