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

人工智能A*算法-爬坡路段增加移动代价，在狭窄街道考虑车辆的转弯半径

article 2025/2/10 0:47:27

以下是一个基于 Python 实现的 A* 算法示例，其中考虑了爬坡路段增加移动代价，以及在狭窄街道考虑车辆的转弯半径和可通行空间，从而规划出更合理、安全的路径。

思路分析

地图表示：使用二维数组表示地图，不同的值代表不同的地形和路况，如普通道路、爬坡路段、狭窄街道等。
代价函数：在计算节点的代价时，根据地形和路况调整移动代价。例如，爬坡路段的移动代价更高，狭窄街道需要考虑车辆的转弯半径和可通行空间。
A 算法*：使用标准的 A* 算法进行路径规划，在扩展节点时，根据调整后的代价函数选择最优路径。

代码实现

import heapq
import math

# 定义地图，0 表示普通道路，1 表示爬坡路段，2 表示狭窄街道
map_grid = [
    [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]
]

# 定义节点类
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, vehicle_turn_radius):
    x1, y1 = current
    x2, y2 = next_node

    # 普通道路代价
    base_cost = 1

    # 根据地形调整代价
    terrain = map_grid[x2][y2]
    if terrain == 1:  # 爬坡路段
        cost_factor = 2  # 爬坡路段代价翻倍
    elif terrain == 2:  # 狭窄街道
        # 检查是否能通过（简单模拟，假设车辆转弯半径影响）
        if not can_pass(x1, y1, x2, y2, vehicle_turn_radius):
            return float('inf')
        cost_factor = 1.5  # 狭窄街道代价增加 50%
    else:
        cost_factor = 1

    return base_cost * cost_factor

# 检查是否能通过狭窄街道
def can_pass(x1, y1, x2, y2, vehicle_turn_radius):
    # 简单模拟，假设车辆转弯半径过大则无法通过
    dx = abs(x2 - x1)
    dy = abs(y2 - y1)
    if dx + dy > vehicle_turn_radius:
        return False
    return True

# A* 算法实现路径规划
def astar(start, goal, vehicle_turn_radius):
    rows = len(map_grid)
    cols = len(map_grid[0])
    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 = current_node.x + dx
            new_y = 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), vehicle_turn_radius)
                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)
vehicle_turn_radius = 2  # 假设车辆转弯半径为 2

# 进行路径规划
path = astar(start, goal, vehicle_turn_radius)

if path:
    print("规划的路径:", path)
else:
    print("未找到可行路径！")

代码解释

地图表示：map_grid 是一个二维数组，用于表示地图。不同的值代表不同的地形和路况，如 0 表示普通道路，1 表示爬坡路段，2 表示狭窄街道。
节点类：Node 类用于表示地图中的每个节点，包含节点的坐标、实际代价 g、估计代价 h、总代价 f 和父节点。
启发式函数：heuristic 函数使用曼哈顿距离作为启发式函数，用于估计从当前节点到目标节点的代价。
移动代价计算：get_move_cost 函数根据地形和路况调整移动代价。爬坡路段的代价翻倍，狭窄街道的代价增加 50%，并检查车辆是否能通过狭窄街道。
狭窄街道通过检查：can_pass 函数简单模拟了车辆在狭窄街道的通过情况，根据车辆的转弯半径判断是否能通过。
A 算法*：astar 函数实现了标准的 A* 算法，在扩展节点时，根据调整后的代价函数选择最优路径。
主程序：定义起点、终点和车辆的转弯半径，调用 astar 函数进行路径规划，并输出规划结果。