Main Content

derivative

이동체 상태의 시간 도함수

R2019b 이후

페이지 내 모두 축소

구문

stateDot = derivative(motionModel,state,cmds)

설명

예제

stateDot = derivative(motionModel,state,cmds)는 모션 모델이 bicycleKinematics 객체, differentialDriveKinematics 객체 또는 unicycleKinematics 객체일 경우 현재 상태 도함수 stateDot를 요소를 3개 가진 벡터 [xDot yDot thetaDot]로 반환합니다. 모션 모델이 ackermannKinematics 객체인 경우에는 state를 요소를 4개 가진 벡터 [xDot yDot thetaDot psiDot]로 반환합니다. xDot와 yDot는 이동체 속도를 나타내며, 초당 미터 단위로 지정됩니다. thetaDot는 이동체 방향의 각속도이고 psiDot는 이동체 조향의 각속도이며, 둘 다 초당 라디안 단위로 지정됩니다.

예제

모두 축소

라이브 스크립트 열기

이 예제에서는 하나의 환경에서 다양한 로봇 기구학 모델을 모델링하고 비교하는 방법을 보여줍니다.

기구학 제약 조건을 가진 이동 로봇 정의하기

이동 로봇의 기구학을 모델링하는 방법은 여러 가지가 있습니다. 모든 모델링 방법은 바퀴 속도가 로봇 상태 [x y theta], 즉 xy 좌표와 라디안 단위의 로봇 방향 theta와 어떻게 연결되는지를 규정합니다.

외륜 기구학 모델

이동 로봇 이동체 기구학을 나타내는 가장 간단한 방법은 외륜 모델입니다. 이 모델은 중심 차축을 중심으로 하는 회전으로 바퀴 속도가 설정되고 z축을 중심으로 회전할 수 있습니다. 입력값이 이동체 속도와 방향 변화율로 제공되고 그 외 제약 조건이 고려되지 않을 경우 차동 구동 모델과 자전거 기구학 모델은 모두 외륜 기구학으로 축소됩니다.

unicycle = unicycleKinematics(VehicleInputs="VehicleSpeedHeadingRate");

차동 구동 기구학 모델

차동 구동 모델은 후방 구동 차축을 사용하여 이동체 속도와 방향 변화율을 모두 제어합니다. 구동 차축의 바퀴는 양방향으로 회전할 수 있습니다. 대부분의 이동 로봇은 로우 레벨의 바퀴 명령 인터페이스만 일부 가지므로 이 모델은 마찬가지로 이동체 속도와 방향 변화율을 입력값으로 사용하여 이동체 제어를 단순화합니다.

diffDrive = differentialDriveKinematics(VehicleInputs="VehicleSpeedHeadingRate");

외륜 모델과 동작을 구별하기 위해 차동 구동 기구학 모델에 바퀴 속도 제약 조건을 추가합니다.

diffDrive.WheelSpeedRange = [-10 10]*2*pi;

자전거 기구학 모델

자전거 모델은 로봇을 2개의 차축, 즉 후방 구동 차축과 z축을 중심으로 회전하는 전방 차축을 가진 차륜 모델로 처리합니다. 자전거 모델은 각 차축의 바퀴가 하나의 중심 바퀴로 모델링될 수 있고, 자전거처럼 전방 바퀴의 방향을 직접 설정할 수 있다는 가정하에 작동합니다.

bicycle = bicycleKinematics(VehicleInputs="VehicleSpeedHeadingRate",MaxSteeringAngle=pi/8);

기타 모델

Ackermann 기구학 모델은 변형된 차륜 모델로, Ackermann 조향을 가정합니다. 대부분의 차륜 이동체에서 앞바퀴는 같은 축을 중심으로 회전하는 것이 아니라, 이동체 회전의 가운데를 중심으로 동심원을 그리며 주행할 수 있도록 약간 다른 축에서 회전합니다. 이 회전 각도의 차이를 Ackermann 조향이라고 부르며, 일반적으로 실제 차량에서는 메커니즘에 따라 적용됩니다. 이동체 및 바퀴 기구학 관점에서는 조향각을 비율 입력으로 처리하여 적용할 수 있습니다. 이 시뮬레이션은 이 기구학 모델을 포함하지 않지만, 모델에 대한 자세한 내용은 ackermannKinematics 항목을 참조하십시오.

carLike = ackermannKinematics;

