INTRODUCTION TO ROBOTICS
CPSC - 460
Lecture 5B – Control
CONTROL PROBLEM
 Determine
the time history of joint inputs
required to cause the end-effector to execute a
command motion.
 The
joint inputs may be joint forces or torques.
CONTROL PROBLEM


Given: A vector of desired position, velocity and
acceleration.
Required: A vector of joint actuator signals
using the control law.
ROBOT MOTION CONTROL (I)

Joint level PID control





each joint is a servo-mechanism
adopted widely in industrial robot
neglect dynamic behavior of whole arm
degraded control performance especially in high speed
performance depends on configuration
e = qd - q
Trajectory q d q& d e e&
Controller
q&&d _
Planner
q q&
tor
Robot
4
ROBOT MOTION CONTROL (II) – COMPUTED TORQUE
 The
dynamic model of the robot has the form:
 = M ()  V (, )  G()
is the torque about zk ,if joint k is revolute

joint and is a force if joint k is prismatic joint
Where: M(Θ) is n x n inertia matrix,
V (, ) is n x 1 vector of centrifugal terms
G(Θ) is a n x 1 vector of gravity terms
PD CONTROL
 The
control law takes the form
 = K P E  K D E&
Where:
E = d - 
& -
&
E& = 
d
PD CONTROL
e = qd - q
Trajectory q d q& d e e&
Controller
q&&d _
Planner
q q&
tor
Robot
d
e
+
KP

+
&

d
e
+
-
KD
Torque
Robot
&

MODEL BASED CONTROL
e = qd - q
Trajectory q d q& d e e&
Controller
q&&d _
Planner
 The
q q&
tor
Robot
control law takes the form:
 = M ()(d  KD E  K p E)  V (, )  G()
E = d - 
& -
&
E& = 
d
Kp and KD are diagonal matrices.
CONTROL PROBLEM

STABLE RESPONSE
Evaluating the response
overshoot
settling time
steady-state error
ss error -- difference from the
system’s desired value
overshoot -- % of final value
exceeded at first oscillation
rise time -- time to span from
10% to 90% of the final value
settling time -- time to reach
within 2% of the final value
rise time
PROJECT

The equations of motion:
(x , y)
M ( )  v( , )  g ( ) = 
2
l2
1 l1
PROJECT
SIMULATION AND DYNAMIC CONTROL OF A 2
DOF PLANAR ROBOT
 Problem
-
-
-
statement:
The task is to take the end point of the RR
robot from (0.5, 0.0, 0.0) to (0.5, 0.3, 0.0) in
the in a period of 5 seconds.
Assume the robot is at rest at the starting
point and should come to come to a
complete stop at the final point.
The other required system parameters
are: L1 = L2 = 0.4m, m1 = 10kg, m2 = 7kg, g
= 9.82m/s2.
PROJECT

1.
2.
Planning
Perform inverse position kinematic
analysis of the serial chain at initial and
final positions to obtain (1i, 2i) and (1f,
2f).
Then, obtain fifth order polynomial
functions for 1 and 2 as functions of
time such that the velocity and
acceleration of the joints is zero at the
beginning and at the end. These fifth
order polynomials can be differentiated
twice to get the desired velocity and
acceleration time histories for the joints.
PROJECT
 Use
a PD control law where Kp and Kv are
2x2 diagonal matrices, and s is the
current(sensed) value of the joint angle as
obtained from the simulation. Tune the
control gains to obtain good performance
 = K p (d - )  KD (d - )
BLOCK DIAGRAM
2DOF ROBOT
 The
forward kinematic equations:
x = l1 cos1  l2 cos(1  2 )
 The
y = l1 sin 1  l2 sin( 1  2 )
inverse kinematic equations:
y
x
1 = tan -1 ( ) - tan -1 (
2
l2 sin  2
)
l1  l2 cos  2
x 2  y 2 - l12 - l22
cos  2 = (
)
2l1l2
 The
(x , y)
Jacobian matrix
- l sin 1 - l2 sin( 1   2 ) - l2 sin( 1   2 )
J = 1

 l1 cos 1  l2 cos(1   2 ) l2 cos(1   2 ) 
l2
1 l1