具身智能（四）：从传感器到世界状态

上一篇文章介绍了机器人如何通过运动学、动力学与控制，将目标位姿转化为身体运动。但在行动之前，机器人首先要知道自己在哪里、周围有什么，以及环境正在怎样变化。相机、深度传感器、IMU 和编码器提供的只是带有噪声的原始观测，还不能直接成为规划与控制所需的世界状态。

本文沿着“传感器观测到世界状态”的链路，介绍相机模型与标定、RGB-D 与点云、物体位姿估计、滤波融合和 SLAM 的基本原理，理解机器人如何从离散的传感器信号中恢复空间关系，并形成连续、可信的状态估计。

观测为什么不等于状态

传感器只能测量物理世界的某个侧面。RGB 相机通过红、绿、蓝三个通道记录可见光，生成二维彩色图像；深度相机通过双目、结构光或飞行时间等方式测量物体表面距离，生成深度图；IMU通常由陀螺仪和加速度计组成，直接测量角速度与比力；编码器安装在电机或关节上，用于测量转角或线性位移。

这些传感器读数共同构成 Observation（观测），但它们只是对机器人与环境的局部测量。机器人真正决策时需要的 State（状态），还要通过标定、坐标变换和状态估计从观测中推断出来。

状态并不是对真实世界的完整复刻，而是机器人根据任务需要，从观测中提取和估计出的关键信息。例如：

场景	原始观测	任务需要的状态
机械臂抓取	RGB-D 图像、关节角、夹爪读数	杯子六维位姿、末端位姿、抓取是否稳定
移动机器人导航	相机、激光雷达、IMU、轮速	机器人位姿、速度、可通行区域和障碍物
人形机器人行走	编码器、IMU、足底力传感器	基座姿态、质心运动、双脚接触状态

从观测到状态通常需要经过一条连续处理链：

传感器采样
    -> 时间同步与标定
    -> 图像和深度预处理
    -> 几何与语义感知
    -> 数据关联与目标跟踪
    -> 滤波融合或 SLAM
    -> 带时间戳和不确定性的任务状态

其中，感知主要从观测中提取物体、深度、特征和空间关系；状态估计进一步结合历史观测与运动模型，推断当前最可能的状态。二者没有绝对边界，但可以用一句话区分：感知回答“这一帧看到了什么”，状态估计回答“综合过去和现在，世界目前最可能是什么样”。

现代 VLA 可以直接从图像生成动作，但这并不意味着状态消失了。状态可能由独立模块显式维护，也可能编码在模型的隐表示和观察历史中。只要机器人需要处理遮挡、延迟、接触和连续运动，就必须以某种形式记住环境如何变化。

相机如何获得空间含义

相机是具身系统最常见的外部传感器。它能提供丰富的颜色、纹理和语义信息，但图像本质上是三维世界在二维平面上的投影。要根据像素恢复空间关系，首先要理解相机模型，并通过标定确定模型参数。

针孔相机模型

针孔相机模型用一个理想的小孔近似成像过程。设三维点在相机坐标系中的坐标为 $(X_C,Y_C,Z_C)$，投影后的像素为 $(u,v)$，则有：

$$\displaystyle Z_C\begin{bmatrix}u\\v\\1\end{bmatrix}=\mathbf K\begin{bmatrix}X_C\\Y_C\\Z_C\end{bmatrix},\qquad\mathbf K=\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}$$

展开后可以得到：

$$\displaystyle u=f_x\frac{X_C}{Z_C}+c_x,\qquad v=f_y\frac{Y_C}{Z_C}+c_y$$

$f_x$ 和 $f_y$ 是以像素为单位的焦距，$(c_x,c_y)$ 是主点，通常接近图像中心。机器人领域常用的相机光学坐标系约定是：$x$ 轴指向图像右侧，$y$ 轴指向图像下方，$z$ 轴沿光轴向前。

这个模型揭示了两个关键事实。第一，相机记录的是方向而不是完整三维位置：同一条视线上的多个点可能投影到同一个像素。第二，物体越远，投影尺寸越小，因此单张普通 RGB 图像通常无法仅靠几何关系确定绝对深度。

标定确定相机与机器人的关系

真实镜头并不完全符合理想针孔模型，广角镜头尤其会出现径向畸变，镜头装配还会带来切向畸变。因此，相机标定通常需要估计三类参数：

• 内参：$f_x$、$f_y$、$c_x$、$c_y$ 等成像参数；
• 畸变参数：描述直线在图像边缘发生弯曲的程度；
• 外参：相机坐标系与机器人基座、末端或世界坐标系之间的刚体变换。

常见内参标定方法会从多个角度拍摄尺寸已知的棋盘格或 ChArUco 标定板，建立标定板三维点与图像二维点的对应关系，再优化相机参数，使预测投影与实际检测点之间的重投影误差尽可能小。

