机器人运动控制是机器人学中衔接感知、决策与执行的基石领域，其核心在于构建一套从底层伺服到高层行为规划的完整算法体系。该体系不仅要求对物理模型有深刻理解，还需融合现代计算与优化理论，以实现复杂动态环境下的鲁棒、高效与智能行为。本文旨在系统性地归纳并深入剖析机器人运动控制中的关键算法，呈现一个从经典理论到前沿研究的立体技术图谱，为相关领域的研究者与工程师提供一份结构化的参考。

---

一、基础数学与核心理论

1. 空间运动描述

机器人是一个多体系统，其运动描述是分析的起点。

• 刚体位姿描述：旋转矩阵 R∈SO(3)是描述刚体方向的标准数学工具，但其存在奇异性与冗余性。四元数 (qw,qx,qy,qz)因其计算高效、无奇异性（万向节锁问题）而被广泛用于姿态插值与滤波（如 IMU 数据融合）。欧拉角或RPY角直观，常用于任务定义与人类交互，但存在顺序依赖和奇异性。

• 齐次变换矩阵：T=[R0p1]∈SE(3)统一描述了刚体的位置 p和姿态 R，是实现连杆变换、坐标转换和视觉手眼标定的核心工具。

• 运动旋量与速度：对于运动学与动力学分析，更高效的方式是使用运动旋量 V=[ωT,vT]T描述刚体瞬时速度。伴随矩阵 (Adjoint Map) AdT实现了旋量在不同坐标系间的变换，这是现代机器人学（如 PoE 建模）的基础。

2. 运动学

运动学研究运动本身，不涉及力。

• 正运动学：通过指数积公式 (PoE) 或改进的 D-H 参数法（如 MDH, Craig 修正）建立从关节空间到末端执行器位姿的映射。PoE 法在建模连续体机器人或并联机构时更具优势。

• 逆运动学 (IK)：

  • 解析法：对具有 Pieper 准则（三个相邻关节轴相交或平行）的6自由度机械臂，可求得封闭解，计算高效、确定。这是工业机器人实时控制的基石。

  • 数值法：对于通用构型或冗余机器人，采用基于雅可比矩阵的迭代法。阻尼最小二乘法 (Δq=JT(JJT+λ2I)−1e)是处理奇异点的标准方案。近年来，基于二次规划 (QP) 的优化方法能更自然地融入关节限位、避障等约束。

• 微分运动学：几何雅可比矩阵 Jg(q)建立了关节速度与末端空间速度（线速度与角速度）的映射。解析雅可比则关联关节速度与末端位姿参数（如欧拉角）的导数。奇异点（rank(J)<6）意味着任务空间自由度的丢失，需通过任务优先级或零空间投影来处理。对于冗余机器人（n>6），其零空间 N(J)可用于优化次目标（如避关节极限、最小化能耗）。

---

二、轨迹规划算法

轨迹规划旨在生成满足运动学、动力学约束及任务需求的光滑、可行的时变轨迹。

1. 关节空间规划

直接在关节空间生成轨迹，避免了笛卡尔空间奇异点。

• 多项式插值：三次多项式可保证位置和速度连续；五次多项式可额外保证加速度连续，减少冲击。高阶多项式易产生“龙格现象”，通常分段使用。

• 梯形与S型速度曲线：工业中主流的位置指令生成方式。梯形速度规划（T型曲线）实现简单，但在加速度切换点存在跃变。S型速度规划（加加速度受限的七段曲线）通过限制加加速度，使加速度平滑变化，显著减小了对机械结构的冲击，是高精度、高速运动的标配。时间最优的S型曲线规划通常转化为带约束的优化问题求解。

2. 笛卡尔空间规划

保证末端执行器在任务空间按预定路径运动。

• 路径插补：直线插补和圆弧插补是CNC和工业机器人中的基础功能。对于复杂路径，采用B样条或NURBS曲线进行参数化，可实现高阶连续性。

• 姿态插补：简单的欧拉角线性插值会导致旋转轴“翻转”。球面线性插值 (SLERP) 在四元数空间进行，能保证恒定的角速度和最短旋转路径，是姿态平滑插值的标准方法。

3. 时间最优轨迹规划

在给定路径下，寻找满足执行器速度、加速度、加加速度乃至力矩约束的最快运动轨迹。

• 时间最优路径参数化 (TOPP/TOP-PRS)：将问题转化为沿路径弧长的一维规划。核心思想是构建“速度极限曲线”，并利用相平面法或数值积分求解。这是实时性要求高的场景（如四足奔跑、高速分拣）的关键预处理步骤。

---

三、动力学与控制算法

1. 动力学建模

描述运动与作用力/力矩之间的关系。

• 拉格朗日法：基于能量，系统性地推导出机器人动力学方程：M(q)q¨+C(q,q˙)q˙+g(q)=τ。该形式清晰展示了惯性矩阵 M、科氏力与向心力矩阵 C和重力项 g。适用于控制器设计和理论分析。

