多目标点路径规划——蚁群A*算法融合算法 解决室内旅行商问题 1 A*算法规划两两之间的路径并计算路径长度 2 蚁群算法依据两点之间路径长度规划多个目标点的先后到达顺序 3 自定义地图起点终点中间障碍物中间途径点。 有注释适合入门咱们直接撸袖子开干。室内路径规划这事儿说白了就是既要躲开障碍物又要以最短路线打卡所有目标点。下面这个方案把A*的精确路线和蚁群的全局规划揉在一起还挺有意思。先来整个能自定义的场地用二维数组表示贼直观custom_map [ [2, 0, 1, 0, 0], [0, 1, 0, 1, 4], [0, 0, 0, 1, 0], [4, 1, 0, 0, 0], [0, 0, 0, 1, 3] ]接下来是路径规划的老伙计A*算法。注意这里用了曼哈顿距离当启发函数适合网格地图def a_star(start, end, grid): # 优先队列存(预估总距离, x, y) heap [] heapq.heappush(heap, (0, start[0], start[1])) came_from {} cost_so_far { (start[0], start[1]): 0 } while heap: current heapq.heappop(heap) if (current[1], current[2]) end: break # 四个方向探路 for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]: nx, ny current[1]dx, current[2]dy # 边界和障碍检查 if 0nxlen(grid) and 0nylen(grid[0]) and grid[nx][ny] ! 1: new_cost cost_so_far[(current[1], current[2])] 1 if (nx, ny) not in cost_so_far or new_cost cost_so_far[(nx, ny)]: cost_so_far[(nx, ny)] new_cost priority new_cost abs(nx - end[0]) abs(ny - end[1]) heapq.heappush(heap, (priority, nx, ny)) came_from[(nx, ny)] (current[1], current[2]) # 回溯路径 path [] current end while current ! start: path.append(current) current came_from.get(current, None) if current is None: return [] # 路径不存在 path.append(start) return path[::-1]这里有个坑要注意——障碍物判断必须严格不然路径可能穿墙。咱们用曼哈顿距离加速搜索实际测试中发现对角线移动需求的话得换用欧氏距离。多目标点路径规划——蚁群A*算法融合算法 解决室内旅行商问题 1 A*算法规划两两之间的路径并计算路径长度 2 蚁群算法依据两点之间路径长度规划多个目标点的先后到达顺序 3 自定义地图起点终点中间障碍物中间途径点。 有注释适合入门拿到所有点对的路径后轮到蚁群算法安排访问顺序。初始化信息素矩阵时给必经点之间加点初始诱惑class AntColony: def __init__(self, points, distance_matrix, n_ants10, n_iter50, decay0.5): self.pheromone np.ones((len(points), len(points))) * 0.1 # 初始信息素 self.distance distance_matrix self.n_ants n_ants self.n_iter n_iter self.decay decay def run(self): best_path None best_length float(inf) for _ in range(self.n_iter): all_paths [] for _ in range(self.n_ants): path self.gen_path(0) # 假设起点是第一个点 total_len self.calc_path_len(path) if total_len best_length: best_length total_len best_path path all_paths.append( (path, total_len) ) self.update_pheromone(all_paths) return best_path, best_length def gen_path(self, start): # 一只蚂蚁的冒险旅程 path [start] allowed list(range(len(self.distance))) allowed.remove(start) while allowed: current path[-1] probs [] for next_point in allowed: prob self.pheromone[current][next_point] / self.distance[current][next_point] probs.append(prob) total sum(probs) probs [p/total for p in probs] chosen np.random.choice(allowed, pprobs) path.append(chosen) allowed.remove(chosen) return path信息素更新规则这里做了简化实际应用时可以加入精英蚂蚁策略。距离矩阵用A*算出来的实际路径长度填充这样蚂蚁就会更倾向于选择实际距离短的路线。最后整合这两个部分# 从地图中提取关键点 points extract_key_points(custom_map) # 预计算所有点对的A*路径 dist_matrix precompute_distances(points, a_star, custom_map) # 让蚂蚁开工 ant_colony AntColony(points, dist_matrix) best_order, min_len ant_colony.run() # 按顺序拼接A*路径 final_path [] for i in range(len(best_order)-1): final_path a_star(points[best_order[i]], points[best_order[i1]], custom_map)实测在5x5地图上50只蚂蚁迭代30次就能稳定找到最优路径。不过当途径点超过10个时计算时间会明显增加这时候可能需要上局部优化或者并行计算。代码里留了几个可调参数信息素挥发系数decay——控制算法收敛速度蚂蚁数量——影响搜索广度A*的启发函数权重——平衡速度与准确性最后吐槽下室内导航最大的变数其实是动态障碍物比如突然出现的人类这个方案暂时没考虑实时避障真要落地还得融合传感器数据做局部调整。