无锁数据结构与并发原语

本章定位：让控制、估计、规划、日志线程在不互相拖慢的前提下交换数据。 无锁编程不是追求“没有锁”的炫技，而是为实时路径建立可解释的等待边界和内存可见性规则。

前置自测

1. 数据竞争和逻辑竞争有什么区别？
2. std::atomic<int> 默认内存序是什么？为什么它通常比 acquire/release 更贵？
3. 单生产者单消费者队列为什么比多生产者多消费者队列容易做得快？
4. cache line 伪共享会造成什么性能现象？
5. 控制线程读取“最新状态”与读取“每一条消息”是同一个需求吗？

本章目标

学完本章后，你应该能解释 C++ 内存序的 acquire/release 语义。 你应该能根据数据语义选择双缓冲、三缓冲、SPSC 队列或 seqlock。 你还应该能为无锁结构写出压力测试，避免只在 x86 桌面上“看起来正确”。

---

3.1 并发问题的三层分类 ⭐

这一节解决的问题是：为什么“加 atomic”不能自动得到正确并发程序。

动机：机器人软件天然多线程

一个典型机器人进程同时包含：

线程	频率	数据	关注点
传感器接收	100 Hz 到 2 kHz	IMU、关节、点云	不丢关键数据
状态估计	100 Hz 到 1 kHz	位姿、速度、偏置	一致性
控制循环	500 Hz 到 2 kHz	状态、命令	不阻塞
规划线程	10 Hz 到 100 Hz	轨迹、约束	可被新目标替换
日志线程	尽力而为	调试记录	不影响控制

这些线程的需求不同。 有的需要“最新完整状态”。 有的需要“每条事件都处理”。 有的只需要“统计值大致正确”。 如果不先分类，就会把所有共享数据都塞进一个 mutex，最终控制线程被低优先级任务拖住。

三类问题

类别	典型错误	例子	正确工具
数据竞争	两线程同时读写非原子对象	一个线程写状态，另一个线程读状态	atomic、锁、双缓冲
可见性错误	标志位可见，但数据未必可见	ready=true 早于数据写入被观察	release/acquire
语义错误	数据无竞争但业务含义错	读到新位置和旧速度组合	整帧发布、seqlock

可见性错误最隐蔽。 代码在 x86 上可能长期正常，因为 x86 TSO 内存模型较强。 在 ARM 或 RISC-V 上，同样代码可能暴露乱序。

反面失败：原子标志位没有 release/acquire

#include <atomic>

struct SensorData {
    double q[12];
    double dq[12];
};

SensorData shared_data;
std::atomic<bool> ready{false};

void producerBad() {
    for (int i = 0; i < 12; ++i) {
        shared_data.q[i] = i;
        shared_data.dq[i] = 0.1 * i;
    }
    // 错误：relaxed 只保证 ready 自身原子，不保证 shared_data 写入对消费者可见。
    ready.store(true, std::memory_order_relaxed);
}

void consumerBad() {
    if (ready.load(std::memory_order_relaxed)) {
        // 在弱序平台上可能看到 ready=true，但 shared_data 仍是旧值或部分新值。
        // process(shared_data);
    }
}

正确写法使用 release/acquire 建立同步关系。

#include <atomic>

void producerGood() {
    for (int i = 0; i < 12; ++i) {
        shared_data.q[i] = i;
        shared_data.dq[i] = 0.1 * i;
    }
    // release：保证此写之前的普通写入不会被重排到 ready 发布之后。
    ready.store(true, std::memory_order_release);
}

void consumerGood() {
    if (ready.load(std::memory_order_acquire)) {
        // acquire：看到 ready=true 后，也能看到 producerGood 发布前写入的数据。
        // process(shared_data);
    }
}

本质洞察：atomic 的原子性只回答“这个变量会不会被撕裂读写”。 内存序回答“其他普通数据何时对另一个线程可见”。 无锁程序真正难的往往不是原子变量本身，而是原子变量与普通数据之间的因果关系。

练习

1. 把上面的 ready 改成 seq_cst，解释为什么正确但可能过重。
2. 在 ARM 平台或模拟弱序的工具中运行 relaxed 版本，观察是否可能出现旧数据。
3. 对一个“状态 + 时间戳”结构，解释为什么时间戳也必须属于同一次发布。

---

3.2 C++ 内存序：从规则到模式 ⭐⭐

这一节解决的问题是：怎样用最少的内存序规则覆盖机器人中最常见的数据交换。

内存序速查

内存序	保证	典型用途	机器人例子
relaxed	只保证该原子对象读写不撕裂	统计计数	丢包计数、调试计数
release	发布之前的写入	生产者发布数据	状态帧写完后发布索引
acquire	消费之后可见发布数据	消费者读取数据	控制线程读取状态帧
acq_rel	同时读改写并建立顺序	exchange、fetch_add	三缓冲交换索引
seq_cst	全局单一顺序	保守默认	初始化阶段或低频路径

不要把 relaxed 理解成“不安全”。 它适合不参与同步的计数器。 也不要把 seq_cst 理解成“总是正确选择”。 它更强，但可能隐藏你对同步关系的理解不足。

模式一：统计计数器

#include <atomic>

class DropCounter {
public:
    void increment() {
        // 只统计数量，不用它同步其他数据，因此 relaxed 足够。
        dropped_.fetch_add(1, std::memory_order_relaxed);
    }

    int value() const {
        return dropped_.load(std::memory_order_relaxed);
    }

private:
    std::atomic<int> dropped_{0};
};

模式二：发布完整帧

#include <array>
#include <atomic>

template <typename T>
class LatestFrame {
public:
    void publish(const T& frame) {
        // 写入非活动槽，避免覆盖消费者正在读取的最新槽。
        const int next = 1 - active_.load(std::memory_order_relaxed);
        frames_[next] = frame;
        // 发布新槽索引，使 frame 的普通写入对读取者可见。
        active_.store(next, std::memory_order_release);
    }

    T read() const {
        // acquire 与 publish 的 release 配对，保证读到完整帧。
        const int idx = active_.load(std::memory_order_acquire);
        return frames_[idx];
    }

private:
    std::array<T, 2> frames_{};
    std::atomic<int> active_{0};
};

这个双缓冲有一个隐含前提：写者两次 publish 之间，读者至少能完成一次完整 read。 如果写者连续快速 publish 三次，第三次计算出的 next 槽位可能正被读者读取，构成数据竞争。 当写者频率远高于读者、或写者存在突发时，应改用下面的三缓冲 TripleBuffer。

模式三：交换所有权

三缓冲正是为了消除上述频率限制而引入的。 它用 exchange 交换索引，使写者和读者各自独占一个槽位，第三个槽位作为交接区。 exchange 是读改写操作，需要同时关心读和写的顺序。

#include <array>
#include <atomic>

template <typename T>
class TripleBuffer {
public:
    T& writeBuffer() { return buffers_[write_idx_]; }

    void publish() {
        // 写者完成 writeBuffer 后，把 write_idx_ 与 ready_idx_ 交换。
        // acq_rel 保证写入先发生，也保证拿回的旧 ready_idx_ 可安全复用。
        write_idx_ = ready_idx_.exchange(write_idx_, std::memory_order_acq_rel);
    }

    const T& readLatest() {
        read_idx_ = ready_idx_.exchange(read_idx_, std::memory_order_acq_rel);
        return buffers_[read_idx_];
    }

private:
    std::array<T, 3> buffers_{};
    std::atomic<int> ready_idx_{1};
    int write_idx_{0};
    int read_idx_{2};
};

伪共享

无锁结构也会慢。 常见原因是生产者和消费者频繁写入的原子变量落在同一个 cache line。 这会导致缓存行在核心间反复迁移。

#include <atomic>
#include <cstddef>

struct alignas(64) PaddedAtomicSize {
    // 单独占用缓存行，避免与另一个热点索引产生伪共享。
    std::atomic<std::size_t> value{0};
};

class QueueIndices {
public:
    // 生产者频繁写 head，消费者只读取。
    PaddedAtomicSize head;
    // 消费者频繁写 tail，生产者只读取。
    PaddedAtomicSize tail;
};

对 1 kHz 控制，伪共享未必立刻致命。 对高频日志队列或点云流水线，它会造成明显抖动。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
概念	以为 atomic 自动同步所有数据	偶发读旧帧	缺少 release/acquire	发布索引用 release，读取索引用 acquire
编程	对所有原子用 seq_cst	性能差且意图不清	过强内存序掩盖设计	按同步模式选择
工程	head 和 tail 紧挨着	高并发下吞吐下降	伪共享	cache line 对齐
思维	在 x86 正常就认为正确	ARM 上崩溃	弱序架构暴露乱序	用标准内存模型推理

练习

1. 为 LatestFrame 写一个多线程测试：生产者写递增序号，消费者验证序号和数据内容一致。
2. 把 head 和 tail 放在同一 cache line 与分开 cache line 做基准测试，比较吞吐。
3. 找出一个可以用 relaxed 的统计量，并说明它为什么不参与同步。

---

3.3 队列、双缓冲、三缓冲与 seqlock ⭐⭐⭐

这一节解决的问题是：不同数据交换语义应该用哪种结构。

先问语义

需求	推荐结构	是否保留每条数据	读者是否可能重试	典型场景
最新完整状态	双缓冲	否	否	控制读估计状态
最新完整大帧	三缓冲	否	否	点云、地图切片
每条事件都处理	SPSC 队列	是，容量内	否	日志、命令事件
单写多读一致快照	seqlock	否	是	关节状态快照
多生产者事件	MPSC/MPMC 队列	是，容量内	否	多传感器事件汇聚

这张表比具体实现更重要。 误用结构会带来语义错误。 例如用双缓冲传日志会丢中间记录。 用队列传控制状态会让控制线程处理过期状态。

SPSC 队列骨架

#include <array>
#include <atomic>
#include <cstddef>

template <typename T, std::size_t Capacity>
class SpscRing {
public:
    bool push(const T& value) {
        // 只有生产者线程写 head，因此本线程读取可用 relaxed。
        const std::size_t head = head_.load(std::memory_order_relaxed);
        const std::size_t next = increment(head);
        if (next == tail_.load(std::memory_order_acquire)) {
            return false;
        }
        buffer_[head] = value;
        // 发布新元素，消费者看到 head 后即可读取 buffer_[head]。
        head_.store(next, std::memory_order_release);
        return true;
    }

