多目标点移动机器人改进路径规划算法代码 送餐机器人，AGV室内机器人仿真路径规划 采用改进A*算法融合模拟退火算法，规划多目标点路径规划。 解决路径与障碍物相撞，AGV不斜穿室内区间，采用水平垂直方向移动路径规划，圆弧转弯。 室内旅行商问题——送餐移动机器人（从厨房出发到达多个目标点，最后返回厨房） 1，改进A*算法规划两两之间的路径，并计算路径长度； 2，模拟退火算法依据两点之间路径长度，规划多个目标点的先后到达顺序； 3，组合最优顺序的路径，输出最后路线

在如今智能化的时代，送餐机器人、AGV室内机器人等在室内环境中的应用越来越广泛。而其中关键的一环便是路径规划，今天咱就聊聊采用改进A*算法融合模拟退火算法的多目标点路径规划。

面临的挑战

想象一下，室内环境到处是障碍物，机器人要是规划的路径与障碍物相撞，那可就尴尬了。而且，还不能斜穿室内区间，得采用水平垂直方向移动路径规划，转弯还得是圆弧。这就如同给机器人戴上了“紧箍咒”，但也正是这些约束，让路径规划更符合实际室内场景。

算法融合思路

改进A*算法规划两两之间路径

A算法大家都不陌生，它通过启发式函数来寻找最优路径。咱这里的改进A算法，主要是针对室内水平垂直移动和圆弧转弯做优化。

import heapq


def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def modified_a_star(grid, start, goal):
    open_set = []
    heapq.heappush(open_set, (0, start))
    came_from = {}
    g_score = {node: float('inf') for node in grid.keys()}
    g_score[start] = 0
    f_score = {node: float('inf') for node in grid.keys()}
    f_score[start] = heuristic(start, goal)

    while open_set:
        _, current = heapq.heappop(open_set)
        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            path.reverse()
            return path

        for neighbor in grid[current]:
            tentative_g_score = g_score[current] + 1
            if tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = tentative_g_score + heuristic(neighbor, goal)
                if neighbor not in [i[1] for i in open_set]:
                    heapq.heappush(open_set, (f_score[neighbor], neighbor))
    return None

这段代码中，heuristic函数计算两点之间的曼哈顿距离作为启发式值。modifiedastar函数则是改进的A*算法主体，它维护了openset（优先队列）、camefrom（记录路径）、gscore（从起点到当前点的实际代价）和fscore（gscore与启发式值之和）。通过不断从openset中取出f_score最小的节点进行扩展，最终找到从start到goal的路径。

模拟退火算法确定目标点顺序

模拟退火算法就像给机器人一个“聪明的脑袋”，让它依据两点之间路径长度，规划多个目标点的先后到达顺序。它模拟物理退火过程，在高温时接受较差的解，随着温度降低，逐渐只接受较好的解。

import random
import math


def simulated_annealing(target_points, distance_matrix, initial_temperature=1000, cooling_rate=0.95,
                        min_temperature=1e - 3):
    current_order = list(range(len(target_points)))
    current_distance = calculate_total_distance(current_order, distance_matrix)
    best_order = current_order[:]
    best_distance = current_distance
    temperature = initial_temperature

    while temperature > min_temperature:
        new_order = current_order[:]
        i, j = random.sample(range(len(target_points)), 2)
        new_order[i], new_order[j] = new_order[j], new_order[i]
        new_distance = calculate_total_distance(new_order, distance_matrix)
        if new_distance < current_distance or random.random() < math.exp((current_distance - new_distance) / temperature):
            current_order = new_order
            current_distance = new_distance
            if current_distance < best_distance:
                best_order = current_order[:]
                best_distance = current_distance
        temperature *= cooling_rate
    return best_order


def calculate_total_distance(order, distance_matrix):
    total_distance = 0
    for i in range(len(order) - 1):
        total_distance += distance_matrix[order[i]][order[i + 1]]
    return total_distance

在这段代码里，simulatedannealing函数实现了模拟退火算法。currentorder是当前的目标点顺序，通过随机交换两个点的位置产生新的顺序neworder，计算新顺序的总路径长度newdistance。如果新距离更短或者满足一定概率条件（由温度控制），就接受新顺序。calculatetotaldistance函数则用于计算给定顺序下的总路径长度。

组合路径并输出

最后，将模拟退火算法确定的最优顺序的路径组合起来，输出最终路线。

def combine_paths(paths, order):
    final_path = []
    for i in range(len(order) - 1):
        start = order[i]
        end = order[i + 1]
        final_path.extend(paths[(start, end)])
        if i < len(order) - 2:
            final_path.pop()  # 去除重复点
    return final_path

这个combine_paths函数接收各个两点间的路径paths和目标点顺序order，将路径按顺序组合起来，并且去除重复点，得到最终的机器人行驶路径。

采用这种改进A*算法融合模拟退火算法的多目标点路径规划方法，能很好地解决送餐机器人等在室内环境中的路径规划问题，让机器人更高效、更安全地完成任务。无论是餐厅的送餐场景，还是工厂内AGV的物料搬运，都能看到这种算法融合的魅力所在。