Simulink에서 차동 구동 로봇의 경로 계획하기

이 예제는 Simulink®에서 주어진 맵의 두 위치 사이에 장애물 없는 경로를 실행하는 방법을 보여줍니다. 경로는 확률적 로드맵(PRM) 계획 알고리즘(mobileRobotPRM))을 사용하여 생성됩니다. 이 경로를 탐색하기 위한 제어 명령은 Pure Pursuit 제어기 블록을 사용하여 만듭니다. 차동 구동 기구학 모션 모델은 이러한 명령을 기반으로 로봇 모션을 시뮬레이션합니다.

맵 및 Simulink 모델 불러오기

맵 내에서 맵 제한과 장애물을 정의하는 점유 맵을 불러옵니다. exampleMaps.mat에는 이 예제에서 사용하는 simpleMap을 비롯한 여러 가지 맵이 있습니다.

load exampleMaps.mat

맵 내에서 시작 위치와 끝 위치를 지정합니다.

startLoc = [5 5];
goalLoc = [20 20];

모델 개요

Simulink 모델을 엽니다.

open_system('pathPlanningSimulinkModel.slx')

모델은 세 가지 주요 부분으로 구성됩니다.

• 계획

• 제어

• 플랜트 모델

계획

Planner MATLAB® Function 블록은 mobileRobotPRM 경로 플래너를 사용하며 시작 위치, 목표 위치, 맵을 입력으로 받습니다. 이 블록은 웨이포인트로 구성된 배열을 출력합니다. 로봇은 이러한 웨이포인트를 따라가서 목표 지점에 도달합니다. 계획된 웨이포인트는 Pure Pursuit 제어기 블록에서 다운스트림으로 사용됩니다.

제어

Pure Pursuit

Pure Pursuit 제어기 블록은 웨이포인트와 로봇의 현재 자세를 기반으로 선속도 명령과 각속도 명령을 생성합니다.

목표 도달 여부 검사

Check Distance to Goal 서브시스템은 목표까지의 현재 거리를 계산하고 임계값 내에 있으면 시뮬레이션을 중지합니다.

플랜트 모델

Differential Drive Kinematic Model 블록은 단순한 이동체 기구학을 시뮬레이션하는 이동체 모델을 만듭니다. 이 블록은 선속도와 각속도를 Pure Pursuit 제어기 블록의 명령 입력으로 받고 현재 위치 상태와 속도 상태를 출력합니다.

모델 실행하기

simulation = sim('pathPlanningSimulinkModel.slx');

로봇 모션 시각화하기

모델을 시뮬레이션한 후 맵에서 장애물 없는 경로를 주행하는 로봇을 시각화합니다.

map = binaryOccupancyMap(simpleMap);
robotPose = simulation.Pose;
thetaIdx = 3;

% Translation
xyz = robotPose;
xyz(:, thetaIdx) = 0;

% Rotation in XYZ euler angles
theta = robotPose(:,thetaIdx);
thetaEuler = zeros(size(robotPose, 1), 3 * size(theta, 2));
thetaEuler(:, end) = theta;

% Plot the robot poses at every 10th step.
for k = 1:10:size(xyz, 1) 
    show(map)
    hold on;
    
    % Plot the start location.
    plotTransforms([startLoc, 0], eul2quat([0, 0, 0]))
    text(startLoc(1), startLoc(2), 2, 'Start');
    
    % Plot the goal location.
    plotTransforms([goalLoc, 0], eul2quat([0, 0, 0]))
    text(goalLoc(1), goalLoc(2), 2, 'Goal');
    
    % Plot the xy-locations.
    plot(robotPose(:, 1), robotPose(:, 2), '-b')
    
    % Plot the robot pose as it traverses the path.
    quat = eul2quat(thetaEuler(k, :), 'xyz');
    plotTransforms(xyz(k,:), quat, 'MeshFilePath',...
        'groundvehicle.stl');
    light;
    drawnow;
    hold off;
end

© Copyright 2019 The MathWorks, Inc.