轨迹规划与控制

有了运动学和动力学模型，最终目标是让机器人按照期望轨迹精确运动——这需要轨迹规划生成期望运动、控制算法消除误差。

---

一、轨迹规划基础

1.1 路径 vs 轨迹

概念	含义	数学描述
路径（Path）	空间中的几何曲线，不含时间信息	\(q = f(s)\)，\(s\) 是路程参数
轨迹（Trajectory）	路径 + 时间规律（每个时刻在哪里）	\(q = q(t)\)，包含速度、加速度信息

轨迹规划的目标：生成 \(q(t)\)，使得运动满足位置、速度、加速度的边界条件，并且平滑可执行。

1.2 关节空间 vs 笛卡尔空间

空间	描述	优点	缺点
关节空间	对每个关节变量 \(\theta_i(t)\) 分别规划	简单，天然满足关节限位	无法直接控制末端路径形状
笛卡尔空间	对末端位姿 \((x, y, z, \alpha, \beta, \gamma)(t)\) 规划	可精确控制末端走直线/圆弧	每步需逆运动学，可能遇奇异

---

二、关节空间轨迹规划

2.1 三次多项式插值

问题：关节从 \(q_0\) 运动到 \(q_f\)，总时间 \(t_f\)，起止速度分别为 \(\dot{q}_0, \dot{q}_f\)。

设：

\[ q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 \]

4 个未知数由 4 个边界条件确定：

\[ q(0) = q_0, \quad q(t_f) = q_f, \quad \dot{q}(0) = \dot{q}_0, \quad \dot{q}(t_f) = \dot{q}_f \]

求解系数：

由 \(q(0) = a_0 = q_0\)，\(\dot{q}(0) = a_1 = \dot{q}_0\)，以及

\[ \begin{cases} a_0 + a_1 t_f + a_2 t_f^2 + a_3 t_f^3 = q_f \\ a_1 + 2a_2 t_f + 3a_3 t_f^2 = \dot{q}_f \end{cases} \]

联立得：

\[ a_0 = q_0, \quad a_1 = \dot{q}_0 \]

\[ a_2 = \frac{3(q_f - q_0) - (2\dot{q}_0 + \dot{q}_f) t_f}{t_f^2} \]

\[ a_3 = \frac{-2(q_f - q_0) + (\dot{q}_0 + \dot{q}_f) t_f}{t_f^3} \]

数值示例

设 \(q_0 = 10°\)，\(q_f = 60°\)，\(t_f = 2\text{s}\)，起止速度均为 0：

\[a_0 = 10, \quad a_1 = 0, \quad a_2 = \frac{3 \times 50}{4} = 37.5, \quad a_3 = \frac{-2 \times 50}{8} = -12.5\]

\[q(t) = 10 + 37.5 t^2 - 12.5 t^3 \quad (°)\]

\[\dot{q}(t) = 75 t - 37.5 t^2 \quad (°/\text{s})\]

最大速度在 \(t = 1\text{s}\)（中点）处：\(\dot{q}_{\max} = 37.5°/\text{s}\)。

2.2 五次多项式插值

三次多项式只能约束位置和速度，加速度不连续会导致力矩跳变。五次多项式增加加速度约束：

\[ q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \]

6 个系数由 6 个边界条件确定：\(q, \dot{q}, \ddot{q}\) 在起止点各一个。

多项式阶数	可约束条件	连续性保证	应用场景
三次	位置 + 速度	速度连续，加速度可能不连续	一般场景
五次	位置 + 速度 + 加速度	加速度连续	高精度/高速场景

2.3 多路径点的三次多项式

经过多个中间路径点 \(q_0, q_1, \ldots, q_m\) 时，在相邻点之间各用一段三次多项式，要求在中间点处速度和加速度连续。

中间点速度的选取方法：

• 指定速度：直接给出
• 连续性条件自动计算：要求前后段的加速度在中间点相等，联立方程组求解

2.4 用抛物线过渡的线性插值（LSPB）

Linear Segments with Parabolic Blends——工业机器人中最常用的轨迹类型。

