02 Robotics 算法栈

本章抓手

• Robotics 算法栈的主线是“传感器数据如何变成可靠动作”。
• 每个模块都要看输入输出、运行频率、不确定性和下游影响。
• Learning 可以增强很多模块，但物理闭环仍需要估计、规划、控制和系统约束。

它解决什么问题

Robotics 算法栈把原始传感器数据变成物理动作。它不是单个大模型，而是一组有明确接口的模块：感知提供环境信息，估计提供机器人状态，规划生成意图，控制把意图变成稳定动作，系统层保证它们实时协作。

从数据到动作的层级

Sensors -> Perception -> State Estimation -> World Model
-> Planning -> Trajectory Generation -> Control -> Actuation

学习 robotics 时，不要只问“哪个算法最强”，更要问：

• 它输入输出是什么。
• 它运行频率是多少。
• 它的不确定性如何表达。
• 它的错误会被下游如何放大。
• 它是否能在真实硬件上实时运行。

传感器建模 Sensor Modeling

传感器建模描述测量值如何由真实状态产生，以及噪声如何进入系统。

常见传感器：

• Camera：RGB、stereo、event camera。
• LiDAR：2D/3D point cloud。
• IMU：accelerometer, gyroscope。
• Encoder：wheel encoder, joint encoder。
• Force/Torque sensor：接触力和力矩。
• Tactile sensor：触觉图像、压力分布。

核心概念：

• Measurement model：z = h(x) + noise。
• Calibration：intrinsic / extrinsic calibration。
• Noise model：Gaussian noise, bias, drift, outlier。
• Time synchronization：多传感器时间对齐。

代表算法：

• Camera calibration, hand-eye calibration。
• IMU preintegration。
• Point cloud filtering and registration。
• Outlier rejection with RANSAC / robust loss。

学习关键词：sensor model, calibration, extrinsic, IMU bias, time synchronization。

状态估计 State Estimation

状态估计回答“机器人现在在哪里、速度是多少、姿态是什么、关键对象在哪里”。它是 robotics 中连接传感器和控制的核心。

核心数学对象：

• State x：position, orientation, velocity, bias, joint state。
• Observation z：传感器测量。
• Belief p(x | z_1:t)：给定历史观测后的状态分布。
• Motion model p(x_t | x_{t-1}, u_t)。
• Measurement model p(z_t | x_t)。

代表算法：

• Kalman Filter, Extended Kalman Filter, Unscented Kalman Filter。
• Particle Filter。
• Factor Graph Optimization。
• Bundle Adjustment。
• Pose Graph Optimization。

和其他模块的关系：

• 控制依赖状态估计的低延迟和稳定性。
• SLAM 依赖状态估计同时优化机器人轨迹和地图。
• 学习策略在真实机器人上也通常需要状态估计作为输入或监督信号。

学习关键词：Bayes filter, EKF, factor graph, pose graph, sensor fusion。

SLAM 与地图 SLAM and Mapping

SLAM 同时解决定位 localization 和建图 mapping：机器人不知道自己在哪，也不知道地图是什么，需要同时估计。

核心表示：

• Occupancy grid：栅格是否被占据。
• Point cloud map：点云地图。
• Feature map：路标、角点、线特征。
• TSDF / ESDF：用于三维重建和路径规划。
• Semantic map：带物体、类别和关系的地图。

代表算法：

• EKF-SLAM, FastSLAM。
• ORB-SLAM, visual SLAM。
• LOAM, LiDAR odometry and mapping。
• Cartographer, graph-based SLAM。
• Neural implicit mapping, NeRF / Gaussian Splatting inspired mapping。

学习关键词：SLAM, visual odometry, LiDAR odometry, loop closure, occupancy grid。

路径与运动规划 Planning

规划回答“应该怎么走或怎么动”。路径规划常处理几何可达性，运动规划还要处理机器人运动学约束，轨迹规划进一步加入时间、速度、加速度和动力学限制。

核心概念：

• Configuration space：机器人所有可能配置组成的空间。
• Free space / obstacle space：可行与碰撞区域。
• Feasibility：是否满足约束。
• Optimality：是否优化路径长度、能耗、时间、平滑度或安全距离。

代表算法：

• Graph search：Dijkstra, A*, D*, Hybrid A*。
• Sampling-based planning：PRM, RRT, RRT*, RRT-Connect。
• Optimization-based planning：CHOMP, STOMP, TrajOpt。
• Trajectory optimization：direct collocation, shooting methods。
• Task and Motion Planning：symbolic planning + geometric planning。

学习关键词：path planning, motion planning, configuration space, RRT, trajectory optimization。

控制 Control

控制回答“如何让真实机器人跟上期望状态或轨迹”。规划可以慢一些，控制通常必须高频、稳定、鲁棒。

核心概念：

• Feedback：根据误差实时修正动作。
• Stability：误差是否会收敛或有界。
• Tracking：跟踪目标轨迹。
• Robustness：模型不准或扰动存在时仍能工作。

代表算法：

• PID。
• LQR / iLQR。
• MPC。
• Nonlinear control。
• Impedance / admittance control。
• Whole-body control。

学习关键词：feedback control, stability, LQR, MPC, impedance control, whole-body control。

强化学习 Reinforcement Learning

RL 把机器人看作与环境交互的智能体。它适合处理难以手工建模的策略问题，但真实机器人上的数据成本和安全风险很高。

核心概念：

• State / observation。
• Action。
• Reward。
• Policy。
• Value function。
• Exploration。

代表算法：

• PPO, SAC, TD3。
• Model-based RL。
• Offline RL。
• Hierarchical RL。
• Safe RL。

和 robotics 的特殊关系：

• Sim-to-real 是关键难点。
• Reward design 很容易导致非预期行为。
• 控制频率、动作空间和安全约束决定算法能否落地。

学习关键词：robot reinforcement learning, sim-to-real, domain randomization, offline RL, safe RL。

模仿学习 Imitation Learning

模仿学习从专家演示中学习策略，常用于 manipulation、驾驶、遥操作数据和大规模机器人数据集。

代表方法：

• Behavior Cloning。
• DAgger。
• Inverse Reinforcement Learning。
• Diffusion Policy。
• Action chunking / sequence modeling。

关键问题：

• Distribution shift：策略犯错后进入专家数据没有覆盖的状态。
• Demonstration quality：演示质量决定策略上限。
• Action representation：末端位姿、关节速度、力控制或语言条件动作。

学习关键词：imitation learning, behavior cloning, DAgger, diffusion policy, robot demonstration。

视觉语言动作模型 Vision-Language-Action Models

VLA 模型试图把视觉、语言和动作统一起来，让机器人根据语言指令和视觉状态输出动作。它连接 foundation models 和 robot learning。

核心能力：

• Grounding：把语言指令绑定到场景物体和动作。
• Long-horizon reasoning：分解长期任务。
• Policy learning：从多机器人、多任务数据中学习动作。
• Generalization：跨对象、跨场景、跨任务迁移。

代表方向：

• Vision-language planning + low-level controller。
• End-to-end action prediction。
• Hierarchical policy with foundation model as planner。
• Large-scale robot dataset pretraining。

学习关键词：vision-language-action, robot foundation model, embodied AI, language-conditioned manipulation。

模块接口心智模型

• Perception 输出“世界里有什么”。
• State estimation 输出“机器人和关键对象现在是什么状态”。
• Planning 输出“下一段可行意图”。
• Control 输出“当前应该给执行器什么命令”。
• Learning 可以替代、增强或连接上述任何模块，但仍要服从物理和系统约束。