一、问题的本质：为什么简单插值会让机械臂“走走停停”

在许多简易的机械臂编程模式中，工程师先示教一串路径点（via-points），控制器在相邻两点之间做直线或圆弧插值，并在每段轨迹的起点和终点都把速度规划为零。这种做法在几何上是正确的，但在时间维度上，速度曲线会呈现“锯齿状”：加速—减速—停止—再加速。

这种模式的代价是全方位的：

• 动力学层面：频繁加减速带来较大惯性力，容易激起机械结构共振，末端出现残余振动，直接损害定位精度；
• 电气层面：电机电流反复冲击，驱动器发热加剧，能耗上升；
• 机械层面：减速器与传动机构承受循环冲击载荷，长期磨损加速；
• 生产层面：每段轨迹都完整经历“起—停”过程，节拍时间被拉长，产能受损。

究其根源，是这种做法把“路径”（纯几何信息）与“轨迹”（状态随时间的演化）混为一谈，且没有在全局层面对整条轨迹做统一优化。要同时做到速度平滑与位置精确，正确思路是把轨迹规划表述为一个最优化问题：在“经过指定路径点”的目标约束下，最小化与平滑性相关的代价。

二、建模第一步：选择合适的状态空间

轨迹优化的第一步是确定状态向量，它直接决定了平滑的“阶数”：

状态向量	控制输入 u 的含义	平滑性来源
[x]	速度	位置连续、速度受限
[x, dx]	加速度	速度连续、加速度受限
[x, dx, ddx]	加加速度（jerk）	加速度连续、冲击最小

以最常用的 [位置, 速度] 二阶模型为例，按采样间隔 Δt 离散化后：

x_{t+1} = A · x_t + B · u_t
A = [ 1   Δt ]      B = [ Δt²/2 ]
    [ 0   1  ]          [  Δt   ]

其中 u 即加速度指令。这一离散线性模型是后续所有求解的基础；若选用三阶模型，矩阵按同样方式扩展即可。

值得注意的是，“可指定状态变量维度”不只是接口上的灵活性，它对应了不同应用对平滑性的物理要求：搬运重物或需要抑制末端残余振动时，往往要限制加加速度，宜选三阶模型；而对节拍敏感的轻载高速场景，二阶模型配合合适的权重即可满足要求。

三、把路径点变成“跟踪目标”：代价函数设计

这是整个方法的灵魂。将稀疏的 via-points 编码进一个时变的跟踪代价中：

J = Σ_t [ (x_t − x*_t)ᵀ · Q_t · (x_t − x*_t)  +  u_tᵀ · R · u_t ]

两项代价各司其职：

• 跟踪项：x*_t 是参考状态，仅在“应当经过某个路径点的时刻”取该点位姿，其余时刻 Q_t 置零（不设目标）。这一稀疏设计让轨迹在路径点之间自由“滑行”，而不是被强制贴着某条参考曲线运动。
• 控制项：R 全程惩罚控制量（加速度或加加速度）。u 越小，速度曲线越平缓，机械冲击越小。

这两项天然地编码了“位置精确”与“速度平滑”这对看似矛盾的目标，其相对权衡由 Q 与 R 的比值决定：加大跟踪权重，轨迹更贴点但更“急”；加大控制权重，轨迹更平缓但可能偏离路径点。工程上通常以“末端到位误差满足公差、加速度不超限”为整定准则。

对比传统做法更能看出优势：经典插值在每个路径点处施加硬约束（速度必须为零），相当于把局部决策强加给全局；而软目标允许规划器在全局视角下权衡——例如在误差公差范围内“圆滑地掠过”中间点，只在真正需要停靠的终点才把速度降为零。

四、LQT：线性二次跟踪的标准求解

经典 LQR 面向“调节到固定设定值”的问题，而 LQT（Linear Quadratic Tracking）把参考目标推广为随时间变化的序列，恰好匹配“稀疏路径点”的表达方式。

对于线性动力学与二次型代价，最优解可通过动态规划解析求得，核心是两遍扫描：

1. 后向传递：从终端时刻出发，按 Riccati 递推逐时刻计算价值函数的展开项与反馈增益 K_t；
2. 前向传递：从初始状态出发，用 u_t = −K_t·(x_t − x*_t) + 前馈项 逐步 rollout，得到完整的状态序列与控制序列。

结果具备三个特点：

