로봇 팔의 최소 저크 궤적 계획하기

이 예제에서는 로봇 매니퓰레이터의 최소 저크 다항식 궤적을 계획하는 방법을 보여줍니다. 포함된 로봇 모델을 불러와 장애물이 있는 환경에서 로봇 모델의 경로를 계획하고, 경로에서 최소 저크 궤적을 생성하고, 생성된 궤적과 로봇 모션을 시각화하는 방법을 보여주는 예제입니다.

로봇 모델과 환경 설정하기

이 예제에서는 7자유도 로봇 매니퓰레이터인 KUKA LBR iiwa 모델을 사용합니다. loadrobot을 사용하여 로봇 모델을 rigidBodyTree 객체로 작업 공간에 불러옵니다. 컨피규레이션의 출력 형식을 "row"로 설정합니다.

robot = loadrobot("kukaIiwa14",DataFormat="row");

로봇을 위한 환경을 생성합니다. 충돌 객체를 만들고 로봇 베이스를 기준으로 자세를 지정합니다. 환경을 시각화합니다.

env = {collisionBox(0.5,0.5,0.05,Pose=trvec2tform([0 0 -0.05])), ...
       collisionSphere(0.3,Pose=trvec2tform([0.1 0.2 0.8]))};
env{1}.Pose(3,end) = -0.05;
env{2}.Pose(1:3,end) = [0.1 0.2 0.8];

show(robot);
hold on
showCollisionArray(env);
axis([-1 1 -1 1 -0.1 1.75])
hold off

manipulatorRRT를 사용하여 경로 계획하기

manipulatorRRT를 사용하여 로봇 모델의 RRT 플래너를 만듭니다. 경로를 보간하는 동안 중간 상태의 개수를 늘리도록 ValidationDistance 속성을 설정합니다.

rrt = manipulatorRRT(robot,env);
rrt.ValidationDistance = 0.2;
rrt.SkippedSelfCollisions = "parent";

시작 컨피규레이션과 목표 컨피규레이션을 지정합니다.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

경로를 계획합니다. RRT 알고리즘은 임의성이 있으므로 반복성을 위해 rng 시드값을 설정합니다.

rng(0)
path = plan(rrt,startConfig,goalConfig);

경로를 보간하고 웨이포인트를 가져옵니다.

interpPath = interpolate(rrt,path);
wpts = interpPath';

최소 저크 다항식 궤적 생성하기

플래너는 경로를 웨이포인트의 순서 집합으로 반환합니다. 이를 로봇에 전달하려면 먼저 이러한 웨이포인트를 통과하는 궤적을 결정해야 합니다. minjerkpolytraj 함수는 지정된 모든 웨이포인트를 지나는 매끄러운 최소 저크 궤적을 만듭니다.

로봇 팔이 웨이포인트에 도착하는 시간에 대한 초기 추측값을 제공합니다.

initialGuess = linspace(0,size(wpts,2)*0.2,size(wpts,2));

궤적을 추정하는 동안 사용할 샘플 개수를 지정합니다.

numSamples = 100;

최소 저크 다항식 궤적을 계산합니다.

[q,qd,qdd,qddd,pp,tpts,tSamples] = minjerkpolytraj(wpts,initialGuess,numSamples);

조인트 궤적과 웨이포인트 시각화하기

시간 경과에 따른 궤적과 웨이포인트를 플로팅합니다.

minJerkPath = q';
figure
plot(tSamples,q)
hold on
scatter(tpts,wpts,40)
title("Joint Trajectories and Waypoints")
xlabel("Time")
ylabel("Joint Angle (rad)")

로봇 모션 시각화하기

show 객체 함수를 사용하여 결과 모션을 애니메이션으로 표시합니다. 이 시각화를 통해 빠른 업데이트가 가능하여 매끄러운 애니메이션이 보장됩니다.

figure
ax = show(robot,startConfig);
hold on
showCollisionArray(env);
axis([-1 1 -1 1 -0.1 1.75])
title(["Animation of Robot Arm Following","Minimum Jerk Trajectory"])
for i = 1:size(minJerkPath,1)
    show(robot,minJerkPath(i,:),PreservePlot=false,FastUpdate=true);
    drawnow;
end
hold off