基本思路：匀速段 + 抛物线过渡段（匀加速/匀减速）

  q
  │        ╱‾‾‾‾‾‾‾‾╲
  │      ╱              ╲
  │    ╱                  ╲
  │  ╱                      ╲
  │╱                          ╲
  └──────────────────────────── t
     加速     匀速       减速
    (抛物线)  (直线)    (抛物线)

三段的数学描述：

设总时间 \(t_f\)，过渡时间 \(t_b\)，加速度 \(\ddot{q}\)：

阶段	时间范围	位置	速度
加速段	\(0 \leq t \leq t_b\)	\(q(t) = q_0 + \frac{1}{2}\ddot{q} t^2\)	\(\dot{q} = \ddot{q} t\)
匀速段	\(t_b \leq t \leq t_f - t_b\)	\(q(t) = q_0 + V(t - \frac{t_b}{2})\)	\(\dot{q} = V\)
减速段	\(t_f - t_b \leq t \leq t_f\)	\(q(t) = q_f - \frac{1}{2}\ddot{q}(t_f - t)^2\)	\(\dot{q} = \ddot{q}(t_f - t)\)

其中匀速段速度 \(V\) 和过渡时间 \(t_b\) 满足：

\[ V = \ddot{q} \cdot t_b = \frac{q_f - q_0}{t_f - t_b} \]

给定 \(\ddot{q}\)，可以求出：

\[ t_b = t_f - \frac{1}{\ddot{q}} \cdot \frac{q_f - q_0}{t_f - t_b} \quad \Rightarrow \quad t_b = \frac{t_f}{2} - \frac{1}{2}\sqrt{t_f^2 - \frac{4(q_f - q_0)}{\ddot{q}}} \]

约束条件

• \(t_b < t_f / 2\)（否则没有匀速段，退化为纯抛物线）
• \(\ddot{q} > \frac{4(q_f - q_0)}{t_f^2}\)（加速度要足够大，否则无法在规定时间内完成运动）

数值示例

设 \(q_0 = 0°\)，\(q_f = 90°\)，\(t_f = 3\text{s}\)，\(\ddot{q} = 60°/\text{s}^2\)：

\[V = \frac{90}{3 - t_b}, \quad V = 60 t_b\]

解得 \(t_b = 0.634\text{s}\)，\(V = 38.0°/\text{s}\)。

三段运动：0~0.634s 加速，0.634~2.366s 匀速，2.366~3s 减速。

2.5 多路径点的 LSPB

当有多个路径点（via points）时，在每个中间点附近使用抛物线过渡，使机器人无需停下来即可平滑通过。

  q
  │      q₂──────╲
  │    ╱            ╲
  │  ╱     q₁──╲     ╲ q₃
  │╱  q₀                 
  └──────────────────── t

在中间点 \(q_k\) 附近，速度从前一段的 \(V_{k-1}\) 变为后一段的 \(V_k\)，抛物线过渡使速度平滑切换。

---

三、笛卡尔空间轨迹规划

3.1 直线轨迹

末端在笛卡尔空间中走直线：

\[ \mathbf{p}(s) = (1-s)\mathbf{p}_{\text{start}} + s \cdot \mathbf{p}_{\text{end}}, \quad s \in [0, 1] \]

\(s(t)\) 是时间参数化函数——可以用三次多项式或 LSPB 来规划 \(s\) 随时间的变化。

3.2 姿态插值

四元数球面线性插值（SLERP）：

\[ q(s) = \frac{\sin((1-s)\Omega)}{\sin\Omega} q_0 + \frac{\sin(s\Omega)}{\sin\Omega} q_1 \]

其中 \(\Omega = \arccos(q_0 \cdot q_1)\) 是两个四元数之间的角度。SLERP 保证姿态变化角速度恒定，且无万向节死锁。

3.3 笛卡尔规划的完整流程

graph TD
    A[给定起止位姿] --> B[位置: 直线插值]
    A --> C[姿态: SLERP 插值]
    B --> D[合成每个时刻的目标位姿]
    C --> D
    D --> E[逆运动学 → 关节角]
    E --> F[检查关节限位/奇异性]
    F --> G[发送关节指令]

