MTRN9224 Robot Design

Lecture 3: Kinematic Fundamentals

Tomonari Furukawa

School of Mechanical and Manufacturing Engineering University of New South Wales

SUMMARY

Gear ratio

1s

w

R

Rη = >>

Ackerman steering type: Kinematic equations

cos

sin

tan

ww

ww

w w

Rx

Ry

R

L

ω θη

ω θηωθ γη

=

=

=

Differential steering type: Kinematic equations

( )

( )

( )

cos2

sin2

wl r

wl r

wl r

Rx

Ry

R

l

η ω ω θ

η ω ω θ

ηθ ω ω

= + = + = −

1 STATE SPACE REPRESENTATION

1.1 Equations In MTRN3212 Principles of Control, we learned that a linear dynamical system could be

represented in the state space form.

=x Ax + Bu (3.1)

where x and u are state variables and control inputs, respectively. Most of the robot systems are non-linear. We expand Equation (3.1) to a more general form:

2

( , )=x f x u (3.2)

The robot on a three-dimensional plane often has state variables of position [ ],x y and orientation

θ , but the control variables depend upon the robot.

1.2 Feedforward Control The algorithms to simulate the motion of the robot are shown below.

/* Definition: k: Iteration x(k): State variable vector u(k): Control input vector func(x(k), u(k)): Robot model xdot(k): Rate of change of state vector Initially given:

u(k) for all k=0,…,K x0: Initial state variable vector dt: Time step interval

*/ k = 0; // Set initial time step to 0 x(k) = x0; // Substitute initial state x0 into current state do{ xdot(k) = func(x(k), u(k)); x(k+1) = x(k) + dt * xdot(k); k = k+1; }while(until terminal condition is satisfied);

2 GEAR RATIO

DC motors, used for driving wheels of a wheeled robot, are designed to rotate fast. The angular velocity of the motor, mω , should not direcly become the velocity of the robot. Speed reduction is

accomplished by coupling two wheels together through physical contact on their outside diameters. Gears are common speed reducers.

How does the speed of the driving gear relate to the speed of the output gear connected to the shaft? Driving gear’s pitch radius mR is smaller than the output gear’s radius wR . The output gear

thus spins faster ( wω ) than the driving gear mω , and the relationship is given by

mw m

w

R

Rω ω= (3.1)

The actual motors have a more complicated internal structure, but the following gear ratio,

1w

m

R

Rη = >> (3.2)

3

is given as the most important property of the motor gear.

3 ACKERMAN STEERING TYPE

We will derive the equations of motion of a tricycle for simplicity.

3.1 Control Inputs Figure 3.1 illustrates a tricycle. The control inputs of the tricycle are:

Steering angle: γ [rad]

Angular velocity of motor: mω [rad/s]

Note that the angular velocity of the motor directly leads to the velocity of the vehicle in terms of the following equation:

ww w m

Rv R ω ω

η= = (3.3)

where wR is the radius of a rear wheel (We assume that the two rear wheels have the same radius)

and η is the gear ratio.

x

y

0

yx,

θ

γ

v

Figure 3.1 Tricycle model

3.2 Instantaneous Curvature

3.2.1 Representation We will first learn the idea of instantaneous curvature, as it makes us easier to derive and

understand the orientation of the tricycle. Figure 3.2 shows the instantaneous curvature whose radius is cR , as well as variables and parameters of the model when the tricycle is turning left.

4

x

y

0

cR

γ

vcθ

L

Figure 3.2 Instantaneous curvature

The angle cθ in the figure firstly becomes the steering angle of the tricycle:

cθ γ= (3.4)

The radius cR is then given in terms of the length of the vehicle L and steering angle γ by

tanc

LR

γ= (3.5)

3.2.2 Alternative representation for radius We have considered that γ is positive when the vehicle turns left. Therefore, the vehicle is

meant to turn right when cR is negative. However,

0 0

0

0tan

0

0 0

c

LR Singularγ

γ

> > → + → +∞ → == = → − → −∞ < <

(3.6)

It is not easy to represent the instantaneous curvature in terms of cR . Curvature rate κ , which is

