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

Python实现的用于处理协作车辆进入路口遮挡自车路径的情况

news 来源：原创 2025/6/9 5:08:18

下面是一个使用Python实现的示例代码，用于处理协作车辆进入路口遮挡自车路径的情况。我们将分为几个步骤来完成这个任务：

离线混合A*路径规划：使用混合A*算法规划自车从起点到终点的初始路径。
障碍物处理：根据他车的停车点和车辆长宽确定障碍物的位置和大小。
决策模型：判断是否需要进行路径重规划。
在线路径重规划：如果需要重规划，使用 MoveToXY 方法重新规划路径。

import heapq
import numpy as np
import math

# 定义节点类
class Node:
    def __init__(self, x, y, theta, g=0, h=0, parent=None):
        self.x = x
        self.y = y
        self.theta = theta
        self.g = g
        self.h = h
        self.f = g + h
        self.parent = parent

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

# 定义混合A*路径规划器
def hybrid_astar(start, goal, obstacles, vehicle_length, vehicle_width, grid_resolution=0.1, theta_resolution=math.radians(10)):
    open_list = []
    closed_set = set()

    start_node = Node(start[0], start[1], start[2])
    heapq.heappush(open_list, start_node)

    while open_list:
        current_node = heapq.heappop(open_list)

        if (math.sqrt((current_node.x - goal[0])**2 + (current_node.y - goal[1])**2) < grid_resolution and
            abs(current_node.theta - goal[2]) < theta_resolution):
            path = []
            while current_node:
                path.append((current_node.x, current_node.y, current_node.theta))
                current_node = current_node.parent
            return path[::-1]

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

        # 生成子节点
        for steering_angle in [-math.radians(30), 0, math.radians(30)]:
            new_x = current_node.x + math.cos(current_node.theta) * grid_resolution
            new_y = current_node.y + math.sin(current_node.theta) * grid_resolution
            new_theta = current_node.theta + steering_angle

            new_node = Node(new_x, new_y, new_theta,
                            g=current_node.g + grid_resolution,
                            h=math.sqrt((new_x - goal[0])**2 + (new_y - goal[1])**2),
                            parent=current_node)

            if (new_x, new_y, new_theta) in closed_set:
                continue

            # 检查碰撞
            if not is_collision(new_node, obstacles, vehicle_length, vehicle_width):
                heapq.heappush(open_list, new_node)

    return None

# 检查碰撞
def is_collision(node, obstacles, vehicle_length, vehicle_width):
    for obstacle in obstacles:
        ox, oy, ol, ow = obstacle
        # 简单的矩形碰撞检测
        min_x = ox - ol / 2
        max_x = ox + ol / 2
        min_y = oy - ow / 2
        max_y = oy + ow / 2

        if (min_x < node.x < max_x) and (min_y < node.y < max_y):
            return True

    return False

# 决策模型：判断是否需要重规划路径
def decision_model(path, obstacles):
    for point in path:
        for obstacle in obstacles:
            ox, oy, ol, ow = obstacle
            min_x = ox - ol / 2
            max_x = ox + ol / 2
            min_y = oy - ow / 2
            max_y = oy + ow / 2

            if (min_x < point[0] < max_x) and (min_y < point[1] < max_y):
                return True

    return False

# 在线路径重规划（简化的MoveToXY）
def online_replanning(start, goal, obstacles, vehicle_length, vehicle_width):
    return hybrid_astar(start, goal, obstacles, vehicle_length, vehicle_width)

# 示例参数
start = (0, 0, 0)  # 起点 (x, y, theta)
goal = (10, 10, 0)  # 终点 (x, y, theta)
obstacles = [(5, 5, 2, 2)]  # 障碍物 (x, y, length, width)
vehicle_length = 4
vehicle_width = 2

# 离线混合A*路径规划
initial_path = hybrid_astar(start, goal, obstacles, vehicle_length, vehicle_width)
if initial_path:
    print("初始路径规划成功！")
else:
    print("初始路径规划失败！")

# 决策是否需要重规划
if decision_model(initial_path, obstacles):
    print("需要重规划路径！")
    new_path = online_replanning(start, goal, obstacles, vehicle_length, vehicle_width)
    if new_path:
        print("重规划路径成功！")
    else:
        print("重规划路径失败！")
else:
    print("不需要重规划路径。")