주차 알고리즘 matlab

2024.01.29 - [자율주행 자동차] - 자동주차 알고리즘

자동주차 알고리즘

여기서 시작해서 계속 진행중인 내용 입니다

mapLayers = loadParkingLotMapLayers;
plotMapLayers(mapLayers)

% For simplicity, combine the three layers into a single costmap.
costmap = combineMapLayers(mapLayers);

figure
plot(costmap, Inflation="off");
legend off

여기선 맵을 인식해서 넣어주기

vehicleDims      = vehicleDimensions;
maxSteeringAngle = 35; % in degrees

차량정보 집어넣어주기

costmap.CollisionChecker.VehicleDimensions = vehicleDims;

맵에 넣어서 충돌 정보 인식시켜주기

currentPose = [4 12 0]; % [x, y, theta]

차량 현재 위치 여기선 전부 도 단위를 사용합니다 (0~ 360)

data = load("routePlan.mat");
routePlan = data.routePlan %#ok<NOPTS>

차량의 중간 목표지점 넣어주기

% Plot vehicle at current pose
hold on
helperPlotVehicle(currentPose, vehicleDims, DisplayName="Current Pose")
legend(Location="northwest")

for n = 1 : height(routePlan)
    % Extract the goal waypoint
    vehiclePose = routePlan{n, "EndPose"};

    % Plot the pose
    legendEntry = "Goal " + n;
    helperPlotVehicle(vehiclePose, vehicleDims, DisplayName=legendEntry);
end
hold off

그래프 상에 차량 위치 표시해주기

BehavioralPlanner = HelperBehavioralPlanner(routePlan, maxSteeringAngle);

중간지점과 스티어링 최대각을 넣어서 경로 탐색

ss = stateSpaceSE2;
ss.StateBounds = [costmap.MapExtent(1, 1:2); costmap.MapExtent(1, 3:4); -pi, pi];

정보 넣어주기(아마 차량의 가동 범위)

validator = validatorVehicleCostmap(ss, Map=costmap);

cost map을 우리가 만들 수 있을 지 모르겠는데... 차량이 지나갈 수 있는지 판단

minTurningRadius = 10; % in meters
motionPlanner = plannerHybridAStar(validator, MinTurningRadius=minTurningRadius, ...
    MotionPrimitiveLength=4); % length in meters

하이브리드 A*를 활용하여 부드러운 경로를 만든다.

goalPose = routePlan{1, "EndPose"};
currentPoseRad = [currentPose(1:2) deg2rad(currentPose(3))];
goalPoseRad = [goalPose(1:2) deg2rad(goalPose(3))];
refPath = plan(motionPlanner, currentPoseRad, goalPoseRad);

plannerAxes = show(motionPlanner, Tree="off", HeadingLength=0); % Visualize the planned path.

이전에 만든 경로를 보여준다.

weights = struct(Time=10, Smoothness=100, Obstacle=50);
vehicleInfo = struct("Dimension", [vehicleDims.Length, vehicleDims.Width], "Shape", "Rectangle");

localPlanner = controllerTEB(zeros(3,3),...
    MaxVelocity=[5 0.5],... % in m/s and rad/s
    MaxAcceleration=[2 0.5],... % in m/s/s and rad/s/s
    LookAheadTime=3,... % in seconds
    MinTurningRadius=minTurningRadius, NumIteration=2,...
    CostWeights=weights, RobotInformation=vehicleInfo);

부드러운 궤적을 생성하기 위한 TEB알고리즘 사용

% The local planner does not require knowledge of the full parking lot map
% since it is only planning a short local trajectory. Providing the local
% planner with a smaller map increases planner performance.
maxLocalPlanDistance = 15; % in meters
localPlanner.Map = getLocalMap(costmap, currentPose, maxLocalPlanDistance);
localPlanner.ReferencePath = refPath;

% Along with an optimized trajectory, |localPath|, the local planner provides
% a reference velocity for each point on the path, |refVel|.
[refVel, ~, localPath, ~] = localPlanner(currentPoseRad, [0 0]);

% Show the local trajectory alongside the reference path.
hold(plannerAxes,"on");
plot(plannerAxes, localPath(:,1), localPath(:,2), "g", LineWidth=2, DisplayName="Local Path");
legend(plannerAxes, "show", findobj(plannerAxes, Type="Line"), {"Local Path","ReferencePath"});

주차 지점까지 많은 정보가 필요하지 않으므로 15까지만 본다.

closeFigures; % Close all

% Create the vehicle simulator.
vehicleSim = HelperVehicleSimulator(costmap, vehicleDims);