시뮬레이션 파라미터 설정하기

이러한 이동 로봇은 다양한 기구학으로 인해 발생하는 몇 가지 차이를 보여주도록 설계된 일련의 웨이포인트를 따라갑니다.

waypoints = [0 0; 0 10; 10 10; 5 10; 11 9; 4 -5];
% Define the total time and the sample rate
sampleTime = 0.05;               % Sample time [s]
tVec = 0:sampleTime:20;          % Time array
initPose = [waypoints(1,:)'; 0]; % Initial pose (x y theta)

이동체 제어기 만들기

이동체는 Pure Pursuit 제어기를 사용하여 일련의 웨이포인트를 따라갑니다. 일련의 웨이포인트, 로봇의 현재 상태, 몇 가지 다른 파라미터가 주어지면 제어기는 이동체 속도와 방향 변화율을 출력합니다.

% Define a controller. Each robot requires its own controller
controller1 = controllerPurePursuit(Waypoints=waypoints,DesiredLinearVelocity=3,MaxAngularVelocity=3*pi);
controller2 = controllerPurePursuit(Waypoints=waypoints,DesiredLinearVelocity=3,MaxAngularVelocity=3*pi);
controller3 = controllerPurePursuit(Waypoints=waypoints,DesiredLinearVelocity=3,MaxAngularVelocity=3*pi);

ODE 솔버를 사용하여 모델 시뮬레이션하기

모델은 derivative 함수를 사용하여 상태를 업데이트하면서 시뮬레이션됩니다. 이 예제에서는 ODE(상미분 방정식) 솔버를 사용하여 해를 생성합니다. 또 다른 방법은 차동 구동 로봇의 경로 추종에 나와 있듯이 루프를 사용하여 상태를 업데이트하는 것입니다.

ODE 솔버에서는 모든 출력값이 단일 출력값으로 제공되어야 하므로, Pure Pursuit 제어기는 선형 속도와 방향 각속도를 단일 출력값으로 출력하는 함수에 래핑되어야 합니다. 예제 헬퍼 exampleHelperMobileRobotController가 이 목적으로 사용됩니다. 또한 이 예제 헬퍼는 로봇이 목표의 지정된 반경 내에 있을 때 정지하도록 합니다.

goalPoints = waypoints(end,:)';
goalRadius = 1;

ode45는 각 모델 유형에 대해 한 번씩 호출됩니다. 도함수는 initPose에 의해 설정된 초기 상태로 상태 출력값을 계산합니다. 각 도함수는 대응하는 기구학 모델 객체, 현재 로봇 자세, 해당 자세에서의 제어기 출력값을 받습니다.

% Compute trajectories for each kinematic model under motion control
[tUnicycle,unicyclePose] = ode45(@(t,y)derivative(unicycle,y,exampleHelperMobileRobotController(controller1,y,goalPoints,goalRadius)),tVec,initPose);
[tBicycle,bicyclePose] = ode45(@(t,y)derivative(bicycle,y,exampleHelperMobileRobotController(controller2,y,goalPoints,goalRadius)),tVec,initPose);
[tDiffDrive,diffDrivePose] = ode45(@(t,y)derivative(diffDrive,y,exampleHelperMobileRobotController(controller3,y,goalPoints,goalRadius)),tVec,initPose);

결과 플로팅하기

ODE 솔버의 결과는 plotTransforms를 사용해 모든 궤적의 결과를 한 번에 시각화하여 단일 플롯에서 쉽게 볼 수 있습니다.

자세 출력값은 먼저 평행 이동과 쿼터니언으로 구성된, 인덱싱된 행렬로 변환되어야 합니다.

unicycleTranslations = [unicyclePose(:,1:2) zeros(length(unicyclePose),1)];
unicycleRot = axang2quat([repmat([0 0 1],length(unicyclePose),1) unicyclePose(:,3)]);

bicycleTranslations = [bicyclePose(:,1:2) zeros(length(bicyclePose),1)];
bicycleRot = axang2quat([repmat([0 0 1],length(bicyclePose),1) bicyclePose(:,3)]);

diffDriveTranslations = [diffDrivePose(:,1:2) zeros(length(diffDrivePose),1)];
diffDriveRot = axang2quat([repmat([0 0 1],length(diffDrivePose),1) diffDrivePose(:,3)]);

다음으로, 모든 변환의 집합을 위에서 내려보도록 플로팅할 수 있습니다. 외륜, 자전거, 차동 구동 로봇의 경로는 각각 빨간색, 파란색, 녹색입니다. 플롯을 단순화하기 위해, 출력값을 10개마다 하나씩 표시합니다.

figure
plot(waypoints(:,1),waypoints(:,2),"kx-",MarkerSize=20);
hold all
plotTransforms(unicycleTranslations(1:10:end,:),unicycleRot(1:10:end,:),MeshFilePath="groundvehicle.stl",MeshColor="r");
plotTransforms(bicycleTranslations(1:10:end,:),bicycleRot(1:10:end,:),MeshFilePath="groundvehicle.stl",MeshColor="b");
plotTransforms(diffDriveTranslations(1:10:end,:),diffDriveRot(1:10:end,:),MeshFilePath="groundvehicle.stl",MeshColor="g");
axis equal
view(0,90)

라이브 스크립트 열기

조향각에 제약 조건이 있는 Ackermann 조향을 사용하는 이동 로봇 모델을 시뮬레이션합니다. 시뮬레이션하는 동안 모델은 조향 한도에 도달한 후 최대 조향각을 유지합니다. 조향 포화의 효과를 확인하기 위해 조향각에 대한 제약 조건이 있는 로봇과 조향 제약 조건이 없는 로봇의 궤적을 비교합니다.

모델 정의하기

Ackermann 기구학 모델을 정의합니다. 이 차륜 모델에서는 두 앞바퀴가 주어진 거리만큼 떨어져 있습니다. 두 바퀴가 동심원을 그리며 회전할 수 있도록 서로 다른 조향각을 가집니다. 회전하는 동안 앞바퀴는 조향각의 변화율을 조향 입력값으로 받습니다. 

carLike = ackermannKinematics;

시뮬레이션 파라미터 설정하기

이동 로봇이 일정한 선형 속도를 따르고 일정한 조향률을 입력값으로 받도록 설정합니다. 조향 포화를 보여주기 위해 제약 조건이 있는 로봇을 장시간 시뮬레이션합니다. 

velo = 5;    % Constant linear velocity 
psidot = 1;  % Constant left steering rate 

% Define the total time and sample rate 
sampleTime = 0.05;                  % Sample time [s]
timeEnd1 = 1.5;                     % Simulation end time for unconstrained robot 
timeEnd2 = 10;                      % Simulation end time for constrained robot 
tVec1 = 0:sampleTime:timeEnd1;      % Time array for unconstrained robot 
tVec2 = 0:sampleTime:timeEnd2;      % Time array for constrained robot  

initPose = [0;0;0;0];               % Initial pose (x y theta phi)

ODE 솔버에 대한 Options 구조체 생성하기

이 예제에서는 ODE 솔버에 options 구조체를 인수로 전달합니다. options 구조체는 조향각 한도에 대한 정보를 포함합니다. options 구조체를 만들기 위해 odeset의 Events 옵션과 생성된 이벤트 함수 detectSteeringSaturation을 사용합니다. detectSteeringSaturation은 최대 조향각을 45도로 설정합니다.

detectSteeringSaturation을 정의하는 방법에 대한 설명은 이 예제의 끝에 있는 이벤트 함수 정의하기를 참조하십시오. 

options = odeset('Events',@detectSteeringSaturation);

ODE 솔버를 사용하여 모델 시뮬레이션하기

다음으로 derivative 함수와 ODE 솔버 ode45를 사용하여 모델을 풀고 해를 구합니다. 

% Simulate the unconstrained robot 
[t1,pose1] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec1,initPose);