• 牛顿-欧拉法：基于力和力矩平衡，采用前向递推（计算速度和加速度）和后向递推（计算力和力矩）的递归算法。其计算复杂度为 O(n)，远低于拉格朗日法的 O(n4)，是实时动力学计算的实际工业标准。

2. 经典控制

• PID控制：在关节空间独立控制每个关节，结构简单，对模型精度要求低，是绝大多数机器人底层的伺服控制器。但其在高速、高动态或强耦合系统中性能受限。

• 计算力矩控制：又称逆动力学控制。利用动力学模型进行前馈补偿：τ=M(q)a+C(q,q˙)q˙+g(q)，其中 a=q¨d+Kd(q˙d−q˙)+Kp(qd−q)。该策略通过反馈线性化将复杂的非线性系统解耦为一组简单的双积分器，然后用PD控制进行跟踪。其性能严重依赖于模型准确性。

3. 高级控制算法

• 自适应控制：如 模型参考自适应控制 (MRAC)，在线估计未知或时变的动力学参数（如负载），以维持控制器在参数不确定性下的性能。Slotine 和 Li 提出的基于跟踪误差的自适应控制是经典方案。

• 鲁棒控制：滑模变结构控制 (SMC) 通过设计一个滑模面，使系统状态在有限时间内被吸引并保持在面上，对匹配不确定性具有强鲁棒性。但其固有的高频“抖振”现象可能激发未建模动态。

• 阻抗/导纳控制：实现与环境顺应交互的典范。阻抗控制调节机器人末端使其表现得像一个质量-弹簧-阻尼系统：F=Mdx¨+Bdx˙+Kd(x−x0)，它将位置作为输入，输出力。导纳控制则相反，根据力误差调整期望位置。二者选择取决于机器人的力控精度和带宽。

• 力/位混合控制：Raibert 和 Craig 提出的经典框架，将任务空间分解为位置控制子空间和力控制子空间。例如，在插孔任务中，轴向进行力控制，径向进行位置控制。其关键在于根据任务几何正确选择选择矩阵。

4. 基于模型的控制

• 反馈线性化：如上所述的计算力矩控制，是非线性系统控制的标准工具。

• 操作空间控制：Khatib 提出的框架，直接在任务空间进行动力学和控制律设计：F=Λ(q)a+μ(q,q˙)+p(q)，其中 Λ是操作空间惯性矩阵。该框架能自然地集成零空间投影以实现多优先级任务。

---

四、运动规划算法

在复杂环境中寻找从起点到目标的无碰撞路径或轨迹。

1. 路径规划

• 基于图搜索：将连续空间离散化为图。

  • A*：启发式搜索的黄金标准，在已知地图的全局规划中广泛应用。

  • D：增量式 A，适用于部分未知或动态变化的环境。

• 基于采样：

  • 快速探索随机树 (RRT)：适用于高维空间，概率完备。RRT* 是其渐近最优版本。

  • 概率路图法 (PRM)：适用于多查询场景，先构建路图，再查询。

• 基于优化：

  • 轨迹优化：将规划问题表述为带约束的优化问题，如直接转录法。能直接处理动力学约束和光滑性要求，是生成高质量轨迹的主流方法。CHOMP, STOMP, TrajOpt 等是代表性算法。

2. 避障规划

• 动态窗口法 (DWA)：适用于差分轮式机器人的局部避障，在速度空间中采样，并评估短时间内轨迹的安全性和趋近目标程度。

• 速度障碍法 (VO) 及其扩展（RVO, ORCA）：用于多机器人或人机共享空间的动态避障，通过计算速度集合来保证无碰撞。

• 模型预测控制 (MPC)：将局部避障建模为一个滚动时域优化问题。在每一控制周期，基于当前状态和预测模型，求解未来有限时域内的最优控制序列，并执行第一个控制量。MPC 能显式处理约束，是动态环境下实时运动规划与控制融合的强有力框架。

3. 任务与运动规划

• 分层规划框架：高层任务规划器（基于逻辑或 PDDL）生成符号化的动作序列，底层运动规划器为每个动作生成可行的几何路径。二者通过可行性检查和回溯进行交互。

• 逻辑几何规划：旨在更紧密地耦合符号推理与几何约束，是当前研究热点。

---

五、状态估计与感知

1. 传感器融合

• 卡尔曼滤波器族：状态估计的核心。

  • 扩展卡尔曼滤波 (EKF)：通过一阶泰勒展开处理非线性系统，是机器人定位与建图（如 EKF-SLAM）的长期主力。

  • 无迹卡尔曼滤波 (UKF)：使用 Sigma 点传递统计特性，精度通常优于 EKF，且无需计算雅可比。

• 粒子滤波 (PF)：适用于高度非线性和非高斯系统，如机器人 kidnapped 问题，但计算量较大。

• 互补滤波：融合高频的陀螺仪（短时准确）与低频的加速度计/磁力计（长时稳定），是无人机和 IMU 姿态估计中轻量高效的方案。

