2. 垂直泊车路径规划与碰撞检测

任务目标

在已知车位尺寸、车辆尺寸及障碍物位置的条件下，规划一条从起始位姿到垂直车位内的可行路径，并对路径进行碰撞检测，确保车辆在全程不与障碍物发生干涉。

问题定义与关键参数

车辆简化为矩形（长4.7m，宽1.9m），垂直车位宽2.5m、长5.3m，车位两侧有障碍车辆（宽2.0m，长4.5m）。路径采用“圆弧-直线-圆弧”三段式几何规划：车辆先沿圆弧后退转向，再直线倒车，最后反向圆弧摆正入位。碰撞检测通过计算车辆矩形四角点在路径离散位姿下的坐标，判断是否侵入障碍物矩形区域。

核心实现

import numpy as np
import math

# 车辆与车位参数
CAR_LEN = 4.7
CAR_WID = 1.9
PARK_W = 2.5
PARK_DEPTH = 5.3
OBS_W = 2.0
OBS_LEN = 4.5

# 车位坐标系：车位入口在y=0，车位深度沿y负方向
# 车位范围: x ∈ [-PARK_W/2, PARK_W/2], y ∈ [-PARK_DEPTH, 0]
# 左侧障碍物: x ∈ [-PARK_W/2-OBS_W, -PARK_W/2], y ∈ [-OBS_LEN, 0]
# 右侧障碍物: x ∈ [PARK_W/2, PARK_W/2+OBS_W], y ∈ [-OBS_LEN, 0]

def vehicle_corners(x, y, theta):
    """计算车辆矩形四角坐标（车辆坐标系原点为后轴中心）"""
    # 后轴中心到车尾距离
    rear_overhang = (CAR_LEN - 2.7) / 2  # 简化：轴距2.7m，前后悬均分
    front_axle_to_front = CAR_LEN - 2.7 - rear_overhang
    # 车辆矩形相对后轴中心的半尺寸
    dx = CAR_LEN / 2
    dy = CAR_WID / 2
    corners_local = np.array([
        [-dx, -dy], [dx, -dy], [dx, dy], [-dx, dy]
    ])
    # 旋转矩阵
    R = np.array([
        [math.cos(theta), -math.sin(theta)],
        [math.sin(theta), math.cos(theta)]
    ])
    corners_world = corners_local @ R.T + np.array([x, y])
    return corners_world

def point_in_rect(px, py, rect_xmin, rect_xmax, rect_ymin, rect_ymax):
    """判断点是否在矩形内（含边界）"""
    return rect_xmin <= px <= rect_xmax and rect_ymin <= py <= rect_ymax

def check_collision(x, y, theta):
    """检测车辆在给定位姿下是否与障碍物碰撞"""
    corners = vehicle_corners(x, y, theta)
    # 障碍物区域列表
    obstacles = [
        (-PARK_W/2-OBS_W, -PARK_W/2, -OBS_LEN, 0),   # 左侧障碍
        (PARK_W/2, PARK_W/2+OBS_W, -OBS_LEN, 0)      # 右侧障碍
    ]
    for cx, cy in corners:
        for (ox_min, ox_max, oy_min, oy_max) in obstacles:
            if point_in_rect(cx, cy, ox_min, ox_max, oy_min, oy_max):
                return True
    return False

def plan_vertical_parking():
    """生成垂直泊车路径并检测碰撞"""
    # 路径参数（单位：m, rad）
    R = 4.5          # 转向半径
    x_start, y_start = -4.0, -1.0   # 起始点（车位左侧）
    theta_start = 0.0
    
    # 三段路径离散采样
    path_points = []
    
    # 第一段：圆弧转向（从起始点绕圆心旋转）
    cx1 = x_start + R  # 圆心在起始点右侧R处
    cy1 = y_start
    theta_arc1_start = math.pi  # 起始角度（圆心指向车辆）
    theta_arc1_end = math.pi / 2  # 转90度使车头朝向车位
    
    t_arc1 = np.linspace(theta_arc1_start, theta_arc1_end, 20)
    for t in t_arc1:
        x = cx1 + R * math.cos(t)
        y = cy1 + R * math.sin(t)
        theta = t - math.pi / 2  # 车辆航向角
        path_points.append((x, y, theta))
    
    # 第二段：直线倒车
    x_line_end = 0.0
    y_line_end = -2.0
    x_line_start = path_points[-1][0]
    y_line_start = path_points[-1][1]
    t_line = np.linspace(0, 1, 20)
    for t in t_line:
        x = x_line_start + (x_line_end - x_line_start) * t
        y = y_line_start + (y_line_end - y_line_start) * t
        theta = -math.pi / 2  # 车头朝上（y正方向），倒车入位
        path_points.append((x, y, theta))
    
    # 第三段：反向圆弧摆正
    cx3 = x_line_end - R  # 圆心在车位入口左侧
    cy3 = y_line_end
    theta_arc3_start = math.pi / 2
    theta_arc3_end = math.pi
    
    t_arc3 = np.linspace(theta_arc3_start, theta_arc3_end, 20)
    for t in t_arc3:
        x = cx3 + R * math.cos(t)
        y = cy3 + R * math.sin(t)
        theta = t - math.pi / 2
        path_points.append((x, y, theta))
    
    # 碰撞检测
    collision_free = True
    collision_point = None
    for (x, y, theta) in path_points:
        if check_collision(x, y, theta):
            collision_free = False
            collision_point = (x, y, theta)
            break
    
    return path_points, collision_free, collision_point

# 执行规划
path, is_safe, bad_pose = plan_vertical_parking()
print(f"路径点数: {len(path)}")
print(f"碰撞检测结果: {'通过' if is_safe else '碰撞!'}")
if not is_safe:
    print(f"碰撞位姿: x={bad_pose[0]:.2f}, y={bad_pose[1]:.2f}, theta={bad_pose[2]:.2f}")

# 输出终点位姿验证
final_pose = path[-1]
print(f"终点位姿: x={final_pose[0]:.2f}, y={final_pose[1]:.2f}, theta={final_pose[2]:.2f} rad")
print(f"终点是否在车位内: {'是' if -PARK_W/2 <= final_pose[0] <= PARK_W/2 and -PARK_DEPTH <= final_pose[1] <= 0 else '否'}")

运行与验证

运行方式：将代码保存为 vertical_parking.py，执行 python vertical_parking.py。需要安装NumPy（pip install numpy）。

验证方法：

• 程序输出“碰撞检测结果: 通过”表示路径安全；若输出“碰撞!”则需调整路径参数（如增大转向半径R或调整直线段长度）。
• 终点位姿应满足 x ∈ [-1.25, 1.25] 且 y ∈ [-5.3, 0]，输出“终点是否在车位内: 是”即验证路径终点正确。
• 可通过修改 R 值（如改为3.0）观察碰撞检测结果变化，验证检测逻辑的有效性。

关键说明

本实现采用简化的矩形车辆模型，忽略转向动力学约束，路径由三段几何曲线拼接而成。碰撞检测基于离散点采样，实际应用中应增加采样密度或使用连续碰撞检测算法。若需处理更复杂的障碍物布局，可将矩形障碍物替换为多边形并采用SAT（分离轴定理）检测。

上图展示了垂直泊车的几何路径规划结果。蓝色虚线为车辆后轴中心的运动轨迹，包含两段半径相同的圆弧和一段直线倒车段。碰撞检测在每个离散路径点上验证车辆矩形是否侵入红色障碍区域。