    bool pop(T* value) {
        // 只有消费者线程写 tail，因此本线程读取可用 relaxed。
        const std::size_t tail = tail_.load(std::memory_order_relaxed);
        if (tail == head_.load(std::memory_order_acquire)) {
            return false;
        }
        *value = buffer_[tail];
        // 归还槽位，生产者看到 tail 后可以覆盖旧元素。
        tail_.store(increment(tail), std::memory_order_release);
        return true;
    }

private:
    static constexpr std::size_t increment(std::size_t x) {
        return (x + 1) % Capacity;
    }

    std::array<T, Capacity> buffer_{};
    alignas(64) std::atomic<std::size_t> head_{0};
    alignas(64) std::atomic<std::size_t> tail_{0};
};

这个队列牺牲一个槽位区分满和空。 Capacity 应大于最大突发量。 队列满时应选择丢弃、覆盖或触发降级，而不是在实时线程等待。

Seqlock 快照

Seqlock 适合单写者、多读者。 写者先把序号变成奇数，写数据，再把序号变成偶数。 读者如果读到奇数或前后序号不同，就重试。

#include <atomic>

struct JointSnapshot {
    double q[12];
    double dq[12];
};

class JointSeqlock {
public:
    void write(const JointSnapshot& s) {
        // 序号变奇数，标记快照正在更新；acquire 保证后续数据写入不会被重排到此操作之前。
        seq_.fetch_add(1, std::memory_order_acquire);
        snapshot_ = s;
        // 序号变偶数，标记快照处于稳定状态；release 保证之前的数据写入对读者可见。
        seq_.fetch_add(1, std::memory_order_release);
    }

    bool read(JointSnapshot* out) const {
        for (int attempt = 0; attempt < 3; ++attempt) {
            // 读取前后两次序号一致，才说明拷贝期间没有写者打断。
            const auto before = seq_.load(std::memory_order_acquire);
            if (before & 1U) {
                continue;
            }
            JointSnapshot copy = snapshot_;
            const auto after = seq_.load(std::memory_order_acquire);
            if (before == after) {
                *out = copy;
                return true;
            }
        }
        return false;
    }

private:
    mutable std::atomic<unsigned> seq_{0};
    JointSnapshot snapshot_{};
};

Seqlock 的读者可能重试。 因此它不适合强硬实时中不可容忍重试的路径，除非限制尝试次数并有降级策略。

ABA 问题

无锁栈和自由链表常遇到 ABA。 线程看到指针从 A 变成 B 又变回 A，以为没有变化，但 A 代表的对象可能已经被释放并重新使用。 机器人工程中，如果不是必须写通用 MPMC 结构，优先使用成熟库。 自写 SPSC、双缓冲、seqlock 已足以覆盖多数实时控制路径。

本质洞察：无锁不是一个单一技术，而是一组受限场景下的协议。 场景越受限，协议越简单、越可靠、越容易测试。 单生产者单消费者不是缺点，而是工程上主动收窄问题边界。

练习

1. 说明为什么 SPSC 队列不能直接让两个生产者同时 push。
2. 用 seqlock 读取 12 个关节位置和速度，统计重试次数，并解释写者频率升高时重试如何变化。
3. 为日志队列设计“满队列策略”：丢最新、丢最旧、计数后丢弃，比较三者对调试的影响。

---

3.4 机器人案例：规划到控制、估计到控制、控制到日志 ⭐⭐

这一节解决的问题是：把抽象并发原语落到机器人系统的数据流。

规划到控制

规划线程低频输出轨迹。 控制线程高频读取当前最新轨迹。 控制线程不应该等待规划线程。 因此适合双缓冲或 RealtimeBuffer 风格结构。

数据	写频率	读频率	推荐
速度指令	10 到 100 Hz	1 kHz	双缓冲
MPC 轨迹	20 到 50 Hz	500 Hz	双缓冲
单个急停事件	低频	1 kHz	atomic 标志 + release/acquire
用户命令历史	低频	非实时	队列

估计到控制

估计状态必须整帧一致。 不要把 q、dq、base pose、timestamp 分开多个 atomic 发布。 否则控制线程可能读到新 q 和旧 dq 的组合。

正确做法是把它们放入一个结构体并整帧发布。

控制到日志

日志不应反向影响控制。 实时线程 push 固定大小记录。 日志线程尽力 pop。 队列满时实时线程丢弃记录并增加计数。

struct ControlTrace {
    int64_t stamp_ns;
    double solve_time_us;
    double torque_norm;
    int status_code;
};

using TraceQueue = SpscRing<ControlTrace, 8192>;

void realtimeTracePush(TraceQueue& q, const ControlTrace& trace, DropCounter& drops) {
    if (!q.push(trace)) {
        // 日志队列满时只计数，不阻塞实时控制线程。
        drops.increment();
    }
}

PX4 uORB 的启发

PX4 的 uORB 把发布订阅做成轻量共享数据通道。 关键思想不是“复制 ROS2”，而是理解嵌入式实时系统如何看待消息：

1. 话题是固定类型。
2. 订阅者读取最新数据或检测生成计数。
3. 发布路径尽量短。
4. 中断上下文也可能发布，因此不能依赖复杂阻塞。

这个思想对 ROS2 控制器也有启发。 高频控制路径内部不一定要走完整 DDS 通信。 同进程内可以用更受限、更可控的数据结构。

练习

1. 为“规划轨迹到控制器”选择数据结构，并说明为什么不是 MPMC 队列。
2. 设计一个控制 trace 记录结构，要求固定大小、无 std::string、可被二进制写盘。
3. 把一个机器人状态拆成多个 atomic 的设计改成整帧发布，说明修复了什么一致性问题。

---

3.5 测试无锁结构 ⭐⭐

这一节解决的问题是：无锁结构如何验证，而不是靠肉眼检查。

测试维度

测试	目的	方法
单线程边界	满、空、环绕	小容量队列穷举
多线程压力	顺序和丢包	生产递增序号
内存序	弱序可见性	ARM/RISC-V 或专用工具
性能	延迟尾部	记录 p99 和最大值
退化策略	满队列处理	人为阻塞消费者

递增序号测试

#include <atomic>
#include <thread>
#include <vector>
#include <gtest/gtest.h>

TEST(SpscRingTest, PreservesOrderForSingleProducerSingleConsumer) {
    SpscRing<int, 1024> queue;
    std::atomic<bool> done{false};
    std::vector<int> received;
    received.reserve(10000);

    std::thread producer([&] {
        for (int i = 0; i < 10000; ++i) {
            while (!queue.push(i)) {
                // 测试代码可以等待；实时代码不能在这里无限自旋。
                std::this_thread::yield();
            }
        }
        done.store(true, std::memory_order_release);
    });

    std::thread consumer([&] {
        int value = 0;
        while (!done.load(std::memory_order_acquire)) {
            if (queue.pop(&value)) {
                received.push_back(value);
            } else {
                std::this_thread::yield();
            }
        }
        while (queue.pop(&value)) {
            received.push_back(value);
        }
    });

    producer.join();
    consumer.join();

    ASSERT_FALSE(received.empty());
    for (std::size_t i = 1; i < received.size(); ++i) {
        EXPECT_GT(received[i], received[i - 1]);
    }
}

这段测试故意强调一个点：测试里的等待策略不能照搬到实时代码。 实时代码要有有界失败策略。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
测试	只测单线程 push/pop	上机后乱序	没测并发可见性	加多线程递增序号
测试	只看吞吐	仍有延迟尖峰	无锁结构也会抖动	记录尾部延迟
概念	认为 lock-free 等于 wait-free	偶发长时间重试	lock-free 保证整体前进，不保证单线程步数	实时路径优先 wait-free 或有界尝试
工程	自写 MPMC 队列	难以验证 ABA 和回收	问题过宽	优先成熟库或收窄为 SPSC

练习

1. 扩展上面的测试：记录消费者空转次数，并解释它与队列容量和生产速度的关系。
2. 对 SPSC 队列记录每次 push 的耗时最大值，观察消费者暂停时的退化行为。
3. 用 ThreadSanitizer 检查一个故意非原子共享变量的示例，理解数据竞争报告。

---

3.6 内存模型的因果链：从 happens-before 到发布协议 ⭐⭐⭐

这一节解决的问题是：如何把“某个线程写了数据”严格推理成“另一个线程一定能读到这组数据”。

为什么需要因果链

回顾前面两节：std::atomic 只保证原子对象本身不会被撕裂读写。 但机器人系统真正共享的通常不是一个整数，而是一整帧状态、一个轨迹段、一个参数包。 这些普通对象不能靠“旁边有一个 atomic”自动变安全。

把内存模型想成实验室门禁。 普通写入像把工具放进柜子。 release store 像给柜子贴上“已整理完成”的封条。 acquire load 像另一个人看到封条后才打开柜子。 如果没有封条，对方可能看到“有人来过”，却不保证柜子里的工具已经按同一批次摆好。

术语	直觉	工程含义
sequenced-before	单线程内的程序顺序	同一函数内先写数据后写标志
synchronizes-with	release 与 acquire 的跨线程握手	发布者与读取者建立可见性
happens-before	因果顺序的传递闭包	读者能看到发布者之前完成的普通写入
modification order	同一原子对象的修改总序	所有线程对同一个 atomic 的写入顺序一致

内存模型难，是因为它不是“CPU 会不会乱序”这么简单。 标准关心的是：在所有合法优化、所有硬件架构、所有编译器转换之后，程序是否仍有定义良好的跨线程因果关系。 如果只靠在 x86 桌面上试跑，得到的是某个平台的经验，而不是 C++ 程序的正确性证明。

release/acquire 的最小同步图

发布一帧数据时，至少需要三条边。

1. 生产者线程内：普通数据写入 sequenced-before 发布索引。
2. 发布索引：release store synchronizes-with 消费者的 acquire load。
3. 消费者线程内：读取索引 sequenced-before 读取普通数据。

把三条边连起来，就得到完整的 happens-before：

生产者: 写 frame.q, frame.dq, frame.stamp
   │
   │ sequenced-before
   ▼
生产者: active.store(next, release)
   │
   │ synchronizes-with
   ▼
消费者: active.load(acquire)
   │
   │ sequenced-before
   ▼
消费者: 读 frame.q, frame.dq, frame.stamp

如果把中间的 release/acquire 换成 relaxed，图中间那条跨线程边就断了。 断边的后果不是“每次都错”，而是“标准不再保证它对”。 这种错误在机器人上尤其危险，因为它可能只在长时间运行、温度变化、CPU 负载变化或换成 ARM 板卡后暴露。

