【二维路径规划】基于hybridAStar实现自主泊车库路径规划附matlab代码

1 简介

一种基于混合A*算法的两段式自主泊车路径规划方法,包括:将泊车路径分为第一段和第二段;第一段为从车辆进入停车场到车辆行驶到最小泊车距离点的路径,第二段为车辆从最小泊车距离点行驶到泊车终止点的路径​。

2 部分代码

% Main entry:​ObstList = [-25:25;15*ones(1,51)]';            % Obstacle point listObstList = [ObstList; [-10: 10; 0*ones(1,21)]'];ObstList = [ObstList; [-25:-10; 5*ones(1,16)]'];ObstList = [ObstList; [ 10: 25; 5*ones(1,16)]'];ObstList = [ObstList; [ 10*ones(1,6);0:  5;]'];ObstList = [ObstList; [-10*ones(1,6);0:  5;]'];% Park lot line for collision checkObstLine = [-25, 15 , 25, 15;            -25,  5, -10,  5;            -10,  5, -10,  0;            -10,  0,  10,  0;             10,  0,  10,  5;             10,  5,  25,  5;            -25,  5, -25, 15;             25,  5,  25, 15];% ObstList and ObstLineObstInfo.ObstList = ObstList;ObstInfo.ObstLine = ObstLine;% ObstInfo.ObstMap = GridAStar(ObstList,End,XY_GRID_RESOLUTION);​Vehicle.WB = 3.7;         % [m] wheel base: rear to front steerVehicle.W  = 2.6;         % [m] width of vehicleVehicle.LF = 4.5;         % [m] distance from rear to vehicle front end of vehicleVehicle.LB = 1.0;         % [m] distance from rear to vehicle back end of vehicleVehicle.MAX_STEER = 0.6;  % [rad] maximum steering angle Vehicle.MIN_CIRCLE = Vehicle.WB/tan(Vehicle.MAX_STEER); % [m] mininum steering circle radius​% Motion resolution defineConfigure.MOTION_RESOLUTION = 0.1;             % [m] path interporate resolutionConfigure.N_STEER = 20.0;                      % number of steer commandConfigure.EXTEND_AREA = 0;                     % [m] map extend lengthConfigure.XY_GRID_RESOLUTION = 2.0;            % [m]Configure.YAW_GRID_RESOLUTION = deg2rad(15.0); % [rad]% Grid boundConfigure.MINX = min(ObstList(:,1))-Configure.EXTEND_AREA;Configure.MAXX = max(ObstList(:,1))+Configure.EXTEND_AREA;Configure.MINY = min(ObstList(:,2))-Configure.EXTEND_AREA;Configure.MAXY = max(ObstList(:,2))+Configure.EXTEND_AREA;Configure.MINYAW = -pi-0.01;Configure.MAXYAW = pi;% Cost related defineConfigure.SB_COST = 0;             % switch back penalty costConfigure.BACK_COST = 1.5;         % backward penalty costConfigure.STEER_CHANGE_COST = 1.5; % steer angle change penalty costConfigure.STEER_COST = 1.5;        % steer angle change penalty costConfigure.H_COST = 10;             % Heuristic cost​StartState = [22, 13, pi  ];EndState =   [7,   2, pi/2];[x,y,th,~,~] = HybridAStar(StartState,EndState,Vehicle,Configure,ObstInfo);if isempty(x)    disp("Failed to find path!")else    hold on;    VehicleAnimation(x,y,th,Configure,Vehicle,ObstInfo)end

3 仿真结果

4 参考文献

[1]张瑶港, 陈国迎, 高振海,等. 一种基于混合A*算法的两段式自主泊车路径规划方法:, CN112606830A[P]. 2021.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。