轨迹规划是机器人运动控制的核心问题之一，它决定了机器人如何从初始状态平滑、高效地运动到目标状态。对于轮足机械臂系统，轨迹规划不仅要考虑运动学约束，还必须满足动力学限制、避障需求以及任务特定的优化目标。本章将深入探讨关节空间与笛卡尔空间规划的权衡、时间与能量优化的数学基础、动力学约束的处理方法，以及实时系统中的轨迹修正策略。通过学习本章内容，读者将掌握现代轨迹规划算法的理论基础和工程实现技巧。
轨迹规划的第一个关键决策是选择在哪个空间进行规划。关节空间规划直接在机器人的配置空间中工作，而笛卡尔空间规划则在任务空间（通常是3D位置和姿态）中进行。这个选择深刻影响着算法复杂度、计算效率和轨迹质量。
关节空间规划的优势：
| 速度和加速度约束易于施加：$ | \dot{q}_i | \leq \dot{q}_{max,i}$，$ | \ddot{q}_i | \leq \ddot{q}_{max,i}$ |
笛卡尔空间规划的优势：
设机器人有$n$个自由度，关节空间轨迹表示为$q(t) \in \mathbb{R}^n$，笛卡尔空间轨迹表示为$x(t) \in SE(3)$。两者通过正运动学映射关联：
\[x(t) = f_{kin}(q(t))\]笛卡尔速度与关节速度的关系由雅可比矩阵描述：
\[\dot{x} = J(q)\dot{q}\]其中$J(q) = \frac{\partial f_{kin}}{\partial q}$是位置依赖的雅可比矩阵。
在笛卡尔空间规划时，需要通过逆运动学或雅可比伪逆计算关节空间轨迹：
\[\dot{q} = J^{\dagger}(q)\dot{x} + (I - J^{\dagger}J)z\]这里$J^{\dagger} = J^T(JJ^T)^{-1}$是Moore-Penrose伪逆，$z$是零空间速度，可用于优化次要目标。
实践中常采用混合策略：在笛卡尔空间定义关键路径点，然后在关节空间进行插值。这种方法结合了两者优势：
笛卡尔空间路径点：x₀ → x₁ → x₂ → ... → xₙ
↓ IK
关节空间配置：q₀ → q₁ → q₂ → ... → qₙ
↓ 插值
连续轨迹：q(t), t ∈ [0, T]
路径点生成策略：
在实际应用中，路径点的选择直接影响轨迹质量。密集的路径点能更好地控制笛卡尔路径形状，但增加了逆运动学求解的计算负担。稀疏的路径点计算高效，但可能导致实际路径偏离期望。工程实践中常用的自适应采样策略基于路径曲率：
\[\Delta s = \min\left(\Delta s_{max}, \frac{\epsilon}{\kappa(s)}\right)\]其中$\kappa(s)$是路径曲率，$\epsilon$是允许的弦高误差。在直线段使用大步长，在曲线段自动加密。
逆运动学求解的连续性：
多解性是逆运动学的固有特征。对于冗余机械臂，每个笛卡尔位置可能对应无穷多个关节配置。为保证轨迹连续性，需要在相邻路径点间选择”最近”的解：
\[q_{i+1} = \arg\min_{q \in IK(x_{i+1})} ||q - q_i||_W\]其中$W$是权重矩阵，反映各关节的运动代价。对于非冗余机械臂，可能存在多个离散解（如肘部向上/向下），需要通过配置空间的连通性分析来选择。
插值方法的选择：
关节空间插值决定了轨迹的动力学特性。常用方法包括：
对于高速运动，推荐使用保证加加速度（jerk）连续的七次多项式或S型速度曲线，减少机械冲击。
时间最优问题可表述为：
\[\min_{q(t)} T\]subject to: \(q(0) = q_{start}, \quad q(T) = q_{goal}\) \(|\dot{q}_i(t)| \leq v_{max,i}, \quad \forall i, t\) \(|\ddot{q}_i(t)| \leq a_{max,i}, \quad \forall i, t\)
经典的解决方法是时间缩放法。首先规划几何路径$s \mapsto q(s)$，$s \in [0,1]$，然后优化时间参数化$s(t)$：
\(q(t) = q(s(t))\) \(\dot{q} = \frac{dq}{ds}\dot{s}\) \(\ddot{q} = \frac{dq}{ds}\ddot{s} + \frac{d^2q}{ds^2}\dot{s}^2\)
速度约束转化为： \(|\frac{dq_i}{ds}|\dot{s} \leq v_{max,i}\)
这导出最大允许速度： \(\dot{s}_{max}(s) = \min_i \frac{v_{max,i}}{|\frac{dq_i}{ds}|}\)
能量优化目标通常包括机械功和电能消耗：
\[E = \int_0^T \sum_i \tau_i(t)\dot{q}_i(t) dt\]其中$\tau_i$是关节扭矩，由动力学方程确定：
\[\tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q)\]能量最优问题的拉格朗日函数：
\[L = \dot{q}^T\tau + \lambda^T(constraints)\]通过变分法可得出最优性条件（Euler-Lagrange方程）。实践中常用的简化目标是最小化加速度平方和：
\[\min \int_0^T ||\ddot{q}(t)||^2 dt\]这产生平滑的轨迹，间接降低能耗。
实际应用中常需平衡时间和能量：
\[J = \alpha T + \beta \int_0^T ||\tau||^2 dt + \gamma \int_0^T ||\dddot{q}||^2 dt\]其中第三项（jerk）用于限制轨迹平滑度，减少机械冲击。
帕累托最优前沿：
多目标优化的本质是在相互冲突的目标间寻找平衡。帕累托最优解集形成一个前沿，任何解都无法在不恶化其他目标的情况下改进某个目标。对于时间-能量权衡，典型的帕累托前沿呈现凸曲线形状：
生成帕累托前沿的方法包括加权和法、ε-约束法和进化算法。加权和法通过改变权重系数扫描前沿：
\[J(\lambda) = \lambda T + (1-\lambda)E, \quad \lambda \in [0,1]\]但这种方法无法获得非凸部分的解。ε-约束法通过将一个目标作为约束来克服这个限制：
\[\min T \quad \text{s.t.} \quad E \leq \epsilon\]任务相关的权重选择：
权重系数的选择应根据具体应用场景：
实践中可通过机器学习从历史数据中学习最优权重，或使用自适应权重根据当前系统状态（如电池电量）动态调整。
层次化优化：
对于复杂的多目标问题，层次化方法更实用：
这种方法避免了权重选择的困难，且物理意义明确。
在高速运动或重载操作中，必须考虑完整的动力学约束。机器人动力学方程：
\[M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) + F_{friction}(\dot{q}) = \tau\]其中：
扭矩约束： \(\tau_{min} \leq \tau(t) \leq \tau_{max}\)
功率约束（考虑电机特性）： \(|\tau_i \cdot \dot{q}_i| \leq P_{max,i}\)
将连续轨迹离散化为N个配点：
\[q_k = q(t_k), \quad k = 0, 1, ..., N\]动力学约束在每个配点处强制满足：
\[M(q_k)\frac{q_{k+1} - 2q_k + q_{k-1}}{\Delta t^2} + C(q_k, \frac{q_{k+1} - q_{k-1}}{2\Delta t})\frac{q_{k+1} - q_{k-1}}{2\Delta t} + G(q_k) = \tau_k\]优化问题变为：
\[\min_{q_0,...,q_N, \tau_0,...,\tau_N} \sum_{k=0}^{N} L(q_k, \dot{q}_k, \tau_k)\]这是一个大规模非线性规划(NLP)问题，可用IPOPT、SNOPT等求解器处理。
将轨迹分段，每段内进行前向积分：
段1: q₀ → integrate → q₁'
段2: q₁ → integrate → q₂'
...
添加连续性约束： \(q_{i+1} = q_i', \quad i = 0, ..., N-1\)
相比直接配点法，多重打靶法：
原始的非凸问题可通过以下技术部分凸化：
1. 序列凸规划(SCP)： 在当前轨迹$q^{(i)}$附近线性化：
\[M(q) \approx M(q^{(i)}) + \sum_j \frac{\partial M}{\partial q_j}|_{q^{(i)}}(q_j - q_j^{(i)})\]序列凸规划的关键在于迭代求解一系列凸近似问题。每次迭代解决：
\[\min_{q,\tau} J(q,\tau) \quad \text{s.t. } \text{linearized constraints}\]| 收敛条件通常基于解的变化量：$ | q^{(i+1)} - q^{(i)} | < \epsilon$。为加速收敛，可使用拟牛顿更新估计海森矩阵，或采用内点法的暖启动策略。SCP的收敛性依赖于初始猜测的质量，实践中常用简化模型（如忽略动力学的运动学轨迹）生成初值。 |
2. 信赖域方法： 添加信赖域约束确保线性化有效：
\[||q - q^{(i)}||_{\infty} \leq \delta\]信赖域半径$\delta$的自适应调整至关重要。定义模型预测质量指标：
\[\rho = \frac{J(q^{(i)}) - J(q^{(i+1)})}{J_{model}(q^{(i)}) - J_{model}(q^{(i+1)})}\]其中$J_{model}$是线性化模型的目标函数。根据$\rho$调整信赖域：
这种自适应机制在非线性强的区域自动减小步长，在近似良好的区域加速收敛。
3. 凸松弛： 将非凸约束松弛为凸约束。例如，碰撞避免约束：
\[d(q) \geq d_{safe}\]可松弛为： \(d(q^{(i)}) + \nabla d(q^{(i)})^T(q - q^{(i)}) \geq d_{safe}\)
但简单的线性化可能过于保守。更精确的凸松弛技术包括：
二阶锥松弛（SOCP）： 对于球形障碍物，精确的避碰约束是： \(||p(q) - p_{obs}|| \geq r\)
可reformulate为二阶锥约束： \(\begin{bmatrix} r \\ p(q) - p_{obs} \end{bmatrix} \in \mathcal{K}_{SOC}\)
其中$\mathcal{K}_{SOC}$是二阶锥。
McCormick松弛： 对于双线性项$xy$，在区间$[x_L, x_U] \times [y_L, y_U]$内： \(w \geq x_Ly + xy_L - x_Ly_L\) \(w \geq x_Uy + xy_U - x_Uy_U\) \(w \leq x_Uy + xy_L - x_Uy_L\) \(w \leq x_Ly + xy_U - x_Ly_U\)
这将非凸的双线性约束转化为线性约束系统。
实时轨迹修正常采用模型预测控制(MPC)框架：
预测时域$H$的选择需要平衡：
基于误差的触发： \(||q_{actual} - q_{planned}|| > \epsilon_{position}\) \(||\dot{q}_{actual} - \dot{q}_{planned}|| > \epsilon_{velocity}\)
基于预测的触发： 预测未来违反约束的概率： \(P(violation|current\_state) > threshold\)
事件驱动触发：
完全重规划计算代价高，增量式方法更高效：
1. 局部修正： 只修改违反约束的轨迹段：
\[q_{new}(t) = \begin{cases} q_{old}(t), & t \notin [t_{start}, t_{end}] \\ q_{corrected}(t), & t \in [t_{start}, t_{end}] \end{cases}\]2. 弹性带方法： 将轨迹建模为弹性带，通过虚拟力场变形：
\[F_{total} = F_{goal} + F_{obstacle} + F_{smooth}\]3. 梯度优化： 沿约束违反的负梯度方向调整：
\[q^{(k+1)} = q^{(k)} - \alpha \nabla_{q} V(q)\]其中$V(q)$是违反度量函数。
1. 热启动： 使用上一时刻的解作为初始猜测：
\[q_{init}^{t+1} = shift(q_{solution}^t)\]热启动的效果取决于系统动态和采样频率。对于1kHz的控制频率，相邻时刻的最优解通常非常接近，热启动可将求解迭代次数减少50-80%。具体实现时，需要处理时域滑动：
新的初始猜测 = [q₁, q₂, ..., qₙ, qₙ]（丢弃q₀，复制qₙ）
对于周期性任务，可维护一个解的库，根据当前状态选择最接近的历史解作为初值。
2. 并行化：
并行化的粒度选择影响效率。细粒度并行（如矩阵运算）适合GPU，粗粒度并行（如多起点优化）适合多核CPU。对于轨迹优化，有效的并行化策略包括：
空间分解：将机器人分解为多个子系统（如基座、手臂），并行优化后通过拉格朗日乘子耦合：
\[L = \sum_i J_i(q_i) + \lambda^T(c_{couple}(q_1, q_2, ...))\]时间分解：将长时域问题分解为多个短时域子问题，使用ADMM（交替方向乘子法）协调：
\[q^{k+1}_i = \arg\min(J_i(q_i) + \frac{\rho}{2}||q_i - z^k + u^k_i||^2)\]场景并行：同时优化多个可能场景（如障碍物位置不确定），选择鲁棒解。
3. 自适应分辨率： 远离当前时刻的轨迹使用粗分辨率：
\[\Delta t(k) = \Delta t_{min} \cdot (1 + \beta \cdot k)\]这种非均匀离散化在保持近期精度的同时减少了决策变量数量。更高级的自适应策略包括：
基于误差的细化：监控离散化误差，在误差大的区域自动加密：
\[e_k = ||q(t_k) - \frac{q_{k-1} + q_{k+1}}{2}||\]若$e_k > \epsilon$，在$[t_{k-1}, t_{k+1}]$插入新节点。
多重网格方法：先在粗网格求解获得大致轨迹，然后逐级细化：
每级使用上级结果插值作为初值，总计算时间often少于直接细网格求解。
4. 问题简化技术：
模型降阶：在远离奇异性和约束边界时，使用简化动力学模型：
\[M_{reduced}\ddot{q} + G_{reduced} = \tau\]忽略科氏力、摩擦等次要项，误差通过反馈控制补偿。
约束聚合：将多个相似约束合并为单个保守约束：
\[\max_i(g_i(q)) \leq 0 \quad \text{代替} \quad g_i(q) \leq 0, \forall i\]稀疏性利用：轨迹优化的KKT系统通常具有带状稀疏结构：
\[\begin{bmatrix} H & & & G^T \\ & H & & G^T \\ & & \ddots & \vdots \\ G & G & \cdots & 0 \end{bmatrix}\]使用专门的稀疏求解器（如MA57）可将复杂度从$O(N^3)$降至$O(N)$。
Canadarm2是国际空间站(ISS)上的关键机械臂系统，长17.6米，7个自由度，用于空间站组装、载荷操作和宇航员辅助。其轨迹规划面临独特挑战：
1. 振动抑制轨迹设计
采用输入整形(Input Shaping)技术，设计零振动(ZV)轨迹：
\[u_{shaped}(t) = \sum_{i=1}^{n} A_i \cdot u(t - t_i)\]其中$A_i$和$t_i$根据系统固有频率$\omega_n$和阻尼比$\zeta$计算：
\(A_1 = \frac{1}{1 + K}, \quad A_2 = \frac{K}{1 + K}\) \(t_1 = 0, \quad t_2 = \frac{\pi}{\omega_d}\) \(K = e^{-\zeta\pi/\sqrt{1-\zeta^2}}\)
2. 分层规划架构
任务层：高级任务分解（抓取卫星、移动载荷）
↓
路径层：笛卡尔空间无碰撞路径（RRT*算法）
↓
轨迹层：时间参数化，考虑动力学约束
↓
控制层：PID + 前馈补偿
3. 碰撞检测优化
使用包围体层次结构(BVH)加速碰撞检测：
4. 自适应重规划
根据任务紧急度调整规划策略：
许多轨迹优化问题可转化为半定规划：
\[\begin{align} \min_{X} \quad & \text{tr}(CX) \\ \text{s.t.} \quad & \text{tr}(A_iX) = b_i, \quad i = 1, ..., m \\ & X \succeq 0 \end{align}\]其中$X \succeq 0$表示$X$是半正定矩阵。
例：最小能量轨迹
对于线性系统$\dot{x} = Ax + Bu$，最小能量控制问题可表述为：
\[\min_u \int_0^T u^Tu \, dt\]通过可控性格拉姆矩阵：
\[W_c = \int_0^T e^{At}BB^Te^{A^Tt} dt\]最优控制为： \(u^*(t) = B^Te^{A^T(T-t)}W_c^{-1}(x_f - e^{AT}x_0)\)
对于多项式系统，使用SOS技术验证轨迹可行性：
一个多项式$p(x)$是SOS的，当且仅当存在多项式$q_i(x)$使得：
\[p(x) = \sum_i q_i^2(x)\]这等价于存在半正定矩阵$Q$使得：
\[p(x) = z(x)^T Q z(x)\]其中$z(x)$是单项式基向量。
应用：障碍函数验证
验证轨迹$q(t)$避开障碍物：
\[B(q) = ||q - q_{obs}||^2 - r^2 > 0\]通过SOS分解证明$B(q(t)) > 0, \forall t \in [0, T]$。
使用SOS优化同时设计轨迹和反馈控制器：
\[\begin{align} \text{find} \quad & q(t), K(q) \\ \text{s.t.} \quad & V(q) - \epsilon||q||^2 \in SOS \\ & -\dot{V}(q, K(q)) \in SOS \\ & ||K(q)|| \leq u_{max} \end{align}\]其中$V(q)$是李雅普诺夫函数，保证闭环稳定性。
本章系统介绍了轮足机械臂系统的轨迹规划与优化技术。核心要点包括：
规划空间选择：关节空间规划计算高效但路径不直观；笛卡尔空间规划任务描述自然但需处理奇异性。混合策略结合两者优势。
优化目标权衡：时间最优追求效率，能量最优降低功耗，实际应用需多目标平衡。时间缩放法是处理速度/加速度约束的有效方法。
动力学约束处理：直接配点法和多重打靶法是两种主流数值方法。凸化技术（SCP、信赖域）可将非凸问题转化为可求解的凸问题序列。
实时性保证：MPC框架提供反馈修正能力。增量式重规划、热启动、并行化是提升实时性的关键技术。
高级优化技术：SDP和SOS优化提供了严格的理论保证，适用于安全关键应用。
关键公式回顾：
练习8.1 考虑一个2自由度平面机械臂，连杆长度分别为$l_1 = l_2 = 1$米。若要使末端从$(1, 0)$移动到$(0, 1)$，比较直线路径和圆弧路径在关节空间的表现差异。
| 练习8.2 给定关节约束$ | q | \leq 2$rad，$ | \dot{q} | \leq 1$rad/s，$ | \ddot{q} | \leq 2$rad/s²。计算从$q = 0$到$q = 1.5$的时间最优轨迹。 |
练习8.3 对于质量$m = 10$kg的单自由度系统，比较最小时间和最小能量轨迹的扭矩需求。设移动距离$d = 1$m，最大加速度$a_{max} = 1$m/s²。
练习8.4 设计一个轨迹规划器，使7自由度机械臂在避开障碍物的同时，利用冗余自由度最小化关节运动范围。建立优化问题并描述求解策略。
练习8.5 推导弹性关节机器人的最优轨迹条件。考虑关节弹性$k$和电机惯量$I_m$，建立包含振动抑制的轨迹优化问题。
练习8.6 对于空间机械臂（无固定基座），如何修改轨迹规划以保持系统角动量为零？给出约束方程和实现方法。
练习8.7（开放题）讨论如何将强化学习与传统轨迹优化结合，设计一个能够在线学习和改进的轨迹规划系统。考虑采样效率和安全性保证。