基于Matlab程序泊车路径规划b自动垂直泊车和自动平行泊车

1. 系统设计概述

自动泊车路径规划是自动驾驶领域的重要研究方向，主要包括以下步骤：

1. 车位检测：通过传感器（如超声波雷达或摄像头）检测停车位的位置和大小。
2. 路径规划：根据车辆当前状态和车位位置生成泊车路径。
3. 轨迹跟踪：控制车辆沿规划路径移动，完成泊车。

本文将分别实现 垂直泊车 和 平行泊车 的路径规划算法，并使用 Matlab 进行仿真。

---

2. 路径规划方法

2.1 垂直泊车

• 车辆从初始位置倒车进入垂直车位。
• 使用几何方法计算倒车轨迹，确保车辆不与周围障碍物发生碰撞。

2.2 平行泊车

• 车辆从初始位置通过多次前进和后退调整姿态，最终停入平行车位。
• 使用分段路径规划方法，逐步调整车辆位置和方向。

---

3. 代码实现

3.1 垂直泊车路径规划

% 垂直泊车路径规划
clc; clear; close all;

% 车辆参数
L = 4.5; % 车辆长度 (m)
W = 1.8; % 车辆宽度 (m)
r_min = 5; % 最小转弯半径 (m)

% 泊车位参数
parking_spot_x = 0; % 泊车位中心 x 坐标
parking_spot_y = 0; % 泊车位中心 y 坐标
parking_spot_width = W + 0.5; % 泊车位宽度 (m)
parking_spot_length = L + 0.5; % 泊车位长度 (m)

% 初始位置
x0 = -5; % 初始 x 坐标
y0 = 2; % 初始 y 坐标
theta0 = deg2rad(90); % 初始航向角 (rad)

% 计算倒车轨迹
theta_end = deg2rad(-90); % 目标航向角 (rad)
delta_theta = theta_end - theta0; % 航向角变化量
R = r_min; % 转弯半径
arc_length = abs(delta_theta) * R; % 圆弧长度

% 轨迹点
t = linspace(0, arc_length, 100);
x_traj = x0 - R * sin(theta0) + R * sin(theta0 + t / R);
y_traj = y0 + R * cos(theta0) - R * cos(theta0 + t / R);

% 绘制结果
figure;
hold on;
grid on;

% 绘制泊车位
rectangle('Position', [parking_spot_x - parking_spot_width/2, ...
                       parking_spot_y - parking_spot_length/2, ...
                       parking_spot_width, parking_spot_length], ...
          'EdgeColor', 'r', 'LineWidth', 2);

% 绘制车辆初始位置
plot_vehicle(x0, y0, theta0, L, W, 'b');

% 绘制轨迹
plot(x_traj, y_traj, 'g--', 'LineWidth', 2);

% 绘制车辆目标位置
plot_vehicle(parking_spot_x, parking_spot_y, theta_end, L, W, 'r');

title('垂直泊车路径规划');
xlabel('X (m)');
ylabel('Y (m)');
axis equal;
legend('泊车位', '初始车辆位置', '泊车轨迹', '目标车辆位置');

% 绘制车辆函数
function plot_vehicle(x, y, theta, L, W, color)
    % 车辆四个角点
    corners = [L/2, W/2; -L/2, W/2; -L/2, -W/2; L/2, -W/2];
    % 旋转矩阵
    R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
    rotated_corners = (R * corners')';
    % 平移
    rotated_corners(:,1) = rotated_corners(:,1) + x;
    rotated_corners(:,2) = rotated_corners(:,2) + y;
    % 绘制
    fill(rotated_corners(:,1), rotated_corners(:,2), color, 'FaceAlpha', 0.5, 'EdgeColor', 'k');
end

---

3.2 平行泊车路径规划

% 平行泊车路径规划
clc; clear; close all;

% 车辆参数
L = 4.5; % 车辆长度 (m)
W = 1.8; % 车辆宽度 (m)
r_min = 5; % 最小转弯半径 (m)

% 泊车位参数
parking_spot_x = 0; % 泊车位起点 x 坐标
parking_spot_y = 0; % 泊车位起点 y 坐标
parking_spot_width = W + 0.5; % 泊车位宽度 (m)
parking_spot_length = L + 0.5; % 泊车位长度 (m)

% 初始位置
x0 = -10; % 初始 x 坐标
y0 = 2; % 初始 y 坐标
theta0 = deg2rad(0); % 初始航向角 (rad)

% 分段路径规划
theta1 = deg2rad(-45); % 第一段终点航向角
theta2 = deg2rad(0); % 第二段终点航向角
theta3 = deg2rad(-90); % 第三段终点航向角

R = r_min; % 转弯半径
arc_length1 = abs(theta1 - theta0) * R;
arc_length2 = abs(theta2 - theta1) * R;
arc_length3 = abs(theta3 - theta2) * R;

% 轨迹点
t1 = linspace(0, arc_length1, 50);
t2 = linspace(0, arc_length2, 50);
t3 = linspace(0, arc_length3, 50);

x_traj1 = x0 + R * cos(theta0) - R * cos(theta0 + t1 / R);
y_traj1 = y0 + R * sin(theta0) - R * sin(theta0 + t1 / R);

x_traj2 = x_traj1(end) + R * sin(theta1) - R * sin(theta1 + t2 / R);
y_traj2 = y_traj1(end) - R * cos(theta1) + R * cos(theta1 + t2 / R);

x_traj3 = x_traj2(end) - R * cos(theta2) + R * cos(theta2 + t3 / R);
y_traj3 = y_traj2(end) - R * sin(theta2) + R * sin(theta2 + t3 / R);

% 绘制结果
figure;
hold on;
grid on;

% 绘制泊车位
rectangle('Position', [parking_spot_x, ...
                       parking_spot_y - parking_spot_width/2, ...
                       parking_spot_length, parking_spot_width], ...
          'EdgeColor', 'r', 'LineWidth', 2);

% 绘制车辆初始位置
plot_vehicle(x0, y0, theta0, L, W, 'b');

% 绘制轨迹
plot([x_traj1, x_traj2, x_traj3], [y_traj1, y_traj2, y_traj3], 'g--', 'LineWidth', 2);

% 绘制车辆目标位置
plot_vehicle(parking_spot_x, parking_spot_y, theta3, L, W, 'r');

title('平行泊车路径规划');
xlabel('X (m)');
ylabel('Y (m)');
axis equal;
legend('泊车位', '初始车辆位置', '泊车轨迹', '目标车辆位置');

% 绘制车辆函数
function plot_vehicle(x, y, theta, L, W, color)
    corners = [L/2, W/2; -L/2, W/2; -L/2, -W/2; L/2, -W/2];
    R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
    rotated_corners = (R * corners')';
    rotated_corners(:,1) = rotated_corners(:,1) + x;
    rotated_corners(:,2) = rotated_corners(:,2) + y;
    fill(rotated_corners(:,1), rotated_corners(:,2), color, 'FaceAlpha', 0.5, 'EdgeColor', 'k');
end

---

4. 运行结果

• 垂直泊车：车辆从初始位置倒车进入垂直车位，轨迹为一段圆弧。
• 平行泊车：车辆通过多次前进和后退调整姿态，最终停入平行车位。

---