3. 轨迹队列的数据结构设计

轨迹队列是运动控制器中连接“轨迹规划”与“实时插补”的关键中间层。规划器以较低频率（例如 1 kHz 或更低）向前方压入轨迹段，插补器则以固定周期（例如 1 kHz、2 kHz 甚至 4 kHz）从队列头部取出并消费。队列一旦设计不当，就会出现两类典型故障：一是入队/出队操作耗时抖动大，导致插补周期被拉长；二是内存访问不连续，缓存命中率低，在高速插补时产生不可预测的延迟。因此，数据结构选型不是“能用就行”，而是直接决定实时性与确定性。

三种候选结构的本质差异

环形缓冲区（Ring Buffer）：用一段连续内存加读、写两个索引实现 FIFO。入队时写指针前移并回绕，出队时读指针前移并回绕。它的核心优势是内存连续、索引运算为常数时间、无动态分配，非常适合“定长轨迹段、先进先出”的插补消费模型。缺点是容量固定，队列满时需要背压或覆盖策略。

链表（Linked List）：每个节点独立分配，通过指针串联。它天然支持任意长度、动态增删，适合轨迹段长度差异大、需要频繁插入或删除中间节点的场景。但节点内存分散，遍历时缓存不友好，且每次插入/删除涉及指针改写与潜在的内存分配，实时性抖动明显。

优先级队列（Priority Queue）：通常用二叉堆实现，按优先级（如截止时间、运动轴优先级、安全等级）出队。它适合多轴协同中“高优先级轨迹段必须先执行”的场景，但堆操作是 O(log n)，且同样存在内存跳跃，不适合作为主插补队列的高频消费结构。

选型对比：从实时性、缓存与工程复杂度出发

在运动控制器的典型工况下，插补周期为 1 ms，每个周期消费 1 个轨迹段，队列深度通常设计为 32～256 段。此时环形缓冲区的读写下标运算仅需几条指令，且连续内存使预取器高效工作，缓存命中率可达 95% 以上。链表在同样深度下，每次出队都要解引用指针，节点分散在不同 cache line，命中率可能降到 60% 以下，且插入时的 malloc/free 会引入不可控延迟。优先级队列则更适合作为“调度层”而非“消费层”：主队列仍用环形缓冲区保证 FIFO 实时性，另设一个小容量优先级堆处理急停、回零、安全覆盖等特殊段。

队列深度、内存布局与缓存友好性

队列深度不是越大越好。深度过小会导致规划器频繁等待，产生“饥饿”；深度过大则增加端到端延迟，使急停响应变慢。工程上常用经验公式：深度 ≥ 最大规划周期 / 插补周期 + 安全余量。例如规划周期 1 ms、插补周期 0.5 ms，则每毫秒消费 2 段，若规划器每 1 ms 批量压入 4 段，深度取 32 已足够；若规划器有 5 ms 的抖动，则深度至少 5×2+8=18，取 32 或 64 更稳妥。

内存布局上，环形缓冲区应保证每个轨迹段大小对齐到 cache line（通常 64 B），避免一个段跨越两条 cache line。读索引和写索引应分属不同 cache line，防止多核环境下伪共享。若控制器为单核，仍建议将索引与数据区分开，减少写回冲突。

链表若要用于轨迹队列，建议改用静态节点池 + 空闲链表：预分配 N 个节点，插入时从空闲链表取，删除时归还，避免运行时 malloc。但即便如此，节点分散仍会带来缓存缺失，因此只适合低频、非实时的轨迹管理，不适合 1 kHz 插补消费。

可参考的队列参数配置示例

总结来看，环形缓冲区是轨迹队列主结构的首选，链表适合非实时轨迹管理，优先级队列适合作为调度层补充。三者并非互斥，而是分层协作：主队列保实时与缓存，优先级堆保安全与抢占，链表保灵活与动态。理解这一分工，才能在工程中做出正确的数据结构选型。