차동 구동 로봇의 경로 추종

이 예제는 로봇 시뮬레이터를 사용하여 로봇이 목표 경로를 추종하도록 제어하는 방법을 보여줍니다. 이 예제에서는 Pure Pursuit 경로 추정 제어기를 사용해 미리 지정된 경로를 따라 시뮬레이션 로봇을 구동합니다. 목표 경로란 명시적으로 정의되거나 경로 플래너를 사용하여 계산되는 일련의 웨이포인트입니다(Path Planning in Environments of Different Complexity 항목 참조). 시뮬레이션 차동 구동 로봇에 대한 Pure Pursuit 경로 추정 제어기가 만들어지고 지정된 경로를 추종하도록 제어 명령을 계산합니다. 계산된 제어 명령을 사용하여 Pure Pursuit 제어기를 기반으로 목표 경로를 추종하도록 목표 궤적을 따라 시뮬레이션 로봇을 구동합니다.

웨이포인트 정의하기

로봇의 목표 경로에 대한 일련의 웨이포인트를 정의합니다.

path = [2.00    1.00;
        1.25    1.75;
        5.25    8.25;
        7.25    8.75;
        11.75   10.75;
        12.00   10.00];

경로에 의해 정의된 로봇의 현재 위치와 목표 위치를 설정합니다.

robotInitialLocation = path(1,:);
robotGoal = path(end,:);

초기 로봇 방향을 가정합니다. (로봇 방향은 로봇이 향하는 방향과 반시계 방향으로 측정되는 양의 x축 사이의 각입니다.)

initialOrientation = 0;

로봇의 현재 자세 [x y theta]를 정의합니다.

robotCurrentPose = [robotInitialLocation initialOrientation]';

기구학적 로봇 모델 만들기

로봇 모델을 초기화하고 초기 자세를 할당합니다. 시뮬레이션 로봇은 2륜 차동 구동 로봇의 모션에 대한 기구학 방정식을 가집니다. 이 시뮬레이션 로봇에 대한 입력값은 선속도와 각속도입니다.

robot = differentialDriveKinematics("TrackWidth", 1, "VehicleInputs", "VehicleSpeedHeadingRate");

목표 경로를 시각화합니다.

figure
plot(path(:,1), path(:,2),'k--d')
xlim([0 13])
ylim([0 13])

경로 추종 제어기 정의하기

위에서 정의한 경로와 로봇 모션 모델을 기반으로 로봇이 경로를 추종하도록 구동하는 경로 추종 제어기가 필요합니다. controllerPurePursuit 객체를 사용하여 경로 추종 제어기를 만듭니다.

controller = controllerPurePursuit;

위에서 정의한 경로를 사용하여 목표 웨이포인트를 제어기에 설정합니다.

controller.Waypoints = path;

경로 추종 제어기의 파라미터를 설정합니다. 이 예제에서는 목표 선속도를 0.6미터/초로 설정합니다.

controller.DesiredLinearVelocity = 0.6;

최대 각속도는 회전 속도의 포화 한도 역할을 하며, 이 예제에서는 2라디안/초로 설정합니다.

controller.MaxAngularVelocity = 2;

일반적으로 매끄러운 경로를 만들려면 전방 주시 거리(look-ahead distance)가 목표 선속도보다 커야 합니다. 전방 주시 거리가 너무 커지면 로봇이 코너를 무시하게 될 수도 있습니다. 반대로, 전방 주시 거리가 너무 작으면 경로 추종 동작이 불안정해질 수 있습니다. 이 예제에서는 값을 0.3m로 선택합니다.

controller.LookaheadDistance = 0.3;

경로 추적 제어기를 사용하여 목표 웨이포인트를 따라 로봇 구동하기

경로 추적 제어기가 로봇에 입력 제어 신호를 보내고, 로봇은 이 신호를 사용하여 목표 경로를 따라 구동합니다.

목표 반경을 정의합니다. 목표 반경은 로봇의 최종 위치와 목표 위치 사이의 원하는 거리 임계값입니다. 로봇이 목표 위치로부터 이 거리 내에 도달하면 정지합니다. 또한 로봇 위치와 목표 위치 사이의 현재 거리를 계산합니다. 이 거리를 목표 반경을 기준으로 지속적으로 확인하다가 이 거리가 목표 반경보다 작아지면 로봇이 정지합니다.

목표 반경 값이 너무 작으면 로봇이 목표를 놓칠 수 있으며, 이렇게 되면 목표 지점 근처에서 예기치 않은 동작이 발생할 수 있습니다.

goalRadius = 0.1;
distanceToGoal = norm(robotInitialLocation - robotGoal);

