基于C语言的A和D寻迹算法实现

一、A*算法实现

1. 数据结构定义

typedef struct {
    int x, y;           // 节点坐标
    int g;              // 起点到当前节点的实际代价
    int h;              // 启发式估计代价
    int f;              // f = g + h
    struct Node* parent; // 父节点指针
} Node;

typedef struct {
    Node** array;       // 节点数组
    int capacity;       // 队列容量
    int size;           // 当前元素数量
} PriorityQueue;

2. 核心算法实现

// 启发式函数（曼哈顿距离）
int heuristic(int x1, int y1, int x2, int y2) {
    return abs(x1-x2) + abs(y1-y2);
}

// 优先队列操作
void pq_insert(PriorityQueue* pq, Node* node) {
    pq->array[pq->size++] = node;
    int i = pq->size - 1;
    while (i > 0 && 
           (pq->array[(i-1)/2]->f > node->f ||
            (pq->array[(i-1)/2]->f == node->f &&
             pq->array[(i-1)/2]->h > node->h))) {
        Node* temp = pq->array[i];
        pq->array[i] = pq->array[(i-1)/2];
        pq->array[(i-1)/2] = temp;
        i = (i-1)/2;
    }
}

Node* pq_extract_min(PriorityQueue* pq) {
    Node* min = pq->array[0];
    pq->array[0] = pq->array[--pq->size];
    int i = 0;
    while (1) {
        int left = 2*i+1, right = 2*i+2;
        int smallest = i;
        if (left < pq->size && 
            (pq->array[left]->f < pq->array[smallest]->f ||
             (pq->array[left]->f == pq->array[smallest]->f &&
              pq->array[left]->h < pq->array[smallest]->h))) {
            smallest = left;
        }
        if (right < pq->size && 
            (pq->array[right]->f < pq->array[smallest]->f ||
             (pq->array[right]->f == pq->array[smallest]->f &&
              pq->array[right]->h < pq->array[smallest]->h))) {
            smallest = right;
        }
        if (smallest != i) {
            Node* temp = pq->array[i];
            pq->array[i] = pq->array[smallest];
            pq->array[smallest] = temp;
            i = smallest;
        } else {
            break;
        }
    }
    return min;
}

// A*算法主函数
Node* a_star(int map[ROWS][COLS], int start[2], int goal[2]) {
    PriorityQueue open_set = {0};
    Node** all_nodes = calloc(ROWS*COLS, sizeof(Node*));
    
    // 初始化所有节点
    for (int i=0; i<ROWS; i++) {
        for (int j=0; j<COLS; j++) {
            Node* node = calloc(1, sizeof(Node));
            node->x = i; node->y = j;
            all_nodes[i*COLS + j] = node;
        }
    }
    
    Node* start_node = all_nodes[start[0]*COLS + start[1]];
    Node* goal_node = all_nodes[goal[0]*COLS + goal[1]];
    
    start_node->g = 0;
    start_node->h = heuristic(start[0], start[1], goal[0], goal[1]);
    start_node->f = start_node->g + start_node->h;
    
    pq_insert(&open_set, start_node);
    
    while (open_set.size > 0) {
        Node* current = pq_extract_min(&open_set);
        
        if (current == goal_node) {
            // 路径回溯
            Node* path = current;
            while (path->parent) {
                path = path->parent;
            }
            free(open_set.array);
            return path;
        }
        
        for (int d[2][8] = {{-1,0},{1,0},{0,-1},{0,1},{-1,-1},{1,-1},{-1,1},{1,1}}) {
            int nx = current->x + d[0][i];
            int ny = current->y + d[1][i];
            
            if (nx < 0 || nx >= ROWS || ny < 0 || ny >= COLS || map[nx][ny]) continue;
            
            Node* neighbor = all_nodes[nx*COLS + ny];
            int tentative_g = current->g + 1;
            
            if (tentative_g < neighbor->g) {
                neighbor->parent = current;
                neighbor->g = tentative_g;
                neighbor->h = heuristic(nx, ny, goal[0], goal[1]);
                neighbor->f = neighbor->g + neighbor->h;
                pq_insert(&open_set, neighbor);
            }
        }
    }
    
    free(open_set.array);
    return NULL; // 未找到路径
}

---

二、D*算法实现

1. 数据结构定义

typedef struct {
    int x, y;
    int g;      // 实际代价
    int rhs;    // 右手边值
    int key[2]; // 优先级队列键值
} DStarNode;

typedef struct {
    DStarNode** array;
    int capacity;
    int size;
} DStarQueue;

2. 核心算法实现

