ST AN2350 Application note
AN2350
Application note
Guidelines for the control of a multiaxial planar robot
with ST10F276
Introduction
This application note describes how to implement a PID control with the ST10F276 16-bit
microcontroller for the control of a multiaxial planar robot.
The document provides guidelines for the complete development of a control system, able
to fulfill all the requirements needed to drive an industrial manipulator.
The first chapter is an introduction to the robotic manipulators. It focuses on their working
space, forward kinematics and the problem of the inverse kinematics. In particular it
describes the main characteristics of an industrial wafer handler used as a case study for a
multiaxial planar manipulator family.
The second chapter is a brief description of the ST10F276 16-bit microcontroller with a
focus on its architecture and its peripherals. Moreover, an overview is given of the control
board, named Starter Development Kit - ST10F276 and its three dedicated connectors for
motion control.
The third chapter provides an overview of the hardware and mechanical equipment of a
wafer handler. More specifically, it describes the encoder conditioning and motor driver
circuits.
The fourth chapter is dedicated to the description of the basic routines for implementing PID
control. The inverse kinematics of the wafer handler and the planning of the trajectory are
also explained. The implementation of the teach and repeat technique and the homing
procedure are shown.
See associated datasheets and technical literature for details of the components related to
the devices and board used in this application note:
http://www.st.com/stonline/books/ascii/docs/9944.htm (L6205 Product Page)
November 2006 Rev 1 1/41
www.st.com
Contents AN2350
Contents
1 An overview on robotics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.1 Structure of a manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.1.1 Kinematics analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.1.2 Singularity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2 The industrial wafer handler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2.1 Forward kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.2.2 Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2 The SDK-ST10F276 control board . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.1 Brief description of the SDK-ST10F276 . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.1.1 User Interfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.2 On board motor control connectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.2 ST10F276 16-bit microcontroller - architectural overview . . . . . . . . . . . . 14
2.2.1 Basic CPU concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.2.2 Memory organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.3 On-chip peripheral blocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.4 Managing Interrupts (hardware) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3 Hardware and mechanical equipments . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.1 The Dual DC motor and the power stage . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2 Cables and connectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.3 The encoders and the conditioning circuit . . . . . . . . . . . . . . . . . . . . . . . . 20
3.4 Schematics of the driver board and the interface board. . . . . . . . . . . . . . 23
4 Control algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.1 Motion and path planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.2 PID position control algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.3 Homing procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.4 Teach and Repeat procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5 Revision history . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
2/41
AN2350 An overview on robotics
1 An overview on robotics
1.1 Structure of a manipulator
A manipulator, from a mechanical point of view, can be seen as an open kinematic chain
constituted of rigid bodies (links) connected in cascade by revolute or prismatic joints, which
represent the degrees of mobility of the structure. These manipulators are also known as
serial manipulators.
Only relatively few commercial robots are composed of a closed kinematic chain (parallel)
structure. In this case there is a sequence of links that realize a loop. From this point on, we
refer only to serial manipulators.
In the chain mentioned above, it is possible to identify two end-points: one end-point is
referred to as the base, and it is normally fixed to ground, the other end-point of the chain is
named the end-effector and is the functional part of the robot. The structure of an end
effector, and the nature of the programming and hardware that drives it, depends on the
intended task.
The overall motion of the structure is realized through a composition of elementary motions
of each link respect to previous one.
A revolute joint allows a relative rotation about a single axis, and a prismatic joint permits a
linear motion along a single axis, namely an extension or retraction.
It is assumed throughout that all joints have only a single degree-of-freedom: the angle of
rotation in the case of a revolute joint, and the amount of linear displacement in the case of
a prismatic joint.
The degrees of mobility must be suitably distributed along the mechanical structure in order
to furnish the needed degrees of freedom (DOF) to execute a task. If there are more
degrees of mobility than degrees of freedom the manipulator is said to be redundant.
The workspace of a point H of the end-effector is the set of all points which H occupies as
the joint variables are varied through their entire ranges. The point H is usually chosen as
either the center of the end-effector, or the tip of a finger, or the end of the manipulator itself.
The workspace is also called work volume or work envelope.
Size and shape of the workspace depend on the coordinate geometry of the robot arm, and
also on the number of degrees of freedom.
The workspace of a robot is a fundamental criterion in evaluating manipulator geometries.
Manipulator workspace may be described in terms of the dexterous workspace and the
accessible workspace. Dexterous workspace is the volume of space which the robot can
reach with all orientations. That is, at each point in the dexterous workspace, the endeffector can be arbitrarily oriented.
The accessible workspace is the volume of space which the robot can reach in at least one
orientation. In the dexterous workspace the robot has complete manipulative capability.
However, in the accessible workspace, the manipulator's operational capacity is limited
because of the terminal device can only be placed in a restricted range of orientations.
In other words, the dexterous workspace is a subset of the accessible workspace.
Ta bl e 1 shows a classification of the manipulators accordingly to the type and sequence of
the degrees of mobility of the structure, and of their workspaces.
3/41
An overview on robotics AN2350
Table 1. Open chain manipulators classification
Type Workspace Joints
– Three prismatic joints
Cartesian
Cylindrical
– A Cartesian degree of
freedom corresponds to every
joint
– One revolute joint and two
prismatic joints
– Cylindrical coordinates
Spherical
SCARA
Anthropomorphic
– Two revolute joints and one
prismatic joint
– Spherical coordinates
– Two revolute joints and one
prismatic joint
– Selective Compliance
Assembly Robot Arm
– Three revolute joints
– It is the most dexterous
structure
4/41
AN2350 An overview on robotics
1.1.1 Kinematics analysis
Robot arm kinematics deals with the analytical study of the geometry of motion of a robot
arm with respect to a fixed reference coordinate system as a function of time without regard
to the forces/moments that causes the motion.
Thus, it deals with the analytical description of the spatial displacement of the robot as a
function of time, in particular the relations between the joint space and the position and
orientation of the end-effector of a robot arm.
In kinematics, we consider two issues:
1. Forward analysis: for a given manipulator, given the joint angle vector
q(t)=(q1(t), q2(t), …., qN(t))
and the geometric link parameters, where n is the number of degrees of freedom,
what is the position and orientation of the end-effector with respect to a reference
coordinate system?
2. Inverse analysis: given a desired position and orientation of the end effector and the
geometric link parameters with respect to a reference coordinate system, can the
manipulator reach the desired manipulator hand position and orientation. And if it can,
how many different manipulator configurations will satisfy the same condition?
For serial robots, the forward analysis problem is usually easy and straightforward.
Unfortunately, the inverse analysis problem is of much more interest. For example, in
industrial applications, the end-effector must follow some desired path; then, we need to find
the joint angles for each position in the path.
T
For redundant robots, the inverse kinematics problem has then an infinite number of
solutions. The extra degrees of freedom can then be used for other purposes, for example
for fault tolerance, obstacle avoidance, or to optimize some performance criteria.
A simple block diagram indicating the relationship between these two problems is shown in
the following figure.
Figure 1. The direct and inverse kinematics problems
Link parameters
Position and
Joint angles
q
(t), q2(t), ..., qN(t)
1
Direct
kinematics
Link parameters
orientation
of the end-effector
Joint angles
q1(t), q2(t), ..., qN(t)
Inverse
kinematics
5/41
An overview on robotics AN2350
Since the links of a robot arm may rotate and/or translate with respect to a reference
coordinate frame, the total spatial displacement of the end-effector is due to the angular
rotations and linear translations of the links.
Denavit and Hartenberg proposed a systematic and generalized approach of utilizing matrix
algebra to describe and represent the spatial geometry of the links of a robot arm with
respect to a fixed reference frame. This method uses a 4 x 4 homogeneous transformation
matrix to describe the relationship between two adjacent rigid mechanical links and reduces
the direct kinematics problem to finding an equivalent 4 x 4 homogeneous transformation
matrix that relates the spatial displacement of the hand coordinate frame to the reference
coordinate frame. These homogeneous transformation matrices are also useful in deriving
the dynamic equation of motion of a robot arm.
In general, the inverse kinematics problem can be solved by several techniques. Most
commonly used approaches are matrix algebraic, iterative, or geometric. A geometric
approach based on both the link coordinate systems and the manipulator configuration has
been used for the industrial wafer handler to which this application note refers.
1.1.2 Singularity
A significant issue in kinematic analysis surrounds so-called singular configurations.
Physically, these configurations correspond to situations where the robot joints have been
aligned in such a way that there is at least one direction of motion (the singular direction[s])
for the end effector that physically cannot be achieved by the mechanism. This occurs at
workspace boundaries, and when the axes of two (or more) joints line up and are
redundantly contributing to an end effector motion, at the cost of another end effector DOF
being lost.
1.2 The industrial wafer handler
This section describes the main characteristics of an industrial wafer handler, used as a
case study for a multiaxial planar manipulator family.
The structure of the manipulator consists of two arms with six joints (one prismatic joint and
five revolute joints) arranged in order to have the motion axes parallel to each other.
The illustration below shows the relation between the axis control and the robot
components.
6/41
AN2350 An overview on robotics
Figure 2. The structure of the wafer handler
Paddle 1
1
H axis
Y axis
Link 2
2
Link 1 Link 1
3
4
5
8
Z axis
6
1. Paddle
2. Link 2
7
3. Link 1
4. Dual Motor Cover
The robot uses 6 electric motors placed as follows:
● Z Axis Motor (Vertical elevation)
● X Axis Motor (Rotation entire robot)
● Dual Motors (Link 2 and Link 1)
Paddle 0
Link 2
A axis
W axis
X axis
5. Torso Beam
6. Robot Body Mounting Flange
7. Chassis Skin
8. Z-Tube
1.2.1 Forward kinematics
Accordingly to the Denavit-Hartenberg convention, an orthonormal cartesian coordinate
system (x
, yi, zi) has been established for each link, in order to characterize the forward
i
kinematics through a D-H homogeneous transformation matrix.
Figure 3. The Denavit Hartenberg convention for one arm
y
y
y
0
1
x
0
y
4
x
4
x
2
y
2
θ
2
x
1
θ
3
y
3
θ
4
x
3
x
Regarding the base coordinates (x
prismatic joint. The origin O
of the base coordinates has been placed at the same height of
0
, y0, z0) , the z0 axis lies along the axis of motion of the
0
7/41
An overview on robotics AN2350
the end effector, when the prismatic joint is at its lower level. The x0 axis lies along the
longitudinal direction of the link 1.
Since all the motion axes are parallel to each other, the origins of the other coordinate
frames have been placed at the same height of the origin O
.
0
The Denavit-Hartenberg parameters are described in the following table:
Table 2. The Denavit Hartenberg parameters for one arm
Link ai [mm] α [rad] di [mm] θ [rad]
1 154 0 d1 0
2 133 0 0 2
3 133 0 0 3
4 215 0 0 4 =- 3/2
In this application note we refer only to one arm, without considering the Z Axis (Vertical
elevation - prismatic joint) and X Axis (Rotation entire robot - revolute joint).
Through a motion simulation it has been possible to obtain the workspace of the
manipulator: for each value of the angle of the joint 1, the joint 2 performs a complete
revolution. As shown in the Figure 4, the workspace has a circular form. Nevertheless
because the joint 3 is under-actuated, there are some regions of the workspace
characterized of different orientations of the end-effector.
Figure 4. The workspace of the wafer handler
8/41
AN2350 An overview on robotics
Out from the circumference, with origin (154 mm, 0 mm) and radius 51 mm, the manipulator
is able to reach a point of the workspace with two possible configurations of the joint
variables; the points inside this circumference are reachable with four possible
configurations of the joint variables as shown in Figure 5.
The center of the workspace is reachable with every orientation.
Figure 5. A detail of the central area of the wafer handler workspace
1.2.2 Inverse kinematics
A geometric approach based on the link coordinate systems and on the manipulator
configuration has been used for the manipulator. Since we refer to the control of only one
arm (link 1 - shoulder, link 2 - elbow), the origin of the base coordinates has been placed in
such way that the z
First of all, we consider the manipulator in the configuration described in the following figure.
axis lies along the rotation axis of the revolute joint of the shoulder.
0
9/41
An overview on robotics AN2350
α
Figure 6. The geometric approach for the inverse kinematics
y
α
= -2α’
2
l
1
α
’
1
l
2
1
l
3
/ 2 = α’
x
3
1
x
1
x
2
α
= -
α
3
2
There is a mechanical connection between the link 3 and the link 2, which imposes
2
α
3
Since
. This implies that the joint 3 can not be actuated independently from the joint 2.
−=
2
lll==
21
⎧
=
lx
⎪
⎨
⎪
⎩
cos
'
α
11
'
cos2cos
=+=
cos2
αα
llxx
1212
'
α
+=+=
lllxx
3
1323
From the last equation and from geometric considerations follows:
−
'
α
arccos
=
1
'
αα
−=
2
12
α
α
3
2
2
lx
33
2
l
'
α
=−=
1
Consider now a rotation of an angle ϕ:
x
10/41
AN2350 An overview on robotics
l
Figure 7. The inverse kinematics after a rotation
y
α
= -2α’
2
l
l
1
α
1
α
’
1
Knowing the position of the end-effector in the workspace, it is possible to deduce its
distance from the origin and the angle ϕ:
2
1
l
3
ϕ
α
= -
α
3
22
yxz
+=
/ 2 = α’
2
Ζ
1
x
α
while
manipulator.
and
2
y
atn
=
ϕ
x
lz
−
arccos
+=
ϕα
1
α
remains the same, because they are independent from the rotation of the
3
3
2
11/41
The SDK-ST10F276 control board AN2350
2 The SDK-ST10F276 control board
2.1 Brief description of the SDK-ST10F276
The SDK-ST10F276 is a two-layer, low-cost development board with an ST10F276 16-bit
Embedded Flash Memory Microcontroller (see Figure 8). It is considered a general purpose
application board used for developing advanced motor control solutions and processing
external data (e.g., sensor outputs).
Three dedicated connectors for motion control allow the user to control different kinds of
motors by plugging in external power motor boards.
Figure 8. The SDK-ST10F276 board
12/41
AN2350 The SDK-ST10F276 control board
2.1.1 User Interfaces
The main interfaces used to communicate with the SDK-ST10F276 platform include:
● Two CAN 2.0B interfaces which operate on one or two CAN buses (30 or 2x15
message objects)
● One RS232 connector for serial signal communication with external devices
● One I2C connector for additional external device management
● Three dedicated connectors for motion control (external power motor boards)
● A series of general purpose LEDs and DIP switches
● One potentiometer and a push-button
For the user's convenience, the ST10 I/O pins on the Development board are split into four
main connectors located on each side of the ST10 device
The five general purpose Light Emitting Diodes (LEDs) may be used for end-user
applications.
They can be activated via the JP2 connector, or by using a series of dip switches on the
board.
The potentiometer and push-button may be set in different and independent ways for
general purpose uses.
2.1.2 On board motor control connectors
The SDK-ST10F276 board provides an embedded solution (3 modular connectors) for
power requirements up to 3000W, so they are well-suited for driving motors (e.g. BLDC,
Brushless Direct Current) via an appropriate external motor board. The various power
boards that can be connected to the SDK-ST10F276 are optimized in order to support a
wide variety of advanced motor control applications.
Each of the three power connectors on the SDK-ST10F276 development board has 26 pins
(see Figure 9). When a connector is used with a compatible motor board, the user can drive
motors with the following signals:
● PWM
● Hall Effect sensor/GPIO
● Power/Brake
● Encoder
● Tachometer/Resolver
● Shutdown, and Alarm
13/41
Loading...