controllerPurePursuit 객체는 로봇의 제어 명령을 계산합니다. 다음의 제어 명령을 사용하여 로봇이 목표 반경 이내에 도달할 때까지 로봇을 구동합니다. 외부 시뮬레이터 또는 물리적 로봇을 사용하는 경우 제어기 출력값을 로봇에 적용해야 하며, 로봇의 자세를 업데이트하기 위해 위치추정 시스템이 필요할 수 있습니다. 제어기는 10Hz로 실행됩니다.

% Initialize the simulation loop
sampleTime = 0.1;
vizRate = rateControl(1/sampleTime);

% Initialize the figure
figure

% Determine vehicle frame size to most closely represent vehicle with plotTransforms
frameSize = robot.TrackWidth/0.8;

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    
    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    hold all
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
    plotTransforms(plotTrVec', plotRot, "MeshFilePath", "groundvehicle.stl", "Parent", gca, "View","2D", "FrameSize", frameSize);
    light;
    xlim([0 13])
    ylim([0 13])
    
    waitfor(vizRate);
end

PRM과 함께 경로 추종 제어기 사용하기

일련의 목표 웨이포인트가 경로 플래너로 계산된다면 경로 추종 제어기도 같은 방식으로 사용할 수 있습니다. 먼저 맵을 시각화합니다.

load exampleMaps
map = binaryOccupancyMap(simpleMap);
figure
show(map)

PRM 경로 계획 알고리즘을 사용하여 path를 계산할 수 있습니다. 자세한 내용은 Path Planning in Environments of Different Complexity 항목을 참조하십시오.

mapInflated = copy(map);
inflate(mapInflated, robot.TrackWidth/2);
prm = robotics.PRM(mapInflated);
prm.NumNodes = 100;
prm.ConnectionDistance = 10;

시작 위치와 끝 위치 사이의 경로를 구합니다. PRM 알고리즘의 확률적 특성 때문에 path가 다를 수 있습니다.

startLocation = [4.0 2.0];
endLocation = [24.0 20.0];
path = findpath(prm, startLocation, endLocation)

path = 8×2

    4.0000    2.0000
    3.1703    2.7616
    7.0797   11.2229
    8.1337   13.4835
   14.0707   17.3248
   16.8068   18.7834
   24.4564   20.6514
   24.0000   20.0000

확장형 맵, 로드맵, 최종 경로를 표시합니다.

show(prm);

위에서 정의한 경로 추종 제어기를 재사용하여 이 맵에서 로봇의 제어 명령을 계산할 수 있습니다. 이 제어기를 재사용해서 다른 정보는 동일하게 유지하면서 웨이포인트를 재정하려면 release 함수를 사용합니다.

release(controller);
controller.Waypoints = path;

경로에 의해 정의된 로봇의 초기 위치와 목표를 설정합니다.

robotInitialLocation = path(1,:);
robotGoal = path(end,:);

초기 로봇 방향을 가정합니다.

initialOrientation = 0;

로봇 모션의 현재 자세 [x y theta]를 정의합니다.

robotCurrentPose = [robotInitialLocation initialOrientation]';

목표 위치까지의 거리를 계산합니다.

distanceToGoal = norm(robotInitialLocation - robotGoal);

목표 반경을 정의합니다.

goalRadius = 0.1;

주어진 맵에서, 제어기 출력값을 사용하여 로봇이 목표에 도달할 때까지 로봇을 구동합니다. 제어기는 10Hz로 실행됩니다.

reset(vizRate);

% Initialize the figure
figure

while( distanceToGoal > goalRadius )
    
    % Compute the controller outputs, i.e., the inputs to the robot
    [v, omega] = controller(robotCurrentPose);
    
    % Get the robot's velocity using controller inputs
    vel = derivative(robot, robotCurrentPose, [v omega]);
    
    % Update the current pose
    robotCurrentPose = robotCurrentPose + vel*sampleTime; 
    
    % Re-compute the distance to the goal
    distanceToGoal = norm(robotCurrentPose(1:2) - robotGoal(:));
    
    % Update the plot
    hold off
    show(map);
    hold all

    % Plot path each instance so that it stays persistent while robot mesh
    % moves
    plot(path(:,1), path(:,2),"k--d")
    
    % Plot the path of the robot as a set of transforms
    plotTrVec = [robotCurrentPose(1:2); 0];
    plotRot = axang2quat([0 0 1 robotCurrentPose(3)]);
    plotTransforms(plotTrVec', plotRot, 'MeshFilePath', 'groundvehicle.stl', 'Parent', gca, "View","2D", "FrameSize", frameSize);
    light;
    xlim([0 27])
    ylim([0 26])
    
    waitfor(vizRate);
end

참고 항목

• Path Planning in Environments of Different Complexity

• Mapping with Known Poses

• Simulate Different Kinematic Models for Mobile Robots