使用栅格法通过蚁群算法规划机器人全局路径

上周帮实验室的学弟调他的机器人路径规划代码，他对着满屏的栅格地图挠头：明明地图里堵了个外卖柜，为啥机器人非要往那撞？后来聊到用蚁群算法做全局规划，才发现不少人把栅格法和蚁群算法拆成俩东西硬套，其实俩搭起来才是工业界入门级别的常用路子。

先不说啥算法，先讲栅格法到底是啥——其实就是把真实的机器人运行地图，切成跟扫雷游戏一样的小格子！每个格子要么是能走的平地（标0），要么是障碍物（墙、外卖柜、充电桩，标1），起点和终点就标上坐标，比如(0,0)是机器人当前位置，(9,9)是要送外卖的目的地。这么一来，复杂的连续地图就变成了离散的格子点，电脑算起来就简单多了，不用处理那种动不动就几百万像素的原图。

举个最简单的例子，我们整一个10x10的测试地图，中间加个L型的外卖柜挡路：

import numpy as np
import random

grid_map = np.zeros((10, 10), dtype=int)
# 中间加L型障碍物，模拟外卖柜挡路
grid_map[3:7, 4] = 1
grid_map[4, 3:7] = 1
# 起点(0,0)，终点(9,9)
start = (0, 0)
end = (9, 9)

这样跑出来的地图，直接看的话就是中间卡了个方块，机器人没法直接直线穿过去，得绕路。

接下来就是蚁群算法上场了，这玩意儿完全是抄蚂蚁找吃的逻辑：蚂蚁出门找食物，走的路会留下一种叫“信息素”的东西，别的蚂蚁路过的时候，会更愿意选信息素浓的路。要是某条路特别短，蚂蚁来回走的次数多，信息素就会越来越浓，后来的蚂蚁就全往这条路上挤，最后整个蚁群就找到了最短路径。

放到栅格地图里，逻辑就更直白了：每只蚂蚁从起点出发，每次只能往上下左右四个方向挪一格（毕竟机器人一般不会斜着走），不能撞障碍物，也不能走回头路（不然就绕圈子了）。每到一个格子，就按概率选下一个要走的格子——不是瞎选，也不是直接挑离终点最近的，而是平衡了“走信息素多的老路”和“探索没走过的新路”。

使用栅格法通过蚁群算法规划机器人全局路径

这里贴一下选下一个格子的代码，比干讲好懂：

# 初始化所有格子的信息素，一开始大家都一样，都是1
tau = np.ones_like(grid_map, dtype=float)

# 启发式函数：算当前格子到终点的曼哈顿距离倒数，离终点越近，数值越大
def heuristic(pos):
    return 1 / (abs(pos[0] - end[0]) + abs(pos[1] - end[1]) + 1e-6)

# 蚂蚁选下一步的逻辑
def select_next_node(current_pos, visited):
    # 上下左右四个方向
    dirs = [(-1,0),(1,0),(0,-1),(0,1)]
    neighbors = []
    for d in dirs:
        x, y = current_pos[0] + d[0], current_pos[1] + d[1]
        # 检查格子在地图里、不是障碍物、没走过
        if 0<=x<10 and 0<=y<10 and grid_map[x][y]==0 and (x,y) not in visited:
            neighbors.append((x,y))
    if not neighbors:
        return None  # 走到死路了
    
    # 计算每个邻居的选择概率：信息素多的+离终点近的，权重更高
    prob = []
    total = 0
    for n in neighbors:
        # alpha是信息素权重，beta是启发式权重，调参全靠试
        p = tau[n[0]][n[1]]**1 * heuristic(n)**2
        prob.append(p)
        total += p
    # 按概率随机选一个，不是硬选最大的，留了探索的空间
    prob = [p/total for p in prob]
    return random.choices(neighbors, weights=prob, k=1)[0]

我最开始写这段代码的时候，忘了加visited集合，结果蚂蚁在一个格子里来回打转，跟个无头苍蝇似的，跑了半天都没结果——这就跟你出门买奶茶，绕着同一个路口转三圈，纯纯浪费时间。

然后就是迭代的过程了：一次迭代就是让所有蚂蚁都从起点出发，要么走到终点，要么走到死路。每只蚂蚁走完之后，把它走过的路径上的信息素加一点，路径越短，加的信息素越多。同时还要给所有格子的信息素加一个“挥发”的操作——比如每过一段时间，旧的信息素会消失一部分，不然最早的蚂蚁走了弯路留的信息素会一直占着坑，导致后来的蚂蚁都跟着走弯路，这叫“早熟收敛”，是蚁群算法最常见的坑。

贴一下迭代和更新信息素的代码：

# 蚁群的超参数，全靠调
n_ants = 20       # 一次迭代的蚂蚁数量
n_iter = 100      # 迭代次数
rho = 0.1         # 信息素挥发率，0.1就是每轮消失10%
Q = 100           # 信息素增量系数，越大则短路径的信息素涨得越快

best_path = None
best_length = float('inf')

for iter in range(n_iter):
    all_paths = []
    for ant in range(n_ants):
        current_pos = start
        path = [current_pos]
        visited = set(path)
        # 蚂蚁一直走，直到到终点或者死路
        while current_pos != end:
            next_pos = select_next_node(current_pos, visited)
            if next_pos is None:
                break
            current_pos = next_pos
            path.append(current_pos)
            visited.add(current_pos)
        # 要是走到了终点，就记录这条路径的长度
        if current_pos == end:
            path_length = sum(abs(path[i][0]-path[i+1][0]) + abs(path[i][1]-path[i+1][1]) for i in range(len(path)-1))
            all_paths.append((path, path_length))
            if path_length < best_length:
                best_length = path_length
                best_path = path
    # 更新所有格子的信息素：先挥发，再加上每只蚂蚁留下的信息素
    tau *= (1 - rho)
    for path, length in all_paths:
        for (x,y) in path:
            tau[x][y] += Q / length
    # 每10轮打印一下当前的最优结果，看看进度
    if iter % 10 == 0:
        print(f"第{iter}次迭代，当前最优路径长度：{best_length}")

学弟最开始把rho设成了0.01，结果跑了50轮还在乱逛，后来改成0.1，第30轮左右就收敛到了最优路径——说白了就是挥发率太低的话，旧的信息素留太久，新的好路径的信息素盖不过旧的，就跟你刷到一个好视频忘了点赞，算法过段时间就不给你推同类了。

最后跑出来的最优路径其实很符合预期：从(0,0)先往右走到(0,3)，绕开障碍物的左边，再往下走到(3,3)，然后往右走到(3,7)，再往下走到(9,7)，最后往右走到(9,9)，全程没有撞障碍物，而且是最短的绕路方案。

其实这套组合拳不是最快的路径规划算法，比如A*算法跑这种小地图比蚁群快多了，但蚁群算法胜在好理解，调参也相对灵活，而且群智能的逻辑特别有意思——你看，一群没有脑子的蚂蚁，居然能凑出来最优路径，放到机器人身上也是一样，单个机器人可能笨，但一群机器人凑起来就能搞定复杂的路径规划。

要是你想本地跑一下试试，最后加个matplotlib可视化代码就行，看着地图上画出来的红线圈出来的路径，比看打印的日志直观多了。