本质洞察：无锁数据通道的核心不是“用了几个 atomic”，而是“能不能画出完整的 happens-before 路径”。 只要路径断开，就算所有共享变量都是原子变量，业务数据也可能没有一致的发布语义。

为什么 seq_cst 不能替代理解

seq_cst 提供所有 seq_cst 原子操作之间的全局单一顺序。 这像把所有路口都改成红绿灯。 它确实更容易推理，但成本和表达力都不一定合适。

问题	用 seq_cst 的效果	更精确的选择
发布完整帧	正确，但比需要的更强	release/acquire
统计丢包数	正确，但无同步必要	relaxed
环形队列 head/tail	可能正确，但隐藏所有权边界	release/acquire + 单写原则
多原子变量协议	仍可能语义错误	重新设计为单发布点

反事实地看，如果把所有原子都写成 seq_cst，代码短期内可能更容易通过测试。 但团队会失去两个关键信号： 第一，哪些原子变量真正参与同步。 第二，哪些普通数据依赖哪个发布点可见。 当性能出现尾延迟，或者要迁移到更弱序的处理器时，缺少这些信号会让排查成本急剧上升。

release sequence：读改写的连续发布

一个容易遗漏的细节是 release sequence。 当一个线程对原子变量做 release store，后续对同一个原子变量的读改写操作可能延续这条发布序列。 这在引用计数、任务队列和索引交换中会出现。

但对初学者最重要的不是背诵术语，而是记住工程原则： 如果一个原子变量既承担“计数”又承担“发布数据”的角色，必须仔细证明每个读改写操作不会破坏数据归属。 如果证明写不清楚，就拆成两个变量或改用更受限的数据结构。

三个常用推理模板

模板	发布者	读取者	适合
标志位发布	写普通数据，release 写 ready	acquire 读 ready，再读数据	初始化完成、急停状态
索引发布	写 inactive buffer，release 写索引	acquire 读索引，复制该 buffer	双缓冲、最新帧
序号发布	写数据前后更新序号	acquire 读序号并验证一致	seqlock 快照

代码推理时建议把注释写成“这条原子操作同步哪一批普通数据”。 例如“发布状态帧”比“使用 release”更有价值。 因为前者表达业务因果，后者只是实现细节。

#include <atomic>
#include <cstdint>

struct ImuFrame {
    std::int64_t stamp_ns = 0;
    double accel[3] = {};
    double gyro[3] = {};
};

class ImuMailbox {
public:
    void publish(const ImuFrame& frame) {
        const int next = 1 - active_.load(std::memory_order_relaxed);
        slots_[next] = frame;
        // 发布完整 IMU 帧：stamp、accel、gyro 属于同一次可见性协议。
        active_.store(next, std::memory_order_release);
    }

    ImuFrame readLatest() const {
        // acquire 与 publish 中的 release 配对，读取者随后能看到完整帧。
        const int idx = active_.load(std::memory_order_acquire);
        return slots_[idx];
    }

private:
    ImuFrame slots_[2] = {};
    std::atomic<int> active_{0};
};

常见陷阱

类型	错误做法	现象	根本原因	正确做法
概念	把 atomic 当作内存屏障	普通数据偶发旧值	原子性不等于同步关系	明确 release/acquire 配对
编程	多个字段各自用 atomic 发布	新旧字段混合	缺少整帧边界	一个索引发布一整帧
思维	只看本机测试结果	换平台后失败	平台内存模型更强或更弱	按 C++ 标准推理
工程	低频参数也走复杂无锁协议	代码难测	过度追求形式	低频路径可用锁，高频路径再无锁

练习

1. 画出 LatestFrame<T> 的 happens-before 图，并标出普通写入、release、acquire、普通读取四个节点。
2. 设计一个“参数包发布”结构，要求 kp、kd、limit 和 stamp 必须属于同一帧。
3. 解释为什么丢包计数器可以用 relaxed，而“是否急停”的标志位通常不能只用 relaxed。

---

3.7 SPSC 环形队列：性能来自收窄问题边界 ⭐⭐⭐

这一节解决的问题是：为什么单生产者单消费者队列可以做到简单、快、可验证。

SPSC 的工程含义

SPSC 不是“低级版本的队列”。 它是一个强约束协议： 只有一个线程写 head，只有一个线程写 tail。 生产者永远不修改 tail。 消费者永远不修改 head。 双方只读取对方的索引来判断满或空。

这种所有权划分让队列少掉了最困难的竞争。 如果两个生产者同时写 head，它们必须通过 CAS 抢占槽位。 CAS 成功之后还要处理发布顺序、槽位构造、消费者可见性。 SPSC 直接避开这些问题。

索引	写入者	读取者	内存序含义
head	生产者	消费者	生产者 release 发布新元素
tail	消费者	生产者	消费者 release 归还槽位
buffer[head]	生产者	消费者	由 head 的 release/acquire 保护
buffer[tail]	消费者读取	生产者后续覆盖	由 tail 的 release/acquire 保护

这和机器人控制里的“单写原则”一致。 一个数据通道应当有明确所有者。 规划线程写轨迹，控制线程读轨迹。 控制线程写 trace，日志线程读 trace。 如果多个模块都能写同一个通道，架构问题往往已经先于并发问题出现。

容量为什么通常留一个空槽

环形队列中 head == tail 可以表示空。 但满的时候也可能出现 head == tail。 为了避免再引入额外计数器，常见做法是牺牲一个槽位。 当 next(head) == tail 时认为队列满。

方案	空判断	满判断	代价
留一个空槽	head == tail	next(head) == tail	可用容量少 1
额外 size	size == 0	size == Capacity	size 被双方写，增加同步
每槽序号	槽位序号判断	槽位序号判断	更复杂，适合 MPMC

对实时控制来说，少一个槽位通常不是问题。 更重要的是代码可证明。 如果容量是 8192，实际可用 8191 条 trace，换来的简单性非常划算。

峰值容量的估算方法

队列容量不是随便取一个 1024。 它应该覆盖最大突发量。 设控制线程以 \(f_p\) 频率写入，日志线程最坏可能暂停 \(T_{\text{pause}}\) 秒。 那么最低容量应满足：

\[N_{\min} \ge f_p \cdot T_{\text{pause}} + N_{\text{burst}} + 1\]

最后的 +1 是留空槽。 例如 1 kHz 控制线程，日志线程可能因为磁盘刷新暂停 20 ms，额外突发 50 条，则：

\[N_{\min} \ge 1000 \times 0.02 + 50 + 1 = 71\]

工程上会再乘安全系数，取 256 或 512。 如果 trace 每条很大，就应该先减小 trace，而不是盲目增大队列。

可移动对象与固定大小对象

实时队列最好存放固定大小、无堆分配的对象。 std::string、std::vector、std::shared_ptr 都可能把内存管理带进实时路径。 这不是说它们不能进队列，而是它们会改变队列的延迟边界。

元素类型	是否推荐进入实时 SPSC	原因
POD trace 结构	推荐	固定大小，复制成本稳定
Eigen 固定维向量	推荐	栈内数据，大小明确
std::array<double, N>	推荐	无堆分配
std::string	不推荐	可能分配和释放
std::vector<T>	不推荐	容量变化引入堆操作
std::shared_ptr<T>	谨慎	引用计数是原子读改写

带缓存行隔离的 SPSC 版本

下面的版本强调两个点： 第一，索引分离到不同缓存行，减少伪共享。 第二，生产者和消费者只写自己的索引。

#include <array>
#include <atomic>
#include <cstddef>
#include <new>

template <typename T, std::size_t Capacity>
class PaddedSpscRing {
public:
    static_assert(Capacity >= 2, "容量至少为 2，因为需要保留一个空槽");

    bool push(const T& value) {
        const std::size_t head = head_.value.load(std::memory_order_relaxed);
        const std::size_t next = increment(head);
        if (next == tail_.value.load(std::memory_order_acquire)) {
            // 队列已满：实时线程返回失败，由上层决定丢弃或降级。
            return false;
        }

        buffer_[head] = value;
        // 发布槽位内容：消费者 acquire 看到新 head 后可以读取 buffer_[head]。
        head_.value.store(next, std::memory_order_release);
        return true;
    }

    bool pop(T* value) {
        const std::size_t tail = tail_.value.load(std::memory_order_relaxed);
        if (tail == head_.value.load(std::memory_order_acquire)) {
            // 队列为空：消费者可以让出 CPU 或执行低优先级任务。
            return false;
        }

        *value = buffer_[tail];
        // 归还槽位：生产者 acquire 看到新 tail 后可以覆盖旧元素。
        tail_.value.store(increment(tail), std::memory_order_release);
        return true;
    }

private:
    // 用 alignas(64) 而非 std::hardware_destructive_interference_size：
    // 后者语义更精确（“破坏性缓存干扰大小”），但可移植性差——部分编译器/标准库
    // 至今未定义该常量（如某些 libstdc++/libc++ 配置），会直接编译失败。
    // 64 字节是主流 x86-64 与多数 ARM64 的缓存行大小，作为保守默认足够。
    struct alignas(64) PaddedIndex {
        std::atomic<std::size_t> value{0};
    };

    static constexpr std::size_t increment(std::size_t x) {
        return (x + 1) % Capacity;
    }

    std::array<T, Capacity> buffer_{};
    PaddedIndex head_{};
    PaddedIndex tail_{};
};

注意上面用的是 alignas(64)：64 字节是主流平台的缓存行大小，可移植且无需特殊支持。 C++17 引入的 std::hardware_destructive_interference_size 是更精确的替代——它直接表达“避免破坏性缓存干扰”的意图，但部分编译器/标准库实现至今未定义该常量，启用前需确认工具链支持。

队列满时的策略

实时线程不能在满队列上无限等待。 策略必须由数据语义决定。

数据	满队列策略	理由
控制 trace	丢弃当前记录并计数	日志不能影响控制
低频用户命令	保留并报警	命令丢失可能影响交互
传感器原始流	丢旧或丢新取决于算法	点云可丢旧，事件相机可能保序
急停事件	不应依赖普通队列	使用单独原子状态或实时安全通道

本质洞察：SPSC 队列的实时性来自失败有界。 push 成功是常数时间，push 失败也是常数时间。 真正危险的不是丢一条可选日志，而是在控制周期中等待一个不受控消费者。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
编程	两个线程同时调用 push	元素覆盖或乱序	head 只有单写假设	每个生产者一条 SPSC，或改用 MPSC/MPMC
概念	以为容量 N 可存 N 条	最后一条总是失败	留空槽区分满和空	文档写明可用容量为 N-1
工程	队列元素包含动态内存	周期偶发尖峰	复制或析构触发堆操作	固定大小 trace 结构
思维	队列满就自旋等待	控制周期超时	消费者速度不受控制	立即失败并记录丢弃

