1. 基于几何方法的平行泊车路径规划

任务目标

实现平行泊车场景下的几何路径规划，生成由两段圆弧和一段直线组成的可行泊车路径，并输出离散路径点序列。路径需满足车辆最小转弯半径约束，且起点与终点位姿（位置+航向角）精确匹配。

实现原理

采用经典圆弧-直线-圆弧几何法：车辆从起始位姿以最小转弯半径R_min倒车转弯，中间以直线段过渡，最后以相反方向圆弧切入目标车位。路径由三段拼接而成，通过几何关系求解各段长度与圆心位置。

关键代码

import numpy as np
import math

def parallel_parking_path(start, goal, R_min, step=0.1):
    """
    平行泊车几何路径规划
    start: (x, y, theta) 起始位姿
    goal:  (x, y, theta) 目标位姿（车位内）
    R_min: 最小转弯半径
    step:  路径点采样步长(弧度)
    返回: Nx3 路径点数组 [x, y, theta]
    """
    x0, y0, theta0 = start
    xg, yg, thetag = goal

    # 第一段圆弧：从起点以半径R_min右转（顺时针）
    # 圆心在起点右侧垂直于航向方向
    cx1 = x0 + R_min * math.sin(theta0)
    cy1 = y0 - R_min * math.cos(theta0)

    # 第三段圆弧：到目标点以半径R_min左转（逆时针）
    # 圆心在目标点左侧垂直于航向方向
    cx3 = xg - R_min * math.sin(thetag)
    cy3 = yg + R_min * math.cos(thetag)

    # 计算两圆心距离
    d_center = math.hypot(cx3 - cx1, cy3 - cy1)

    # 直线段长度（两圆心距离需大于2R_min才能有解）
    if d_center < 2 * R_min:
        raise ValueError("车位空间不足，无法完成泊车")

    # 直线段方向角（两圆心连线方向）
    alpha = math.atan2(cy3 - cy1, cx3 - cx1)

    # 直线段长度
    L_line = math.sqrt(d_center**2 - (2 * R_min)**2)

    # 直线段与圆心连线的夹角
    beta = math.atan2(2 * R_min, L_line)

    # 第一段圆弧结束点（直线起点）
    theta1 = theta0 - math.pi / 2  # 右转90度
    # 直线段方向
    theta_line = alpha - beta

    # 计算第一段圆弧结束点坐标
    x1 = cx1 + R_min * math.cos(theta_line - math.pi/2)
    y1 = cy1 + R_min * math.sin(theta_line - math.pi/2)

    # 第三段圆弧起始点（直线终点）
    x2 = x1 + L_line * math.cos(theta_line)
    y2 = y1 + L_line * math.sin(theta_line)

    # 采样路径点
    points = []

    # 第一段圆弧：从theta0转到theta_line（右转，角度减小）
    theta = theta0
    while theta > theta_line:
        x = cx1 + R_min * math.sin(theta)
        y = cy1 - R_min * math.cos(theta)
        points.append([x, y, theta])
        theta -= step
    # 确保终点精确
    points.append([x1, y1, theta_line])

    # 直线段
    dist = 0
    L_total = L_line
    while dist < L_total:
        t = dist / L_total
        x = x1 + (x2 - x1) * t
        y = y1 + (y2 - y1) * t
        points.append([x, y, theta_line])
        dist += 0.1
    points.append([x2, y2, theta_line])

    # 第三段圆弧：从theta_line转到thetag（左转，角度增大）
    theta = theta_line
    while theta < thetag:
        x = cx3 - R_min * math.sin(theta)
        y = cy3 + R_min * math.cos(theta)
        points.append([x, y, theta])
        theta += step
    # 确保终点精确
    points.append([xg, yg, thetag])

    return np.array(points)

# 示例：平行泊车场景
if __name__ == "__main__":
    # 起始位姿：道路行驶方向（x轴正向），位于车位旁
    start = (0.0, 2.0, 0.0)
    # 目标位姿：车位内，与道路平行
    goal = (5.0, 0.0, 0.0)
    R_min = 2.0

    path = parallel_parking_path(start, goal, R_min, step=0.05)
    print(f"生成路径点数量: {len(path)}")
    print("前3个路径点:")
    print(path[:3])
    print("最后3个路径点:")
    print(path[-3:])

    # 验证起点和终点位姿
    assert np.allclose(path[0], start, atol=1e-6), "起点不匹配"
    assert np.allclose(path[-1], goal, atol=1e-6), "终点不匹配"
    print("起点与终点位姿验证通过")

运行方式

python parallel_parking.py

验证方法

运行脚本后观察输出：

1. 程序正常输出路径点数量（约数百个点），无异常报错；
2. 前3个路径点与起始位姿(0, 2, 0)一致，最后3个路径点与目标位姿(5, 0, 0)一致；
3. 控制台显示“起点与终点位姿验证通过”，说明路径端点精确匹配；
4. 路径点序列中航向角theta单调变化（先减小后不变再增大），符合圆弧-直线-圆弧的几何特征。

关键参数说明

• R_min：车辆最小转弯半径，需根据实际车辆参数设定，本案例取2.0m；
• step：圆弧采样步长（弧度），越小路径点越密；
• 若车位空间不足（两圆心距离小于2R_min），程序会抛出异常提示。