轨迹规划与控制¶

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

一、轨迹规划基础¶

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[发送关节指令]

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

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

3.4 工程实操：7 自由度仿人机械臂的轨迹规划流程¶

实际项目里，轨迹规划不是只生成一串点，而是要生成电机能执行、动力学能承受、不会撞到自己的运动。以 7 自由度仿人机械臂从当前姿态移动到抓取位姿为例，可以按下面流程做：

graph TD
    A[任务目标: 末端目标位姿] --> B{末端路径是否重要?}
    B -->|不重要| C[关节空间规划]
    B -->|重要| D[笛卡尔空间规划]
    D --> E[逐点 IK 得到 q 路径]
    C --> F[时间参数化]
    E --> F
    F --> G[速度/加速度/力矩检查]
    G --> H{是否满足约束?}
    H -->|否| I[放慢/改路径/调整姿态权重]
    I --> F
    H -->|是| J[下发 q qdot qddot 或位置序列]

1）先选规划空间¶

任务	推荐方法	原因
机械臂回 home、摆到预抓取位	关节空间规划	简单稳定，关节限位容易处理
末端必须走直线靠近物体	笛卡尔直线 + 姿态插值	避免末端绕路碰撞
拿着杯子保持竖直移动	笛卡尔规划，姿态权重大	姿态约束比关节路径更重要
快速大幅度换姿态	关节空间规划 + 碰撞检查	更容易控制速度和力矩

7 自由度臂不要默认所有任务都走笛卡尔直线。很多时候末端路径不重要，关节空间轨迹更平滑、更不容易遇到 IK 跳解。

2）笛卡尔规划时逐点求 IK¶

位置可以线性插值：

\[ p(s) = (1-s)p_0 + sp_f \]

姿态用 SLERP：

\[ R(s) = \text{SLERP}(R_0, R_f, s) \]

然后对每个 \(s_k\) 求 IK：

\[ q_k = \text{IK}(T(s_k), q_{k-1}) \]

这里一定要把上一帧的 \(q_{k-1}\) 作为下一帧 IK 初值，这样可以降低跳解概率，并让冗余自由度保持连续。

IK 失败时不要硬插

某个路径点 IK 失败，通常说明路径穿过奇异点、工作空间边界或碰撞区。优先改中间路径点、降低姿态权重或允许末端绕行，不要直接用上一帧关节角填过去。

3）做时间参数化¶

得到离散路径点 \(q_0,q_1,\ldots,q_m\) 后，还需要给它们分配时间，得到 \(q(t),\dot{q}(t),\ddot{q}(t)\)。最常见做法：

按每个关节的最大速度估计每段最短时间
用最大加速度约束把时间再放大
对每段用五次多项式或 LSPB 生成平滑轨迹
若有任一关节超速或超加速度，整体放慢

对于多关节同步运动，每段时间取所有关节需求时间的最大值：

\[ \Delta t_k = \max_i \left(\frac{|q_{k+1,i}-q_{k,i}|}{\dot{q}_{i,\max}}\right) \]

再乘一个安全系数，例如 \(1.2 \sim 2.0\)，给加速度和动力学留余量。

4）用动力学做最后验算¶

时间参数化完成后，把轨迹送入逆动力学：

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

逐点检查：

检查项	不满足时怎么改
关节位置限位	改 IK 零空间目标或换路径
关节速度限位	增大总时长
关节加速度限位	增大过渡时间，使用五次曲线
关节力矩限位	降低加速度、减少负载、换姿态
奇异性	增大阻尼、改肘部构型、绕开路径
碰撞距离	加中间点或启用避障规划

5）下发到控制器¶

常见下发接口有三种：

接口	下发内容	适用
位置序列	\(q_d\)	最简单，依赖底层位置环
位置 + 速度	\(q_d,\dot{q}_d\)	跟踪更平滑
位置 + 速度 + 加速度 + 力矩前馈	\(q_d,\dot{q}_d,\ddot{q}_d,\tau_{\text{ff}}\)	高速或高精度机械臂

对自研仿人机械臂，建议按“位置序列 → 位置速度 → 加速度与力矩前馈”的顺序逐步打开能力。每加一层前馈，都先在仿真中验证，再低速上实机。

四、关节伺服控制¶

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）
力控制	控制接触力	阻抗控制、力/位混合控制