defined as

1 tan

cR L

γκ ≡ = , (3.7)

is often used as an alternative variable to represent the instantaneous curvature. Continuity is guaranteed in this representation:

5

0 0

0 0tan

0 0

0 0

0 0

L

γγ κ

> > → + → + → == = → − → − < <

(3.8)

3.3 Orientation The tricycle moves along the tangent of the instantaneous curvature. The velocity of the tricycle

v is given in terms of the rate of change of the curvature by

cv R θ= (3.9)

The substitution of Equation (3.5) into Equation (3.9) yields the equation for the orientation of the tricycle

tan tanw m

c

Rv v

R L L

ωθ γ γη

= = = (3.10)

3.4 Position The velocity of the vehicle can be given as a vector in Cartesian coordinates

( [ ], ,Tx yv v x y ≡ = v , 2 2v x y≡ = +v ). The tricycle moves tangentially to the circumference of

the instantaneous curvature, so that the position of the tricycle is expressed in state form as

cos cos

sin sin

wm

wm

Rx v

Ry v

θ ω θη

θ ω θη

= = = =

(3.11)

4 DIFFERENTIAL STEERING TYPE

We will consider in this section the differetial steering type of wheeled robot where two wheels are each driven by a motor.

Question 2

Does the attachment of a support wheel influence the motion of the differential steering type of wheeled robot?

4.1 Control Inputs Figure 3.3 illustrates the robot. Each wheel has the same radius of wR , and the wheels have a

distance of l . The control inputs of the robot are:

6

Angular velocity of motor on left wheel: lω [rad/s]

Angular velocity of motor on right wheel: rω [rad/s]

x

y

0

yx,

θ

lωv

rω

cRω

l

Figure 3.3 Differential steering robot

4.2 Instantaneous Curvature Let us find an instantaneous curvature when lω > rω . Figure 3.3 shows the instantaneous

curvature of such a robot. The linear and angular velocities of the center of the robot are

( )2

wl r

Rv ω ω

η= + (3.12)

( )wl r

R

lω ω ω

η= − (3.13)

The radius of the instantaneous curvature is given by

c

vR

ω= (3.14)

4.3 Orientation State space equation describing the orientation of the robot is given from Equation (3.13) by

( )wl r

R

lθ ω ω

η= − (3.15)

4.4 Position From Equation (3.12), state space equations describing the position of the robot is given by

7

( )

( )

cos2

sin2

wl r

wl r

Rx

Ry

ω ω θη

ω ω θη

= + = +

(3.11)

5 SIMULATION PROGRAM

5.1 Program Files The simulation program uses the following files:

rad2deg.m

deg2rad.m

rpm2rads.m

rads2rpm.m

robot_model.m

model_config.m

plot_rectangular.m

simulate.m

5.2 Running the Program To run the program, take the following three steps:

1. Start MATLAB by double-clicking MATLAB icon.

2. Change the working directory to where your programs are. For instance, if your working directory is

C:\Documents and Settings\student\My Documents\ Program\

then type

>> cd C:\’Documents and Settings’\student\’My Documents’\ Program\

3. Run the program by typing

>> simulate

5.3 Source Codes

5.3.1 rad2deg.m % ----------------------------------------------------- % rad2deg.m % This M-function converts radian to degree % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % -----------------------------------------------------

8

function deg = rad2deg(rad) deg = rad / pi * 180;

5.3.2 deg2rad.m % ----------------------------------------------------- % deg2rad.m % This M-function converts degree to radian % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % ----------------------------------------------------- function rad = deg2rad(deg) rad = deg / 180 * pi;

5.3.3 rpm2rads.m % ----------------------------------------------------- % rpm2rads.m % This M-function converts rpm to rad/s % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % ----------------------------------------------------- function rads = rpm2rads(rpm) rads = rpm * 2*pi / 60;

5.3.4 rads2rpm.m % ----------------------------------------------------- % rads2rpm.m % This M-function converts rad/s to rpm % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % ----------------------------------------------------- function rpm = rads2rpm(rads) rpm = rads * 2*pi / 60;