练习

1. 按 \(N_{\min} \ge f_p T_{\text{pause}} + N_{\text{burst}} + 1\) 估算 2 kHz trace 队列容量，假设日志线程最坏暂停 50 ms。
2. 把 ControlTrace 增加一个 std::string 字段，分析它为什么破坏实时边界。
3. 为“每个电机驱动线程向诊断线程上报事件”设计通道数量，比较“12 条 SPSC”和“1 条 MPMC”的复杂度。

---

3.8 MPSC 与 MPMC：什么时候不应自己写 ⭐⭐⭐

这一节解决的问题是：多生产者、多消费者队列为什么难，以及机器人系统如何避开不必要的复杂性。

多方并发增加了哪些问题

SPSC 中，槽位归属非常清楚。 生产者独占 head，消费者独占 tail。 MPMC 中，这个清晰边界消失了： 多个生产者会同时争抢下一个写入槽。 多个消费者会同时争抢下一个读取槽。 每个槽位还必须知道自己当前处于“空、可写、已写、可读、已读”的哪一种状态。

结构	需要解决的问题	常见技术
SPSC	单写索引可见性	release/acquire
MPSC	多生产者抢写槽	CAS、fetch_add、每生产者缓冲
SPMC	多消费者抢读槽	CAS、引用计数、每消费者游标
MPMC	双向多方竞争	每槽序号、CAS 循环、内存回收

这就是为什么成熟 MPMC 队列通常看起来比 SPSC 复杂得多。 复杂不是实现者喜欢炫技，而是问题本身多了槽位归属、竞争失败、ABA、回收和公平性。

CAS 循环的实时含义

CAS 的语义是：如果当前值仍等于我刚才看到的旧值，就把它改成新值。 这很适合抢占一个共享索引。 但 CAS 可能失败。 失败后通常要重新读取、重新计算、再次尝试。

对普通服务器程序，这叫无锁前进。 对实时控制线程，这叫迭代次数没有上界。 lock-free 保证系统整体持续前进，不保证某个具体线程在固定步数内完成。 wait-free 才保证每个线程都有有界步数，但 wait-free 通用结构更难写、更难验证。

性质	含义	对控制线程的意义
blocking	可能等待锁或条件变量	不适合硬实时路径
lock-free	至少有线程能前进	单个控制周期仍可能失败很多次
wait-free	每个线程有界完成	最符合强实时，但实现受限
bounded try	尝试有限次后失败	工程上常用的折中

反事实地看，如果在 1 kHz 控制循环中直接使用竞争激烈的 MPMC 队列，平均耗时可能很好看。 但一次 CAS 风暴就足以让周期超过 1 ms。 机器人控制关心的是尾部延迟，而不是只看均值。

架构层面的替代方案

很多 MPMC 需求可以通过架构调整变成多个 SPSC 或 MPSC。

原需求	直接方案	更可控方案
12 个电机线程上报诊断	1 条 MPMC	12 条 SPSC 汇入诊断线程
多传感器写状态估计输入	1 条 MPMC	每个传感器一个输入通道，估计线程按时间戳融合
多模块写日志	1 条 MPMC 日志队列	每模块 SPSC 到日志汇聚线程
多控制器写同一命令	1 条 MPMC	上层仲裁器单写命令通道

这种改法看似增加了通道数量，但减少了共享状态。 并发设计中，少共享通常比少对象更重要。 多个小通道还能给故障诊断带来好处：某个通道拥塞时，可以直接定位是哪类数据超过预算。

每槽序号的 MPMC 思路

成熟有界 MPMC 队列常用每槽序号。 每个槽位带一个 sequence。 生产者通过 CAS 抢占全局写位置，然后检查槽位序号是否表示可写。 消费者通过 CAS 抢占全局读位置，然后检查槽位序号是否表示可读。

下面的代码只展示槽位状态思想，不建议在控制路径中把它当作完整工程实现。 真正项目应优先使用充分测试过的库，并用压力测试验证目标平台上的尾延迟。

#include <atomic>
#include <cstddef>

template <typename T>
struct MpmcCell {
    std::atomic<std::size_t> sequence{0};
    T value{};
};

bool cellLooksWritable(std::size_t cell_sequence, std::size_t expected_pos) {
    // 序号等于期望写位置时，表示该槽位已经被消费者归还，可以写入。
    return cell_sequence == expected_pos;
}

bool cellLooksReadable(std::size_t cell_sequence, std::size_t expected_pos) {
    // 序号等于期望读位置加一时，表示生产者已经发布该槽位。
    return cell_sequence == expected_pos + 1;
}

每槽序号解决的是“槽位现在属于谁”。 但它没有自动解决所有问题。 元素构造失败怎么办？ 消费者读取后析构是否可能阻塞？ 容量不是 2 的幂时索引取模是否昂贵？ CAS 竞争失败是否造成尾延迟？ 这些都必须由工程实现逐一处理。

多生产者日志的折中设计

对日志系统，一个常见折中是“每个实时生产者一个 SPSC，非实时汇聚线程轮询”。 汇聚线程慢一点没关系，因为它不在控制路径。 实时生产者只写自己的队列，失败就丢弃并计数。

#include <array>
#include <cstdint>

struct MotorTrace {
    std::int64_t stamp_ns = 0;
    int motor_id = 0;
    double position = 0.0;
    double velocity = 0.0;
    double torque = 0.0;
};

template <std::size_t MotorCount, std::size_t QueueSize>
class MotorTraceFanIn {
public:
    bool pushFromMotor(std::size_t motor_id, const MotorTrace& trace) {
        if (motor_id >= MotorCount) {
            // 非法编号直接失败，避免写越界。
            return false;
        }
        // 每个电机只写自己的 SPSC 队列，避免多生产者争抢同一个 head。
        return queues_[motor_id].push(trace);
    }

    bool popAny(MotorTrace* out) {
        for (auto& queue : queues_) {
            if (queue.pop(out)) {
                // 汇聚线程读取到一条记录即可返回，下一轮继续轮询。
                return true;
            }
        }
        return false;
    }

private:
    std::array<SpscRing<MotorTrace, QueueSize>, MotorCount> queues_;
};

这个设计的缺点是汇聚线程需要轮询多个队列。 但它把不确定性放在非实时线程，实时线程只执行一次 SPSC push。 在控制系统中，这通常是正确的取舍。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
概念	lock-free 等于实时安全	p99 很好但最大值爆炸	CAS 失败次数无上界	控制路径使用 SPSC 或有界尝试
编程	自写 MPMC 不测 ABA	长时间运行后崩溃	指针复用欺骗 CAS	使用成熟库或带标签指针
工程	多模块直接写控制命令	命令互相覆盖	缺少单写仲裁	设置命令仲裁器
思维	用一个通用队列解决所有数据流	难调试、难隔离	数据语义混合	按语义拆通道

练习

1. 画出“4 个传感器输入状态估计”的通道设计，要求状态估计线程能知道每个传感器最近一次更新时间。
2. 解释为什么“控制命令”通常应该单写，而不是让多个模块并发写入同一队列。
3. 调研一个成熟 MPMC 队列的接口，列出它对元素类型、容量和内存回收的约束。

---

3.9 ABA、内存回收与对象生命周期 ⭐⭐⭐

这一节解决的问题是：为什么 CAS 看到“指针没变”不等于对象没变。

ABA 的具体时间线

ABA 最常见于无锁栈、自由链表和对象池。 考虑一个无锁栈顶指针 top。 线程 A 读取到 top == A_node，准备把栈顶改成 A_node->next。 此时线程 A 被抢占。 线程 B 弹出 A_node，又弹出 B_node，然后把 A_node 放回栈顶。 线程 A 恢复运行，再看 top 仍然是 A_node，CAS 成功。 但这时 A_node->next 的含义已经不是线程 A 最初看到的含义。

时刻	线程 A 看到	线程 B 操作	真实状态
t0	top = A	无	A -> B -> C
t1	准备 CAS	pop A	B -> C
t2	A 暂停	pop B	C
t3	A 暂停	push A	A -> C
t4	top 仍是 A	无	A 的 next 已变化
t5	CAS 成功	无	栈结构被旧假设破坏

ABA 的可怕之处在于，指针值相同掩盖了对象身份变化。 这和机器人里“同一个缓冲区地址被重复使用”很像。 地址没变，并不表示内容属于同一批数据。 因此无锁算法常常需要版本号、危险指针、epoch 回收或固定对象池协议。

带标签指针

一种常见做法是在指针旁边加版本号。 每次修改栈顶时，不只比较指针，还比较版本。 即使指针从 A 到 B 再回到 A，版本号也会变化。

#include <atomic>
#include <cstdint>

struct Node {
    Node* next = nullptr;
    int value = 0;
};

struct TaggedPtr {
    Node* ptr = nullptr;
    std::uint64_t tag = 0;
};

class TaggedTop {
public:
    TaggedPtr load() const {
        // 这里用互斥自由的原子对象表达思路；实际可用平台相关双宽 CAS。
        return top_.load(std::memory_order_acquire);
    }

    bool compareExchange(TaggedPtr& expected, Node* desired_ptr) {
        TaggedPtr desired{desired_ptr, expected.tag + 1};
        // CAS 同时比较指针和值标签，避免 A-B-A 被误认为没有变化。
        return top_.compare_exchange_weak(
            expected,
            desired,
            std::memory_order_acq_rel,
            std::memory_order_acquire);
    }

private:
    std::atomic<TaggedPtr> top_{TaggedPtr{}};
};

这段代码表达的是思想。 在真实平台上，std::atomic<TaggedPtr> 是否 lock-free 取决于 ABI、结构大小和硬件是否支持双宽 CAS。 如果它内部退化成锁，就不再满足实时路径对等待边界的要求。 因此标签指针不是万能解法，它只是 ABA 防护的一类工具。

内存回收为什么比入栈出栈更难

无锁栈看起来只需要改几个指针。 但一旦节点可以释放，问题立刻变复杂。 某个线程可能已经读取了节点指针，还没来得及访问节点内容。 另一个线程把这个节点弹出并释放。 第一个线程再访问时就变成 use-after-free。

常见回收策略有三类：

