第四讲：路径规划算法入门——A*算法原理、Dijkstra算法、栅格地图构建

各位同学，今天咱们进入路径规划的核心地带。说实话，我刚入行那会儿，觉得路径规划就是“找条路走过去”，后来在植保机器人项目里被现实狠狠教育了一顿——田里有电线杆、有水沟、有没长齐的庄稼，随便一条路都可能让机器卡住。嗯，今天讲的这两个算法，就是解决“怎么走最划算”这个问题的。

4.1 栅格地图：把现实世界变成格子

在做路径规划之前，得先把环境“翻译”成机器能懂的语言。我个人习惯用栅格地图，说白了就是把农田拍扁，切成一个个小方格。

栅格地图的构建思路：

• 每个格子代表一块区域，比如1米×1米
• 格子分两种：可通行（庄稼间隙、空地）和不可通行（障碍物、边界）
• 用0表示空地，1表示障碍物，存成一个二维数组

举个例子，一块10米×10米的田，有3个障碍物，地图大概长这样：

// 栅格地图示例（10x10）
int map[10][10] = {
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,1,0,0,0,0,0,0},  // 障碍物1
    {0,0,0,1,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,1,1,0,0,0},  // 障碍物2和3
    {0,0,0,0,0,1,1,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0}
};

4.2 Dijkstra算法：老实人的最短路径

Dijkstra算法，你想想看，它就像个老实巴交的快递员——从起点开始，一层一层往外扩，直到找到终点。它保证找到的路径是全局最短的，但代价是慢。

核心思想：

1. 从起点出发，标记起点距离为0，其他点距离为无穷大
2. 每次选一个距离最小的未访问点，更新它邻居的距离
3. 重复直到所有点都访问过

代码实现（C语言风格）：

#define INF 99999
#define N 100  // 栅格数量

void dijkstra(int map[N][N], int start, int end) {
    int dist[N];      // 最短距离
    int visited[N];   // 是否已访问
    int prev[N];      // 前驱节点
    
    // 初始化
    for (int i = 0; i < N; i++) {
        dist[i] = INF;
        visited[i] = 0;
        prev[i] = -1;
    }
    dist[start] = 0;
    
    for (int count = 0; count < N-1; count++) {
        // 找当前距离最小的未访问节点
        int u = -1, minDist = INF;
        for (int i = 0; i < N; i++) {
            if (!visited[i] && dist[i] < minDist) {
                minDist = dist[i];
                u = i;
            }
        }
        if (u == -1) break;  // 所有可达点已访问
        visited[u] = 1;
        
        // 更新邻居
        for (int v = 0; v < N; v++) {
            if (!visited[v] && map[u][v] > 0 && 
                dist[u] + map[u][v] < dist[v]) {
                dist[v] = dist[u] + map[u][v];
                prev[v] = u;
            }
        }
    }
}

4.3 A*算法：带“方向感”的寻路

A*算法比Dijkstra聪明在哪？它有个启发式函数，说白了就是“猜一下终点在哪边”。Dijkstra是四面开花，A*是朝着终点方向优先探索。

A*的核心公式：

f(n) = g(n) + h(n)

• g(n)：从起点到当前点的实际代价
• h(n)：从当前点到终点的估计代价（启发式）
• f(n)：总估计代价，越小越优先探索

启发式函数怎么选？我常用曼哈顿距离（只能上下左右走时）或欧几里得距离（可以斜着走时）。

// A*算法核心逻辑（伪代码）
typedef struct {
    int x, y;
    int g, h, f;
    Node* parent;
} Node;

Node* a_star(Node* start, Node* end, int map[][]) {
    openList.push(start);  // 待探索列表
    closedList.clear();    // 已探索列表
    
    while (!openList.empty()) {
        Node* current = openList.popMinF();  // 取f值最小的
        
        if (current == end) {
            return reconstructPath(current);  // 回溯路径
        }
        
        closedList.push(current);
        
        // 遍历四个邻居
        for (每个邻居 neighbor) {
            if (neighbor 是障碍物 || neighbor 在 closedList) 
                continue;
            
            int tentative_g = current->g + distance(current, neighbor);
            
            if (neighbor 不在 openList) {
                neighbor->g = tentative_g;
                neighbor->h = heuristic(neighbor, end);
                neighbor->f = neighbor->g + neighbor->h;
                neighbor->parent = current;
                openList.push(neighbor);
            } else if (tentative_g < neighbor->g) {
                // 找到更短路径，更新
                neighbor->g = tentative_g;
                neighbor->f = neighbor->g + neighbor->h;
                neighbor->parent = current;
            }
        }
    }
    return NULL;  // 无路可走
}

4.4 两种算法的对比

4.5 植保作业中的实际选择

我在做植保机器人时，遇到过这么个情况：田块有50米×30米，栅格化后1500个格子。用Dijkstra跑一次路径要200ms，而A*只要30ms。你想想看，机器人要实时避障，200ms的延迟可能就撞上突然出现的树枝了。

我的建议：

• 如果地图小（< 500格子），用Dijkstra，代码简单不容易出bug
• 如果地图大（> 500格子），必须上A*，否则MCU扛不住
• 启发式函数选曼哈顿距离，计算量小，适合嵌入式

好了，这一讲的内容就是这些。下节课咱们会把这些算法真正跑在STM32上，看看实际效果。记住，理论再漂亮，不上板子跑一跑都是纸上谈兵。