5.3.5 robot_model.m % ----------------------------------------------------- % robot_model.m % This M-function defines the robot model % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % ----------------------------------------------------- function zdot = robot_model(z, u) global L; % State variables x = z(1);

9

y = z(2); th = z(3); % Control variables v = u(1); st = u(2); % Robot model xdot = v * cos(th); ydot = v * sin(th); thdot = v / L * tan(st); % Outputs zdot(1) = xdot; zdot(2) = ydot; zdot(3) = thdot;

5.3.6 model_config.m % ----------------------------------------------------- % model_config.m % This M-function acquires the configuration of the robot % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % ----------------------------------------------------- function [xb, yb, xfw, yfw, xrlw, yrlw, xrrw, yrrw] = model_config(z, u) global L; global d; global R_w; x = z(1); % Length of the vehicle y = z(2); th = z(3); v = u(1); st = u(2); xfc = x + L*cos(th); yfc = y + L*sin(th); xfl = xfc - 0.5*d*sin(th); yfl = yfc + 0.5*d*cos(th); xfr = xfc + 0.5*d*sin(th); yfr = yfc - 0.5*d*cos(th); xrl = x - 0.5*d*sin(th); yrl = y + 0.5*d*cos(th); xrr = x + 0.5*d*sin(th); yrr = y - 0.5*d*cos(th); xfwf = xfc + R_w*cos(th+st); yfwf = yfc + R_w*sin(th+st); xfwr = xfc - R_w*cos(th+st); yfwr = yfc - R_w*sin(th+st); xrlwf = xrl + R_w*cos(th); yrlwf = yrl + R_w*sin(th); xrlwr = xrl - R_w*cos(th); yrlwr = yrl - R_w*sin(th);

10

xrrwf = xrr + R_w*cos(th); yrrwf = yrr + R_w*sin(th); xrrwr = xrr - R_w*cos(th); yrrwr = yrr - R_w*sin(th); zfc = [xfc, yfc]; xb = [xfl, xfr, xrr, xrl, xfl]; yb = [yfl, yfr, yrr, yrl, yfl]; xfw = [xfwf, xfwr]; yfw = [yfwf, yfwr]; xrlw = [xrlwf, xrlwr]; yrlw = [yrlwf, yrlwr]; xrrw = [xrrwf, xrrwr]; yrrw = [yrrwf, yrrwr];

5.3.7 plot_rectangular.m % ----------------------------------------------------- % plot_rectangular.m % This M-function plots an rectangular object % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % ----------------------------------------------------- function r = plot_rectangular(x, y, c) xtl = x(1); xtr = x(2); ytb = y(1); ytt = y(2); xt = [xtl, xtr, xtr, xtl, xtl]; yt = [ytb, ytb, ytt, ytt, ytb]; plot(xt, yt, c);

5.3.8 simulate.m % ----------------------------------------------------- % simulate.m % Main M-script file that simulates the robot movement % Programmed by Tomonari Furukawa for MTRN9224 % March 11, 2004 % % ----------------------------------------------------- clear all; % Clear all variables close all; % Close all figures global L; global d; global R_w; % Physical parameters of the robot L = 0.25; % Length between the front wheel axis and rear wheel axis [m] d = 0.18; % Distance between the rear wheels [m] m_max_rpm = 10000; % Motor max speed [rpm]

11