2. 同步定位与建图 (SLAM)

• 基于优化的 SLAM：将 SLAM 问题构建为一个位姿图优化问题，利用李代数在流形上进行求解。g2o, GTSAM, Ceres Solver 等开源库使其成为现代 SLAM 的标配，精度远高于基于滤波的方法。

3. 环境感知

• 点云处理：使用 PCL (Point Cloud Library) 进行滤波、分割、特征提取和配准，是构建 3D 环境模型的基础。

• 深度学习感知：卷积神经网络 (CNN) 已主导了 2D/3D 物体检测、语义分割等任务，为机器人提供了丰富的场景理解能力。

---

六、机器学习与优化方法

1. 模仿学习 (IL)

• 行为克隆 (BC)：将示教数据视为监督学习数据，直接学习状态到动作的映射。存在分布偏移问题。

• 逆强化学习 (IRL)：不直接模仿动作，而是从专家示教中反推出其潜在的奖励函数，再通过强化学习优化策略。能获得更鲁棒和泛化的策略。

2. 强化学习 (RL)

在机器人中，RL 通常用于学习复杂的、难以精确建模的操控技能。

• 深度确定性策略梯度 (DDPG)：适用于连续动作空间的 Actor-Critic 算法。

• 软演员-评论家 (SAC)：最大熵框架下的 RL 算法，鼓励探索，在机器人任务中表现出优异的稳定性和样本效率。

• 近端策略优化 (PPO)：通过裁剪等方式约束策略更新步长，易于调参，实践中最流行的 RL 算法之一。

• 仿真到现实 (Sim2Real)：通过在仿真中引入域随机化（随机化纹理、质量、摩擦等）来训练策略，以迁移到真实世界。这是当前解决 RL 数据效率和安全挑战的关键路径。

3. 优化算法

• 二次规划 (QP) 求解：是许多控制（如MPC）和规划（如轨迹优化）问题的核心子问题。OSQP, qpOASES 是高效的求解库。

• 序列二次规划 (SQP)：解决一般非线性约束优化问题的迭代方法。

---

七、多机器人系统

1. 协同控制

• 编队控制：基于一致性理论，使多个机器人保持特定几何构型。

• 冲突避免搜索 (CBS)：用于多智能体路径规划 (MAPF) 的最优或次优算法，通过解决智能体间的冲突来规划无碰撞路径。

---

八、前沿与进阶方向

1. 基于学习的控制

• 神经网络动力学模型：用深度网络学习复杂的系统动力学或环境交互模型，用于 MPC 的预测或规划。World Models 是一种代表性思路。

• 元学习：旨在学习“如何快速学习”，使机器人能用少量新任务数据快速适应新环境或新技能。

2. 优化与控制结合

• 微分动态规划 (DDP) / 迭代线性二次调节器 (iLQR)：基于动态规划的最优控制算法，能高效处理非线性系统，常用于离线轨迹生成和模型预测控制。

• 迭代学习控制 (ILC)：针对重复性任务，利用前次周期的误差信息来修正本次控制输入，以渐近提升跟踪精度。

3. 人机交互 (HRI)

• 物理人机交互 (pHRI)：通过自适应阻抗控制等方式，使机器人能安全、自然地与人进行物理协作。

• 共享控制：结合人的高层意图与机器的局部自主能力（如避障），共同完成操作任务。

---

学习与实践建议

1. 循序渐进：遵循“运动学→轨迹规划→动力学/控制→运动规划→感知/学习”的路径，夯实每一层基础。

2. 仿真优先：在 PyBullet, MuJoCo, Isaac Sim 或 Gazebo 中实现算法，进行安全、高效的验证。ROS (Robot Operating System) 是集成算法模块的事实标准中间件。

3. 深入开源：研究 MoveIt（运动规划）、OMPL（规划算法库）、Drake（MIT 的机器人控制与规划工具箱）、ros_control（控制器框架）等优秀开源项目的设计与实现。

4. 数学为本：线性代数、微积分、微分方程、优化理论和概率论是理解上述所有算法的共同语言。

5. 领域聚焦：算法需与具体机器人形态结合。足式机器人关注动态平衡与全身控制；无人机关注欠驱动控制；机械臂关注高精度轨迹与力控；移动机器人关注SLAM与导航。

总结与展望

机器人运动控制正经历一场深刻的范式融合。以 MPC 和优化理论为代表的模型驱动方法，与以深度强化学习为代表的数据驱动方法之间的界限日益模糊。混合模型（如将神经网络嵌入 MPC 预测模型）和离线优化与在线自适应相结合的框架，正成为解决复杂、不确定环境中机器人控制问题的新前沿。同时，随着硬件算力的提升和开源生态的成熟，这些先进的算法正加速从实验室走向真实的工业与服务场景。未来的挑战将更多地集中在安全性、可解释性、长期自主性以及与复杂动态环境的共生交互上。掌握这一系统的算法体系，是参与并推动这场变革的关键。