当前位置：首页 > article >正文

人工智能A*算法代价函数中加入时间因素和能耗因素

article 2025/2/9 7:12:59

在自动驾驶的路径规划里，为了让规划出的路径在距离、时间和能耗之间达到最优平衡，我们可以在 A* 算法的代价函数中加入时间因素和能耗因素。以下是具体的代码实现：

思路分析

地图表示：使用二维数组表示地图，不同的值代表不同的路况，路况会影响行驶的速度和能耗。
代价函数：在计算节点的代价时，综合考虑距离、时间和能耗。距离通过曼哈顿距离等计算，时间根据路况对应的速度计算，能耗根据路况对应的能耗系数计算。
A 算法*：在标准 A* 算法的基础上，使用新的代价函数进行节点扩展和路径搜索。

代码实现

import heapq
import numpy as np

# 地图表示，不同值代表不同路况
# 0: 普通道路，速度快，能耗低
# 1: 拥堵道路，速度慢，能耗高
# 2: 爬坡路段，速度慢，能耗高
map_grid = np.array([
    [0, 0, 0, 0, 0],
    [0, 1, 1, 0, 0],
    [0, 0, 0, 2, 0],
    [0, 0, 1, 1, 0],
    [0, 0, 0, 0, 0]
])

# 不同路况的速度和能耗系数
speed_factors = {
    0: 2,  # 普通道路速度
    1: 0.5,  # 拥堵道路速度
    2: 0.3  # 爬坡路段速度
}

energy_factors = {
    0: 1,  # 普通道路能耗系数
    1: 3,  # 拥堵道路能耗系数
    2: 4  # 爬坡路段能耗系数
}

# 节点类
class Node:
    def __init__(self, x, y, g=float('inf'), h=float('inf'), parent=None):
        self.x = x
        self.y = y
        self.g = g
        self.h = h
        self.f = g + h
        self.parent = parent

    def __lt__(self, other):
        return self.f < other.f

# 启发式函数（曼哈顿距离）
def heuristic(current, goal):
    return abs(current[0] - goal[0]) + abs(current[1] - goal[1])

# 计算移动代价，考虑距离、时间和能耗
def get_move_cost(current, next_node):
    x1, y1 = current
    x2, y2 = next_node

    # 距离代价
    distance_cost = 1

    # 获取路况
    terrain = map_grid[x2][y2]
    speed = speed_factors[terrain]
    energy = energy_factors[terrain]

    # 时间代价
    time_cost = 1 / speed

    # 能耗代价
    energy_cost = energy

    # 综合代价，这里简单加权求和，可根据实际情况调整权重
    total_cost = distance_cost + time_cost + energy_cost
    return total_cost

# A* 算法
def astar(grid, start, goal):
    rows, cols = grid.shape
    open_list = []
    closed_set = set()

    start_node = Node(start[0], start[1], g=0, h=heuristic(start, goal))
    heapq.heappush(open_list, start_node)

    while open_list:
        current_node = heapq.heappop(open_list)

        if (current_node.x, current_node.y) == goal:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]

        closed_set.add((current_node.x, current_node.y))

        neighbors = [(0, 1), (0, -1), (1, 0), (-1, 0)]
        for dx, dy in neighbors:
            new_x, new_y = current_node.x + dx, current_node.y + dy

            if 0 <= new_x < rows and 0 <= new_y < cols and (new_x, new_y) not in closed_set:
                new_g = current_node.g + get_move_cost((current_node.x, current_node.y), (new_x, new_y))
                new_h = heuristic((new_x, new_y), goal)
                new_node = Node(new_x, new_y, g=new_g, h=new_h, parent=current_node)

                found = False
                for i, node in enumerate(open_list):
                    if node.x == new_x and node.y == new_y:
                        if new_g < node.g:
                            open_list[i] = new_node
                            heapq.heapify(open_list)
                        found = True
                        break

                if not found:
                    heapq.heappush(open_list, new_node)

    return None

# 主程序
start = (0, 0)
goal = (4, 4)
path = astar(map_grid, start, goal)
if path:
    print("规划的路径:", path)
else:
    print("未找到可行路径！")

代码解释

地图和路况：map_grid 表示地图，不同的值代表不同的路况。speed_factors 和 energy_factors 分别存储了不同路况下的速度和能耗系数。
节点类：Node 类用于存储节点的信息，包括坐标、g 值（从起点到该节点的实际代价）、h 值（从该节点到目标节点的估计代价）、f 值（总代价）和父节点。
启发式函数：heuristic 函数使用曼哈顿距离作为启发式函数，估计从当前节点到目标节点的距离。
移动代价计算：get_move_cost 函数计算从一个节点移动到另一个节点的代价，综合考虑了距离、时间和能耗。时间代价根据路况对应的速度计算，能耗代价根据路况对应的能耗系数计算。
A 算法*：astar 函数实现了 A* 算法的核心逻辑，使用新的代价函数进行节点扩展和路径搜索。