Simulink에서 외륜 로봇의 경로 계획하기

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

맵 및 Simulink 모델 불러오기

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

load exampleMaps.mat

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

startLoc = [5 5];
goalLoc = [12 3];

모델 개요

Simulink 모델을 엽니다

open_system('pathPlanningUnicycleSimulinkModel.slx')

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

• 계획

• 제어

• 플랜트 모델

계획

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

제어

Pure Pursuit

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

목표 도달 여부 검사

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

플랜트 모델

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

모델 실행하기

모델을 시뮬레이션합니다

simulation = sim('pathPlanningUnicycleSimulinkModel.slx');

로봇 모션 시각화하기

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

map = binaryOccupancyMap(simpleMap)

map = 
  binaryOccupancyMap with properties:

   mapLayer Properties
              LayerName: 'binaryLayer'
               DataType: 'logical'
           DefaultValue: 0
    GridLocationInWorld: [0 0]
      GridOriginInLocal: [0 0]
     LocalOriginInWorld: [0 0]
             Resolution: 1
               GridSize: [26 27]
           XLocalLimits: [0 27]
           YLocalLimits: [0 26]
           XWorldLimits: [0 27]
           YWorldLimits: [0 26]

robotPose = simulation.UnicyclePose

robotPose = 428×3

    5.0000    5.0000         0
    5.0000    5.0000   -0.0002
    5.0001    5.0000   -0.0012
    5.0006    5.0000   -0.0062
    5.0031    5.0000   -0.0313
    5.0156    4.9988   -0.1569
    5.0707    4.9707   -0.7849
    5.0945    4.9354   -1.1140
    5.1075    4.9059   -1.1828
    5.1193    4.8759   -1.2030
      ⋮

numRobots = size(robotPose, 2) / 3;
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;

for k = 1:size(xyz, 1)
    show(map)
    hold on;
    
    % Plot Start Location
    plotTransforms([startLoc, 0], eul2quat([0, 0, 0]))
    text(startLoc(1), startLoc(2), 2, 'Start');
    
    % Plot Goal Location
    plotTransforms([goalLoc, 0], eul2quat([0, 0, 0]))
    text(goalLoc(1), goalLoc(2), 2, 'Goal');
    
    % Plot Robot's XY locations
    plot(robotPose(:, 1), robotPose(:, 2), '-b')
    
    % Plot Robot's pose as it traverses the path
    quat = eul2quat(thetaEuler(k, :), 'xyz');
    plotTransforms(xyz(k,:), quat, 'MeshFilePath',...
        'groundvehicle.stl');
    
    pause(0.01)
    hold off;
end

© Copyright 2019 The MathWorks, Inc.