策略	思路	优点	代价
固定对象池	节点不释放，只在池中循环	简单，适合嵌入式	容量固定
hazard pointer	线程声明自己正在访问的指针	可精确保护	每次访问有额外原子操作
epoch 回收	所有线程离开旧 epoch 后再释放	批量高效	长时间停顿线程会延迟回收

机器人实时路径最常见的选择是固定对象池。 不是因为它最通用，而是因为它边界清楚。 容量固定、生命周期固定、失败策略固定。 如果容量不足，实时线程返回失败，而不是临时分配。

对象池与自由链表的边界

对象池适合“对象类型固定、数量上界可估算”的场景。 例如控制 trace、关节命令包、固定大小轨迹片段。 不适合任意大小图像、点云或可变长度字符串。

场景	对象池适合度	原因
1 kHz 控制 trace	高	固定大小，数量可估算
关节命令包	高	结构固定，生命周期短
MPC 轨迹片段	中	可固定 horizon 后使用
点云消息	低	大小随场景变化
调试字符串	低	长度不可控

不在实时路径释放内存

在实时控制中，释放内存也可能带来不可预测延迟。 析构函数可能释放嵌套对象。 内存分配器可能加锁。 共享指针的最后一次引用释放可能触发复杂析构链。

因此设计实时数据通道时，要同时回答三个问题：

1. 谁构造对象？
2. 谁拥有对象？
3. 谁在什么时候销毁对象？

如果第三个问题答不清楚，队列再无锁也不能保证实时。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
编程	无锁栈 pop 后立即 delete 节点	偶发崩溃	其他线程仍持有指针	hazard pointer、epoch 或对象池
概念	指针相同就认为对象相同	CAS 错误成功	ABA 改变了对象历史	使用标签或避免指针复用
工程	实时路径使用 shared_ptr 传大对象	尾延迟尖峰	引用计数和析构不可控	固定缓冲或显式所有权
思维	为学习自写通用无锁栈并上机	长期稳定性差	回收难度被低估	生产系统优先成熟实现

练习

1. 按时间线写出一个三节点无锁栈的 ABA 过程，并说明哪一次 CAS 是错误成功。
2. 设计一个固定大小 TrajectorySegment 对象池，说明容量不足时实时线程应如何失败。
3. 对比 unique_ptr、shared_ptr 和对象池在实时数据通道中的所有权语义。

---

3.10 false sharing 与缓存一致性：无锁结构为什么会慢 ⭐⭐

这一节解决的问题是：为什么明明没有锁，CPU 时间却消耗在缓存行来回迁移上。

缓存行不是变量级别同步

CPU 缓存一致性通常以 cache line 为单位，常见大小是 64 字节。 如果两个线程分别修改两个不同变量，但这两个变量落在同一 cache line，硬件仍必须让该 cache line 在核心之间迁移。 这就是 false sharing。

直觉上像两个人编辑同一个表格文件的不同单元格。 虽然内容不冲突，但文件锁以整份文件为单位，结果两个人仍然互相打断。 缓存行也是类似的粒度问题。

情况	变量关系	缓存行关系	性能
真共享	两线程写同一变量	同一缓存行	必然竞争
伪共享	两线程写不同变量	同一缓存行	无语义竞争但有硬件竞争
无共享	两线程写不同变量	不同缓存行	最理想

在无锁队列中的典型位置

SPSC 队列中，生产者频繁写 head。 消费者频繁写 tail。 如果 head 和 tail 相邻，两个核心会反复抢同一 cache line。 队列越高频，抖动越明显。

伪共享在 1 kHz 控制中未必每次都超过周期，但会污染尾部延迟。 在日志、点云预处理、批量 trace 等高吞吐路径中，它可能让吞吐下降数倍。

对齐不是越多越好

对齐会增大对象尺寸。 如果每个小对象都 alignas(64)，内存占用和缓存利用率反而变差。 对齐应优先用于被不同线程频繁写入的热点字段。

字段	是否需要隔离	原因
SPSC head 和 tail	通常需要	分别被两个线程高频写
只读配置参数	不需要	没有写竞争
大缓冲区数据	不一定	关键是访问模式
丢包计数器数组	可能需要	多线程分别写不同计数器

多计数器的伪共享示例

#include <array>
#include <atomic>
#include <cstddef>

struct alignas(64) PaddedCounter {
    std::atomic<std::uint64_t> value{0};
};

class PerMotorDropCounters {
public:
    void increment(std::size_t motor_id) {
        if (motor_id >= counters_.size()) {
            // 非法编号忽略，避免诊断代码影响控制路径。
            return;
        }
        // 每个电机线程写自己的计数器，relaxed 足够表达统计语义。
        counters_[motor_id].value.fetch_add(1, std::memory_order_relaxed);
    }

    std::uint64_t read(std::size_t motor_id) const {
        // 诊断线程读取统计值，不用它同步其他数据。
        return counters_[motor_id].value.load(std::memory_order_relaxed);
    }

private:
    std::array<PaddedCounter, 12> counters_{};
};

这里每个计数器占一个缓存行，适合“12 个电机线程分别高频写自己的计数器”。 如果只有一个线程写所有计数器，就不需要这样对齐。 优化必须服务访问模式，而不是套固定格式。

如何测 false sharing

测量时不要只看总吞吐。 至少记录：

1. 平均耗时。
2. p95、p99。
3. 最大耗时。
4. CPU 迁移次数。
5. cache miss 或 cache line invalidation 相关事件。

在 Linux 上可用 perf stat 观察缓存相关指标。 在机器人控制进程中，还应结合控制周期耗时直方图。 一个优化如果只提升吞吐但放大最大延迟，对控制不一定是好事。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
概念	不同变量就一定互不影响	无锁结构仍慢	缓存一致性按行工作	隔离热点写变量
编程	对所有字段强行 64 字节对齐	内存膨胀	缓存空间被浪费	只隔离跨线程热点
工程	只测平均吞吐	实机仍超时	尾部延迟未观察	记录 p99 和最大值
思维	优化前不确认瓶颈	代码复杂但收益小	伪共享不是唯一原因	先测量再对齐

练习

1. 构造两个版本的计数器数组：普通数组和 alignas(64) 数组，比较 12 个线程分别写不同计数器时的耗时。
2. 解释为什么“一个线程写、多个线程只读”的配置对象通常不需要每个字段单独对齐。
3. 在 SPSC 队列中把 head 和 tail 的位置互换或合并，观察压力测试的尾部延迟变化。

---

3.11 机器人实时数据通道设计案例 ⭐⭐⭐

这一节解决的问题是：把内存序、队列、缓冲、ABA 和缓存行知识组合成一个可落地的数据通道。

案例设定

考虑一个腿足机器人控制进程：

数据流	写入者	读取者	频率	语义
关节状态	驱动接收线程	状态估计、控制	1 kHz	最新完整状态
估计状态	状态估计线程	控制线程	500 Hz	最新完整状态
MPC 轨迹	规划线程	控制线程	20-50 Hz	最新轨迹，可覆盖旧轨迹
控制命令	控制线程	驱动发送线程	1 kHz	每周期一帧
trace 日志	控制线程	日志线程	1 kHz	尽力记录，不影响控制
参数更新	ROS2 回调线程	控制线程	低频	验证后整包生效

每条数据流的语义不同。 如果全部用一个队列，会混合“最新值”和“每条都要处理”的需求。 如果全部用 mutex，低优先级线程可能影响控制线程。 如果全部用 MPMC，又会把不必要的竞争带进高频路径。

推荐通道图

驱动接收线程 ── LatestFrame<JointState> ──► 状态估计线程
       │                                      │
       └── LatestFrame<JointState> ──────────► 控制线程

状态估计线程 ── LatestFrame<RobotState> ────► 控制线程
规划线程     ── TripleBuffer<Trajectory> ───► 控制线程
ROS2参数回调 ─ LatestFrame<ControllerParams>► 控制线程
控制线程     ── SpscRing<ControlTrace> ─────► 日志线程
控制线程     ── SpscRing<MotorCommand> ─────► 驱动发送线程

这张图的核心是单写原则。 每条箭头都有明确生产者和消费者。 多消费者场景不强行共享同一个读游标，而是为关键消费者提供独立通道或使用整帧快照。

为什么关节状态不拆成 12 个 atomic

关节状态不仅是 12 个位置。 它至少包含位置、速度、力矩估计、采样时间戳和驱动状态位。 控制律使用的是同一采样时刻的状态。 如果每个字段独立发布，控制线程可能读到新位置、旧速度、新时间戳的混合。

设计	数据竞争	语义一致性	结论
每个 double 一个 atomic	无数据竞争	可能混帧	不推荐
一个 mutex 保护结构	正确	可能阻塞	适合非实时或低频
双缓冲整帧发布	正确	最新帧语义清楚	推荐
seqlock 快照	正确但可能重试	适合单写多读	可选

反事实地看，如果把 12 个关节状态拆成 24 个 atomic，代码表面上没有数据竞争。 但控制器计算阻抗项 \(\tau = K_p(q_d-q)+K_d(\dot{q}_d-\dot{q})\) 时，q 和 dq 可能不来自同一周期。 这种错误不会被 ThreadSanitizer 报告，因为它不是数据竞争，而是业务一致性错误。

控制线程读取策略

控制线程应该把所有输入在周期开始时采样到局部变量。 本周期内只使用局部快照。 这样可以避免同一周期前半段用旧状态、后半段用新状态。

struct RobotState {
    std::int64_t stamp_ns = 0;
    double q[12] = {};
    double dq[12] = {};
    double base_pose[7] = {};
    double base_twist[6] = {};
};

struct TrajectoryPoint {
    double q_ref[12] = {};
    double dq_ref[12] = {};
    double torque_ff[12] = {};
};

void controlTick(
    const LatestFrame<RobotState>& state_buffer,
    TripleBuffer<TrajectoryPoint>& trajectory_buffer,
    SpscRing<ControlTrace, 8192>& trace_queue)
{
    // 周期开始时采样输入，本周期后续计算只使用这些局部副本。
    RobotState state = state_buffer.read();
    TrajectoryPoint ref = trajectory_buffer.readLatest();

    ControlTrace trace{};
    trace.stamp_ns = state.stamp_ns;
    trace.status_code = 0;

    // 这里执行控制律计算；示例省略具体力矩公式。
    trace.torque_norm = 0.0;

    // trace 写入失败不阻塞控制周期，日志线程之后读取成功记录。
    (void)trace_queue.push(trace);
}

参数更新的两阶段提交

参数更新来自 ROS2 回调或配置服务。 它属于非实时上下文。 正确流程是：

