AN2350 STMICROELECTRONICS | Alldatasheet
Document overview
- Manufacturer or author: Provided By ALLDATASHEET.COM(FREE DATASHEET DOWNLOAD SITE)
- PDF pages: 41
Technical content
Datasheet sections
- 1 An overview on robotics
- 1.1 Structure of a manipulator
- 1.1.1 Kinematics analysis
- 1.1.2 Singularity
- 1.2 The industrial wafer handler
- 1.2.1 Forward kinematics
- 1.2.2 Inverse kinematics
- 2 The SDK-ST10F276 control board
- 2.1 Brief description of the SDK-ST10F276
- 2.1.1 User Interfaces
- 2.1.2 On board motor control connectors
- 2.2 ST10F276 16-bit microcontroller - architectu ral overview
- 2.2.1 Basic CPU concepts
- 2.2.2 Memory organization
- 2.2.3 On-chip peripheral blocks
- 2.2.4 Managing Interrupts (hardware)
- 3 Hardware and mechanical equipments
- 3.1 The Dual DC motor and the power stage
- 3.2 Cables and connectors
- 3.3 The encoders and the conditioning circuit
- 3.4 Schematics of the driver board and the interface board
- 4 Control algorithm
- 4.1 Motion and path planning
- 4.2 PID position control algorithm
- 4.3 Homing procedure
- 4.4 Teach and Repeat procedure
- 5 Revision history
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)
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 end- effector 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. Table 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.
Table 1. Open chain manipulators classification
1.1.1 Kinematics analysis
to the forces/moments that causes the motion. orientation of the end-effector of a robot arm.
- Forward analysis: for a given manipulator, given the joint angle vector
- Inverse analysis: given a desired position and orientation of the end effector and the
For serial robots, the forward analysis problem is usually easy and straightforward. the joint angles for each position in the path. for fault tolerance, obstacle avoidance, or to optimize some performance criteria. Figure 1. The direct and inverse kinematics problems
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.
Figure 2. The structure of the wafer handler
- Z Axis Motor (Vertical elevation)
- X Axis Motor (Rotation entire robot)
- Dual Motors (Link 2 and Link 1)
1.2.1 Forward kinematics
kinematics through a D-H homogeneous transformation matrix. Figure 3. The Denavit Hartenberg convention for one arm
- Robot Body Mounting Flange
longitudinal direction of the link 1. frames have been placed at the same height of the origin O0. Table 2. The Denavit Hartenberg parameters for one arm elevation - prismatic joint) and X Axis (Rotation entire robot - revolute joint). characterized of different orientations of the end-effector. Figure 4. The workspace of the wafer handler
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
0 axis lies along the rotation axis of the revolute joint of the shoulder. First of all, we consider the manipulator in the configuration described in the following figure.
Figure 6. The geometric approach for the inverse kinematics . This implies that the joint 3 can not be actuated independently from the joint 2.
Figure 7. The inverse kinematics after a rotation
2 The SDK-ST10F276 control board
2.1 Brief descriptio n of the SDK-ST10F276
external data (e.g., sensor outputs). motors by plugging in external power motor boards. Figure 8. The SDK-ST10F276 board
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
Figure 9. One of the 26 pin connectors of the SDK-ST10F276 board
2.2 ST10F276 16-bit microcontrol ler - architectural overview
The ST10F276 is a high performance 64MHz 16-bit microcontroller with DSP functions. the ST10F276. (see Figure 10).
Figure 10. ST10F276 functional block diagram
2.2.1 Basic CPU concepts
logic unit (ALU) and dedicated SFRs. Several areas of the processor core have been optimized for performance and flexibility.
- High instruction bandwidth / fast execution
- High function 8-bit and 16-bit arithmetic and logic unit
- Extended bit processing and peripheral control
- High performance branch, call, and loop processing
- Consistent and optimized instruction formats
- Programmable multiple priority interrupt structure
The SDK-ST10F276 control board AN2350
2.2.2 Memory organization
The memory space of the ST10F276 is organized as a unified memory. Code memory, data memory, registers and I/O ports are organized within the same linear address space. The main characteristics are summarized below:
- 512K Bytes of on-chip single voltage Flash memory with Erase/Program controller
- 320K Bytes of on-chip extension single voltage Flash memory with Erase/Program controller (XFLASH)
- Up to 100K Erase/Program cycles
- Up to 16 MB of linear address space for code and data (5 MB with CAN or I2C)
- 2K Bytes of on-chip internal RAM (IRAM)
- 66K Bytes of on-chip extension RAM (XRAM)
2.2.3 On-chip peripheral blocks
The ST10F276 generic peripherals are:
- Two General Purpose Timer Blocks (GPT1 and GPT2)
- Serial channel: Two Synchronous/Asynchronous, two High-Speed Synchronous, I2C standard interface
- A Watchdog Timer
- Two 16-channel Capture / Compare units (CAPCOM1 and CAPCOM2)
- A 4-channel Pulse Width Modulation unit and 4-channel XPWM
- A 10-bit Analog / Digital Converter
- Up to 111 General Purpose I/O lines Each peripheral also contains a set of Special Function Registers (SFRs), which control the functionality of the peripheral and temporarily store intermediate data results. Each peripheral has an associated set of status flags. Individually selected clock signals are generated for each peripheral from binary multiples of the CPU clock.
2.2.4 Managing Interrupts (hardware)
- 8-channel Peripheral Event Controller for single cycle, interrupt-driven data transfer
- 16-levels of priority for the Interrupt System with 56 sources, and a sampling rate down to 15.6ns at 64MHz Note: Refer to the ST10 information listed in the Introduction on page 1 for device details.
3 Hardware and mechanical equipments
3.1 The Dual DC motor and the power stage
and having concentric drive hubs, as shown in the Figure 12. Figure 11. The Dual DC motor through a drive belt with a transmission ratio of 1/2.
Figure 12. The chassis of the dual DC motor Each DC motor is driven by a monolithic solution (L6205PD) that is a dual full bridge driver. current controller disables the power drivers. the motor moves in the opposite direction. For a duty cycle of 50% the motor is stopped. algorithm refers to the control of only one arm (two links). the power stage is shown in the following figure.
Figure 13. A detail of the power stage
3.2 Cables and connectors
On the chassis skin of the wafer handler are present six 15 pin D-SUB male connectors. harness connectors of the links are located under the torso beam. below shows the pins assignment and the related description. Table 3. The sectors of the absolute encoder
3.3 The encoders and th e conditioning circuit
ring it is possible to identify four sector named A, B, C and D, and the encoder read head.
33 V i o l e t Absolute encoder
59 W h i t e Absolute encoder
Figure 14. The encoder ring of one DC motor (grey wire) represents the least significant bit. Table 4. The sectors of the absolute encoder
Figure 15. The conditioning circuit for one absolute encoder offset of 2.5 V and a peak-to-peak voltage of 1.1 V. necessary to develop a conditioning circuit whose outputs are digital signals.
Figure 17. Driver board schematic (page 1)
Figure 18. Driver board schematic (page 2) interface board is shown below.
0.22 Ohm 2W 1%
Figure 19. The interface board schematic
4 Control algorithm
4.1 Motion and path planning
configuration (goal) respecting certain constraints. for problems requiring time-parameterized solution trajectories. order to ensure the correct execution of the planned trajectory by the manipulator. been used in this work according to the scheme of the following figure. Figure 20. General scheme for the path planning variables that specify the position and orientation of the end-effector in the workspace. performed on the feedback of the incremental encoders of the two joints.
Figure 21. Example of trapezoidal velocity profile c (also called the cruising speed).
Now follows the algorithm used for the motion planning in order to build a trajectory table whose points (as joint variables after the reversal kinematics and expressed as incremental encoder pulses) are given as inputs to the PID control interrupt service routine every T R , (fixed in 20 ms). Let the coordinates of the start point and the coordinates of the stop point, vc the cruising speed and a the acceleration. From the previous equations it is possible to obtain the time t1 and the time t3 , where: Consider as the generic straight-line that intersect the two points and , where is the angular coefficient and is the intersection with the Y axis. It is possible that the cruising speed it is not compatible with the imposed acceleration and the distance to cover: this implies that the velocity profile is not trapezoidal. For this reason a convergent loop procedure has been developed (shown in the following scheme) that redefines the values of t 1 , vc , and t3 . 12max )()( yyxxs −+−= cmxy += ),( 11 yx ),( 22 yx xx yym − +−−+= )( ))(( 1212 xx xxyyyyc
Figure 22. Convergent loop procedure for a trapezoidal velocity profile feedback of the incremental encoders of the two joints. of the trajectory impossible. with the arms aligned, as shown in the following figure.
As shown in the figure above, the trajectory in this case is made of three tracts.
- A straight-line tract with a trapezoidal velocity profile that brings the end-effector from the start point to the origin .
- A change of the configuration in which the entire arm is rotated of 180°. We can see how the position of the end-effector remains the same while its orientation is changed.
- A straight-line tract with a trapezoidal velocity profile that brings the end-effector from the origin to the stop point .
Figure 25. Motion path planning scheme to avoid singularities
4.2 PID position control algorithm
routine (ISR) is performed every 1ms. one for the elbow (elbow_array[ ]), obviously of the same dimension. trajectory table has been associated to the compare unit CC3. timers T2 and T4, respectively, used in incremental interface mode.
To ensure that the point has been reached, a check on the error has been performed in a such way that if the total error (as sum of the error of the shoulder and of the elbow) is higher than a fixed value (MAX_ERR_POS=150 ) no new point is passed. The figures below show the general schemes for the control algorithm ISR and for the scanning of the trajectory table. The variables sh_pulses_desired, elb_pulses_desired, total_error, index, flag_error are global variables.
Figure 26. PID position control ISR
Figure 27. Trajectory table scanning ISR additional control algorithm is performed using the ISR associated to timer T6, every 200ms. drastically reducing the oscillations.
4.3 Homing procedure
a configuration that allows it to perform the other tasks correctly. the position in which it is placed and to the tasks that it must execute. absolute encoder for the shoulder and for the elbow.
home procedure for the elbow there's no movement of the shoulder. The scheme below shows the homing procedure. Figure 30. The homing procedure
4.4 Teach and Repeat procedure
Robots are mainly programmed using a technique known as teach and repeat. Using this technique the control system stores internal joint poses specified by a human operator and then recalls them in sequence to execute a task. In this way it is possible to store complex trajectories allowing the robot can execute them with a good reliability and repeatability. During the teach phase the control of the manipulator is disabled and the human operator can move the arm as he wants. With a periodic task of 20 ms (associated to the Compare Unit CC8 and the timer T0) the value of the incremental encoders of the two joints (the shoulder and the elbow) are stored in the two corresponding arrays: shoulder_array_teach[] and elbow_array_teach[]. The teach phase duration depends on the size of the two arrays. Using 3 KByte of the XRAM2 for each array, it is possible to store a trajectory of 30 seconds. In the repeat phase the couple of points of the two arrays are passed to the control algorithm as explained before, as well the scanning of the two arrays. The scheme for teach and repeat procedure is shown in the figure below.
Figure 31. The teach and repeat procedure
5 Revision history
Table 5. Document revision history 30-Nov-2006 1 Initial release.