4. 笛卡尔空间轨迹规划：直线插补、圆弧插补、样条曲线插补
各位，欢迎来到第四章。前面我们聊了关节空间的规划，说白了就是让每个关节自己算好怎么转。但实际干活的时候，你更关心的是——机器人末端到底走了个什么路径。
比如你要让机械臂画一条直线焊缝，或者沿着圆弧打磨一个曲面。这时候关节空间规划就不够直观了。你得在笛卡尔空间里，直接控制末端的位置和姿态。
这一章，我们就来拆解笛卡尔空间里最常用的三种插补方式：直线、圆弧、样条曲线。我个人习惯把这三者看作一个递进关系——直线是基础，圆弧是进阶，样条则是应对复杂曲面的利器。
核心认知：笛卡尔空间规划的本质，是在末端位姿的连续流形上，找到一条平滑且可达的路径。关节空间规划管的是“能不能动”，笛卡尔空间规划管的是“动成什么样”。
4.1 直线插补：最基础，也最容易踩坑
直线插补，名字听着简单——让机器人末端从A点直线走到B点。但这里有个坑：你规划的是直线，机器人实际走的可能不是直线。
为什么会这样？因为笛卡尔空间的直线，映射到关节空间就是一条复杂的曲线。如果你只给起点和终点的关节角度，让机器人自己跑，那末端轨迹大概率是弧线。
正确的做法是：在笛卡尔空间里密集采样中间点，然后逐点做逆运动学求解。
具体步骤我总结为三步：
- 位置插值：在起点P₁和终点P₂之间，按时间比例做线性插值。P(t) = P₁ + t * (P₂ - P₁)，t ∈ [0, 1]。
- 姿态插值：姿态用四元数表示，做球面线性插值（SLERP）。千万别用欧拉角直接插，否则你会看到机器人疯狂翻转——我吃过这个亏。
- 逐点逆解：对每个插值点，调用逆运动学算出关节角度，同时检查是否超限或接近奇异点。
我的经验：插值步长不是越密越好。步长太密，计算量大，而且容易在奇异点附近震荡。我一般先按固定步长粗插，然后在曲率大的地方自适应加密。说白了，就是“该省省，该花花”。
来看一段伪代码，帮你理解核心逻辑：
function LinearInterpolation(start_pose, end_pose, step_size):
// 提取位置和姿态
pos_start = start_pose.position
pos_end = end_pose.position
quat_start = start_pose.orientation // 四元数
quat_end = end_pose.orientation
// 计算总距离
distance = norm(pos_end - pos_start)
num_points = ceil(distance / step_size)
trajectory = []
for i = 0 to num_points:
t = i / num_points
// 位置线性插值
pos_i = pos_start + t * (pos_end - pos_start)
// 姿态球面插值
quat_i = slerp(quat_start, quat_end, t)
// 逆运动学求解
joint_i = inverse_kinematics(pos_i, quat_i)
// 检查可达性和奇异点
if is_valid(joint_i):
trajectory.append(joint_i)
else:
// 触发重规划或报错
handle_exception()
return trajectory注意：直线插补遇到奇异点时，末端速度会瞬间飙升。我曾经在调试焊接机器人时，就因为没做奇异点检测，结果机器人“嗖”一下甩出去，差点撞到夹具。所以，逆解之后一定要做速度校验。
4.2 圆弧插补：三点定乾坤
圆弧插补在工业场景里太常见了——弧焊、打磨、涂胶，到处都是圆弧轨迹。
实现思路也很直接：给定三个不共线的点，确定一个唯一的圆弧。然后在这段圆弧上均匀采样，生成插补点。
具体流程：
- 第一步：用三点P₁、P₂、P₃计算圆心O和半径R。这步是纯几何计算，初中数学水平就能搞定。
- 第二步：计算起始角和终止角，确定圆弧的扫掠范围。注意要区分顺时针和逆时针。
- 第三步：按角度步长或弧长步长，在圆弧上生成中间点。
- 第四步：姿态插值——圆弧的姿态变化通常也是平滑的，继续用SLERP。
这里有个细节：圆弧的圆心角超过180度时，你要明确告诉机器人走优弧还是劣弧。我见过有工程师忘了指定方向，结果机器人绕了个大圈，把工件给撞了。
| 参数 | 说明 | 典型值 |
|---|---|---|
| 圆心 O | 三点共圆的几何中心 | 由三点坐标计算 |
| 半径 R | 圆心到任意点的距离 | 由三点坐标计算 |
| 起始角 θ₁ | 起点相对于圆心的角度 | atan2(P₁ - O) |
| 终止角 θ₂ | 终点相对于圆心的角度 | atan2(P₃ - O) |
| 方向标志 | 0=顺时针, 1=逆时针 | 由用户指定 |
避坑指南：我曾经在写圆弧插补时，直接用角度等分来采样。结果在半径很大的圆弧上，末端线速度忽快忽慢。后来改成弧长等分，速度就均匀了。你想想看，角度等分对应的是角速度恒定，但线速度 = 角速度 × 半径，半径不变时没问题，但姿态变化会导致实际路径长度不均匀。
4.3 样条曲线插补：应对复杂曲面的终极武器
直线和圆弧能解决大部分规则路径。但遇到自由曲面——比如汽车覆盖件、手机外壳——你就得请出样条曲线了。
样条插补的核心思想是：用分段低次多项式拼接成一条光滑曲线。最常用的是三次样条和B样条。
为什么是三次？因为三次多项式能保证位置、速度、加速度都连续（C²连续）。机器人运动时，加速度连续意味着力不会突变，轨迹更平滑，对减速机也更友好。
我个人的经验是：B样条比三次样条更适合机器人。原因有三：
- 局部性：修改一个控制点只影响局部曲线，不会牵一发动全身。
- 凸包性：曲线始终在控制点构成的凸包内，方便做碰撞检测。
- 可扩展：可以轻松增加控制点来细化局部形状。
样条插补的工程实现，比直线和圆弧复杂一个量级。你需要处理：
- 节点向量生成：根据数据点分布，生成合理的节点序列。
- 控制点反算：给定型值点，反算出B样条的控制点。
- 曲线求值：用De Boor算法或Cox-de Boor递推公式，快速计算曲线上任意参数对应的点。
- 参数化速度规划：样条曲线的参数和实际弧长不是线性关系，需要做弧长参数化，否则速度会忽快忽慢。
我的建议：如果你刚开始接触样条插补，别急着自己造轮子。先用现成的库（比如Eigen、Ceres）把流程跑通，再根据实际需求做定制。我当年就是太自信，手写了一个B样条库，结果调试了两个月才稳定——得不偿失。
4.4 三种插补方式的对比与选型
说了这么多，到底什么时候用哪种？我整理了一张对比表，方便你快速决策：
| 特性 | 直线插补 | 圆弧插补 | 样条插补 |
|---|---|---|---|
| 路径形状 | 直线段 | 圆弧/整圆 | 任意自由曲线 |
| 计算复杂度 | 低 | 中 | 高 |
| 连续性 | C⁰（位置连续） | C⁰（位置连续） | C²（加速度连续） |
| 适用场景 | 点焊、搬运、简单涂胶 | 弧焊、圆周打磨、旋转装配 | 曲面加工、复杂焊缝、3D打印 |
| 姿态处理 | SLERP | SLERP | 四元数样条 / SLERP |
| 工程难度 | ★☆☆☆☆ | ★★☆☆☆ | ★★★★☆ |
选型建议很简单：能用直线别用圆弧，能用圆弧别用样条。不是样条不好，而是复杂度高、调试周期长。实际项目中，80%的轨迹用直线+圆弧就能搞定。
重要提醒：无论你用哪种插补方式，一定要在仿真环境里先跑一遍。我见过太多人直接上真机，结果轨迹不对，把夹具撞变形了。仿真不花钱，撞机才花钱。
4.5 工程落地：从算法到实机的关键几步
算法写好了，怎么让它真正跑在机器人上？这里有几个关键点：
- 插补点密度自适应：直线段可以稀疏采样，圆弧和样条曲率大的地方要加密。我一般用曲率阈值来控制采样密度。
- 前瞻规划：提前看未来N个插补点，预判速度变化，避免急停急启。这个N我通常取10~20，具体看控制周期。
- 奇异点规避：在逆解之前，先判断当前位姿是否靠近奇异点。如果靠近，就微调路径绕过去。
- 关节限位检查：每个插补点都要检查关节角度是否在允许范围内。超出限位时，要么放弃该点，要么触发重规划。
嗯，这一章的内容就到这里。直线、圆弧、样条——三种插补方式，对应着从简单到复杂的工程需求。掌握了它们，你就掌握了笛卡尔空间轨迹规划的半壁江山。