4. 轨迹规划基础：关节空间规划、笛卡尔空间规划、多项式插值

各位同学，欢迎来到轨迹规划这一章。

说实话，轨迹规划是整个人形机器人运动控制的「灵魂」所在。你想想看，就算你的动力学模型再准，控制算法再牛，如果规划出来的轨迹本身就不合理——比如加速度突变、路径穿模——那机器人跑起来肯定是一顿一顿的，甚至直接摔倒。

我个人习惯把轨迹规划分成两大流派：关节空间规划和笛卡尔空间规划。它们各有各的脾气，也各有各的坑。今天咱们就把这两条路都走一遍，顺便把最常用的多项式插值讲透。

4.1 关节空间 vs 笛卡尔空间：两条路，怎么选？

先问个问题：你希望机器人末端（比如手）走一条直线，还是希望每个关节转得平滑？

这两个目标，其实对应了两种不同的规划思路。

• 关节空间规划：直接在关节角度、角速度、角加速度上做规划。说白了，就是告诉每个电机「你从角度A转到角度B，中间怎么走」。优点是计算量小，不会出现奇异性问题。缺点是末端轨迹不可控——你没法保证手是走直线的。
• 笛卡尔空间规划：在末端执行器的位姿（位置+姿态）上做规划。比如让手从点P1直线运动到点P2。优点是末端轨迹直观可控。缺点是需要实时做逆运动学求解，计算量大，而且可能遇到奇异点。

我在项目中遇到过这样一个场景：让机器人端着一杯水走。如果用关节空间规划，末端路径是弧线，水很容易洒出来。换成笛卡尔空间规划，让杯子始终保持水平直线运动，问题就解决了。所以你看，选哪种规划，完全取决于你的任务需求。

4.2 多项式插值：平滑轨迹的「万能钥匙」

不管选哪种空间，最终都要解决一个问题：如何让运动平滑？

平滑意味着位置连续、速度连续、加速度连续。如果加速度突变，机器人就会抖动，电机也会受冲击。解决这个问题的经典方法就是多项式插值。

4.2.1 三次多项式：最基础的平滑

假设你只有一个起点和一个终点，要求起点和终点的速度都为0。那么最少需要几个参数？

位置和速度，一共4个约束条件。所以用三次多项式：

θ(t) = a0 + a1*t + a2*t² + a3*t³

四个系数a0~a3，正好由四个边界条件解出。嗯，这里要注意：三次多项式只能保证速度连续，加速度是跳变的。如果对加速度有要求，就得升级到五次多项式。

4.2.2 五次多项式：加速度也连续

如果你需要加速度也连续（比如高精度轨迹跟踪），那就用五次多项式：

θ(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵

六个系数，对应位置、速度、加速度各两个边界条件。我在做人形机器人步态规划时，就强制要求加速度连续，否则机器人迈步时会有明显的「顿挫感」。

4.2.3 多段轨迹的衔接

如果轨迹有多个中间点（比如从A到B再到C），就不能只用一段多项式了。需要分段插值，并且在衔接点处保证位置、速度、加速度连续。

我曾经踩过一个坑：分段插值时只保证了位置连续，忘了速度连续。结果机器人每到中间点就「卡」一下，像在打嗝。后来加上速度约束，才彻底解决。

多段轨迹的数学表达稍微复杂一些，但核心思想不变：每段多项式在衔接点处的各阶导数必须相等。

4.3 知识体系总览

下面这张图，是我自己梳理的轨迹规划知识框架。你可以把它当作本章的「地图」：

4.4 代码实战：五次多项式插值

光说不练假把式。下面给出一段C++代码，实现五次多项式插值。这是我在ROS2中实际用过的版本，稍作简化：

#include <vector>
#include <cmath>

struct TrajectoryPoint {
    double pos;      // 位置
    double vel;      // 速度
    double acc;      // 加速度
    double time;     // 时间
};

// 五次多项式系数计算
// 输入：起点位置、速度、加速度，终点位置、速度、加速度，总时间T
// 输出：系数a0~a5
std::vector<double> calcQuinticCoeffs(
    double q0, double v0, double a0,
    double q1, double v1, double a1,
    double T) {
    
    std::vector<double> coeffs(6);
    
    coeffs[0] = q0;
    coeffs[1] = v0;
    coeffs[2] = a0 / 2.0;
    
    double T2 = T * T;
    double T3 = T2 * T;
    double T4 = T3 * T;
    double T5 = T4 * T;
    
    coeffs[3] = (20.0*(q1 - q0) - (8.0*v1 + 12.0*v0)*T - (3.0*a0 - a1)*T2) / (2.0*T3);
    coeffs[4] = (30.0*(q0 - q1) + (14.0*v1 + 16.0*v0)*T + (3.0*a0 - 2.0*a1)*T2) / (2.0*T4);
    coeffs[5] = (12.0*(q1 - q0) - (6.0*v1 + 6.0*v0)*T - (a0 - a1)*T2) / (2.0*T5);
    
    return coeffs;
}

// 根据系数和时间t计算轨迹点
TrajectoryPoint evaluateQuintic(const std::vector<double>& coeffs, double t) {
    TrajectoryPoint pt;
    double t2 = t * t;
    double t3 = t2 * t;
    double t4 = t3 * t;
    double t5 = t4 * t;
    
    pt.pos = coeffs[0] + coeffs[1]*t + coeffs[2]*t2 + coeffs[3]*t3 + coeffs[4]*t4 + coeffs[5]*t5;
    pt.vel = coeffs[1] + 2.0*coeffs[2]*t + 3.0*coeffs[3]*t2 + 4.0*coeffs[4]*t3 + 5.0*coeffs[5]*t4;
    pt.acc = 2.0*coeffs[2] + 6.0*coeffs[3]*t + 12.0*coeffs[4]*t2 + 20.0*coeffs[5]*t3;
    pt.time = t;
    
    return pt;
}

4.5 两种规划的对比总结

对比项	关节空间规划	笛卡尔空间规划
计算量	小（直接对角度插值）	大（需要实时逆运动学）
末端轨迹可控性	不可控	可控（直线、圆弧等）
奇异点问题	无	有（需特殊处理）
适用场景	点到点运动、码垛	焊接、涂胶、避障
插值方法	多项式、梯形速度等	直线插补、圆弧插补

好了，这一章的内容就到这里。轨迹规划说难不难，说简单也不简单。关键是要理解每种方法的适用场景，以及背后的数学原理。多项式插值是基础中的基础，一定要亲手写一遍代码，跑几个例子感受一下。

可可精品资料库