이동 로봇으로 경로 계획 시뮬레이션 수행하기

라이브 스크립트 열기

방을 탐색하는 이동 로봇을 시뮬레이션하는 시나리오를 생성합니다. 이 예제에서는 시나리오를 생성하고, 강체 트리 객체로부터 로봇 플랫폼을 모델링하고, 이 시나리오에서 이진 점유 그리드 맵을 구하고, mobileRobotPRM 경로 계획 알고리즘을 사용하여 이동 로봇이 따를 경로를 계획하는 방법을 보여줍니다.

지면 평면과 정적 메시를 사용하는 시나리오 생성하기

robotScenario 객체는 여러 정적 장애물 그리고 플랫폼이라고 불리는 움직이는 물체들로 구성됩니다. robotPlatform 객체를 사용하여 시나리오 내에서 이동 로봇을 모델링합니다. 이 예제에서는 방을 만들기 위해 지면 평면과 박스 메시로 구성된 시나리오를 작성합니다.

scenario = robotScenario(UpdateRate=5);

평면 메시를 시나리오에 지면 평면으로 추가합니다.

floorColor = [0.5882 0.2941 0];
addMesh(scenario,"Plane",Position=[5 5 0],Size=[10 10],Color=floorColor);

방의 벽은 박스 메시로 모델링됩니다. 정적 메시는 IsBinaryOccupied 값이 true로 설정된 상태로 추가되어, 따라서 해당 장애물이 경로 계획에 사용할 이진 점유 맵에 반영됩니다.

wallHeight = 1;
wallWidth = 0.25;
wallLength = 10;
wallColor = [1 1 0.8157];

% Add outer walls.
addMesh(scenario,"Box",Position=[wallWidth/2, wallLength/2, wallHeight/2],...
    Size=[wallWidth, wallLength, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength-wallWidth/2, wallLength/2, wallHeight/2],...
    Size=[wallWidth, wallLength, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallLength-wallWidth/2, wallHeight/2],...
    Size=[wallLength, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallWidth/2, wallHeight/2],...
    Size=[wallLength, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);

% Add inner walls.
addMesh(scenario,"Box",Position=[wallLength/8, wallLength/3, wallHeight/2],...
    Size=[wallLength/4, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/4, wallLength/3, wallHeight/2],...
    Size=[wallWidth, wallLength/6,  wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, wallHeight/2],...
   Size=[wallLength/2, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallLength/2, wallHeight/2],...
    Size=[wallWidth, wallLength/3, wallHeight],Color=wallColor,IsBinaryOccupied=true);

시나리오를 시각화합니다.

show3D(scenario);
lightangle(-45,30);
view(60,50);

시나리오에서 이진 점유 맵 구하기

경로 계획을 위해 시나리오에서 binaryOccupancyMap 객체 형태로 점유 맵을 만듭니다. 맵의 점유 공간을 0.3m만큼 확장합니다.

map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
                                           MapSize=[15 15],...
                                           MapHeightLimits=[0 3]);
inflate(map,0.3);

2차원 점유 맵을 시각화합니다.

show(map)

경로 계획

mobileRobotPRM 경로 플래너를 사용하여 맵에서 출발 위치부터 목표 위치까지 장애물 없는 경로를 구합니다.

이동 로봇의 출발 위치와 목표 위치를 지정합니다.

startPosition = [1 1];
goalPosition = [8 8];

반복성을 위해 rng 시드값을 설정합니다.

rng(100)

이진 점유 맵을 사용하여 mobileRobotPRM 객체를 만들고 최대 노드 개수를 지정합니다. 연결된 두 노드 사이의 최대 거리를 지정합니다.

numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;

출발 위치와 목표 위치 사이의 경로를 구합니다.

waypoints = findpath(planner,startPosition,goalPosition);

궤적 생성

waypointTrajectory System object™를 사용하여 이동 로봇이 계획된 경로의 웨이포인트를 따라 이동할 궤적을 생성합니다 .

% Robot height from base.
robotheight = 0.12;
% Number of waypoints.
numWaypoints = size(waypoints,1);
% Robot arrival time at first waypoint.
firstInTime = 0;
% Robot arrival time at last waypoint.
lastInTime = firstInTime + (numWaypoints-1);
% Generate waypoint trajectory with waypoints from planned path.
traj = waypointTrajectory(SampleRate=10,...
                          TimeOfArrival=firstInTime:lastInTime, ...
                          Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ...
                          ReferenceFrame="ENU");

로봇 플랫폼을 시나리오에 추가하기

로봇 라이브러리에서 Clearpath Husky 이동 로봇을 rigidBodyTree 객체로 불러옵니다.

huskyRobot = loadrobot("clearpathHusky");

rigidBodyTree 객체로 지정된 로봇 모델과 waypointTrajectory System object로 지정된 궤적을 사용해 robotPlatform 객체를 생성합니다.

platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
                         BaseTrajectory=traj);

로봇을 포함하여 시나리오를 시각화합니다.

[ax,plotFrames] = show3D(scenario);
lightangle(-45,30)
view(60,50)

이동 로봇 시뮬레이션하기

계획된 경로를 시각화합니다.

hold(ax,"on")
plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
               LineWidth=2,...
               MarkerSize=4,...
               MarkerEdgeColor="b",...
               MarkerFaceColor=[0.5 0.5 0.5]);
hold(ax,"off")

시뮬레이션을 설정합니다. 로봇의 모든 자세를 이미 알고 있기 때문에 시뮬레이션을 순차적으로 실행하여 매 스텝마다 시각화를 업데이트하면 됩니다.

setup(scenario)

% Control simulation rate at 20 Hz.
r = rateControl(20);

% Status of robot in simulation.
robotStartMoving = false;

while advance(scenario)
    show3D(scenario,Parent=ax,FastUpdate=true);
    waitfor(r);

    currentPose = read(platform);
    if ~any(isnan(currentPose))
        % implies that robot is in the scene and performing simulation.
        robotStartMoving = true;
    end
    if any(isnan(currentPose)) && robotStartMoving
        % break, once robot reaches goal position.
        break;
    end
end

시뮬레이션을 다시 실행하고 결과를 다시 시각화하려면 시나리오에서 시뮬레이션을 초기화합니다.

restart(scenario)