笛卡尔空间规划的注意事项

• 每个时间步都需要做逆运动学（计算量大）
• 中间路径可能穿越奇异配置或工作空间边界
• 关节速度/加速度不一定平滑，需要事后验证

---

四、关节伺服控制

4.1 独立关节 PID 控制

最基础的控制策略：将每个关节看作独立的单输入-单输出系统，用 PID 控制器消除位置误差。

\[ \tau_i = K_{p_i} e_i + K_{i_i} \int e_i \, dt + K_{d_i} \dot{e}_i \]

其中 \(e_i = q_{d_i} - q_i\) 是第 \(i\) 个关节的位置误差。

graph LR
    A[期望 q_d] --> B((+))
    B -->|e| C[PID 控制器]
    C -->|τ| D[电机 + 关节]
    D -->|q| E[编码器]
    E --> B
    B -.->|"-"| E

PID 控制的局限

• 没有考虑连杆间的动力学耦合
• 在高速运动或变负载时性能下降
• 重力补偿不足——静止时也有稳态误差（除非积分项足够大）

4.2 计算力矩控制（CTC / 逆动力学控制）

利用完整的动力学模型进行前馈补偿，从根源上消除非线性耦合。

控制律：

\[ \tau = M(q)\mathbf{u} + C(q, \dot{q})\dot{q} + G(q) \]

其中辅助控制信号：

\[ \mathbf{u} = \ddot{q}_d + K_d(\dot{q}_d - \dot{q}) + K_p(q_d - q) \]

代入动力学方程后，闭环系统变为线性：

\[ \ddot{e} + K_d \dot{e} + K_p e = 0 \]

这是一个简单的二阶线性系统——通过调整 \(K_p, K_d\) 即可设定期望的响应特性。

为什么 CTC 有效？

CTC 本质上是用动力学模型做了非线性项的精确对消。模型越准，对消越干净，控制越好。模型误差大时需要结合自适应或鲁棒控制。

---

五、力控制

5.1 为什么需要力控制

纯位置控制在接触环境时会出问题：

• 刚性环境碰撞 → 巨大接触力，可能损坏工件/机器人
• 柔性装配需要控制施加的力，而非仅仅位置

典型应用：打磨、装配、擦拭、人机交互。

5.2 阻抗控制（Impedance Control）

让末端像一个弹簧-阻尼系统一样响应外部力：

\[ M_d(\ddot{x} - \ddot{x}_d) + B_d(\dot{x} - \dot{x}_d) + K_d(x - x_d) = F_{\text{ext}} \]

其中 \(M_d, B_d, K_d\) 是期望的惯量、阻尼和刚度——这些是设计参数，决定机器人的"柔顺程度"。

直觉理解

阻抗控制不试图精确控制位置或力，而是控制力与位移的关系。就像用弹簧连接末端——碰到障碍物时弹簧会让开，施加的力由弹簧刚度决定。

5.3 力/位混合控制（Hybrid Force/Position Control）

在不同方向上分别控制力或位置：

• 约束方向：控制力（如磨削时压紧工件的法向力）
• 自由方向：控制位置（如磨削的进给运动）

用选择矩阵 \(S\) 划分：

\[ \tau = J^T[S \cdot F_{\text{ctrl}} + (I - S) \cdot K_p(x_d - x)] \]

其中 \(S\) 是对角矩阵，对角线元素为 0（位控）或 1（力控）。

---

六、总结

graph TD
    A[任务目标] --> B[轨迹规划]
    B -->|"q_d, q̇_d, q̈_d"| C[控制器]
    C -->|τ| D[电机 + 关节]
    D -->|运动| E[机器人本体]
    E -->|传感器反馈| C
    F[动力学模型] -->|前馈| C

层级	功能	关键方法
轨迹规划	生成平滑的运动序列	三次/五次多项式、LSPB、笛卡尔直线 + SLERP
位置控制	跟踪期望轨迹	PID、计算力矩法（CTC）
力控制	控制接触力	阻抗控制、力/位混合控制