derivative

Time derivative of vehicle state

Syntax

stateDot = derivative(motionModel,state,cmds)

Description

stateDot = derivative(motionModel,state,cmds) returns the current state derivative, stateDot, as a three-element vector [xDot yDot thetaDot] if the motion model is a bicycleKinematics, differentialDriveKinematics, or unicycleKinematics object. It returns state as a four-element vector, [xDot yDot thetaDot psiDot], if the motion model is a ackermannKinematics object. xDot and yDot refer to the vehicle velocity, specified in meters per second. thetaDot is the angular velocity of the vehicle heading and psiDot is the angular velocity of the vehicle steering, both specified in radians per second.

Examples

Simulate Ackermann Kinematic Model with Steering Angle Constraints

Simulate a mobile robot model that uses Ackermann steering with constraints on its steering angle. During simulation, the model maintains maximum steering angle after it reaches the steering limit. To see the effect of steering saturation, you compare the trajectory of two robots, one with the constraints on the steering angle and the other without any steering constraints.

Define the Model

Define the Ackermann kinematic model. In this car-like model, the front wheels are a given distance apart. To ensure that they turn on concentric circles, the wheels have different steering angles. While turning, the front wheels receive the steering input as rate of change of steering angle.

carLike = ackermannKinematics; 

Set Up Simulation Parameters

Set the mobile robot to follow a constant linear velocity and receive a constant steering rate as input. Simulate the constrained robot for a longer period to demonstrate steering saturation.

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) 

Create Options Structure for ODE Solver

In this example, you pass an options structure as argument to the ODE solver. The options structure contains the information about the steering angle limit. To create the options structure, use the Events option of odeset and the created event function, detectSteeringSaturation. detectSteeringSaturation sets the maximum steering angle to 45 degrees.

For a description of how to define detectSteeringSaturation, see Define Event Function at the end of this example.

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

Simulate Model Using ODE Solver

Next, you use the derivative function and an ODE solver, ode45, to solve the model and generate the solution.

% 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);

Detect Steering Saturation

When the model reaches the steering limit, it registers a timestamp of the event. The time it took to reach the limit is stored in 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

Simulate Constrained Robot with New Initial Conditions

Now use the state of the constrained robot before termination of integration as initial condition for the second simulation. Modify the input vector to represent steering saturation, that is, set the steering rate to zero.

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 the Results

Plot the trajectory of the robot using plot and the data stored in 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

The unconstrained robot follows a spiral trajectory with decreasing radius of curvature while the constrained robot follows a circular trajectory with constant radius of curvature after the steering limit is reached.

Define Event Function

Set the event function such that integration terminates when 4th state, theta, is equal to maximum steering angle.

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

Calculate Derivative for More States at Once

Create a bicycle kinematic model.

model = bicycleKinematics;

Set the four states and set the control commands the respective states.

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];

Calculate state derivatives for all control commands.

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

Input Arguments

motionModel — Mobile kinematic model object

ackermannKinematics object | bicycleKinematics object | differentialDriveKinematics object | unicycleKinematics object

The mobile kinematics model object, which defines the properties of the motion model, specified as an ackermannKinematics, bicycleKinematics, differentialDriveKinematics, or a unicycleKinematics object.

state — Current vehicle state

three-element vector | N-by-3 matrix | four-element vector | N-by-4 matrix

Current vehicle state returned as a three-element or four-element vector, depending on the motionModel input:

• unicycleKinematics –– [x y theta]

• bicycleKinematics –– [x y theta]

• differentialDriveKinematics –– [x y theta]

• ackermannKinematics –– [x y theta psi]

x and y refer to the vehicle position, specified in meters per second. theta is the vehicle heading and psi is the vehicle steering angle, both specified in radians per second.

You can specify more than one state by specifying state as a N-by-3 or N-by-4 matrix depending on the motion model. Each row corresponds to a state. Note that the number of states must either be 1 or equal to the number of commands. If you specify 1 state, then the derivative is calculated for that state using each command in cmds. If you specify an equal number of states and commands, then the derivative is calculated for each state using the corresponding command.

cmds — Input commands to motion model

two-element vector | N-by-2 matrix

Input commands to the motion model, specified as a two-element vector or N-by-2 matrix that depend on the motion model. When cmds is a N-by-2 matrix, each row is a command.

For ackermannKinematics objects, the commands are [v psiDot].

For other motion models, the VehicleInputs property of motionModel determines the format of the command:

• "VehicleSpeedSteeringAngle" –– [v psiDot]

• "VehicleSpeedHeadingRate" –– [v omegaDot]

• "WheelSpeedHeadingRate" (unicycleKinematics only) –– [wheelSpeed omegaDot]

• "WheelSpeeds" (differentialDriveKinematics only) –– [wheelL wheelR]

v is the vehicle velocity in the direction of motion in meters per second. psiDot is the steering angle rate in radians per second. omegaDot is the angular velocity at the rear axle in radians per second. wheelL and wheelR are the left and right wheel speeds in radians per second, respectively.

Output Arguments

stateDot — State derivative of current state

three-element column vector | 3-by-N matrix | four-element column vector | 4-by-N matrix

The current state derivative returned as a three-element or four-element vector, depending on the motionModel input:

• unicycleKinematics –– [xDot yDot thetaDot]

• bicycleKinematics –– [xDot yDot thetaDot]

• differentialDriveKinematics –– [xDot yDot thetaDot]

• ackermannKinematics –– [xDot yDot thetaDot psiDot]

xDot and yDot refer to the vehicle velocity, specified in meters per second. thetaDot is the angular velocity of the vehicle heading and psiDot is the angular velocity of the vehicle steering, both specified in radians per second.

If cmds is specified as a N-by-2 matrix, then stateDot is either a 3-by-N or 4-by-N matrix depending on the motion model object. N is the number of specified commands. If the number of states specified in state is equal to N, then each column of stateDot corresponds to its respective state and command. For example, the first column is the state derivative of the first state in state using the first command in cmds. If state specifies one state, then each column of stateDot is the state derivative for the corresponding command and the specified state. This means the state derivative is calculated for the single state using each command.

References

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

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019b

R2024b: Specify multiple commands and states at once

The cmds argument of the derivative object function now accepts multiple states and commands at once.