数据结构选型：针对路径规划算法的轻量级数据结构设计原则

做嵌入式路径规划，说白了就是跟内存较劲。我见过太多工程师，算法写得漂亮，一上板子就崩——内存爆了。嗯，这里要聊的，就是怎么用最轻量的数据结构，把路径规划跑起来。

为什么数据结构这么重要？

你想想看，一个典型的A*算法，要维护open list和closed list。如果直接用标准库的vector或者list，在资源受限的MCU上，分分钟把堆栈撑爆。我个人习惯，在选型之前先问三个问题：

• 这个结构要存多少数据？——预估最大节点数
• 访问模式是什么？——频繁插入删除？还是只读遍历？
• 内存是静态分配还是动态分配？——嵌入式里我尽量不用malloc

我在项目中遇到过，一个同事用std::priority_queue做A*的open list，结果在STM32F4上跑了不到100个节点就hard fault了。为什么？动态内存碎片化，加上priority_queue内部用vector，频繁realloc。

原则一：能用数组就别用链表

很多教科书喜欢用链表讲图论。但在嵌入式里，链表是魔鬼。每个节点多一个指针，就是4字节（32位机）或8字节（64位机）。1000个节点，光指针就多占4KB——这在很多MCU上已经是半个RAM了。

我建议用静态数组+索引的方式模拟链表。比如：

// 轻量级邻接表
#define MAX_NODES 128
#define MAX_EDGES 512

typedef struct {
    int16_t next;      // 下一条边的索引，-1表示结束
    int16_t to;        // 目标节点
    uint16_t cost;     // 代价
} Edge;

Edge edges[MAX_EDGES];
int16_t head[MAX_NODES];  // 每个节点的第一条边索引

// 初始化
void graph_init() {
    memset(head, -1, sizeof(head));
    // edge_count 全局维护
}

你看，没有malloc，没有指针，全是int16_t。一个边结构才6字节。我曾经用这个结构在Cortex-M0上跑Dijkstra，512条边只用了3KB内存，稳得很。

原则二：优先队列用二叉堆，别用红黑树

路径规划里，open list需要频繁取最小代价节点。很多人第一反应是红黑树或平衡树。但嵌入式里，二叉堆就够了。

为什么？

• 二叉堆插入O(log n)，删除最小O(log n)，和红黑树一样
• 但二叉堆用数组实现，没有指针开销
• 红黑树每个节点要存颜色、左右子指针、父指针——至少多占12字节

我常用的最小堆实现：

typedef struct {
    int16_t node_id;
    uint16_t f_cost;   // 启发式代价
} HeapNode;

typedef struct {
    HeapNode data[MAX_NODES];
    int16_t size;
} MinHeap;

// 上浮操作
void heap_push(MinHeap* heap, int16_t node, uint16_t cost) {
    int16_t i = heap->size++;
    heap->data[i].node_id = node;
    heap->data[i].f_cost = cost;
    
    while (i > 0) {
        int16_t parent = (i - 1) / 2;
        if (heap->data[parent].f_cost <= heap->data[i].f_cost) break;
        // 交换
        HeapNode tmp = heap->data[parent];
        heap->data[parent] = heap->data[i];
        heap->data[i] = tmp;
        i = parent;
    }
}

注意看，我用int16_t做索引，最大支持32767个节点。如果节点数不超过255，甚至可以用uint8_t。这就是轻量化的细节。

原则三：closed list用位图，别用哈希表

closed list只需要回答一个问题：这个节点访问过没有？

哈希表？太奢侈了。每个节点要存key、value、next指针。嵌入式里，位图（bitmap）是最优解。

// 位图实现closed list
#define NODE_BITS 128  // 支持128个节点
uint8_t closed_bitmap[(NODE_BITS + 7) / 8];

inline void closed_set(uint8_t node) {
    closed_bitmap[node / 8] |= (1 << (node % 8));
}

inline uint8_t closed_test(uint8_t node) {
    return (closed_bitmap[node / 8] >> (node % 8)) & 0x01;
}

128个节点，位图只占16字节。如果用哈希表，至少几百字节。我记得有一次做AGV路径规划，地图有2000个栅格，位图只用了250字节。同事用unordered_set，直接吃了8KB——板子内存总共才64KB。

原则四：路径存储用环形缓冲区

路径规划的结果是一条节点序列。传统做法是用动态数组，push_back一路追加。但嵌入式里，路径长度通常有上限（比如最大100个节点）。用环形缓冲区（ring buffer）更合适。

#define PATH_MAX 64
typedef struct {
    uint8_t nodes[PATH_MAX];
    uint8_t head;
    uint8_t tail;
    uint8_t count;
} PathBuffer;

// 从尾部添加节点
uint8_t path_push_back(PathBuffer* pb, uint8_t node) {
    if (pb->count >= PATH_MAX) return 0;  // 满了
    pb->nodes[pb->tail] = node;
    pb->tail = (pb->tail + 1) % PATH_MAX;
    pb->count++;
    return 1;
}

// 从头部取出节点
uint8_t path_pop_front(PathBuffer* pb, uint8_t* node) {
    if (pb->count == 0) return 0;
    *node = pb->nodes[pb->head];
    pb->head = (pb->head + 1) % PATH_MAX;
    pb->count--;
    return 1;
}

好处是什么？

• 固定内存，没有动态分配
• 支持双向操作（从起点到终点，或者反向回溯）
• 覆盖旧路径时，不需要释放内存

我一般在路径规划完成后，把结果从环形缓冲区拷贝到全局路径数组里。这样规划模块和路径执行模块解耦，各自用各自的数据结构。

原则五：启发式代价用查表法

路径规划里经常要算欧几里得距离或曼哈顿距离。每次实时计算sqrt或者乘法，既慢又费电。我建议预计算+查表。

比如，对于栅格地图，可以预计算每个栅格到目标点的启发式代价：

// 预计算曼哈顿距离
uint16_t heuristic_table[MAX_NODES];

void precompute_heuristic(uint8_t goal_x, uint8_t goal_y, 
                          uint8_t* node_x, uint8_t* node_y, 
                          uint16_t node_count) {
    for (uint16_t i = 0; i < node_count; i++) {
        int16_t dx = (int16_t)node_x[i] - (int16_t)goal_x;
        int16_t dy = (int16_t)node_y[i] - (int16_t)goal_y;
        heuristic_table[i] = (uint16_t)(abs(dx) + abs(dy));
    }
}

这样在A*运行时，取启发式代价就是一次数组访问，O(1)。代价是多了几百字节的表格。但比起每次算sqrt，省下的CPU时间可以跑更多控制逻辑。

总结一下我的选型清单

数据结构	推荐方案	内存开销（1000节点）	适用场景
图/邻接表	静态数组+索引	~6KB（边）	固定拓扑地图
优先队列	二叉堆（数组）	~4KB	A*/Dijkstra
Closed list	位图	~125字节	节点访问标记
路径存储	环形缓冲区	~64字节	路径结果缓存
启发式代价	查表法	~2KB	静态目标点

你看，全部加起来不到12KB。在大多数Cortex-M系列芯片上，都能跑得很舒服。我做过一个项目，用STM32F103（20KB RAM）跑A*，地图200个节点，内存占用不到8KB，还留了12KB给其他任务。

最后说一句：数据结构选型不是炫技，是妥协的艺术。在嵌入式里，每字节都珍贵。你省下的内存，就是留给系统稳定性的余量。嗯，今天就聊到这里，下一章我们聊聊内存池的设计。