hideFigure(vehicleSim); % Hide vehicle simulation figure

% Configure the simulator to show the trajectory.
vehicleSim.showTrajectory(true);
vehicleSim.showLegend(true);

% Set the vehicle pose and velocity.
vehicleSim.setVehiclePose(currentPose);
currentVel = 0;
vehicleSim.setVehicleVelocity(currentVel);

차량 주행 전 여기선 시뮬레이션 준비하므로 실제 주행에선 필요없는 부분이다.

pathAnalyzer = HelperPathAnalyzer(Wheelbase=vehicleDims.Wheelbase);

여기서 방향 및 속도 차량 위치를 고려한 판단을 내린다.

sampleTime = 0.05;
lonController = HelperLongitudinalController(SampleTime=sampleTime);

여기서 속도를 제어한다.

controlRate = HelperFixedRate(1/sampleTime); % in Hertz

여기도 시뮬레이션 전용 속도 보장이다.

% Set the vehicle pose back to the initial starting point.
currentPose = [4 12 0]; % [x, y, theta]
vehicleSim.setVehiclePose(currentPose);

% Reset velocity.
currentVel  = 0; % meters/second
vehicleSim.setVehicleVelocity(currentVel);

% Initialize variables to store vehicle path.
refPath = [];
localPath = [];

% Setup pathAnalyzer to trigger update of local path every 1 second.
localPlanningFrequency = 1; % 1/seconds
pathAnalyzer.PlanningPeriod = 1/localPlanningFrequency/sampleTime; % timesteps

isGoalReached = false;

% Initialize count incremented each time the local planner is updated
localPlanCount = 0; % Used for visualization only

showFigure(vehicleSim); % Show vehicle simulation figure.

while ~isGoalReached
    % Plan for the next path segment if near to the next path segment start
    % pose.
    if planNextSegment(behavioralPlanner, currentPose, 2*maxLocalPlanDistance)
        % Request next maneuver from behavioral layer.
        [nextGoal, plannerConfig, speedConfig] = requestManeuver(behavioralPlanner, ...
            currentPose, currentVel);

        % Plan a reference path using A* planner to the next goal pose.
        if isempty(refPath)
            nextStartRad = [currentPose(1:2) deg2rad(currentPose(3))];
        else
            nextStartRad = refPath(end,:);
        end
        nextGoalRad = [nextGoal(1:2) deg2rad(nextGoal(3))];
        newPath = plan(motionPlanner, nextStartRad, nextGoalRad, SearchMode="exhaustive");

        % Check if the path is valid. If the planner fails to compute a path,
        % or the path is not collision-free because of updates to the map, the
        % system needs to re-plan. This scenario uses a static map, so the path
        % will always be collision-free.
        isReplanNeeded = ~checkPathValidity(newPath.States, costmap);
        if isReplanNeeded
            warning("Unable to find a valid path. Attempting to re-plan.")

            % Request behavioral planner to re-plan
            replanNeeded(behavioralPlanner);
        else
            % Append to refPath
            refPath = [refPath; newPath.States];
            hasNewPath = true;

            vehicleSim.plotReferencePath(refPath); % Plot reference path
        end
    end

    % Update the local path at the frequency specified by
    % |localPlanningFrequency|
    if pathUpdateNeeded(pathAnalyzer)
        currentPose  = getVehiclePose(vehicleSim);
        currentPoseRad = [currentPose(1:2) deg2rad(currentPose(3))];
        currentVel   = getVehicleVelocity(vehicleSim);
        currentAngVel = getVehicleAngularVelocity(vehicleSim);

        % Do local planning
        localPlanner.Map = getLocalMap(costmap, currentPose, maxLocalPlanDistance);

        if hasNewPath
            localPlanner.ReferencePath = refPath;
            hasNewPath = false;
        end

        [localVel, ~, localPath, ~] = localPlanner(currentPoseRad, [currentVel currentAngVel]);

        vehicleSim.plotLocalPath(localPath); % Plot new local path

        % For visualization only
        if mod(localPlanCount, 20) == 0
            snapnow; % Capture state of the figures
        end
        localPlanCount = localPlanCount+1;

        % Configure path analyzer.
        pathAnalyzer.RefPoses     = [localPath(:,1:2) rad2deg(localPath(:,3))];
        pathAnalyzer.Directions   = sign(localVel(:,1));
        pathAnalyzer.VelocityProfile = localVel(:,1);
    end

    % Find the reference pose on the path and the corresponding
    % velocity.
    [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);

    % Update driving direction for the simulator.
    updateDrivingDirection(vehicleSim, direction);

    % Compute steering command.
    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        Direction=direction, Wheelbase=vehicleDims.Wheelbase, PositionGain=4);

    % Compute acceleration and deceleration commands.
    lonController.Direction = direction;
    [accelCmd, decelCmd] = lonController(refVel, currentVel);

    % Simulate the vehicle using the controller outputs.
    drive(vehicleSim, accelCmd, decelCmd, steeringAngle);

    % Get current pose and velocity of the vehicle.
    currentPose  = getVehiclePose(vehicleSim);
    currentVel   = getVehicleVelocity(vehicleSim);

    % Check if the vehicle reaches the goal.
    isGoalReached = helperGoalChecker(nextGoal, currentPose, currentVel, speedConfig.EndSpeed, direction);

    % Wait for fixed-rate execution.
    waitfor(controlRate);