1. 在非实时线程读取参数。
2. 完成范围验证和相容性检查。
3. 构造新的不可变参数包。
4. 通过 release/acquire 缓冲发布给控制线程。
5. 控制线程在周期边界读取并生效。

这样做的意义是避免控制线程执行字符串查找、参数锁、动态分配或复杂校验。 参数不是不能动态更新，而是动态更新不能把不确定成本带进实时循环。

时间戳与超时

最新帧通道有一个风险：它不会告诉你生产者是否已经停止。 因此每个状态帧都应包含时间戳。 控制线程读取最新帧后必须检查数据年龄。

数据	超时策略
关节状态	超过 2 个控制周期进入安全模式
估计状态	超过阈值时降级为关节空间控制
MPC 轨迹	继续使用最后安全轨迹并限速
参数包	参数不更新不构成故障
日志 trace	丢失只计数

通道设计自检表

问题	合格答案
谁是唯一写入者	能指向具体线程或模块
读取者是否需要每条数据	需要则队列，不需要则最新帧
满队列怎么办	有明确失败策略
是否允许重试	控制线程不应无限重试
对象是否固定大小	高频路径应固定大小
数据是否整帧一致	有单一发布点
如何检测生产者死亡	帧内时间戳和看门狗

常见陷阱

类型	错误做法	现象	根本原因	正确做法
概念	控制状态用队列逐条处理	控制器追旧状态	最新状态语义被误用为事件语义	用双缓冲读取最新帧
编程	周期中多次读取共享状态	同周期内数据变化	没有周期快照	周期开始复制到局部变量
工程	日志队列满时阻塞	控制周期超时	诊断反向影响控制	丢弃并计数
思维	所有模块共享一个全局状态对象	修改来源不清	没有数据所有权	每条数据流定义生产者和消费者

练习

1. 为“IMU 1 kHz、里程计 100 Hz、视觉定位 30 Hz”设计状态估计输入通道，并说明每个通道的超时策略。
2. 把一个全局 RobotContext 共享对象改写为三条明确数据流：状态、轨迹、参数。
3. 设计一个控制线程周期开始的采样顺序，要求状态、参数和轨迹都在本周期内保持一致。

---

3.12 RCU 思想在机器人系统中的应用 ⭐⭐⭐

这一节解决的问题是：当配置、地图或参数需要低频更新但高频读取时，怎样让读取路径几乎零开销。

什么是 RCU

RCU（Read-Copy-Update）最初由 Linux 内核开发者发明，用于保护读多写少的共享数据结构。其核心思想非常直接：读者不加任何锁，直接读取当前版本的数据指针；写者不修改旧数据，而是复制一份新数据、在新副本上修改、然后原子地切换指针。旧版本数据要等到所有读者都不再使用后才释放。

这和图书馆的参考书管理很像。参考书不允许带走，但所有人可以同时翻阅。如果要更新参考书，不是从正在阅读的人手里抢走旧版，而是把新版本放到书架上、把索引卡片指向新版。等到所有正在读旧版的人都读完了，再把旧版收走。

在机器人系统中，读多写少的典型数据包括：

数据	写入频率	读取频率	读者
控制增益参数	配置变更时	每个控制周期	控制线程
地图/代价地图	地图更新时	规划每次查询	规划线程
URDF 运动链缓存	初始化或重配置	每个周期	控制、估计
传感器标定参数	标定完成时	每帧处理	估计线程
行为状态机配置	模式切换时	每个决策周期	行为层

对这些数据，加 mutex 保护虽然正确，但会在读取路径引入不可控的等待。如果写者持锁时间长（例如复制整张地图），读者可能在控制周期中等待毫秒级时间。RCU 让读者完全无等待。

RCU 的三个阶段

RCU 操作可以分成三步理解。

阶段	参与者	动作	机器人类比
Read	读者	获取当前指针，在临界区内使用	控制线程从指针读取增益
Copy-Update	写者	分配新对象，修改副本，原子切换指针	参数服务构造新增益包并发布
Reclaim	回收者	确认所有旧读者已退出后释放旧版本	等到没有控制周期在用旧增益后释放

第三步是 RCU 实现中最困难的部分。在 Linux 内核中，这由宽限期（grace period）机制保证：当所有 CPU 都经历过一次上下文切换后，可以确信没有读者仍持有旧指针。在用户态 C++ 中，没有内核那样的调度钩子，因此需要用不同策略。

简化 RCU：基于 shared_ptr 的版本

对非实时路径或中频路径，std::shared_ptr 配合 std::atomic 可以实现一个简化版 RCU。std::atomic_load 和 std::atomic_store（C++11 的自由函数版本）或 C++20 的 std::atomic<std::shared_ptr<T>> 提供了原子读写指针的能力。

#include <atomic>
#include <memory>

struct ControllerParams {
    double kp[12] = {};
    double kd[12] = {};
    double torque_limit = 20.0;
    double friction_coeff = 0.6;
};

class ParamsRcu {
public:
    std::shared_ptr<const ControllerParams> read() const {
        // 读者获取当前版本的共享指针。引用计数增加保证指向对象存活。
        return std::atomic_load(&current_);
    }

    void update(std::shared_ptr<const ControllerParams> new_params) {
        // 写者原子地切换指针。旧版本在所有读者释放后自动析构。
        std::atomic_store(&current_, std::move(new_params));
    }

private:
    std::shared_ptr<const ControllerParams> current_ =
        std::make_shared<const ControllerParams>();
};

这个版本的局限在于 shared_ptr 的引用计数是原子读改写操作。在高频控制路径（1 kHz 以上），每个周期两次原子操作（获取和释放 shared_ptr）可能带来可测量的缓存开销。如果实时要求不严格，这个方案已经足够好。如果要求 wait-free 读取，需要用 hazard pointer 或 epoch-based 回收替代引用计数。

RCU 与双缓冲的对比

RCU 和双缓冲都解决"读写不互相阻塞"的问题，但侧重不同。

特性	双缓冲	RCU
数据副本数	固定 2	动态，由活跃读者决定
写者行为	写入非活动槽，切换索引	分配新对象，切换指针
旧数据释放	写者立即复用旧槽	等所有读者退出后释放
读者等待	无等待	无等待
适用数据大小	小到中	中到大
内存分配	无	写入时分配新对象

对实时控制路径，双缓冲更适合频繁更新的小帧（状态、轨迹点）。RCU 更适合不频繁更新的大对象（地图、参数包、运动链缓存）。两者在同一系统中常常并存：估计状态用双缓冲，控制参数用 RCU。

反事实地看，如果对地图更新也用双缓冲，就需要预分配两份完整地图。对大型 3D 代价地图，这意味着几十 MB 的内存翻倍。RCU 只在更新时分配新版本，旧版本在用完后释放，内存峰值更可控。

本质洞察：RCU 不是"更好的锁"，而是一种"读者不付出任何同步代价"的协议。代价由写者承担——写者负责分配、复制和延迟回收。这种不对称恰好匹配机器人系统中"高频读、低频写"的参数和配置数据模式。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
编程	读者修改通过 RCU 获取的对象	数据损坏	RCU 对象应为 const	只通过写者路径修改
概念	对高频写入数据用 RCU	内存分配频繁	RCU 每次写入分配新对象	高频用双缓冲
工程	忘记回收旧版本	内存泄漏	没有宽限期或引用计数	使用 shared_ptr 或显式回收
思维	认为 RCU 在所有场景都优于锁	代码复杂化	RCU 对读多写少最优	按读写频率比选择

练习

1. 用 shared_ptr RCU 实现一个参数热更新接口，控制线程每周期读取，ROS2 回调线程低频更新。
2. 对比双缓冲和 shared_ptr RCU 在"参数更新"场景中的内存占用和读取开销。
3. 解释为什么 RCU 读者获取的指针必须在使用期间保持存活，以及 shared_ptr 如何自动保证这一点。

---

3.13 与 ROS2 实时数据传递的关系 ⭐⭐

这一节解决的问题是：ROS2 的 executor、DDS 和 intra-process 通信与本章无锁结构之间的关系。

ROS2 通信不是为硬实时设计的

ROS2 的默认通信路径经过 DDS 中间件。DDS 提供可靠传输、QoS 和发现机制，但它的消息序列化、内存分配和互斥锁不满足硬实时路径的可预测性要求。在机器人控制系统中，常见做法是把 ROS2 用于中频和低频通信（参数、诊断、可视化、用户指令），把高频控制路径的数据交换放在进程内部，使用本章讲的无锁结构。

通信路径	频率	延迟要求	推荐机制
用户命令到控制	10-50 Hz	松	ROS2 topic/service
参数更新	偶发	松	ROS2 parameter + RCU
状态到可视化	30-100 Hz	松	ROS2 topic
估计到控制	500-1000 Hz	严	进程内双缓冲
控制到驱动	1000 Hz	严	进程内 SPSC 或直接调用
trace 到日志	1000 Hz	尽力	进程内 SPSC

ros2_control 的 RealtimeBuffer

ros2_control 框架提供了 RealtimeBuffer<T> 和 RealtimePublisher<T>，它们正是本章双缓冲思想的工程落地。RealtimeBuffer 内部用两个槽位和原子索引实现无锁的单写单读发布。控制器在非实时回调中写入新数据，在实时 update() 中读取最新快照。

这不是巧合。ros2_control 的设计者在实时控制中面对的问题和本章一样：控制线程不能等待非实时的 ROS2 回调。RealtimeBuffer 就是双缓冲在 ROS2 生态中的标准答案。

理解本章的原理后，你就能看懂 RealtimeBuffer 的源码：它的 writeFromNonRT() 是 release store，readFromRT() 是 acquire load，两个槽位由原子索引切换。它的局限也很明显：不支持多个实时读者各自独立消费、不支持队列语义、不处理大帧的三缓冲需求。

intra-process 通信

ROS2 Humble 及以后版本支持 intra-process 通信优化。当发布者和订阅者在同一进程中，并且使用 unique_ptr 发布，消息可以跳过 DDS 序列化，通过共享指针或所有权转移直接传递。

这减少了拷贝和序列化开销，但仍然经过 executor 的回调调度。executor 的调度策略（SingleThreaded、MultiThreaded、EventsExecutor）会影响回调的唤醒延迟和优先级。对硬实时控制来说，executor 调度的不确定性仍然是一个问题。

intra-process 模式	适合	不适合
中频数据传递（点云预处理到规划）	需要 ROS2 QoS 和生命周期	1 kHz 控制路径
同进程节点间共享大消息	避免序列化开销	需要确定性延迟
诊断和可视化输出	便利且高效	硬实时 deadline

