使用栅格法通过蚁群算法规划机器人全局路径上周帮实验室的学弟调他的机器人路径规划代码他对着满屏的栅格地图挠头明明地图里堵了个外卖柜为啥机器人非要往那撞后来聊到用蚁群算法做全局规划才发现不少人把栅格法和蚁群算法拆成俩东西硬套其实俩搭起来才是工业界入门级别的常用路子。先不说啥算法先讲栅格法到底是啥——其实就是把真实的机器人运行地图切成跟扫雷游戏一样的小格子每个格子要么是能走的平地标0要么是障碍物墙、外卖柜、充电桩标1起点和终点就标上坐标比如(0,0)是机器人当前位置(9,9)是要送外卖的目的地。这么一来复杂的连续地图就变成了离散的格子点电脑算起来就简单多了不用处理那种动不动就几百万像素的原图。举个最简单的例子我们整一个10x10的测试地图中间加个L型的外卖柜挡路import numpy as np import random grid_map np.zeros((10, 10), dtypeint) # 中间加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, dtypefloat) # 启发式函数算当前格子到终点的曼哈顿距离倒数离终点越近数值越大 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 0x10 and 0y10 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, weightsprob, k1)[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[i1][0]) abs(path[i][1]-path[i1][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可视化代码就行看着地图上画出来的红线圈出来的路径比看打印的日志直观多了。