end

이제 여기부터 시뮬레이션이 실행된다.
초기 시작점을 다시 확인, 속도 설정, 경로 저장하기 위한 변수 초기화, 등 각종 파라미터 초기화 후 도착할 때 까지 시뮬레이션을 시작한다.

시뮬레이션 처음엔 경로를 설정해준다. 경로가 없다면 경로를 재 설정해주고, 있다면 갈곳에 추가해준다.

마지막엔 제어 부분이 들어가게 된다. 그럼 주행을 진행하게 된다.

위와 같은 경로로 차가 움직인다.

이제 주차를 할 시간이다.

만약 우리 대회가 바로 주차만 하면 되는 것 이라면 바로 여기로 들어와도 된다.

hideFigure(vehicleSim); % Hide vehicle simulation figure

일단 위 시뮬레이션 삭제

ccConfig = costmap.CollisionChecker;

figure
plot(ccConfig)
title("Current Collision Checker")

현 차량을 어떻게 볼지 확인.
현재는 엄청 크게 봐서 못 들어가는 부분이 많아 보인다.

ccConfig.NumCircles = 4;

figure
plot(ccConfig)
title("New Collision Checker")

이제 원 큰거 하나를 작은원 4개로 바꿔주면!

좀 더 촘촘하게 드나들 수 있는 부분이 많아진다.

costmap.CollisionChecker = ccConfig;

바꿔준 것을 다시 업데이트 해준다.

figure
plot(costmap)
title("Costmap with updated collision checker")

갈 수 있는 범위가 엄청 많아 졌다.

% Set up the pathPlannerRRT to use the updated costmap.
parkMotionPlanner = pathPlannerRRT(costmap, MinIterations=1000);

% Define desired pose for the parking spot, returned by the V2X system.
parkPose = [36 44 90];
preParkPose = currentPose;

% Compute the required parking maneuver.
refPath = plan(parkMotionPlanner, preParkPose, parkPose);

% Plot the resulting parking maneuver.
figure
plotParkingManeuver(costmap, refPath, preParkPose, parkPose)

이제 RRT*방식을 사용하여 경로를 만듭니다.

곡률이 연속적이지 않을 수 있고, 조향각도가 급격하게 변할수도 있다. => 매끄러워야 한다.

% Retrieve transition poses and directions from the planned path.
[transitionPoses, directions] = interpolate(refPath);

% Specify number of poses to return using a separation of approximately 0.1 m.
approxSeparation = 0.1; % meters

% Smooth the path.
numSmoothPoses   = round(refPath.Length / approxSeparation);
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses);

% Set up the velocity profile generator to stop at the end of the trajectory,
% with a speed limit of 5 mph.
refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 2.2352);

파라메트릭 입방 스플라인을 맞춰 경로를 매끄럽게 만든다.
스플라인 피팅을 통해 컨트롤러가 실행할 수 있는 부드러운 경로 생성

pathAnalyzer.RefPoses     = refPoses;
pathAnalyzer.Directions   = directions;
pathAnalyzer.VelocityProfile = refVelocities;

% Clear old path planning data from the plot
vehicleSim.clearReferencePath();
vehicleSim.clearLocalPath();

% Reset longitudinal controller.
reset(lonController);

isGoalReached = false;

while ~isGoalReached
    % Find the reference pose on the path and the corresponding velocity.
    [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);

    % Update driving direction for the simulator.
    updateDrivingDirection(vehicleSim, direction);

    % Compute steering command.
    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        Direction=direction, Wheelbase=vehicleDims.Wheelbase);

    % Compute acceleration and deceleration commands.
    lonController.Direction = direction;
    [accelCmd, decelCmd] = lonController(refVel, currentVel);

    % Simulate the vehicle using the controller outputs.
    drive(vehicleSim, accelCmd, decelCmd, steeringAngle);

    % Check if the vehicle reaches the goal.
    isGoalReached = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);

    % Wait for fixed-rate execution.
    waitfor(controlRate);

    % Get current pose and velocity of the vehicle.
    currentPose  = getVehiclePose(vehicleSim);
    currentVel   = getVehicleVelocity(vehicleSim);