外参解决的是“相机安装在哪里”。固定在工作区上方的相机通常需要估计 ${}^{B}\mathbf T_C$，将相机观测转换到机器人基座坐标系；安装在机械臂腕部的相机则需要通过手眼标定估计 ${}^{E}\mathbf T_C$。相机移动后，世界位姿会变化，但相机与末端之间的安装关系通常保持不变。

标定不仅是离线求出一组矩阵。图像分辨率、镜头焦距或安装位置改变后，原有参数可能失效；多相机、相机与 IMU 组合还需要校准传感器之间的时间偏移。空间外参正确但时间不同步，同样会让运动中的物体出现位置偏差。

RGB-D 与点云如何恢复三维结构

普通 RGB 图像告诉机器人物体“看起来是什么”，深度图则为每个有效像素提供距离信息。将二者对齐后，就得到 RGB-D 观察：RGB 负责颜色与语义，Depth 负责空间距离。

从深度像素到三维点

深度可以由双目视差、结构光、主动双目或飞行时间等方式获得。不同方案的测量原理不同，但输出通常可以表示为深度图 $D(u,v)$。设像素 $(u,v)$ 的深度为 $Z=D(u,v)$，结合相机内参即可将它反投影到相机坐标系：

$$\displaystyle X_C=\frac{(u-c_x)Z}{f_x},\qquad Y_C=\frac{(v-c_y)Z}{f_y},\qquad Z_C=Z$$

对所有有效深度像素执行这个计算，就能得到三维点集合。这里假设 $D(u,v)$ 表示点在相机 $z$ 轴方向上的坐标；如果传感器接口输出的是沿视线方向的量程，还需要先转换为轴向深度。比如：Open3D 的RGB-D 点云接口使用的也是这组反投影关系。

深度图并不天然可靠。透明、镜面和吸光材料可能导致深度缺失，物体边缘容易混入前后两个表面的测量，测量噪声通常还会随距离增大。RGB 与深度也可能来自两个不同光心，必须通过相机间外参完成配准，才能确保颜色像素与深度像素对应同一条空间射线。

点云如何进入机器人坐标系

点云是许多三维点的集合，每个点至少包含 $(x,y,z)$，还可以附带颜色、法向量和置信度。它比深度图更直接地表达空间几何，但本身通常没有明确的表面连接关系，也不会自动告诉机器人哪些点属于杯子、桌面或障碍物。

由深度相机生成的点云首先位于相机坐标系。若标定已知相机到世界坐标系的变换，则每个点可以通过齐次变换进入统一参考系：

$$\displaystyle {}^{W}\tilde{\mathbf p} = {}^{W}\mathbf T_C {}^{C}\tilde{\mathbf p}, \qquad \tilde{\mathbf p}=[x,y,z,1]^\mathsf T$$

当相机装在机械臂腕部时，${}^{W}\mathbf T_C$ 会随关节运动变化，需要结合正向运动学和手眼标定实时计算。只有把不同时刻、不同传感器的点转换到一致坐标系，机器人才能比较它们是否属于同一个物体或表面。

实际处理还常包含体素降采样、离群点去除、法向估计、平面分割和聚类。它们的目标不是让点云“更漂亮”，而是降低噪声与计算量，并提取规划真正需要的结构，例如桌面平面、可抓取物体和可通行区域。

从单帧感知到连续世界状态

单帧图像或点云只能描述某一瞬间。机器人运动、物体被遮挡或传感器短暂失效后，系统还需要维持连续估计。本章把位姿估计、滤波融合与 SLAM 看成三个逐步扩大的问题：先确定物体相对相机的位置，再融合时间上的多次观测，最后同时估计机器人轨迹与环境地图。

位姿估计确定物体在哪里

检测框只能说明物体位于图像中的哪个区域；机器人抓取通常还需要物体的六维位姿，也就是三维位置和三维方向。设物体坐标系为 $O$、相机坐标系为 $C$，位姿估计的目标是求出物体相对于相机的变换 ${}^{C}\mathbf T_O$。

常见方法可以分为两类：

• PnP（Perspective-n-Point）：根据物体模型上的若干三维点，以及这些点在图像中的二维像素位置，结合相机内参，求解物体相对于相机的旋转和平移。简单来说，PnP 是将“模型上的三维关键点”与“图像中的二维关键点”对齐，常与 RANSAC 配合排除错误匹配。
• ICP（Iterative Closest Point）：用于对齐两组具有重叠区域的三维点云。它反复寻找两组点云中的对应点，再计算使对应点距离最小的旋转和平移，直到结果收敛。ICP 依赖较好的初始位姿和足够的点云重叠，容易受到遮挡、噪声、重复结构和局部最优的影响。

可以简单理解为：PnP 解决三维模型与二维图像的对齐，ICP 解决两组三维点云的对齐。

学习模型可以直接预测关键点、对应关系或位姿分布，但几何约束仍然存在：输出必须明确参考坐标系、长度单位和旋转表示。对于圆杯、瓶子等对称物体，多个方向可能在视觉和任务上等价，因此评测也不能简单使用普通角度差。

滤波与融合让状态保持连续

