机器人从“接收到任务”到“真实完成运动”，通常不是由一个单独算法完成，而是由环境感知、状态估计、路径规划、轨迹生成、反馈控制和安全监控等多个模块共同完成。

“路径规划—轨迹规划—轨迹跟踪”是理解机器人运动系统的一条经典主线，但需要先修正一个常见误区：

现代机器人系统中，这三个阶段并不总是严格分离。
对高速机器人、自动驾驶、四足机器人和动态避障系统，规划器往往会同时考虑位置、时间、速度、动力学和控制约束，即采用运动规划、时空规划、轨迹优化或模型预测控制等一体化方法。

目前的 ROS 2 Nav2 已将导航功能划分为全局规划器、局部控制器、路径平滑器、行为服务器和环境模型等模块；MoveIt 2 则支持 OMPL、CHOMP、STOMP 和工业轨迹生成器等多种规划管线。这说明工程系统通常不是简单的三段串联，而是带有重规划和安全监督的闭环架构。

---

一、理解运动控制前必须明确的基本概念

1. 工作空间与构型空间

1.1 工作空间

工作空间是机器人实际运动所处的物理空间。例如：

• 移动机器人在二维地面上的位置可表示为 ((x,y))；

• 差速轮机器人通常需要考虑位置和航向角，即 ((x,y,\theta))；

• 机械臂末端位姿通常包括三维位置和三维姿态，可以表示为齐次变换矩阵 (T\in SE(3))。

障碍物的几何形状通常最先在工作空间中定义。

1.2 构型空间

规划算法真正搜索的通常不是单纯的物理工作空间，而是机器人的构型空间，记为：C

其中每个点代表机器人完整的构型。

对于一个 (n) 自由度机械臂：

q=[q1​,q2​,…,qn​]T

就是构型空间中的一个点。

构型空间可以进一步分为：

C=Cfree​∪Cobs

其中：

•  C_free：无碰撞构型集合；

• C_obs：机器人与障碍物或自身发生碰撞的构型集合。

因此，机械臂路径规划并不是“让末端点绕过障碍物”这么简单，而是要保证整条机械臂在每个构型下都不与环境、自身以及附着物发生碰撞。

OMPL 将几何运动规划定义在状态空间或构型空间上，并提供 PRM、RRT、RRT*、BIT*、FMT* 等多种采样规划器；MoveIt 负责把机器人模型、碰撞检测和约束交给这些抽象规划算法。(OMPL)

---

2. 路径、轨迹与控制输入的区别

路径

路径只描述机器人“经过哪里”，可以写成：

q=p(s),s∈[0,1]

其中 (s) 是路径参数，而不是时间。路径包含空间几何信息，但通常不规定机器人何时到达某个位置。

轨迹

轨迹描述机器人“在什么时间到达什么状态”：

q=qd​(t)

通常还包括：

q˙​d​(t),q¨​d​(t),q...​​d​(t)

分别表示期望速度、加速度和 jerk，即加加速度。

控制输入

控制器根据期望轨迹和实际状态生成执行器命令：u(t)，不同机器人中的控制输入可能是：

• 电机电流；

• 关节力矩；

• 关节速度；

• 轮速；

• 推力；

• 液压压力；

• 位置设定值。

因此三者的核心关系是：路径：去哪里、轨迹：何时、以什么速度去、控制：实际怎样驱动机器人去

---

二、路径规划

1. 准确定义

路径规划是在状态空间或构型空间中，寻找从初始状态到目标状态的连续可行曲线，基础路径规划通常不显式考虑时间，但不应笼统地说它“完全不考虑机器人约束”。

---

2. 路径规划的输入

比较完整的输入包括：

2.1 环境模型

环境可以表示为：

• 二维占据栅格地图；

• 三维体素地图；

• 八叉树地图；

• 点云；

• 多边形地图；

• 拓扑图；

• 语义地图；

• 时空占据预测模型。

栅格地图中的每个单元通常具有：

• 空闲；

• 占据；

• 未知；

• 代价值。

工程上通常会对障碍物进行“膨胀”，使规划路径与障碍物保持安全距离。

2.2 机器人模型

包括：

• 几何外形；

• 运动学模型；

• 关节限制；

• 碰撞模型；

• 最小转弯半径；

• 是否允许倒车；

• 是否满足非完整运动约束。

2.3 起点与终点

它们可以是：

• 位置点；

• 完整位姿；

• 关节构型；

• 一个目标区域；

• 一个满足任务约束的状态集合。

2.4 代价函数

规划目标不一定只是最短距离，还可能包括：

J=w1​L+w2​Cobs​+w3​Ccurv​+w4​Cenergy​+w5​Crisk​

其中：

• L：路径长度；

• C_obs：靠近障碍物的代价；

• C_curv：曲率或不平滑代价；

• C_energy：能耗估计；

• C_risk：安全风险。

OMPL 的优化规划器支持路径长度、间隙以及长度与间隙加权组合等优化目标。(OMPL)

---

三、现代机器人系统中的完整运动控制逻辑

经典三阶段模型可以扩展为以下闭环系统。

第一层：任务定义

输入：

• 去哪里；

• 做什么；

• 完成时间；

• 精度要求；

• 安全要求；

• 接触要求；

• 能耗要求。

输出：

• 目标位姿；

• 目标区域；

• 操作约束；

• 任务阶段序列。

---

第二层：感知与环境建模

输入：

• 摄像机；

• 激光雷达；