gratio = 20; % Gear ratio R_w = 0.05; % Radius of wheel [m] st_max_deg = 26; % Maximum steering angle of the robot [deg] % Initial location of the robot x0 = 0; % Initial x coodinate of robot [m] y0 = 0; % Initial y coodinate of robot [m] th_deg0 = -26; % Initial orientation of the robot (theta [deg]) % Target and obstacle locations xt = [3.5,4]; % Target yt = [-0.25,0.25]; xo1 = [2.5,2.8]; % Obstacle 1 yo1 = [0,1]; xo2 = [1,1.3]; % Obstacle 2 yo2 = [-1,0]; % Parameters related to simulations t_max = 2; % Simulation time [s] n = 50; % Number of iterations dt = t_max/n; % Time step interval % Derivation of parameters related to performance of the robot m_max_rads = rpm2rads(m_max_rpm); % Motor max speed [m/s] w_max_rads = m_max_rads / gratio; % Wheel max speed [m/s] v_max = w_max_rads * R_w; % Max robot speed [m/s] st_max_rad = deg2rad(st_max_deg); % Maximum steering angle [rad] t = [0:dt:t_max]; % Time vector (n+1 components) v = v_max * ones(1, n+1); % Velocity vector (n+1 components) st_rad = st_max_rad * sin(5*t); % Steering angle vector (n+1 components) [rad] th_rad0 = deg2rad(th_deg0); % Initial orientation [rad] v0 = v(1); % Initial velocity [m/s] st_rad0 = st_rad(1); % Initial steering angle [rad] z0 = [x0, y0, th_rad0]; % Initial state vector u0 = [v0, st_rad0]; % Initial control vector fig1 = figure(1); % Figure set-up (fig1) plot_rectangular(xt, yt, 'r'); axis([-0.2 4.6 -2 2]); hold on; plot_rectangular(xo1, yo1, 'b'); plot_rectangular(xo2, yo2, 'b'); % Acquire the configuration of robot for plot % [xb, yb]: Vertices of rectangular robot base % [xfw, yfw]: Front wheel position vector % [xrlw, yrlw]: Rear left wheel position vector % [xrrw, yrrw]: Rear right wheel position vector [xb, yb, xfw, yfw, xrlw, yrlw, xrrw, yrrw] = model_config(z0, u0); % Plot robot and define plot id plotzb = plot(xb, yb); % Plot robot base plotzfw = plot(xfw, yfw, 'r'); % Plot front wheel plotzrlw = plot(xrlw, yrlw, 'r'); % Plot rear left wheel plotzrrw = plot(xrrw, yrrw, 'r'); % Plot rear right wheel % Draw fast and erase fast set(gca, 'drawmode','fast');

12

set(plotzb, 'erasemode', 'xor'); set(plotzfw, 'erasemode', 'xor'); set(plotzrlw, 'erasemode', 'xor'); set(plotzrrw, 'erasemode', 'xor'); z1 = z0; % Set initial state to z1 for simulation % Biginning of simulation for i = 1:n+1 if v(i) > (v_max * cos(2*st_rad(i))) v(i) = v_max * cos(2*st_rad(i)); end u = [v(i), st_rad(i)]; % Set control input zdot = robot_model(z1, u); % Derive zdot using robot model z2 = z1 + zdot * dt; % Update the state of robot z1 = z2; % Substitute z2 to z1 % Acquire the configuration of robot for plot [xb, yb, xfw, yfw, xrlw, yrlw, xrrw, yrrw] = model_config(z1, u); % Plot robot set(plotzb,'xdata',xb); set(plotzb,'ydata',yb); set(plotzfw,'xdata',xfw); set(plotzfw,'ydata',yfw); set(plotzrlw,'xdata',xrlw); set(plotzrlw,'ydata',yrlw); set(plotzrrw,'xdata',xrrw); set(plotzrrw,'ydata',yrrw); pause(0.2); % Pause by 0.2s for slower simulation end % Plot the resultant velocity and steering angle configurations fig2 = figure(2); % Figure set-up (fig2) subplot(2,1,1); % Upper half of fig1 plot(t, v); % Plot velocity-time curve xlabel('Time [s]'); ylabel('Velocity [m/s]'); subplot(2,1,2); % Lower half of fig1 std = rad2deg(st_rad); % Steering angle vector (n+1 components) [deg] plot(t,std); % Plot steering angle-time curve xlabel('Time [s]'); ylabel('Steering angle [deg]');

13

TUTORIAL QUESTIONS

1. Design a wheeled robot. The items to be designed include:

Type of robot (should not be a tricycle)

Dimensions of base

Dimensions and location of wheels

Physical limitations of the robot (Maximum steering angle, maximum motor speed, etc.)

2. Derive the kinematic model of the robot.