derivative

이동체 상태의 시간 도함수

구문

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는 이동체 조향의 각속도이며, 둘 다 초당 라디안 단위로 지정됩니다.

예제

조향각 제약 조건이 있는 Ackermann 기구학 모델 시뮬레이션하기

조향각에 제약 조건이 있는 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),LineWidth=2)
hold on
plot([pose2(:,1); pose3(:,1)],[pose2(:,2);pose3(:,2)])
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

한 번에 여러 상태에 대한 도함수 계산하기

자전거 기구학 모델을 만듭니다.

model = bicycleKinematics;

4가지 상태를 설정하고 각 상태에 대한 제어 명령을 설정합니다.

state = [0 0 0;
         1 1 0;
         2 2 0;
         3 3 0];
control = [0.1 pi/10;
           1.0 pi/10;
           5.0 pi/10;
           9.0 pi/10];

모든 제어 명령에 대한 상태 도함수를 계산합니다.

stateDot = model.derivative(state,control)

stateDot = 3×4

    0.1000    1.0000    5.0000    9.0000
         0         0         0         0
    0.0325    0.3249    1.6246    2.9243

입력 인수

motionModel — 이동 기구학 모델 객체

ackermannKinematics 객체 | bicycleKinematics 객체 | differentialDriveKinematics 객체 | unicycleKinematics 객체

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

state — 이동체의 현재 상태

요소를 3개 가진 벡터 | N×3 행렬 | 요소를 4개 가진 벡터 | N×4 행렬

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

• unicycleKinematics –– [x y theta]

• bicycleKinematics –– [x y theta]

• differentialDriveKinematics –– [x y theta]

• ackermannKinematics –– [x y theta psi]

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

모션 모델에 따라 state를 N×3 행렬 또는 N×4 행렬로 지정하여 2개 이상의 상태를 지정할 수 있습니다. 각 행은 하나의 상태에 대응합니다. 상태 개수는 1이거나 명령의 개수와 같아야 합니다. 1개의 상태를 지정하면 cmds의 각 명령을 사용하여 해당 상태에 대한 도함수가 계산됩니다. 상태와 명령을 동일한 개수로 지정하면, 대응되는 명령을 사용하여 각 상태에 대한 도함수가 계산됩니다.

cmds — 모션 모델에 대한 입력 명령

요소를 2개 가진 벡터 | N×2 행렬

모션 모델에 대한 입력 명령으로, 모션 모델에 따라 요소를 2개 가진 벡터 또는 N×2 행렬로 지정됩니다. cmds가 N×2 행렬이면, 각 행은 명령입니다.

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

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

• "VehicleSpeedSteeringAngle" –– [v psiDot]

• "VehicleSpeedHeadingRate" –– [v omegaDot]

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

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

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

출력 인수

stateDot — 현재 상태의 상태 도함수

요소를 3개 가진 열 벡터 | 3×N 행렬 | 요소를 4개 가진 열 벡터 | 4×N 행렬

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

• unicycleKinematics –– [xDot yDot thetaDot]

• bicycleKinematics –– [xDot yDot thetaDot]

• differentialDriveKinematics –– [xDot yDot thetaDot]

• ackermannKinematics –– [xDot yDot thetaDot psiDot]

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

cmds가 N×2 행렬로 지정된 경우, stateDot는 모션 모델 객체에 따라 3×N 또는 4×N 행렬입니다. 여기서 N은 지정된 명령의 개수입니다. state에 지정된 상태의 개수가 N이면 stateDot의 각 열은 각 상태와 명령에 대응합니다. 예를 들어 첫 번째 열은 cmds의 첫 번째 명령을 사용하는 state의 첫 번째 상태의 상태 도함수입니다. state가 하나의 상태를 지정하면 stateDot의 각 열은 대응하는 명령과 지정된 상태에 대한 상태 도함수입니다. 즉, 각 명령을 사용하여 단일 상태에 대해 상태 도함수가 계산됩니다.

참고 문헌

[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에 개발됨

R2024b: 한 번에 여러 명령과 상태 지정함

derivative 객체 함수의 cmds 인수가 이제 여러 개의 상태와 명령을 한 번에 받습니다.

참고 항목

ackermannKinematics | bicycleKinematics | differentialDriveKinematics | unicycleKinematics

도움말 항목

• 이동 로봇을 위한 다양한 기구학 모델 시뮬레이션하기
• 이동 로봇 기구학 방정식