• 深度传感器；

• 编码器；

• IMU；

• 力传感器。

处理：

• 定位；

• 障碍物检测；

• 地图构建；

• 动态目标预测；

• 语义识别；

• 状态估计。

输出：估计状态、环境地图和障碍物集合。

---

第三层：全局路径或运动规划

核心问题：

从起点到目标，是否存在满足约束的安全运动？

候选算法：

• Dijkstra；

• A*；

• Hybrid A*；

• State Lattice；

• PRM；

• RRT-Connect；

• RRT*；

• BIT*；

• CHOMP；

• STOMP；

• TrajOpt。

输出：p(s) 或一条尚未完全满足时间约束的候选轨迹。

---

第四层：路径后处理

处理内容：

• 删除冗余节点；

• shortcut 平滑；

• 曲率优化；

• 增加障碍物间隙；

• B 样条拟合；

• 连续碰撞复检；

• 运动学约束复检。

Nav2 独立设置 smoother server，支持信号滤波型平滑器和约束优化型平滑器；其约束平滑器可以同时优化路径长度、平滑性、障碍距离和曲率。

---

第五层：时间参数化与轨迹规划

处理内容：

• 确定持续时间；

• 计算速度；

• 计算加速度；

• 限制 jerk；

• 多关节同步；

• 检查力矩和功率；

• 生成控制周期上的轨迹点。

候选方法：

• 三次多项式；

• 五次多项式；

• B 样条；

• 梯形速度；

• S 曲线；

• 时间最优路径参数化；

• 直接配点；

• 轨迹优化。

输出：

qd​(t),q˙​d​(t),q¨​d​(t)

---

第六层：轨迹跟踪控制

控制器根据：

e=qd​−q

生成：u

候选方法：

• PID；

• 前馈—反馈；

• 计算力矩；

• Pure Pursuit；

• Stanley；

• LQR；

• MPC；

• 自适应控制；

• 滑模控制；

• 阻抗控制。

---

第七层：执行器与机器人本体

控制输入经过：

• 电机驱动器；

• 电流环；

• 速度环；

• 减速器；

• 机械传动；

• 机器人刚柔耦合结构。

最终产生实际运动：x(t)

需要考虑：

• 摩擦；

• 齿隙；

• 饱和；

• 时延；

• 柔性；

• 温度；

• 载荷变化；

• 电池电压变化。

---

四、最终逻辑框架

用户任务与目标
        │
        ▼
任务约束解析
目标位置 / 目标姿态 / 时间 / 精度 / 安全 / 接触要求
        │
        ▼
感知与状态估计
地图、障碍物、机器人位姿、关节状态、动态目标预测
        │
        ▼
构建规划问题
状态空间 + 起点 + 目标 + 碰撞模型 + 运动约束 + 代价函数
        │
        ▼
全局路径或运动规划
Dijkstra / A* / Hybrid A* / PRM / RRT / RRT*
CHOMP / STOMP / TrajOpt
        │
        ▼
路径验证与后处理
碰撞检测、节点简化、平滑、曲率检查、安全距离检查
        │
        ▼
轨迹规划与时间参数化
三次或五次多项式 / 样条 / 梯形速度 / S曲线
时间最优参数化 / 直接配点 / 动力学轨迹优化
        │
        ▼
生成期望轨迹
位置 qd(t)、速度 q̇d(t)、加速度 q̈d(t)、期望力或力矩
        │
        ▼
状态反馈与轨迹跟踪
PID / 前馈-反馈 / 计算力矩 / MPC
自适应 / 滑模 / 阻抗 / 移动机器人几何控制
        │
        ▼
执行器驱动
电流、力矩、速度、轮速、推力等
        │
        ▼
机器人实际运动
        │
        ▼
传感器反馈与状态估计
        │
        ├────────误差较小、环境不变────────┐
        │                                  │
        ▼                                  │
继续跟踪                               正常执行
        │
        └──出现动态障碍、模型失配、跟踪异常───┐
                                             ▼
                               局部调整 / 重规划 / 限速 / 停车
                                             │
                                             └──返回规划或控制层

---

五、算法选择的基本原则

场景	更合适的方法
小型二维静态地图	A*、Dijkstra、Theta*
阿克曼车辆或叉车	Hybrid A*、State Lattice
高维机械臂快速求可行解	RRT-Connect
固定环境、多次机械臂查询	PRM、Lazy PRM
希望逐步改善路径质量	RRT*、Informed RRT*、BIT*
需要平滑避障机械臂轨迹	CHOMP、STOMP、TrajOpt
简单关节点到点运动	三次或五次多项式
工业伺服快速运动	梯形速度或 S 曲线
强调低冲击、高平稳性	jerk-limited S 曲线
简单单轴或低耦合跟踪	PID
有较准确机械臂动力学模型	前馈—反馈、计算力矩
约束多、系统耦合强	MPC
参数随载荷变化	自适应控制或在线辨识补偿
存在显著有界扰动	鲁棒控制、滑模、扰动观测器
接触、装配和打磨	阻抗控制、导纳控制、力位混合控制

最终应把机器人运动控制理解为一个持续循环：

感知→规划→轨迹生成→反馈控制→执行→再感知

其中路径规划解决空间可达性，轨迹规划解决时间和可执行性，轨迹跟踪解决真实系统中的误差抑制；而安全监督和在线重规划使整个系统能够在环境变化、模型不准确和外部扰动存在时继续可靠运行。