% Simulate the constrained robot 
[t2,pose2,te,ye,ie] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec2,initPose,options);

조향 포화 검출하기

모델은 조향 한도에 도달하면 이벤트의 타임스탬프를 등록합니다. 한도에 도달하는 데 걸린 시간은 te에 저장됩니다.

if te < timeEnd2
    str1 = "Steering angle limit was reached at ";
    str2 = " seconds";
    comp = str1 + te + str2; 
    disp(comp)
end 
Steering angle limit was reached at 0.785 seconds

제약 조건이 있는 로봇을 새로운 초기 조건으로 시뮬레이션하기

이제 제약 있는 로봇의 적분 종료 전 상태를 두 번째 시뮬레이션의 초기 조건으로 사용합니다. 입력 벡터가 조향 포화를 나타내도록 수정합니다. 즉, 조향률을 0으로 설정합니다. 

saturatedPsiDot = 0;             % Steering rate after saturation 
cmds = [velo saturatedPsiDot];   % Command vector 
tVec3 = te:sampleTime:timeEnd2;  % Time vector 
pose3 = pose2(length(pose2),:); 
[t3,pose3,te3,ye3,ie3] = ode45(@(t,y)derivative(carLike,y,cmds), tVec3,pose3, options);

결과 플로팅하기