• 解析解：无需迭代搜索，一次计算即得最优解（在给定模型与权重下）；
• 闭环形式：输出天然包含状态反馈增益，执行阶段对扰动具备一定鲁棒性，而不仅是开环时间表；
• 可定制：总时长（时间间隔）与轨迹长度（采样点数）均可作为输入自由指定，便于与底层控制周期对齐。

五、iLQR：向真实非线性动力学的推广

机械臂的真实动力学包含重力项、科氏力/离心项等非线性因素，在高速或力控场景下不可忽略。iLQR（iterative Linear Quadratic Regulator）的处理思路是“线性化—求解—回滚”的迭代：

1. 给定一条初始可行轨迹，在每个时刻对动力学做一阶泰勒展开、对代价做二阶展开，构造一个局部的 LQ 子问题；
2. 用与 LQT 相同的前后向传递求解该子问题，得到新的控制序列；
3. 将新控制回滚出新轨迹，若总代价下降则接受并进入下一轮迭代，直至收敛。

由于每轮只做局部近似，iLQR 得到的是局部最优解，但工程上已足够；且它继承了“输出反馈策略”的优点，可在执行时对模型误差与外部扰动在线补偿。它已成为动态运动、力控制乃至全身控制等任务中广泛使用的轨迹优化基线方法。

六、工程落地的六个要点

算法理论要转化为稳定的产线表现，还需在系统层面完成衔接：

1. 规划空间的选取：任务空间（笛卡尔位姿）规划直观、便于与视觉或工件坐标系对接，但需处理奇异位形；关节空间规划天然满足构型约束，适合搬运类任务。常见做法是先在任务空间给出路径点，经逆运动学映射到关节空间后再优化。
2. 权重整定流程：先固定 R，从较大的 Q 起步收紧到位误差，直至达到公差上限或加速度饱和；再微调两者比例，平衡节拍与平稳性。有条件时可对典型负载做离线参数扫描，形成整定表。
3. 硬约束的处理：加速度/加加速度限幅可在前向传递中做投影，或作为罚项加入代价（软约束）；关节限位可通过在限位附近设置高代价带实现。
4. 与底层控制器对接：优化输出的参考轨迹（位置/速度/加速度序列）应按底层控制周期重采样后下发；若底层为位置接口，需保证参考的速度、加速度连续，避免控制器内部微分运算放大噪声。
5. 时间参数化的分工：另一种流行架构是“路径—时间”解耦：先在几何上规划平滑路径，再单独优化时间缩放函数以逼近时间最优。它与 LQT 类方法互为补充，可按团队能力栈选择。
6. 数值与鲁棒性细节：采样间隔过小会导致递推病态，过大则平滑性下降，通常取底层控制周期的 1～5 倍；对接近奇异的位姿段，提前重新选择路径点比事后补救的成本低得多。

七、示意伪代码

为帮助理解流程，给出与上述公式对应的概念级伪代码（省略推导细节）：

# 以 [位置, 速度] 为状态、加速度为输入的 LQT 流程
dt = 0.02                            # 采样间隔
A, B = 构造二阶积分器转移矩阵(dt)
Q, R = 装配稀疏跟踪权重与控制权重     # Q 仅在路径点时刻非零
P = 终端代价的展开项
for t in 末时刻 -> 初始时刻:          # 后向传递
    K[t], P = Riccati递推(A, B, Q[t], R, P)
x = 初始状态
for t in 初始时刻 -> 末时刻:          # 前向传递
    u[t] = -K[t] · (x - 参考状态)    # 反馈 + 前馈
    x    = A · x + B · u[t]          # rollout 得到轨迹
输出：轨迹（位置、速度序列）与控制（加速度序列）

整个流水线封装后通常只需数十行核心代码——这正是该类方法在工程上受欢迎的原因：问题表达紧凑，求解成熟可靠。

八、小结

回到最初的问题：机械臂如何同时保证速度平滑与位置精确？答案不在某个单点技巧，而在于把运动问题描述成“稀疏路径点跟踪 + 控制量最小化”的统一优化问题：

• 稀疏的时变跟踪目标，保证轨迹在关键位置满足精度要求；
• 对控制量的全局惩罚，从源头消除走走停停；
• LQT 提供解析解，iLQR 将其推广到非线性动力学，覆盖从慢速精密操作到高速动态任务的完整光谱；
• 剩下的工程问题——权重整定、约束处理、与底层控制器的时间对齐——决定了理论优势能否兑现为产线上的节拍与良率。

掌握这套“建模—代价设计—求解—对接”的完整链路，机械臂运动控制系统便具备了可复用、可扩展的方法论基础。