架构模式：ROS2 边界内外

一个工程上可维护的架构是把控制系统分成两层：

┌────────────────────────────────────────────────┐
│                ROS2 边界                        │
│  ┌──────────┐  ┌──────────┐  ┌──────────────┐ │
│  │参数服务   │  │可视化发布 │  │诊断 topic    │ │
│  └────┬─────┘  └────┬─────┘  └──────┬───────┘ │
│       │             │               │          │
│  ┌────▼─────────────▼───────────────▼────────┐ │
│  │         ROS2 适配层（非实时）               │ │
│  └──────┬───────────────────────────┬────────┘ │
│         │ RCU/双缓冲                │ SPSC     │
│  ┌──────▼───────────────────────────▼────────┐ │
│  │         实时核心（无锁数据交换）            │ │
│  │  估计 ──双缓冲──► 控制 ──SPSC──► 日志     │ │
│  └───────────────────────────────────────────┘ │
└────────────────────────────────────────────────┘

ROS2 适配层负责参数解析、消息转换和诊断发布。实时核心只使用本章讲的无锁结构。两层之间的数据流方向明确，每条通道有单一写者。

这种分层让控制核心可以在不依赖 ROS2 的环境中独立测试和基准测试。它也让 ROS2 版本升级不影响控制算法的正确性。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
概念	用 DDS topic 传递 1 kHz 控制状态	延迟抖动	DDS 不保证确定性延迟	进程内无锁结构
工程	控制线程直接调用 ROS2 参数 API	偶发阻塞	参数查找可能加锁或分配	非实时线程预取参数后通过缓冲传入
编程	在实时 update() 中发布 ROS2 消息	周期超时	序列化和 DDS 写入不确定	使用 RealtimePublisher 或 SPSC 到非实时线程
思维	认为 intra-process 等于实时安全	控制偶发延迟	executor 调度仍有不确定性	对 deadline 敏感路径用显式无锁通道

练习

1. 阅读 ros2_control 的 RealtimeBuffer 源码，找出它的 release/acquire 配对位置。
2. 设计一个控制器架构：ROS2 参数通过 RCU 进入实时核心，状态通过双缓冲从估计进入控制。
3. 比较 intra-process topic 和 SPSC 队列在 1 kHz 频率下的延迟分布。

---

3.14 lock-free 与 wait-free 的工程选择 ⭐⭐⭐

这一节解决的问题是：控制路径应该追求 lock-free 还是 wait-free，以及工程中的折中方案。

术语的严格定义

并发数据结构的无阻塞层级是一个精确的理论分类，不是模糊的"快慢"描述。

层级	保证	直觉
阻塞（blocking）	一个线程被延迟可能导致所有线程被延迟	一个人堵门所有人走不了
无阻塞（obstruction-free）	单独运行的线程能在有限步内完成	没人竞争就能完成
无锁（lock-free）	在任意并发下，至少有一个线程在有限步内完成	总有人在前进
无等待（wait-free）	在任意并发下，每个线程都在有限步内完成	所有人都能在有限步完成

lock-free 的关键是"系统级前进保证"。即使某个线程被操作系统抢占、暂停甚至崩溃，其他线程不会因此永久停滞。但 lock-free 不保证某个具体线程的完成时间。一个线程可能因为反复 CAS 失败而被延迟很多次。

wait-free 更强：每个线程都有步数上界。这对硬实时控制最理想，因为控制周期需要的是"这个线程在 deadline 之前完成"，而不是"某个线程完成了"。

为什么 wait-free 通用结构很少见

wait-free 的理论存在性已被 Herlihy（1991）证明：任何具有 consensus number 为无穷大的原语（如 CAS）都能实现任意数据结构的 wait-free 版本。但通用构造的常数开销极大，在实践中几乎不可用。

工程中真正 wait-free 的结构通常是为特定场景定制的：

结构	wait-free 条件	工程限制
SPSC 环形队列	单生产者单消费者	不支持多方
双缓冲最新帧	单写单读	不保留历史
atomic counter	单变量读改写	只适合计数
seqlock 读侧	有界重试	写者不能太频繁

注意 SPSC 队列的 push 和 pop 都是 wait-free 的——每次调用执行固定步数的原子操作，无论另一个线程在做什么。这不是因为 SPSC 使用了特殊算法，而是因为单写者约束消除了竞争。

工程折中：bounded try

实时控制中最常用的折中不是追求理论 wait-free，而是"有界尝试"（bounded try）：操作最多尝试 N 次，超过 N 次则返回失败。

#include <atomic>

template <typename T>
class BoundedTryExchange {
public:
    bool tryUpdate(T desired, int max_attempts = 3) {
        T expected = value_.load(std::memory_order_relaxed);
        for (int i = 0; i < max_attempts; ++i) {
            // 最多尝试 max_attempts 次 CAS，超过则放弃。
            if (value_.compare_exchange_weak(expected, desired,
                                             std::memory_order_release,
                                             std::memory_order_relaxed)) {
                return true;
            }
        }
        // 超过尝试次数，返回失败。上层根据数据语义决定丢弃或降级。
        return false;
    }

    T read() const {
        return value_.load(std::memory_order_acquire);
    }

private:
    std::atomic<T> value_{};
};

bounded try 不满足理论 lock-free 或 wait-free 的定义。但它给出了实时系统最需要的性质：确定性时间边界。操作要么在 N 步内成功，要么立刻失败。控制线程可以在失败后执行降级策略，而不是无限等待。

方案	最坏耗时	适合	不适合
mutex	不可预测	非实时路径	控制线程
lock-free CAS 循环	理论无上界	中频路径	硬实时 deadline
bounded try	N 次操作	硬实时路径	必须成功的场景
wait-free SPSC	固定步数	单写单读	多方竞争

机器人控制中的决策树

数据交换需求
    │
    ├── 单生产者单消费者？
    │   ├── 是 → SPSC 队列或双缓冲（wait-free）
    │   └── 否 ─┐
    │           │
    │   ├── 可以拆成多个 SPSC？
    │   │   ├── 是 → 多条 SPSC 汇聚（每条 wait-free）
    │   │   └── 否 ─┐
    │   │           │
    │   │   ├── 读者在实时路径？
    │   │   │   ├── 是 → bounded try + 降级
    │   │   │   └── 否 → lock-free 库或 mutex

这个决策树反映了一个工程原则：不是选择"最强的无阻塞保证"，而是选择"问题边界最窄的方案"。SPSC 比 MPMC 简单不是因为能力弱，而是因为约束强。约束越强，正确性越容易保证，测试越容易覆盖。

本质洞察：wait-free 不是追求的目标，确定性时间边界才是。SPSC 恰好是 wait-free 的，因为它把竞争彻底消除了。bounded try 给出有限步数保证，因为它用"放弃"替代"无限等待"。控制系统设计的核心是让每个操作的最坏耗时可以被预算。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
概念	把 lock-free 等同于 wait-free	控制偶发超时	lock-free 不保证单线程有界	对控制路径使用 wait-free 或 bounded try
概念	追求通用 wait-free MPMC	代码极其复杂	通用构造常数开销大	收窄为 SPSC 或使用成熟库
思维	认为 bounded try 是"妥协"	设计犹豫	控制系统需要确定性时间	bounded try 是刻意设计的时间边界
工程	不测量最坏耗时只看平均	上机偶发超时	尾延迟未覆盖	p99 和 max 必须在预算内

练习

1. 解释为什么 SPSC 队列的 push 是 wait-free 的，而一个朴素 MPMC 队列的 push 只是 lock-free 的。
2. 为一个"多传感器写入状态估计"场景设计方案：每个传感器一条 SPSC，估计线程轮询。分析为什么这比一条 MPMC 更适合实时系统。
3. 实现一个 bounded try 版本的 compare_exchange，设置最大尝试 5 次，并在测试中制造竞争观察失败比例。

---

3.15 memory_order 在无锁结构中的正确使用 ⭐⭐⭐

这一节解决的问题是：如何在无锁数据结构的每个操作中系统性地选择正确的 memory_order。

选择 memory_order 的思考框架

memory_order 不是性能调参。它是并发协议的一部分。选择 memory_order 时，应该问三个问题：

1. 这个原子操作是否需要同步其他非原子数据？
2. 如果需要同步，谁是数据的发布方，谁是接收方？
3. 同步的范围是什么——单向发布还是双向交换？

答案	选择	例子
不同步其他数据	relaxed	丢包计数、统计采样
发布方写入数据后发布标志	release	写帧后 release store 索引
接收方读取标志后读取数据	acquire	acquire load 索引后读帧
既读又写，且同步双方	acq_rel	exchange 交换缓冲索引
需要全局顺序（罕见）	seq_cst	多个独立原子变量协调

SPSC 队列中的 memory_order 审计

回顾前面的 SPSC 队列实现，每个原子操作的 memory_order 都有明确理由。

push:
  1. head_.load(relaxed)
     理由：只有生产者写 head，本线程读自己的变量不需要同步。

  2. tail_.load(acquire)
     理由：需要看到消费者已经归还的槽位。消费者的 tail_.store(release)
     与这里的 acquire 配对，保证消费者读完的数据不会被生产者覆盖。

  3. buffer_[head] = value
     理由：普通写入，受后续 head_.store(release) 保护。

  4. head_.store(next, release)
     理由：发布新元素。消费者 acquire load 看到新 head 后，
     能看到 buffer_[head] 的完整写入。

pop:
  1. tail_.load(relaxed)
     理由：只有消费者写 tail，本线程读自己的变量不需要同步。

  2. head_.load(acquire)
     理由：需要看到生产者发布的新元素。生产者的 head_.store(release)
     与这里的 acquire 配对。

  3. *value = buffer_[tail]
     理由：普通读取，受前面 head_.load(acquire) 保护。

  4. tail_.store(next, release)
     理由：归还槽位。生产者 acquire load 看到新 tail 后，
     能安全覆盖旧槽位。

这种逐操作审计方法适合所有无锁结构。写代码时在注释中标注"这条操作与哪条操作配对，同步哪些普通数据"，比只写 release 或 acquire 有价值得多。

三缓冲中的 memory_order

三缓冲使用 exchange（读改写），其 memory_order 需要同时考虑读和写。

publish:
  exchange(write_idx, acq_rel)
  理由：
    - release 语义：把 writeBuffer 的普通写入发布出去。
    - acquire 语义：拿回旧 ready_idx，后续可以安全复用该缓冲。