end

closeFigures; % Close all
showFigure(vehicleSim); % Show vehicle simulation figure.

전방 주차 시뮬레이션 실행

후방 주차를 위해선 reeds shepp를 사용해야 한다.

% Specify a parking pose corresponding to a back-in parking maneuver.
parkPose = [49 47.2 -90];

% Change the connection method to allow for reverse motions.
parkMotionPlanner.ConnectionMethod = "Reeds-Shepp";

이제 모션 플래너도 조정하자

parkMotionPlanner.MinTurningRadius   = 8; % meters
parkMotionPlanner.ConnectionDistance = 15; % meters

% Reset vehicle pose and velocity.
currentVel = 0; % m/s
vehicleSim.setVehiclePose(preParkPose);
vehicleSim.setVehicleVelocity(currentVel);

% Compute the parking maneuver.
replan = true;
while replan
    refPath = plan(parkMotionPlanner, preParkPose, parkPose);

    % The path corresponding to the parking maneuver is small and requires
    % precise maneuvering. Instead of interpolating only at transition poses,
    % interpolate more finely along the length of the path.

    numSamples = 10;
    stepSize   = refPath.Length / numSamples;
    lengths    = 0 : stepSize : refPath.Length;

    [transitionPoses, directions] = interpolate(refPath, lengths);

    % Replan if the path contains more than one direction switching poses
    % or if the path is too long.
    replan = sum(abs(diff(directions(1:end-1))))~=2 || refPath.Length > 20;
end

% Visualize the parking maneuver.
figure
plotParkingManeuver(costmap, refPath, preParkPose, parkPose)

% Smooth the path.
numSmoothPoses   = round(refPath.Length / approxSeparation);
minSeparation = 0.5;
[refPoses, directions, cumLengths, curvatures] = smoothPathSpline(transitionPoses, directions, numSmoothPoses, minSeparation);

% Generate velocity profile.
refVelocities = helperGenerateVelocityProfile(directions, cumLengths, curvatures, currentVel, 0, 1);

경로 매끄럽게 만들기

pathAnalyzer.RefPoses     = refPoses;
pathAnalyzer.Directions   = directions;
pathAnalyzer.VelocityProfile = refVelocities;

reset(lonController); % Reset longitudinal controller.

isGoalReached = false;

while ~isGoalReached
    % Get current driving direction.
    currentDir = getDrivingDirection(vehicleSim);

    % Find the reference pose on the path and the corresponding velocity.
    [refPose, refVel, direction] = pathAnalyzer(currentPose, currentVel);

    % If the vehicle changes driving direction, reset vehicle velocity in
    % the simulator and reset longitudinal controller.
    if currentDir ~= direction
        currentVel = 0;
        setVehicleVelocity(vehicleSim, currentVel);
        reset(lonController);
    end

    % Update driving direction for the simulator. If the vehicle changes
    % driving direction, reset and return the current vehicle velocity as zero.
    currentVel = updateDrivingDirection(vehicleSim, direction, currentDir);

    % Compute steering command.
    steeringAngle = lateralControllerStanley(refPose, currentPose, currentVel, ...
        Direction=direction, Wheelbase=vehicleDims.Wheelbase);

    % Compute acceleration and deceleration commands.
    lonController.Direction = direction;
    [accelCmd, decelCmd] = lonController(refVel, currentVel);

    % Simulate the vehicle using the controller outputs.
    drive(vehicleSim, accelCmd, decelCmd, steeringAngle);

    % Check if the vehicle reaches the goal.
    isGoalReached = helperGoalChecker(parkPose, currentPose, currentVel, 0, direction);

    % Wait for fixed-rate execution.
    waitfor(controlRate);

    % Get current pose and velocity of the vehicle.
    currentPose  = getVehiclePose(vehicleSim);
    currentVel   = getVehicleVelocity(vehicleSim);
end

closeFigures;
snapnow; % Capture state of the simulation figure

delete(vehicleSim); % Delete the simulator.

다시 시뮬레이션!

이제 다음 포스팅은 맵을 조금 수정하여 우리의 목표인 평행 주차가 가능할지 확인해보겠다.