// D*初始化
void dstar_init(int map[ROWS][COLS], int start[2], int goal[2]) {
    for (int i=0; i<ROWS; i++) {
        for (int j=0; j<COLS; j++) {
            DStarNode* node = calloc(1, sizeof(DStarNode));
            node->x = i; node->y = j;
            node->g = node->rhs = INT_MAX;
            if (i == goal[0] && j == goal[1]) {
                node->rhs = 0;
            }
            pq_insert(&open_list, node);
        }
    }
}

// 计算键值
void calculate_key(DStarNode* node, int start[2]) {
    node->key[0] = fmin(node->g, node->rhs) + 
                  (abs(node->x - start[0]) + abs(node->y - start[1])) * 2;
    node->key[1] = fmin(node->g, node->rhs);
}

// D*主循环
void dstar_main_loop() {
    while (!pq_empty() || open_list.size > 0) {
        DStarNode* u = pq_extract_min();
        
        if (u->g > u->rhs) {
            u->g = u->rhs;
            for (int d[2][8] = {{-1,0},{1,0},{0,-1},{0,1},{-1,-1},{1,-1},{-1,1},{1,1}}) {
                int nx = u->x + d[0][i];
                int ny = u->y + d[1][i];
                
                if (valid(nx, ny)) {
                    DStarNode* v = get_node(nx, ny);
                    if (v->g > u->g + 1) {
                        v->rhs = u->g + 1;
                        calculate_key(v, start);
                        pq_insert(&open_list, v);
                    }
                }
            }
        } else {
            u->g = INT_MAX;
            for (int d[2][8] = {{-1,0},{1,0},{0,-1},{0,1},{-1,-1},{1,-1},{-1,1},{1,1}}) {
                int nx = u->x + d[0][i];
                int ny = u->y + d[1][i];
                
                if (valid(nx, ny)) {
                    DStarNode* v = get_node(nx, ny);
                    if (v->rhs == u->g + 1) {
                        v->rhs = INT_MAX;
                        for (int d2[2][8] = {{-1,0},{1,0},{0,-1},{0,1},{-1,-1},{1,-1},{-1,1},{1,1}}) {
                            int nnx = v->x + d2[0][i];
                            int nny = v->y + d2[1][i];
                            
                            if (valid(nnx, nny)) {
                                DStarNode* w = get_node(nnx, nny);
                                if (w->rhs != INT_MAX) {
                                    w->rhs = fmin(w->rhs, INT_MAX);
                                    calculate_key(w, start);
                                    pq_insert(&open_list, w);
                                }
                            }
                        }
                    }
                }
            }
        }
    }
}

---

三、算法对比与选型建议

特性	A*算法	D*算法
搜索方向	正向搜索（起点→终点）	反向搜索（终点→起点）
动态障碍处理	需全量重计算	支持局部增量更新
内存消耗	O(b^d)	O(m)
典型应用场景	静态地图路径规划	动态障碍物场景
代码复杂度	中等	高

选型建议：

• 静态环境：优先选择A*算法（代码量少，易于调试）
• 动态环境：使用D*算法（支持实时路径更新）
• 实时性要求高：考虑D* Lite优化版本（搜索效率提升30%+）

---

四、性能优化技巧

1. 启发式函数优化：

   // 对角线距离启发式（A*）
   int heuristic(int x1, int y1, int x2, int y2) {
       return (abs(x1-x2) + abs(y1-y2)) * 10 + 
              (max(abs(x1-x2), abs(y1-y2)) - min(abs(x1-x2), abs(y1-y2))) * 14;
   }
   

2. 内存池管理：

   #define POOL_SIZE 10000
   Node node_pool[POOL_SIZE];
   int pool_idx = 0;
   
   Node* create_node() {
       if (pool_idx >= POOL_SIZE) return NULL;
       return &node_pool[pool_idx++];
   }
   

3. 并行化处理（OpenMP）：

   #pragma omp parallel for
   for (int i=0; i<ROWS; i++) {
       for (int j=0; j<COLS; j++) {
           process_node(i, j);
       }
   }
   

---

五、调试与验证

1. 路径可视化：

   void print_path(Node* path) {
       while (path) {
           printf("(%d,%d) -> ", path->x, path->y);
           path = path->parent;
       }
       printf("END\n");
   }
   

2. 性能分析：

   clock_t start = clock();
   Node* path = a_star(map, start, goal);
   double duration = (double)(clock() - start)/CLOCKS_PER_SEC;
   printf("A*耗时: %.3f秒，路径长度: %d\n", duration, path_length(path));
   

参考代码 Astar和Dstar寻迹算法的实现 www.youwenfan.com/contentcnk/70050.html

六、工程应用示例

1. 机器人导航系统架构：

   传感器数据 → 地图更新 → D*算法 → 运动控制
               ↑           ↓
             障碍检测   路径平滑
   

2. 动态避障流程：

---

通过上述实现方案，可满足不同场景下的路径规划需求。建议结合具体硬件平台进行性能调优，并参考《机器人路径规划算法实战》进行深度优化。