同一个静止杯子的位姿在相邻帧中也可能轻微跳动。原因包括像素噪声、深度误差、遮挡和模型输出波动。状态估计不会简单平均所有测量，而是结合两类信息：

1. 运动模型根据上一时刻的状态和控制输入，预测当前状态；
2. 观测模型判断当前传感器测量与预测状态是否一致，并修正预测。

这一过程可以概括为“预测再更新”。若用 $\mathbf{x}{t}$ 表示状态、$\mathbf{z}{t}$ 表示观测，则贝叶斯滤波的核心关系是：

$$\displaystyle \begin{aligned}\overline{\mathrm{bel}}(\mathbf{x}_{t})&=\int p(\mathbf{x}_{t}\mid\mathbf{x}_{t-1},\mathbf{u}_{t-1})\,\mathrm{bel}(\mathbf{x}_{t-1})\,\mathrm{d}\mathbf{x}_{t-1}\\\mathrm{bel}(\mathbf{x}_{t})&=\eta\,p(\mathbf{z}_{t}\mid\mathbf{x}_{t})\,\overline{\mathrm{bel}}(\mathbf{x}_{t})\end{aligned}$$

直观地说，系统先根据运动规律给出一个带不确定性的预测，再根据新测量修正预测。测量稳定时，估计会更多相信传感器；测量噪声较大或暂时丢失时，估计会更多依赖运动模型，同时让不确定性逐渐增大。

卡尔曼滤波适用于线性模型和高斯噪声；扩展卡尔曼滤波（EKF）通过在当前估计附近线性化，用于处理常见的非线性机器人模型；当分布明显非高斯或存在多个可能状态时，还可以使用粒子滤波等方法。它们的共同重点不是输出一条绝对正确的轨迹，而是同时维护状态估计和不确定性。

多传感器融合利用的是不同传感器的互补性。IMU 频率高，直接测量角速度和比力，适合捕捉快速运动，但积分会产生漂移；相机、激光雷达和轮速可以提供环境或运动约束，却可能频率较低或受场景影响。融合之前必须统一坐标系、时间戳、单位和噪声模型，否则增加传感器反而可能让结果更差。

SLAM 同时估计位置与地图

移动机器人进入未知环境时，会遇到一个相互依赖的问题：要把传感器数据放进地图，必须知道机器人在哪里；要根据地图定位机器人，又必须先有一张地图。SLAM（Simultaneous Localization and Mapping）就是在运动过程中同时估计机器人轨迹与环境地图。一个典型 SLAM 系统可以分为三个部分：

• 前端：从连续图像、点云或激光扫描中提取特征，完成数据关联，并估计相邻时刻的局部运动；
• 后端：将机器人位姿、地图路标和观测关系组织成优化问题，获得整体更一致的轨迹与地图；
• 回环检测：识别机器人重新到达曾经访问过的位置，加入跨越较长时间的约束，修正长期累积漂移。

主要利用近期观测或局部地图连续估计增量运动，通常称为视觉里程计或激光里程计。它能够提供局部连续轨迹，但缺少全局约束时误差仍会逐渐累积。SLAM 通过地图约束、全局优化和回环检测提高长期一致性。Google Cartographer 的算法说明将系统区分为构建局部子图的 Local SLAM 和处理回环约束的 Global SLAM；ORB-SLAM3则展示了单目、双目、RGB-D 与视觉惯性等多种视觉 SLAM 配置。

地图也不是唯一形式。导航系统可能使用二维占据栅格，视觉 SLAM 常维护稀疏特征点，三维重建可能使用稠密点云、体素或表面模型，具身操作还需要在几何地图上叠加物体类别、位姿和可供性。SLAM 通常更擅长估计静态环境与机器人自身位姿，运动的人和物体仍需单独检测、跟踪并写入动态世界状态。

总结：从观测到世界状态

本文沿着“传感器观测到世界状态”的主线，介绍了机器人如何为原始信号赋予空间和时间含义：相机模型建立三维空间与二维像素的投影关系，标定连接不同传感器和机器人坐标系，RGB-D 与点云恢复环境几何，位姿估计确定物体的位置与方向，滤波和跟踪维持状态的时间连续性，SLAM 则为移动机器人提供长期一致的自身位姿与环境地图。

以“抓取杯子”为例，完整的状态生成过程可以概括为：

RGB-D 观测
    -> 标定与深度反投影
    -> 杯子分割与点云提取
    -> 估计相机坐标系中的杯子位姿
    -> 转换到机器人或世界坐标系
    -> 结合历史观测进行滤波与跟踪
    -> 输出结构化杯子状态

从传感器观测到世界状态，机器人需要通过标定、坐标变换、位姿估计、滤波和 SLAM，将离散且带噪声的信号转化为包含参考坐标系、时间戳和不确定性的连续状态。VLA 可以将部分感知与状态估计能力吸收到模型内部，但无法绕过空间几何、时间同步和物理约束；机器人只有持续判断自己在哪里、目标在哪里以及环境如何变化，才能安全地规划下一步动作。