Simulated Dynamic Linear Control of a Differential-Drive Robot in MATLAB/SIMULINK
Jonathan Lipstate, Brian Sweeney, Daniel Scheiber
Control Systems Analysis & Design (AE430)
College of Engineering, Embry-Riddle Aeronautical University, 3700 Willow Creek Rd.Prescott, AZ
-
Abstract
This project consists of creating manual and autonomous control systems for a two-wheel differential-drive robot. A linear state-space method was used in order to control a non-linear system. However, not all of the required state variables were implemented into the model due to limitations in the place() command, which was used to generate the K matrix. The manual controller did not operate in a manner allowing the robot to easily reach the stable point. The autonomous controller returned the robot to the stable point via a spiraling trajectory. The project would require a new method of generating the K matrix to become a full 3-DOF system as it should be.
1.0Introduction
When dealing with the operation of autonomous vehicles, it is essential for these vehicles to be equipped with a reliable control system. These control systems must be able to automatically bring a vehicle from the current orientation and position to the desired orientation and position. The robot analyzed in this project, models a differential drive accurately, even with many of the simplifications to be used.
The motion of the robot in this simulation is characterized by the translation and rotation of the entire robot, as well as its orientation. By using an input device such as a control pad, the motion of the robot can be manually controlled. However, it be useful to have a robot automatically arrive at a desired location. The implementation of such a controller is the objective of this project.
2.0Dynamic Model of the Robot
The robot chosen for this project is a differential-drive robot. This robot type typically has two independently driven wheels, with a third caster wheel for stability. The model was simplified to two wheels with a shaft connecting them & torques applied to each wheel. The system is non-linear and non-holonomic. The robot is capable of rolling in its local forward direction only and no sliding of the wheels.
The derivation of the nonlinear equations of motion is taken from Gholipour & Yazdanpanah [1]. The following equations were derived utilizing the Euler Lagrange equations.
Equation 1
Equation 2
Equation 3
Equation 4
Equation 5
Equation 6
Equation 7
Equation 8
Equation 9
Equation 10
By combining Equation 8 Equation 9, a new variable can be created that will be linear. This new equation describes the forward motion of the robot in its local reference frame.
Equation 11
Equation 12
Additionally, the second order equations must be abandoned for the linear model of the robot as the x-axis (local frame) is not capable of control (No slip condition), therefore making the placement of K values impossible with MATLAB. This is discussed more in the next section.
3.0Designing the Control System
The robot has multiple variables requiring control; therefore Modern control methods are necessary. The method used in this project is linear state-space control. Equation 13 signifies the control system. Vector q is the state vector. Matrix A contains the equations of motion relative to the state vector. Matrix B is the control Matrix. Vector u is the control vector.
Equation 13
If vector u is replaced by-Kq, where K is a new matrix, everything is now in terms of the state-vector.
Equation 14
There are three degrees of freedom in the system. Motion in the X & Z directions, and rotation about the Y-axis.
Equation 15
As Equation 15 shows, the number of states required (n) would be 6. Several attempts to linearize the system with 6 states were made, resulting in a controllability rank of 5/6 at best. Because the place()command would not properly function without having the controllability rank equal to the number of states (6/6), four states were used instead. Equation 16 shows the state-vector used for the control system.
Equation 16
The system neglects Ψ as can be seen in Figure 1. This is an incorrect model for the VR world. This issue will be discussed further in the next section.
Figure 1: Free Body Diagram of Differential-Drive Robot.
The A matrix is generated by creating a matrix that when multiplied by the state-vector results in the equations of motion.
Equation 17
The B matrix is generated by pulling out coefficients of the equations governing control of the system. In this case it is the torque on the wheels.
Equation 18
The control vector (U) is shown in Equation 19.
Equation 19
The -K matrix was generated via the following line of code:
-1*place(A,B,[-4; -3; -2; -1]);
The vector assigned has no particular assignment other than being negative and not the same value as MATLAB’s place()command cannot handle them being the same.
The complete SIMULINK system is shown below. The core of the system is the AB matrix block leading to the integrator which loops back to itself and to the –K matrix. As per project requirements the system is controllable with a joystick. The joystick can be bypassed via a switch on button 1 of the joystick, and the –K matrix becomes active again, reducing the state to zero.
Figure 2: SIMULINK Model of Differential-Drive Robot
Extending from the basic control system are a series of outputs utilized to evaluate the effectiveness of the control system. The four scopes allow viewing the state-vector in real time. The ‘Theta VR World’ subsystem converts θ to a rotation angle that is compatible with the VR world and then sends it into the rotation control for the robot in the VR world. The ‘polar cartesian’ subsystem converts the state-vector (r,θ) to inertial XZ coordinates and feeds them into the XZ position of the robot. Both subsystems are shown in Appendix II.
The robot itself was created in CATIA, using a product design with four individual parts. The parts include two wheels, an axle and the robot body. This model was saved as a .wrl file and imported into the V-Realm Builder in SIMULINK. In V-Realm Builder, an elevation grid of size 20x20, serves as the virtual landscape that the robot travels across. A background that simulated sky and grass was inserted into the world to replace the default black background. The robot was placed into the upper right hand corner of the virtual field which served as the starting point for the robot. This world was saved and selected for use in the VR Sink. Figure 3 below shows the virtual world created for this project:
Figure 3: VR World & Robot.
4.0Simulated Control System
The initial conditions used for the simulation are shown by Equation 20.
Equation 20
When attempting to drive the robot manually with a D-pad, the controls were highly non-intuitive & difficult to use. This was due to Ψ & θ being treated as the same angle in the ‘polar cartesian’ subsystem. In addition, there are only two degrees of freedom being passed through the system when three are necessary.
The following two plots show the linear and angular outputs of the control system as a function of time with autopilot engaged.
Figure 4: Rotation as a function of Time.
Figure 5: Linear Motion as a function of Time.
The control system is critically damped for the radius, as can be seen in Figure 5. Figure 4 shows θ slightly increasing at first, but then decreasing to zero in a manner similar to critical damping. The initial rise could be caused by the integrator ‘gaining its bearings’.
5.0Conclusion
The goal of this project is to make a control system that allows a two wheeled robot to translate (XZ) and rotate (Y). The control system consists of two modes, a manual mode controlled by a joystick and an autonomous mode controlled by the control system. The robot’s mission is to arrive at a predetermined destination on the virtual field. However as the results showed this objective was not quite fulfilled. The lack of two states in the inertial frame caused the manual control of the robot to be nearly impossible. However, autonomous control returned the robot to its starting location on the virtual landscape. The autonomous control returned the robot to this location by utilizing a spiral trajectory. Overall the program worked as well as could be expected for a linear control system that did not take into account all of the necessary state variables. Due to the lack of the Z direction DOF, it is unknown how well the rotation of the robot and its motion are interlinked.
6.0Future Control Analysis Advances
This project would yield the best results via modeling the system in a nonlinear manner, reproducing the model that Gholipour & Yazdanpanah [1] produced.
If the model were to remain linear, however, including the angle Ψ and its derivative in the state-vector would be the next step in advancing the model.
Implementing frictional forces would be necessary for future manually controlled versions of the project. The robot will continue to move around indefinitely otherwise.
7.0References
[1] Gholipour, Ali, & Yazdanpanah, M.J. Dynamic Tracking Control of Non-holonomic Mobile Robot with Model Reference Adaptation for Uncertain Parameters.
[2] Siegward, R, & Nourbakhsh, I. (2004). Introduction to Autonomous Mobile Robots. Massachusetts: MIT.
[3] Nise, N (2004). Control Systems Engineering. Hoboken, NJ: John Wiley & Sons Inc.
8.0Appendix I: MATLAB M-files
jconvert.m
function torque = jconvert(state)
%% Header %%
% The purpose of this script is to generate a sum of torques 1 & 2 based on
% the input from a D-pad or 2-axis joystick.
% Cases:
% When the robot drives forward torque 1 (t1) = torque 2 (t2). (+)
% When the robot drives in reverse t1 = t2. (-)
% When the robot is not moving, and rotates t1 = -t2.
% When Moving & Rotating the F/R component & the L/R component are added
% for each wheel creating a composite torque.
% Example: Moving Forward & Turning Left (F = 1, L = 1).
% F/R Component: t1 = t2 = 1
% L/R Component: t1 = 1, t2 = -1
% Final Components t1 = 2, t2 = 0
% When deriving a Kinematic Model, each wheel contributes to 1/2
% the forward motion. Therefore the forward motion is 1/2*2 = 1
% and the turning motion is to the left. (This is not a perfected
% rotation model, but it does work.)
%% Main Program %%
FR = -state(2); %Forward/Reverse (Sign automatically generated)
LR = state(1); %Left/Right
lr = abs(LR);
if LR > 0 %The IF statements generate the sign of the L/R torques applied.
j=1;
else
j=-1;
end
if LR > 0
k=-1;
else
k=1;
end
t1_fr = FR; % F/R Component of Torque 1
t2_fr = FR; % F/R Component of Torque 2
t1_lr = j*lr; % L/R Component of Torque 1
t2_lr = k*lr; % L/R Component of Torque 2
T1 = t1_fr+t1_lr; % Summed Torque on Wheel 1
T2 = t2_fr+t2_lr; % Summed Torque on Wheel 2
torque = [T1; T2];%*1/250; % Torque Vector
%The 1/250 coefficient reduces the output to make the Robot easier to use.
AB.m
function Qdot =AB(state)
%% Header %%
% The purpose of this script is to generate the state-vector derrivative.
% q_dot =[A]q+[B]u
% q = State-Vector
% u = Control Vector
% [A] = Equations of Motion in Matrix Form ([A]q = EOM)
% [B] = Control Matrix
%% Constants %% **Note: These Constants must be Changed in km.m as well**
M = 3; %[kg] Mass of the Robot
R = .5; %[m] Radius of Wheels
I = 1/3*M*R^2; %[?] Mass Moment of Inertia
L = 1; %[m] Distance from Wheel to Center of Mass
%% Incoming State-Vector (Q) & Control Vector (U) Signals %%
t1 = state(1); % Torque 1
t2 = state(2); % Torque 2
r = state(3);
v = state(4);
th = state(5);
om = state(6);
%% Vectors Q & U%%
Q = [r; v; th; om];
U = [t1; t2];
%% A & B Matricies %%
A = [0 1 0 0;...
0 0 0 0;...
0 0 0 1;...
0 0 0 0];
B = [1/(R*M), 1/(R*M);...
L/(R*I), -L/(R*I)];
%% Combining Equations %%
AQ = A*Q; %[A]*Q Product
BUs = B*U; %[B]*U Product
BU = [0; BUs(1); 0; BUs(2)]; %BU Converted to be equivalent to AQ [1x4]
%% Output %%
Qdot = AQ+BU;
km.m
function u =km(state)
%% Header %%
% The purpose of this script is to generate the K Matrix, and the
% Control Vector (U).
% U = -[K]q
% [q] = State-Vector
% [K] = Matrix based on Eigenvectors
%% Constants %% **Note: These Constants must be Changed in AB.m as well**
M = 3;
R = .5;
I = 1/3*M*R^2;
L = 1;
%% Incoming State-Vector %%
r = state(1);
v = state(2);
th = state(3);
om = state(4);
Q = [r; v; th; om];
%% A & B Matricies %%
A = [0 1 0 0;...
0 0 0 0;...
0 0 0 1;...
0 0 0 0];
B = [0, 0;...
1/(R*M), 1/(R*M);...
0, 0;...
L/(R*I), -L/(R*I)];
% A & B Matricies are used to generate the K Matrix
%% Generating [K] & the U vector %%
KM = -1*place(A,B,[-4; -3; -2; -1]);
u = KM*Q; % Control Vector
Plots.m
%% Header %%
% This script generates two plots, the first shows r & v on a plot,
% the second shows theta & omega on a second plot.
close all
%% Inputs from Simulink Simulation %%
to = Omega_t(:,1); %[s] time
ot = Omega_t(:,2); %[rad/s] omega
tth = Theta_t(:,1); %[s] time
tht = Theta_t(:,2); %[rad] theta
tr = radius_t(:,1); %[s] time
rt = radius_t(:,2); %[m] radius
tv = velocity_t(:,1); %[s] time
vt = velocity_t(:,2); %[m/s] velocity
%% Plots %%
plot(tth, tht,to,ot)
grid
title('Angular Properties')
xlabel('Time [s]')
ylabel('[rad] & [rad/s]')
legend('Theta','Omega')
figure;
plot(tr,rt, tv, vt)
grid
xlabel('Time [s]')
ylabel('[m] & [m/s]')
legend('Radius','Velocity')
title('Linear Properties')
9.0Appendix II: SIMULINK Blocks
Figure 6: SIMULINK Model.
Figure 7: cartesian --> polar subsystem.
Figure 8: Theta --> VR World subsystem.