4. 轨迹规划入门：关节空间与笛卡尔空间

轨迹规划，说白了就是让机械臂“怎么动”的问题。很多新手一上来就调PID，结果臂是稳了，但一动起来要么抖得像帕金森，要么路径歪得离谱。其实根源往往不在底层控制，而在轨迹规划没选对。

我个人习惯把轨迹规划分成两大类：关节空间规划和笛卡尔空间规划。这两者的区别，我刚开始也迷糊了好一阵子。

4.1 关节空间规划

关节空间规划，就是直接对每个关节的角度、速度、加速度做规划。你想想看，六轴臂就是六个独立的电机，每个电机从A点转到B点，怎么转最快、最稳？这就是关节空间规划要解决的。

4.1.1 梯形速度规划

梯形速度曲线，是最简单也最常用的规划方式。它分三段：匀加速、匀速、匀减速。形状像个梯形，所以叫这个名字。

// 梯形速度规划核心参数
float v_max = 0.5;   // 最大速度 (rad/s)
float a_max = 1.0;   // 最大加速度 (rad/s²)
float q_start = 0.0; // 起始角度 (rad)
float q_end = 2.0;   // 目标角度 (rad)

// 计算加速段距离
float d_acc = 0.5 * v_max * v_max / a_max;
// 如果总位移大于2倍加速段距离，则有匀速段
if (abs(q_end - q_start) > 2 * d_acc) {
    // 有匀速段
} else {
    // 只有加速和减速，没有匀速段
}

我在项目中遇到过一个问题：梯形规划在加速段和匀速段切换时，加速度会突变。虽然速度连续，但加加速度（jerk）是无穷大。这就导致机械臂在切换点会有明显的冲击感，尤其在重载时特别明显。

4.1.2 S型曲线规划

S型曲线，说白了就是让加速度也连续变化。它把梯形规划的加速段又细分成了加加速、匀加速、减加速三段。整个曲线更平滑，冲击更小。

嗯，这里要注意：S型曲线有7段！加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，但ROS里MoveIt!已经封装好了，你只需要设置最大速度和最大加速度就行。

// MoveIt!中设置S型曲线参数
move_group.setMaxVelocityScalingFactor(0.5);  // 速度缩放因子
move_group.setMaxAccelerationScalingFactor(0.3); // 加速度缩放因子
move_group.setPlanningTime(5.0);  // 规划超时时间

4.2 笛卡尔空间规划

笛卡尔空间规划，就是直接控制末端执行器在三维空间中的位姿。比如你要让焊枪沿着一条直线焊缝走，或者让吸盘沿着圆弧轨迹运动。这时候关节空间规划就不好使了，因为你不知道每个关节怎么转才能让末端走直线。

4.2.1 直线插补

直线插补，就是让末端从A点直线运动到B点。算法上其实不复杂：在起点和终点之间均匀插值，然后每插一个点就做一次逆运动学求解。

// 直线插补伪代码
for (float t = 0; t <= 1.0; t += step) {
    // 线性插值位置
    x = x_start + t * (x_end - x_start);
    y = y_start + t * (y_end - y_start);
    z = z_start + t * (z_end - z_start);
    
    // 姿态插值（用四元数球面线性插值）
    q = slerp(q_start, q_end, t);
    
    // 逆运动学求解关节角度
    joint_angles = ik_solve(x, y, z, q);
    
    // 发送给底层控制器
    send_to_controller(joint_angles);
}

你想想看，这里有个坑：如果路径上某个点逆运动学无解，整个规划就失败了。我记得有一次调试，臂在中间某个位置突然卡住，查了半天发现是那个点超出了工作空间。

4.2.2 圆弧插补

圆弧插补比直线复杂一些。你需要三个点来确定一个圆弧：起点、中间点、终点。或者指定圆心和半径。

实际项目中，圆弧插补常用于焊接、涂胶这类需要走曲线轨迹的场景。我做过一个手机壳涂胶项目，轨迹就是由多段圆弧拼接而成的。

// 三点确定圆弧插补
// 输入：p1(起点), p2(中间点), p3(终点)
// 输出：圆弧上的插值点序列

// 1. 计算圆心和半径
center = compute_circle_center(p1, p2, p3);
radius = distance(p1, center);

// 2. 计算起始角和终止角
start_angle = atan2(p1.y - center.y, p1.x - center.x);
end_angle = atan2(p3.y - center.y, p3.x - center.x);

// 3. 均匀插值角度
for (float theta = start_angle; theta <= end_angle; theta += step) {
    x = center.x + radius * cos(theta);
    y = center.y + radius * sin(theta);
    z = linear_interp(z1, z3, theta); // Z轴单独线性插值
}

4.3 时间最优规划

时间最优规划，说白了就是在满足电机扭矩、速度、加速度限制的前提下，让机械臂用最短时间走完轨迹。这本质上是一个优化问题。

实际工程中，我们很少从头写时间最优规划算法。ROS里常用的做法是：先用梯形或S型曲线做初步规划，然后通过调整速度缩放因子来逼近时间最优。

规划方法	平滑度	执行时间	适用场景
梯形速度	低（加速度突变）	较短	轻载、低速、简单搬运
S型曲线	高（加速度连续）	略长	重载、高速、精密装配
时间最优	中等	最短	节拍要求高的产线

我个人习惯的做法是：先用S型曲线保证平滑，然后逐步提高速度缩放因子，直到出现跟踪误差或抖动，再回调5%。这样既保证了平滑，又接近时间最优。

最后说一句：轨迹规划没有银弹。不同场景选不同方法，关键是要理解每种方法的优缺点。我见过有人非要用笛卡尔空间规划做码垛，结果逆运动学频繁失败，换成关节空间规划后问题迎刃而解。选对工具，比用好工具更重要。