3、数据结构在自动驾驶中的应用：链表、队列与栈

各位同学，今天我们来聊聊数据结构在自动驾驶中的实战应用。说实话，很多初学者觉得数据结构就是面试题，跟实际工程没啥关系。但我要告诉你，在自动驾驶系统里，数据结构就是骨架，选对了，系统跑得稳；选错了，车可能就停在路中间了。

我个人习惯把数据结构分成三类来理解：管理路径的、管理数据的、管理状态的。正好对应我们今天要讲的链表、队列和栈。

3.1 链表：路径点管理的利器

先说说链表。在自动驾驶中，路径点管理是个典型场景。你想想看，车辆规划出一条路径，可能有几百个点，而且随时可能插入新点、删除旧点、或者调整顺序。用数组？太死板了。用链表？刚刚好。

单向链表：最简单的路径存储

单向链表每个节点只指向下一个节点。我在项目中用它来存储简单的路径点序列，比如从A到B的直线路径。

typedef struct PathNode {
    double x, y;          // 坐标
    double speed;         // 目标速度
    struct PathNode* next; // 指向下一个节点
} PathNode_t;

// 插入一个新路径点
void insertPathNode(PathNode_t** head, double x, double y, double speed) {
    PathNode_t* newNode = (PathNode_t*)malloc(sizeof(PathNode_t));
    newNode->x = x;
    newNode->y = y;
    newNode->speed = speed;
    newNode->next = *head;
    *head = newNode;
}

嗯，这里要注意：单向链表只能单向遍历。如果你需要从后往前回溯路径，那就麻烦了。我曾经在一个项目中用单向链表存储历史轨迹，结果调试时发现没法快速找到上一个点，气得我直接改成了双向链表。

双向链表：灵活的前后操作

双向链表每个节点有prev和next两个指针。这在路径规划中特别有用，比如你要在某个点前后各插入一个避障点。

typedef struct PathNode {
    double x, y;
    double speed;
    struct PathNode* prev;
    struct PathNode* next;
} PathNode_t;

// 在指定节点后插入
void insertAfter(PathNode_t* node, double x, double y) {
    PathNode_t* newNode = (PathNode_t*)malloc(sizeof(PathNode_t));
    newNode->x = x;
    newNode->y = y;
    newNode->prev = node;
    newNode->next = node->next;
    if (node->next) node->next->prev = newNode;
    node->next = newNode;
}

循环链表：环形路径的天然选择

循环链表就是尾节点指向头节点。你想想，自动驾驶在停车场里绕圈找车位，或者做U型掉头，路径本身就是环形的。用循环链表，遍历到末尾自动回到起点，省去了判断边界的麻烦。

// 循环链表遍历示例
void traverseCircular(PathNode_t* start) {
    PathNode_t* current = start;
    do {
        processPoint(current);
        current = current->next;
    } while (current != start);  // 回到起点就停
}

3.2 队列与环形缓冲区：CAN/LIN数据流的守护者

接下来聊队列。自动驾驶系统里，CAN总线和LIN总线上的数据是源源不断的。你不可能每来一帧数据就立刻处理，那样CPU会忙死。所以我们需要一个缓冲区，先把数据存起来，等CPU有空了再慢慢处理。

普通队列：先进先出，简单可靠

队列就是先进先出（FIFO）的结构。CAN数据帧来了，塞进队列尾部；处理线程从队列头部取数据。就这么简单。

#define QUEUE_SIZE 256

typedef struct {
    CAN_Frame_t buffer[QUEUE_SIZE];
    int head;
    int tail;
    int count;
} CAN_Queue_t;

// 入队
int enqueue(CAN_Queue_t* q, CAN_Frame_t* frame) {
    if (q->count >= QUEUE_SIZE) return -1;  // 队列满了
    q->buffer[q->tail] = *frame;
    q->tail = (q->tail + 1) % QUEUE_SIZE;
    q->count++;
    return 0;
}

但普通队列有个问题：如果数据量太大，队列会满。我记得有一次在实车测试中，CAN总线突然爆发大量报文，队列直接溢出，丢了好多关键数据。后来我改用环形缓冲区解决了这个问题。

环形缓冲区：永不溢出的设计

环形缓冲区本质上还是队列，但它允许覆盖旧数据。对于CAN/LIN数据流来说，旧数据丢了就丢了，新数据才重要。说白了，环形缓冲区就是「喜新厌旧」的队列。

typedef struct {
    uint8_t* buffer;
    int size;
    int write_idx;
    int read_idx;
} RingBuffer_t;

// 写入环形缓冲区
void ringBufferWrite(RingBuffer_t* rb, uint8_t data) {
    rb->buffer[rb->write_idx] = data;
    rb->write_idx = (rb->write_idx + 1) % rb->size;
    // 如果写指针追上读指针，说明覆盖了旧数据
    if (rb->write_idx == rb->read_idx) {
        rb->read_idx = (rb->read_idx + 1) % rb->size;
    }
}

3.3 栈：状态机回溯的救星

最后说说栈。自动驾驶的状态机有多复杂？你想想看：车辆启动、自检、等待指令、规划路径、执行控制、紧急制动、故障恢复...每个状态都可能嵌套子状态。一旦出问题，你得知道「我是从哪个状态过来的」。

栈就是干这个的。后进先出（LIFO）的特性，天然适合记录状态历史。

#define STATE_STACK_SIZE 32

typedef struct {
    VehicleState_t states[STATE_STACK_SIZE];
    int top;
} StateStack_t;

// 进入新状态时压栈
void pushState(StateStack_t* stack, VehicleState_t state) {
    if (stack->top < STATE_STACK_SIZE - 1) {
        stack->states[++stack->top] = state;
    }
}

// 回溯到上一个状态
VehicleState_t popState(StateStack_t* stack) {
    if (stack->top >= 0) {
        return stack->states[stack->top--];
    }
    return STATE_IDLE;  // 栈空了就回到空闲状态
}

为什么会用到栈回溯？举个例子：车辆正在执行「自动泊车」状态，突然检测到前方有行人，于是进入「紧急制动」状态。等行人走过后，你需要回到「自动泊车」状态继续执行。这时候从栈里弹出上一个状态，完美恢复现场。

小结

今天的内容就这些。总结一下：

• 链表：路径点管理，单向简单、双向灵活、循环适合环形路径
• 队列/环形缓冲区：CAN/LIN数据流处理，环形缓冲区不怕溢出
• 栈：状态机回溯，后进先出，恢复现场的好帮手

这些数据结构在自动驾驶中每天都在用。你写代码的时候，多想想「这个场景适合哪种结构」，而不是上来就写数组。数据结构选对了，代码质量能提升一个档次。

下一章我们讲哈希表在传感器数据融合中的应用，到时候见。