plot과 pose. 에 저장된 데이터를 사용하여 로봇의 궤적을 플로팅합니다.

figure(1)
plot(pose1(:,1),pose1(:,2),'--r','LineWidth',2); 
hold on; 
plot([pose2(:,1); pose3(:,1)],[pose2(:,2);pose3(:,2)],'g'); 
title('Trajectory X-Y')
xlabel('X')
ylabel('Y') 
legend('Unconstrained robot','Constrained Robot','Location','northwest')
axis equal

제약 조건이 없는 로봇은 곡률 반경이 점점 줄어드는 나선 궤적을 따라가고, 제약 조건이 있는 로봇은 조향 한도에 도달한 후 곡률 반경이 일정한 원형 궤적을 따라갑니다.

이벤트 함수 정의하기

4번째 상태인 theta가 최대 조향각과 같을 때 적분이 종료되도록 이벤트 함수를 설정합니다. 

function [state,isterminal,direction] = detectSteeringSaturation(t,y)
  maxSteerAngle = 0.785;               % Maximum steering angle (pi/4 radians)
  state(4) = (y(4) - maxSteerAngle);   % Saturation event occurs when the 4th state, theta, is equal to the max steering angle    
  isterminal(4) = 1;                   % Integration is terminated when event occurs 
  direction(4) = 0;                    % Bidirectional termination 

end

입력 인수

모두 축소

모션 모델의 속성을 정의하는 이동 기구학 모델 객체로, ackermannKinematics 객체, bicycleKinematics 객체, differentialDriveKinematics 객체 또는 unicycleKinematics 객체로 지정됩니다.

이동체의 현재 상태로, 다음과 같이 motionModel 입력값에 따라 요소를 3개 가진 벡터 또는 요소를 4개 가진 벡터로 반환됩니다.

x와 y는 이동체 위치를 나타내는 것으로, 초당 미터 단위로 지정됩니다. theta는 이동체 방향이고 psi는 이동체 조향각이며, 둘 다 초당 라디안 단위로 지정됩니다.

모션 모델에 대한 입력 명령으로, 모션 모델에 따라 다음과 같이 요소를 2개 가진 벡터로 지정됩니다.

ackermannKinematics 객체의 경우 명령은 [v psiDot]입니다.

그 외의 모션 모델에서는 motionModel의 VehicleInputs 속성에 따라 다음과 같이 명령 벡터가 결정됩니다.

  • "VehicleSpeedSteeringAngle" –– [v psiDot]

  • "VehicleSpeedHeadingRate" –– [v omegaDot]

  • "WheelSpeedHeadingRate" (unicycleKinematics만 해당) –– [wheelSpeed omegaDot]

  • "WheelSpeeds" (differentialDriveKinematics만 해당) –– [wheelL wheelR]

v는 모션 방향의 이동체 속도(단위: 초당 미터)입니다. psiDot는 조향각 속도(단위: 초당 라디안)입니다. omegaDot는 뒤 차축의 각속도입니다. wwheelL과 wheelR은 각각 왼쪽 바퀴 속도와 오른쪽 바퀴 속도입니다.

출력 인수

모두 축소

현재 상태 도함수로, 다음과 같이 motionModel 입력값에 따라 요소를 3개 가진 벡터 또는 요소를 4개 가진 벡터로 반환됩니다.

xDot와 yDot는 이동체 속도를 나타내며, 초당 미터 단위로 지정됩니다. thetaDot는 이동체 방향의 각속도이고 psiDot는 이동체 조향의 각속도이며, 둘 다 초당 라디안 단위로 지정됩니다.

참고 문헌

[1] Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. 1st ed. Cambridge, MA: Cambridge University Press, 2017.

확장 기능

C/C++ 코드 생성
MATLAB® Coder™를 사용하여 C 코드나 C++ 코드를 생성할 수 있습니다.

버전 내역

R2019b에 개발됨

참고 항목

| | |

도움말 항목