derivative
이동체 상태의 시간 도함수
설명
stateDot = derivative(motionModel,state,cmds)bicycleKinematics 객체, differentialDriveKinematics 객체 또는 unicycleKinematics 객체일 경우 현재 상태 도함수 stateDot를 요소를 3개 가진 벡터 [xDot yDot thetaDot]로 반환합니다.
모션 모델이 ackermannKinematics 객체인 경우에는 stateDot를 요소를 4개 가진 벡터 [xDot yDot thetaDot psiDot]로 반환합니다. xDot와 yDot는 이동체 속도를 나타내며, 초당 미터 단위로 지정됩니다. thetaDot는 이동체 방향의 각속도이고 psiDot는 이동체 조향의 각속도이며, 둘 다 초당 라디안 단위로 지정됩니다.
articulatedSteeringKinematics 모션 모델인 경우에는 stateDot를 요소를 4개 가진 벡터 [xDot yDot thetaDot gammaDot]로 반환합니다. xDot와 yDot는 이동체 속도를 나타내며, 초당 미터 단위로 지정됩니다. thetaDot는 전면 바디의 각속도이고 gammaDot는 후면 바디를 기준으로 하는 전면 바디의 각속도이며, 둘 다 초당 라디안 단위로 지정됩니다.
예제
조향각에 제약 조건이 있는 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
입력 인수
출력 인수
참고 문헌
[1] Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. 1st ed. Cambridge, MA: Cambridge University Press, 2017.