readLatest:
  exchange(read_idx, acq_rel)
  理由：
    - acquire 语义：获取最新数据的可见性。
    - release 语义：归还旧 read_idx 指向的缓冲，写者可以复用。

如果把 exchange 改成 relaxed，写者可能在读者还没读完旧缓冲时就开始覆盖它。这不会被 TSan 报告为数据竞争（因为 exchange 本身是原子的），但业务数据的一致性已经被破坏。

常见错误模式

错误	表现	平台差异	修复
标志位用 relaxed 同步数据	ARM 上偶发旧值	x86 TSO 隐藏问题	标志位 release，读取 acquire
双缓冲索引用 relaxed	读到新索引但旧帧	ARM/RISC-V 暴露	索引 release/acquire
统计计数用 seq_cst	浪费性能但不出错	所有平台正确但慢	改 relaxed
CAS 成功用 relaxed	其他线程看不到 CAS 保护的数据	弱序平台	CAS 成功用 acq_rel

用工具验证 memory_order

C++ 标准定义的内存模型是形式化的。对关键代码，可以用以下工具验证：

工具	作用	使用场景
CDSChecker	验证 C/C++ 内存模型下所有可能执行路径	小型无锁协议
CppMem	可视化 happens-before 关系	教学和理解
ThreadSanitizer	运行时检测数据竞争	CI 和压力测试
herd7/litmus7	硬件内存模型测试	跨平台验证

对初学者最实用的方法是：在代码注释中画出 happens-before 图（如 3.6 节所示），然后用 TSan 在真实平台上做压力测试。如果图画对了且 TSan 没有报告，代码在所有合规平台上都应该正确。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
编程	所有操作都用 seq_cst	性能差且意图不清	内存序应表达同步意图	按发布/接收模式选择
编程	CAS 两个分支用不同 memory_order 但不理解	偶发不一致	failure order 也影响可见性	成功 acq_rel，失败至少 acquire
概念	认为 memory_order 是 CPU 指令选择	移植后错误	标准定义的是抽象关系	按 C++ 标准推理，不按特定 CPU
思维	只在 x86 测试就认为 memory_order 选对了	ARM 上出错	x86 TSO 比标准更强	在弱序平台或模拟器上测试

练习

1. 对本章 JointSeqlock 的每个原子操作，写出它同步的普通数据和配对操作。
2. 把 LatestFrame<T> 的 publish 中的 release 改成 relaxed，用 TSan 或 ARM 平台观察是否出问题。
3. 解释为什么 SPSC 队列中生产者读 tail_ 需要 acquire，而读自己的 head_ 可以用 relaxed。

---

3.16 无锁数据结构在 SLAM 与 MPC 中的应用 ⭐⭐⭐

这一节解决的问题是：SLAM 和 MPC 系统中的哪些数据流适合用无锁结构优化。

SLAM 中的典型多线程架构

现代视觉惯性 SLAM 系统（如 VINS-Mono、ORB-SLAM3、LIO-SAM）通常包含多个并行线程：

线程	数据输入	数据输出	频率
IMU 前端	IMU 原始数据	预积分结果	200-1000 Hz
视觉前端	图像帧	特征点、描述子	15-30 Hz
后端优化	因子和变量	优化后位姿	1-10 Hz
回环检测	关键帧描述	回环约束	低频
地图管理	位姿和点云	稀疏/稠密地图	低频

这些线程之间的数据交换恰好覆盖本章所有模式：

数据流	交换模式	推荐结构
IMU 数据到预积分	每条都要处理	SPSC 队列
预积分到后端	关键帧触发	SPSC 队列或事件通知
后端优化结果到前端	最新值	双缓冲
前端位姿到控制	最新值	双缓冲
回环约束到后端	每条都要处理	SPSC 队列
地图快照到可视化	最新值	RCU 或三缓冲

在 LIO-SAM 等激光惯性里程计中，点云和 IMU 的时间对齐是一个关键问题。如果 IMU 前端把预积分结果放入队列，后端需要按时间戳查找对应的预积分段。这时队列的消费策略不是简单的 FIFO，而是"找到与当前关键帧时间最近的预积分"。一种做法是前端把预积分结果放入一个按时间排序的环形缓冲，后端按时间戳二分查找。

MPC 中的典型数据流

MPC（模型预测控制）系统的数据流更简单但频率更高。

估计线程 ──双缓冲(状态)──► MPC 线程 ──双缓冲(轨迹)──► WBC/控制线程
                                                         │
                                                    SPSC(trace)
                                                         │
                                                         ▼
                                                      日志线程

MPC 线程的典型运行频率是 20-100 Hz，但它需要读取 500-1000 Hz 的状态估计结果。状态估计线程不应等待 MPC 线程读完才更新。双缓冲让状态估计线程随时发布最新状态，MPC 线程在每次求解开始时读取最新帧。

MPC 输出的轨迹通常是一个 horizon 内的状态和控制序列。这个轨迹可能有几十到几百个浮点数。三缓冲适合这种"中等大小、频繁覆盖"的数据。

延迟预算分析

在一个 1 kHz 控制系统中，控制周期是 1 ms。无锁数据交换的延迟必须在预算内。

操作	典型延迟	占周期比例
双缓冲 acquire load + 帧拷贝	0.1-1 us	0.01-0.1%
SPSC push（小 trace）	50-200 ns	0.005-0.02%
三缓冲 exchange + 帧拷贝	0.1-2 us	0.01-0.2%
mutex lock/unlock（无竞争）	50-100 ns	0.005-0.01%
mutex lock/unlock（竞争）	1-100 us	0.1-10%
DDS 发布（进程间）	10-100 us	1-10%

无竞争 mutex 的延迟其实很低。无锁结构的优势不在于快几十纳秒，而在于消除竞争路径上的不可预测等待。当 mutex 存在竞争时，最坏延迟可能跳到 100 us，这在 1 ms 周期中占 10%。无锁双缓冲的最坏延迟仍然是帧拷贝时间，完全确定。

本质洞察：无锁结构在 SLAM 和 MPC 中的价值不是"更快"，而是"延迟可预测"。SLAM 后端优化可能运行几百毫秒，如果它持有 mutex 期间控制线程需要读取状态，控制周期就会被拉长到不可接受。无锁双缓冲让控制线程的读取延迟与后端优化的运行时间完全解耦。

常见陷阱

类型	错误做法	现象	根本原因	正确做法
工程	SLAM 后端和控制共享 mutex 保护的状态	控制偶发超时	后端优化持锁时间长	双缓冲解耦
概念	MPC 轨迹用队列逐条处理	控制追旧轨迹	轨迹是最新值语义	双缓冲或三缓冲
编程	IMU 数据用最新帧而非队列	预积分丢中间数据	IMU 每条都要积分	SPSC 队列
思维	所有线程间共享都用同一种结构	语义混乱	不同数据流有不同语义	按语义选择结构

练习

1. 为一个 LIO 系统画出线程数据流图，为每条数据流标注推荐的无锁结构和理由。
2. 估算 500 Hz 状态估计到 20 Hz MPC 到 1 kHz WBC 的延迟链路，找出最大延迟瓶颈。
3. 设计一个 MPC 轨迹发布接口，使用三缓冲，并为轨迹添加版本号和时间戳。

---

本章小结

知识点	关键结论	工程动作
并发分类	原子性、可见性、语义一致性不同	先定义数据交换语义
内存序	release/acquire 建立发布关系	不滥用 relaxed 和 seq_cst
数据结构	场景越受限越可靠	双缓冲、SPSC、seqlock 优先
实时边界	无锁不等于无等待	队列满和重试都要有上界
测试	x86 正常不代表标准正确	多线程、弱序、尾延迟都要测
内存模型	正确性来自完整 happens-before 路径	为每个发布点画出同步图
SPSC/MPMC	多方竞争会引入 CAS、ABA 和回收问题	优先用单写通道或成熟库
缓存一致性	无锁结构也可能被伪共享拖慢	隔离跨线程热点写字段
实时数据通道	每条数据流都有自己的语义	最新帧、队列、参数包分开设计
RCU	读多写少场景让读者零开销	配置和地图用 RCU，状态用双缓冲
ROS2 集成	DDS 不适合硬实时路径	实时核心用进程内无锁结构
lock-free vs wait-free	wait-free 需要问题边界窄	控制路径用 SPSC 或 bounded try
memory_order 审计	每个操作标注同步的数据和配对	逐操作审计 + TSan 验证
SLAM/MPC 应用	不同数据流有不同无锁模式	状态双缓冲、IMU 队列、轨迹三缓冲

累积项目：实时数据交换模块

本章新增模块是 rt_data_exchange。

阶段 1：实现 LatestFrame<T>，用于估计状态到控制线程。 阶段 2：实现 SpscRing<T, N>，用于控制 trace 到日志线程。 阶段 3：实现 DropCounter，使用 relaxed 原子统计丢弃次数。 阶段 4：为三个结构写 gtest 和多线程压力测试。 阶段 5：在 1 kHz 控制骨架中接入这些结构，记录最大 push/read 耗时。 阶段 6：为状态、轨迹、参数和 trace 分别画出数据流图，检查单写原则。 阶段 7：加入伪共享基准测试，对比对齐前后的 p99 和最大延迟。 阶段 8：为队列满、状态超时和参数验证失败设计降级策略。

延伸阅读

资料	难度	阅读目的
C++ 标准库 atomic 文档	⭐⭐	理解内存序正式语义
Anthony Williams, C++ Concurrency in Action	⭐⭐⭐	系统学习并发模式
moodycamel ReaderWriterQueue 文档	⭐⭐	学习成熟 SPSC 队列设计
Boost lockfree 文档	⭐⭐	理解 lock-free 容器约束
PX4 uORB 设计	⭐⭐	学习嵌入式发布订阅思路

故障排查手册

症状	可能原因	排查步骤	处理
ARM 上偶发读到旧数据	用 relaxed 发布数据	检查标志位内存序	改为 release/acquire
控制状态位置速度不匹配	多个字段分别发布	打印时间戳组合	整帧双缓冲发布
队列吞吐突然下降	head/tail 伪共享	perf 观察 cache miss	cache line 对齐
日志丢失严重	消费者慢于生产者	统计队列占用和丢弃计数	增大容量或降低日志频率
seqlock 读取失败	写者频率过高	记录重试次数	降低写频率或换双缓冲
多生产者使用 SPSC	数据损坏或覆盖	检查调用线程数量	改 MPSC/MPMC 或拆分队列