People’s Democratic Republic of Algeria Ministry of Higher Education and Scientific Research University M’Hamed BOUGARA – Boumerdes
Institute of Electrical and Electronic Engineering Department of Power and Control Final Year Project Report Presented in Partial Fulfilment of the Requirements for the Degree of
MASTER In Electrical and Electronic Engineering Option: Control Title: Design Of A Digital Control System And Path Planning Module For The ED-7220C Robot Arm
Presented by: - ELGHERIBI Abdellah Nabih - BOUAMER Tarek Supervisor: Mr. R. GUERNANE
To our families, and friends
ACKNOWLEDGEMENT
At the very outset, all our prayers and thankfulness are to Allah the almighty for facilitating this work and for granting us the opportunity to be surrounded by great and helpful people at IGEE Institute.
We would like to express our everlasting gratitude to our supervisors, Dr. Reda GUERNANE for his valuable encouragement, guidance and monitoring, without which this work would not have reached the point of fruition, so we ask Allah to reward him on our behalf. beh alf.
Our warm heart, our fathers, mothers deserve all the credit here; they have been a source of inspiration to us for years. We would never forget their continuous prayer for the sake of our success.
No acknowledgement would be complete without expressing e xpressing our appreciation and thankfulness for the maintenance laboratory workers for their support a nd help.
Abstract
This Project has the objective to design and Implement a graphical software interface to program typical tasks for a robot arm, these includes pick-and-place, trajectory tracking or basic assembly.
The project should go through the following steps:
1. Implement the power drive (PWM or linear) electronics for the five exes of the ED-7220C arm plus gripper. 2. Design an embedded digital control system for robot positioning. 3. Design a Graphical user interface running on a remote PC using the ROS platform to command the robot. 4. Implement a motion planning algorithm to automatically generate robot trajectories that satisfy the given task while avoiding obstacles.
TABLE OF CONTENTS
ACKNOWLEDGEMENT………………………………………………………………………………….….II ABSTRACT…………………………………………………………………………………………………...III TABLE OF CONTENTS……………………………………………………………………………………...IV LIST OF FIGURES…………...……………………………………………………………………………......V LIST OF TABLES………….…………………………………………………………………………………VI INTRODUCTION………………………………..…………………………………………………………….1 1. 2. 3. 4.
MOTIVATION……………………………………………………………………………………….2 PROBLEM STATEMENT AND GOAL…………………………………………………………….2 SYSTEM OVERVIEW………………………………………………………………………………3 ORGANIZATION OF THE REPORT ………………………………………...…………………….3
CHAPTER I
ROBOT KINEMATICS………………………………...……………………………………5
1.1. INTRODUCTION…………………………………………………………………………………….6 1.2. DIRECT/ FORWARD KINEMATICS……………………………………………………………….7 1.2.1. Assigning the coordinate frames………………………………………………………………..9 1.2.2. ED-7220C DH parameters...........................................................................................................9 1.3. INVERSE KINEMATICS…………………………………………………………………………...12 1.3.1. Geometric approach………….………………………………………………………………...12 1.3.2. Analytical/algebraic approach…………………………………………………………………14 1.4 CONCLUSION………………………………………………………………………………………16 CHAPTER II
DYNAMIC AND CONTROL………………………………………………………………17
2.1. CONTROL TECHNIQUE…………………………………………………………………………..18 2.2. INDEPENDENT JOINT CONTROL..……………………………………………………………...19 2.2.1. Cascade PID compensation ……….…………………………………………………………..20 2.2.2. FeedForward disturbance torque cancellation…………………………………………………21 2.3 CONCLUSION……………………………………………………………………………………….23 CHAPTER III ROBOT TRAJECTORY & MOTION MOTION PLANNING..……………………………………….24 PLANNING..……………………………………….24 3.1 3.2 3.3 3.4
INTRODUCTION…………………………………………………………………………………....25 LINEAR SEGMENT METHOD WITH PARABOLIC BLENDS (LSPB)……………………….....27 ED-7220C MOTION PLANNING…………………………………………………………………...30 CONCLUSION…………………………………………………………………………………...…..33
CHAPTER IV RESULTS AND DESCUSSION…...………………………………………………………..34 4.1 HARDWARE IMPLEMENTATION…………..…………………………………………………….35 4.2 MODELING THE ED-7220C ROBOT IN ROS…………………………………………….……….36 4.3 FORWARD KINEMATICS…………...………………………………………………………….….36 4.4 CONTROL DESIGN………….……………………………………………………………………...39 4.5 TRAJECTORY GENERATION………………...…………………………………………………...42 4.6 MOTION PLANNING……………………………………………………………………………….48 CONCLUSION………………………………………………………………………………………………..53 REFERENCES…....…………………………………………………………………………………………...54 APPENDIX A…………………………………………………………………………………………………56
LIST OF FIGURES
Figure (I.1): Figure (1.1): Figure (1.2): Figure (1.3): Figure (1.4): Figure (1.5): Figure (1.6): Figure (2.1): Figure (2.2): Figure (2.3): Figure (2.4): Figure (2.5): Figure (3.1): Figure (3.2): Figure (3.3): Figure (3.4): Figure (3.5): Figure (3.6): Figure (3.7): Figure (4.1): Figure (4.2): Figure (4.3): Figure (4.4): Figure (4.5): Figure (4.6): Figure (4.7): Figure (4.8): Figure (4.10): Figure (4.11): Figure (4.12): Figure (4.13): Figure (4.14): Figure (4.15): Figure (4.16): Figure (4.17): Figure (4.18): Figure (4.19): Figure (4.20): Figure (4.21): Figure (4.22): Figure (A.1): Figure (A.2): Figure (A.3): Figure (A.4): Figure (A.5): Figure (A.7):
System block diagram……………………… diagram………………………………………………… ………………………………………...3 ……………...3 ED-7220C and its kinematics model………………………………………………….6 model………………………………………………….6 Kinematics block diagram………………………… diagram……………………………………………………… ………………………………….7 …….7 DH frame assignment………………………… assignment…………………………………………………… ………………………………………7 ……………7 ED-7220C robot arm frame assignment……………………… assignment………………………………………………8 ………………………8 Top view vie w of robot………………………… robot…………………………………………………… …………………………………………13 ………………13 Planar view of ED-7220C ED-7220 C robot arm………………………………………………...14 arm………………………………………………...14 Control techniques analysis of manipulators…………………… manipulators………………………………………...18 …………………...18 Control structure for a given robot joint ……………...…………………………….19 Block diagram of closed c losed loop control system...…………………… system...……………………………………..21 ………………..21 Estimator design……...………………… design……...……………………………………………… …………………………………………...21 ……………...21 90° Configuration of ED-7220C robot arm ………………………………………....21 ………………………………………....21 Trajectory planning block diagram………………………… diagram………………………………………………….26 ……………………….26 Typical joint space trajectory…………………… trajectory……………………………………………… …………………………………..26 ………..26 Blend times for LSPB trajectory…………………… trajectory………………………………………………… ……………………………….27 ….27 Trajectory using LSPB………………………… LSPB……………………………………………………… ……………………………………30 ………30 Trajectory generation and motion planning diagram …………...…………………..31 RRT flowchart…………………… flowchart……………………………………………… ………………………………………………….32 ……………………….32 Basic RRT RRT exploring phase of 5 DOF robot arm………………………… arm……………………………………33 …………33 The overall system structure……………………… structure………………………………………………… ………………………………...35 ……...35 The 3D model of ED-7220C in ROS…………………………… ROS………………………………………………..36 …………………..36 Forward Kinematic window for the initial configuration……………………… configuration…………………………...37 …...37 Initial configuration of ED-7220C…………………………… ED-7220C…………………………………………………..37 ……………………..37 FK window for the selected selected configuration at position position (0.03, -41.58, 19.19)……….38 Robot position at (0.03, -41.58, -41.58, 19.19) in ROS………………………… ROS……………………………………..38 …………..38 The actual response of the base joint to 50° setpoint………………… setpoint………………………………..40 ……………..40 The actual response of the wrist joint to 100° setpoint……………………… setpoint……………………………...40 ……...40 Shoulder torque rejecter (case1) from from 110° to 90°………………………………….41 90°………………………………….41 Shoulder torque rejection (case 2) from 90° to 110°………………………………..41 110°………………………………..41 LSPB trajectory curves of the the base………………………………………………….43 base………………………………………………….43 LSPB trajectory curves of the the shoulder……………………………………………..44 shoulder……………………………………………..44 LSPB trajectory curves of the the elbow………………………………………………..45 elbow………………………………………………..45 LSPB trajectory curves of the the wrist…………………………………………… wrist…………………………………………………46 ……46 End effector generated path in XYZ XYZ plane………………………………………….48 plane………………………………………….48 Planned scene with obstacle………………………… obstacle……………………………………………………… ………………………………49 …49 Pick and place position in Rviz……………………………… Rviz……………………………………………………...49 ……………………...49 Waypoints of the planned path in black trial (without obstacle)……………………49 obstacle)……………………49 End-effector curve of the planned path…………………………… path……………………………………………...50 ………………...50 The RRT planned path with obstacle avoidance…………………………………… avoidance…………………………………….50 .50 End-effector curve of the planned path with obstacle avoidance…………………...50 L298 IC H-bridge……………………… H-bridge………………………………………………… ……………………………………………56 …………………56 H-bridge structure structure and its working principle……………………………… principle………………………………………..56 ………..56 The circuit design in proteus and its 3D PCB………………………… PCB……………………………………….57 …………….57 Arduino Due microcontroller……………… microcontroller…………………………………………… ………………………………………..58 …………..58 ED-7220C workspace…………………………… workspace……………………………………………………… ………………………………….59 ……….59 Forward Kinematic Window……………………… Window………………………………………………… ………………………………...60 ……...60
LIST OF TABLES
Table (1.1): Table (1.2): Table (2.1): Table (2.2): Table (4.1): Table (4.2): Table (4.3): Table (4.4): Table (4.5): Table (A.5): Table (A.6):
DH parameter for ED-7220C robot arm................................................................................10
The link lengths ………………………………………………..……………………10 Shoulder’s position with corresponding estimated torque…………………………..22 Elbow’s position with corresponding estimated torque……………………… torque……………………………..23 ……..23 Links’ colors reference………………………… reference…………………………………………………… ……………………………………36 …………36 Initial configuration of ED-7220C…………………………… ED-7220C…………………………………………………..36 ……………………..36 Computed and measured results………………………………………… results……………………………………………………..39 …………..39 Motors’ dead zone……………………………… zone………………………………………………………… …………………………………...39 ………...39 Generated trajectory Waypoints……………………… Waypoints…………………………………………………… ……………………………..47 ..47 Readings of joints’ encoders…………………………… encoders……………………………………………………… ……………………………58 …58 ED-7220C range of motion………………………………………………………………….59
Introduction
Introduction 1 Motivation
In recent years, industrial and commercial systems with high efficiency and great performance have taken advantages of robot technology. Large number of control researches and numerous control applications were presented during the last years, concentrated on control of robotic systems. Robot manipulator field is one of the interested fields in industrial, educational and medical applications. It works in unpredictable, hazard and inhospitable circumstances which human cannot reach. For example, working in chemical or nuclear reactors is very dangerous, while when a robot instead human it involves no risk to human life. Therefore, modeling and analysis of the robot manipulators and applying control techniques are very important before using them in these circumstances to work with high accuracy. In Algeria, many industrial applications can utilize robot technology and develop robot manipulators. It is an attractive field to be applied and developed for industrial applications. This thesis is meant to be suitable for these the se applications. On the other side, some universities and colleges offers, some courses related to robotics. These courses mainly focus on the theoretical concepts without giving much attention for controlling different robot manipulators in the practical side. This thesis may be considered as a valuable educational tool in their laboratories. 2 Problem statement and goal
One of the key components of automation and industrial control are robot manipulators. The main problem with commercial manipulators solutions is that they require (or come with) proprietary (Black-Box) and expensive Control hardware (modules) and programming environments. Our goal is to design a robot manipulator system and demonstrate an easy to use graphical task programming interface. To start, we would need a robot arm, and since as mentioned previously, industrial arms are beyond most universities budgets, we resorted to the inexpensive ED-7220C 5-axes educational robot arm available at our institute. Although less precise and powerful than the professional counterparts, it nevertheless can demonstrate most industrial manipulator tasks. Notice that even e ven though the ED-7220C is educational, its Control hardware and software are unfortunately closed-source and thus cannot be modified or improved upon. Using the robot arm only, our goal is to replace its drive/control box and software with an open one; this requires going thought the following development steps: •
Mathematical analysis for the forward and inverse kinematics of the robot.
•
Design the Actuators Power drive system.
•
Design a digital position controller for the robot arm joints.
2
Introduction •
•
Design an interface to generate and simulate robot trajectories for pick-and-place task in a cell environment with obstacles. Integrate the interface with the robot control system.
3 System overview
The complete system block diagram shown in Figure (I.1) consists of: •
•
•
Personal Computer: This is where the high-level task programming is performed (offline), this contains The Graphical User Interface (GUI) which supplies the de sired trajectory to the controller. Arduino Due microcontroller: this where the robot arm controller arm is ex ecuted, it takes input from the PC and feedback fee dback signals from the arm and issue the command signal to this latter (online) The ED-7220C Robot arm: reacts to the control signals and provides feedback to the Arduino controller.
The Graphical User Interface (GUI) designed by Processing Software. This software consists of two parts; forward and inverse kinematics. The forward kinematics consists of finding the position of the end-effector in the space knowing the movements of its joints. The inverse kinematics consists of the determination of the joint variables corresponding to a given end-effector position and orientation. Path is defined as sequence of robot configurations in particular order without regard for timing of these configurations, trajectory is concerned about when each part of the path must be obtained thus specifying timing.
Figure (I.1): System block diagram
Serial communication is the simplest way to communicate between two devices. A serial interface is established through a serial port object, which can be created using the SERIAL function. The main function of the Arduino Due microcontroller is making interface between PC and ED-7220C robot arm by receiving data from serial port and processing it, then sending a control signal to the arm servo motors then feeding the data from servo motors’ encoders back to the Arduino. 4 Organization of the report
This report is organized as follows:
3
Introduction Chapter 1 brings the analysis of the ED-7220C kinematics (forward and backward), while chapter 2 discusses the low level problem of robot dyna mics, and the control approach used to position the arm. Chapter 3 moves to the higher level problem of path and trajectory planning. Practical results and discussion appears in chapter 4. We finish with a general conclusion, appendices and the references used throughout our work.
4
ROBOT KINEMATICS
Robot kinematics
Chapter 1
1.1 Introduction
Kinematics is the description of motion without regard to the forces that cause it. It deals with the study of position, velocity, acceleration, and higher derivatives of the position variables.
Figure (1.1): ED-7220C and its kinematics model
In robot simulation, system analysis needs to be done, such as the kinematics analysis; its purpose is to carry through the study of the movements of each part of the robot mechanism and its relations between itself. The kinematics analysis is divided into forward and inverse analysis. The forward kinematics consists of finding the position of the end-effector in the space knowing the movements of its joints as F(θ F(θ 1 ,θ 2 ,..,θ n ) = [x, y, z, R], and the inverse kinematics consists of the determination of the joint variables corresponding to a given end-effector position and orientation as F(x, y, z, R) = θ 1 , θ 2 ,…, θ n . Figure (1.1) above shows the ED-7220C ED-7220C arm along with it abstract kinematics view, Figure (1.2) below shows a simplified block diagram of kinematic modeling.
6
Robot kinematics
Chapter 1
Figure (1.2): Kinematics block diagram
A commonly used convention for selecting frames of reference in robotic applications is the Denavit Hartenberg or D-H convention as shown in Figure (1.3). In this convention each homogenous transformation Ti is represented as a product of "four" basic transformations.
ii−1 θ i i i αi
T
Where the four quantities
= Rot (z, θ i ) Trans (z, d i ) Trans (x, a i ) Rot (x, α i )
,a ,d ,
(1.1)
are the parameters of link i and joint i. are
Figure (1.3): DH frame assignment
Where the notation Rot (x, αi ) stands for rotation about xi axis by α i , Trans (x, ai ) is translation along x i axis by a distance a , i Rot(z, θ i ) stands for rotation about z i axis by θ , i and Trans (z, d i ) is the translation along z i axis by a distance d i . 1.2 Direct/Forward Kinematics
The forward kinematics problem can be stated as follows: Given the joint variables of the robot, determine the position and orientation of the end-effector in a workspace frame. Since each
7
Robot kinematics
Chapter 1
1 2
joint has a single degree of freedom, the action of each joint can be described by a single number, i.e. , ,…, the angle of rotation in the case of a revolute joint. The objective of forward kinematic analysis is to determine the cumulative effect of the joint variables. Suppose a robot has n+l links numbered from zero to n starting from the base of the robot, which is taken as link 0. The joints are numbered from 1 to n, and zi is a unit vector along the axis in th space about which the links i-1 and i are connected. The i- joint variable is denoted by q, In the case of a revolute joint, q, is the angle of rotation, while in the case of a prismatic joint q, is the joint translation. Next, a coordinate frame is attached rigidly to each link. To be specific, we choose frames 1 through n such that the frame i is rigidly attached to link i. Figure (1.4) illustrates the idea of attaching frames rigidly to links in the case of an ED-7220C robot.
Figure (1.4): ED-7220C robot arm frame assignment
−1
−1
is a homogenous matrix which is defined to transform the coordinates of a point from frame i
to frame i-1. The matrix
However, the assumption that all joints are either revolute or prismatic means that of only a single joint variable, namely n amely qi. In other words,
−1 −1 =
−1
is not constant, but varies as the configuration of the robot is changed. is a function
( )
(1.2)
The homogenous matrix that transforms the coordinates of a point from frame i to frame j is denoted by
0
0
(i>j). Denoting the position and orientation of the end-effector with respect to the
inertial or the base frame by a three dimensional vector respectively, we define the homogenous matrix.
0 �0 0 =
0
1
8
and a 3x3 rotation matrix
,
(1.3)
Robot kinematics
Chapter 1
Then the position and orientation of the end-effector in the inertial frame are given by
0 1 2 10 1 21 2 −1−1 −1 −1 �−1 −1 +1+1 −1 � (
,
Each homogenous transformation
)=
,…
=
The matrix
)…
(
)
(1.4)
0
1
…
=
0
1
(1.5)
(1.6)
expresses the orientation of frame i relative to frame j (i > j) and is given by the
rotational parts of the
The vectors
(
)
is of the form
=
Hence
(
matrices (i > j) as
+1 −1 +1 − 1 −1 =
…
(1.7)
(i > j) are given recursively by the formula
=
+
(1.8)
1.2.1. Assigning the Coordinate Frames
1 3 4
ED-7220C has five rotational joints and a moving grip as shown in Figure (1.4). Joint 1 represents the base and its axis of motion is z 1 . This joint provides a rotational angular motion around z1 axis in x 1 y 1 plane. Joint 2 is identified as the shoulder and its axis is perpendicular to Joint 1 axis. It provides a rotational angular motion motion around z2 axis in x 2 y 2 plane. z 3 axes of Joint 3 (elbow) and Joint 4 (Wrist) are parallel to Joint 2 z-axis; they provide and angular motions in x 3 y 3 and x 4 y 4 planes respectively. Joint five are identified as the grip rotation. Its z5 axis is vertical to z 4 axis and it provides angular motions in x5 y 5 plane.
5
2
1.2.2. ED-7220C DH Parameters
The Denavit-Hartenberg analysis is one of the most used, in this method the direct kinematics is determined from some parameters that have to be defined, depending on each mechanism. However, it was chosen to use the homogeneous transformation matrix. In this, analysis, once it is easily defined one coordinate transformation between two frames, where the
9
Robot kinematics
Chapter 1
position and orientation are fixed one with respect to the other it is possible to work with elementary homogeneous transformation operations. D-H parameters for ED-7220C defined for the assigned frames in Table 1.1. Table (1.1): DH parameter for ED-7220C robot arm
1
− 0
− 0
2
90
a 1 =2.5
0
3
0
a2
0
4
0
a3
0
5
90
0
d 5
6
0
0
0
i
d 1
1 23 45 6
Table (1.2): The link lengths
joint Symbol Link Length(cm)
Waist d 1
Shoulder a2
Elbow a3
Wrist d 5
37
22.0
22.0
15.0
Using the parameter of table (1.1), the transformation matrices T1 to T6 can be obtained o btained as shown below. For example, T1 shows the transformation between frames 0 and 1 (designating etc.).
1 1 1 1 10 1 1 111 2 2 2 2 21 2 2 22 3 3 3 3 32 3 3 33 =
=
=
0 0
0 0
0 0
0 0 1 0
0 0
0
0 0
0 0 1 0
0 1
0 0
0 0 1 0
0 1
10
(1.9)
(1.10)
(1.11)
Robot kinematics
4 4 43 4 4 5 5 54 5 5 5 =
=
0 0
0 0
0 0 1 0
0 0
0 0
Chapter 1
0 0 0 1
(1.12)
0 0 0 0 1 0 1
(1.13)
Using the above values of the transformation matrices; the link transformations can be concatenated (multiplied together) to find the single transformation that relates frame (5) to frame (0):
50 1021324354 =
=
0
0
0
(1.14)
1
The transformation given by equation (1.14) is a function of all 5 joint variables. From the robots joint position, the Cartesian position and orientation of the last link may be computed using above equation (1.14). The first three columns in the matrices represent the orientation of the end effectors, whereas the last column represents the position of the end effectors. The orientation and position of the end effectors can be calculated in terms of joint angles using:
1123423455 1155 2345 1123423455 1155 2345 23423411 234 11 23423455 332323 2222 11 2345 323 22 1 = =
+
(1.15)
=
= =
+
(1.16)
=
= =
(1.17)
=
= =
=
( (
+ +
+
+ +
+
+ +
) )
(1.18)
+
11
Robot kinematics
Chapter 1
1.3 Inverse kinematics
Inverse Kinematics (IK) analysis determines the joint angles for desired position and orientation in Cartesian space. Total transformation matrix in equation. (1.14) will be used to calculate inverse kinematics equations. IK is more difficult problem than forward kinematics. The solution of inverse kinematic is more complex than direct kinematics and there is not any global analytical solution method. Each manipulator needs a particular method considering the system structure and restrictions. There are two solutions approaches namely, geometric and algebraic used for deriving the inverse kine matics solution. Let’s start with geometric approach. 1.3.1. Geometric Approach
Using IK-Cartesian mode, the user specifies the desired target position of the gripper in Cartesian space as (x, y, z) where z is the height, and the angle of the gripper relative to ground, ψ (see Figure 1.6), 1.6), is held constant. This constant ψ allows users to move objects without changing the object’s orientation (the holding a cup of liquid scenario). In addition, by either keeping ψ fixed in position mode or keeping the wrist fixed relative to the rest of the arm, the inverse kinematic equations can be solved in closed form as we now show for the case of a fixed ψ. The lengths d1, a2, a3 and d5 correspond to the base height, Shoulder length, Elbow length and gripper length, respectively respectively are constant. The angles θ1, θ2, θ3, θ4 θ4 and θ5 correspond to Base rotation, Shoulder, Elbow, wrist, and end effector, respectively. These angles are updated as the specified position in space changes. We solve for the joint angles of the arm, θ1:4 given given desired position (x, y, and z) and ψ which ψ which are inserted by the user. From Figure (1.5), (1.5), we clearly see that θ1= atan2(y, x) and the specified radial distance from the base d are related to x and y by:
2 12 1 = = =
12
+
(1.19)
Robot kinematics
Chapter 1
Figure (1.5): Top view of robot
Moving now to the planar view in Figure (1.6), (1.6), we find a relationship between joint angles θ2, θ3 and θ4 and ψ as follows:
ψθ θ θ 44 55 ψψ 44 22 22 33 22 33 1 = 1 + 2 + 3
(1.20)
Since ψ is given, we can ca n calculate the radial distance and height of the wrist joint:
= =
= cos( cos( ) = sin( ) Or cos( cos( ) + cos( cos( + ) sin( sin( ) + sin( sin( + ) +
(1.21)
Now we want to determine θ2 and θ3. We first solve for α, β and s (from Figure 1.6) uses the law of cosines as:
42 21432 2 4 1 2 42 2 3 4 ψ222332 23 = =
=
2( 2(
(
+
,2
s)
, )
(1.22)
) +
With these intermediate values, we can now find the remaining angle values as:
=
=
2( =
±
,2
13
)
(1.23)
Chapter 1
Robot kinematics
Figure (1.6): Planar view of ED-7220C robot arm
1.3.2. Analytical (algebraic) Approach
Using the X, Y and Z resultants gotten in the direct kinematics:
11 2222 332323 55234234 11 5234 323 22 1 2 ⟹ 22 323 11 2 11 2 5234 ⟹ ⟹ 22 323 5234 1 2 ⟹ 3 −− + −2 + − −−− 234 234 = = =
[ [ [
+ + +
+ + +
+ + ]+
] ]
(1.24) (1.25) (1.26)
The simplified equation is gotten:
2
(1.24) + (1.2 (1.25) 5)
+
=± (
) +(
)
(1.27)
The first joint movement, defined by θ1, can be calculated using geometric parameters only: θ1= atan2(y, x)
Now we can calculate calculate θ3, by using equation (1.26): (1.26):
(1.26) 1.26)
2
(1.27) + (1.28)
=
+
(
=
)
=
± (
,
14
=
)
(
)
(1.28)
(1.29)
Chapter 1
Robot kinematics
−− − − + − + − − ⟹ 3 2 3 32 ⟹ 3 3 3 ± (
=
)
(
)
(1.30)
=± 1 =
2( ,
)
(1.31)
After calculate θ3 we can find θ2 by:
2 5 1 11 2 11 2 5 33 2 33 ⟹ 2 5 1 11 2 11 2 5 33 2 33 2 3 4 ⟹ 4 2 3 5 11111 12121 5 11212 12222 1111 123 123 4 123 123 45 15 1212 123 123 4 123 123 45 15 2121 123 123 4 123 123 45 15 2222 123 123 4 123 123 45 15 ⟹ 5 5 5 =
=
2(
,± (
=
=
2(
2(
,± (
) +(
,
+
)
(1.32)
)
) +(
)
)
)
2(
,
+
)
(1.33)
=
+
+
(1.34)
(1.35)
=
We can find θ5 by using total transformation transformation matrix in equation (1.14):
= =
= (
)
+(
)
+
(1.36)
=
(
= (
=
(
)
)
)
+(
)
+(
)
+(
=
)
2( ,
15
+
)
(1.37)
Robot kinematics
Chapter 1
1.4 Conclusion
This chapter described the Kinematic Model of ED-7220C robot arm by deriving its DH parameters. This model makes it possible to control the manipulator in joint space or Cartesian space to achieve any reachable position and orientation; However, the inverse kinematic is a very tedious task because it results in nonlinear equations having sometimes no solution, unique solution or multiple solutions.
16
ROBOT DYNAMICS AND CONTROL
Robot Dynamics And Control
Chapter 2
2.1 Control Technique
There are two well-known control techniques and methodologies that can be applied to the control of manipulators, coupled and decoupled control techniques. The particular control method chosen as well as the manner in which it is implemented can have a significant impact on the performance of the manipulator and consequently on the range of its possible applications. Coupled control technique or multivariable control considers the system as one box of an interconnected chain of ideal rigid bodies with a generalized force acting at the joints. Decoupled control technique or independent joint control considers each axis of the manipulator is controlled as a single-input/single-output (SISO) system. Any coupling effect in independent control technique due to the motion of the other links is treated as a disturbance. The motion of n degree freedom robot manipulator in coupled control technique can be described by the following set of differential equations [1].
()̈ + (, ̇ )̇ + () =
(2.1)
Where the matrix is called the inertia matrix that is a symmetric positive definite matrix, is vector giving the direction of gravity in the inertial frame, and is matrix defined as Christoffel Symbols of the first kind; however, the equation (2.1) is extremely complicated, and it is difficult the determine the matrices. The figure (2.1) d emonstrates control techniques of manipulators.
Figure (2.1): Control techniques analysis of manipulators.
The disadvantages of coupled control technique are: • •
Extremely complicated and difficult to determine the p arameter of the equation (2.1). There are a number of dynamic d ynamic effects that are not included in the equation (friction at the joints).
18
Robot Dynamics And Control •
Chapter 2
The equation does not give detailed analysis of robot dynamics such as elastic deformation of bearing and gears, deflection of the links under load, and vibration.
In this thesis we consider the more practical type of control strategy, namely, decoupled or independent joint control. 2.2 Independent joint control
Various controllers have been designed and applied in the robot manipulator. Feedback control may be the most widely used controller in the industrial and commercial applications for the early decades, due to its simplicity and cost of implementation. However , Feedback control alone in general is not sufficient in the presence of varying disturbances; this is particularly the case in independent robot joint control. Here, the load on a controlled joint is affected by: -
Gravity, which opposes ascending motion and helps descending motion, thus is it seen as varying The configuration and the motion of the rest arm can also be seen as varying disturbance.
As a result, in addition to feedback , we need a second degree-of-freedom (dof) control to compensate for the disturbances, the latter of which is referred to as feed-forward. In summary our independent joint control is 2-dofs (figure (2.2)), specifically, is composed of: 1. Cascade compensator: this is an in-loop compensator and it is used to achieve adequate control performance (steady-state error, transient error). A suitable type is a PID controller. 2. Feedforward compensator: this is an out-of-loop compensator, and it is used to compensated (cancel) for the undesirable disturbances, a special type is demonstrated in the corresponding section.
Figure (2.2): Control structure for a given robot joint
19
Robot Dynamics And Control
Chapter 2
2.2.1 Cascade PID compensation
PID controllers are considered the most control technique that are widely used in control and robotics applications. PID control offers easy ad-hoc methods for tuning its parameters without precise knowledge of the plant model. Their algorithm is explained below. As shown in Figure 2.3, the error signal, e(t ) , is the difference between the set point, r (t ) , and the process output, y(t ) . If the error between the output and the input values is large, then large input signal u(t) is applied to the physical system. If the error is small, a small input signal is used. As its name suggested, any change in the control signal, u(t ) is directly proportional to change in the error signal for a given proportional gain K p . Mathematically the output of the proportional controller is given as follows:
() = ()
(2.2)
Where e(t ) , the error signal and K p is the proportional parameter. Proportional term is not sufficient to be a controller in practical cases to meet a specified requirement (e.g. small overshoot, good transient response) because the large proportional gain gives fast rising time with large overshoot and oscillatory response. Therefore, a derivative term is added to form PD controller, which tends to adjust the response as the process approaches the set point. The output of PD controller is calculated based on the sum of both current error and c hange of error with respect to time. The effects of PD give a slower response with less overshoot than a proportional controller only. Mathematically, PD controller is represented as:
() = () +
()
(2.3)
The main function of the third term; integral control tends to reduce the effect of steady state error that may be caused by the proportional gain, where a smaller integration time result is the faster change in the controlled signal output. The general form of the PID controller in continuous time formula given as:
() = () +
() + ∫0 ( )
(2.4)
Each term of the three components of PID controller, is amplified by an individual gain, the sum of the three terms is applied as an input to the plant to adjust the process. It will be noted that the purely derivative or integral plus derivative variations never used. In all cases except proportional control, the PID compensator gives at least one pole and one zero.
20
Robot Dynamics And Control
Chapter 2
Figure (2.3): Block diagram of closed loop control system
2.2.2 FeedForward disturbance torque cancellation
The notion of feed forward estimator has been introduced as a method to reject time varying but partially known disturbances (here a torque). Suppose that ()is an arbitrary reference Input. The main role of the compensator is estimate and anticipates the torque at different arm’s positions in order to operate the arm. In this thesis an estimator has been applied only to the joints where the effect of the torque is considerable, for example, the effect of the torque on the base and wrist is not considerable, but on shoulder an d elbow is so. To the way the feedforward compensator is designed is illustrated for the ‘shoulder of ED7220C’ joint, the same approach was used for the elbow. First, consider the figure (2.5).
Figure (2.4): Estimator design
of ED-7220C robot arm Figure (2.5): 90° Configuration of ED-7220C
To identify the estimator, the system in figure (2.4) was used to control the shoulder and the following steps have been followed: •
At 90° shoulder’s position as in figure (2.5) we look for the required torque ℎ−1 (90°)that just moves the shoulder.
21
Robot Dynamics And Control •
•
Choosing another position between 90° and 35° (35°< <90°) we look also for the required torque 1 ( ) that just moves the shoulder too. After taking sufficient data of the required torque ℎ−1 ( ) for each shoulder’s st position until reaching 35° we fit this data using interpolation “1 order order polynomial” to have smooth estimator. P
•
Chapter 2
P
The same procedures have been followed when moving the shoulder from 35° to 90° to have second estimator, third estimator when moving the shoulder from 90° to 115°, and the fourth one in its reverse direction.
Table (2.1) shows different shoulder’s position with corresponding estimated torque Table (2.1): Shoulder’s position with corresponding estimated torque
Shoulder’s position from 90° to 35° Torque estimator
=
3 ∗ ( 90) + 539 15
Shoulder’s position from 35° to 90°
Torque estimator
=
71 ∗ ( 90) + 2695 52
Shoulder’s position from 90° to 115° Torque estimator
=
2 ∗ ( 90) + 245 6
Shoulder’s position between 115° to 90°
Torque estimator
=
22
36 ∗ ( 90) + 1225 20
Robot Dynamics And Control
Chapter 2
Identifying the estimator of the elbow follows the same procedures as the shoulder taking in consideration the position of the elbow with respect to the shoulder and elbow’s positions limitations. Table (2.2) shows different elbow’s position with corresponding estimated torque. Table (2.2): Elbow’s position with corresponding estimated torque
Elbow Position Elbow’s position from 90° to -10° Elbow’s position from -10° to 90° Elbow’s position from -10° to -65° Elbow’s position from -65° to -10° Elbow’s position from 90° to 150° Elbow’s position from 150° to 90° Elbow’s position from 150° to 175° Elbow’s position from 175° to 150°
Torque Estimator
90) + 50 50 = 0.12 ∗ ( 90)
=
(59 ∗ ( 90)2 + 8061 ∗ ( 90) 225000 4500
=
=
6 ∗ ( 90) 90) + 1905 13
7 ∗ ( 90) + 1840 60
=
90) + 250 ( 90) 5
90) + 50 = 0.75 ∗ ( 90)
=
2 ∗ ( 90) + 1070
=
25 ( 90) + 415 5
2.3 Conclusion
In this chapter we introduced 2-dofs control to compensate for the disturbances, the first control is cascade compensator that is used to achieve adequate control performance and this compensator is an in-loop compensator. The second control is feedforward compensator this an out of loop compensator used to cancel undesirable disturbances.
23
ROBOT TRAJECTORY & MOTION PLANNIG
Robot Trajectory & Motion Planning
Chapter 3
3.1 Introduction
During robot motion, the robot controller is provided with a steady stream of goal positions and to track. This specification of the robot position as a function of time is called a trajectory. In some cases, the trajectory is completely specified by the task for example, when the task is simply to move the end-effector from one position to another in a given time, we have freedom to design the trajectory to meet these constraints. This is the domain of trajectory planning. The trajectory should be a sufficiently smooth function of time, and it should respect any given limits on joint velocities, accelerations, or torques. The goal of this chapter is to find a trajectory that connects an initial to a final configuration. Without loss of generality, we will consider planning the trajectory for a single joint, since the trajectories for the remaining joints will be created independently and in exactly the same way. Thus, we will concern ourselves with the problem of determining q (t), where q (t) is a scalar joint variable. We suppose that at time t0 the joint variable satisfies
and we wish to attain the values at tf
() ( ̇ ) ( ) ( ̇ )
(3.1)
(3.2)
(3.3)
(3.4)
Figure (3.2) shows a suitable trajectory for this motion. In addition, we may wish to specify the constraints on initial and final accelerations. In this case, we ha ve two additional equations.
( ̈ ) ( ̈ )
(3.5) (3.6)
The desired path is approximated by a class of polynomial functions. It generates a sequence of time- based based “control set points” for the control of manipulator from the th e initial configuration to its destination. Figure (3.1) shows the trajectory planning block diagram.
25
Robot Trajectory & Motion Planning
Chapter 3
Figure (3.1): Trajectory planning block diagram
To generate a smooth trajectory such as that shown in Figure (3.2) is by a polynomial function of t; however, there many trajectory planning methods require polynomial functions with independent variables those methods are explained briefly as follow:
Cubic Polynomial Trajectories: this method requires a polynomial with four independent coefficients that can be chosen to satisfy the constraints. Thus, we consider a cubic trajectory of the form.
()
For distance
(3.7)
Although, a cubic trajectory gives continuous positions and velocities at the start and finish points times but discontinuities in the acceleration. The derivative of acceleration is called the jerk. A discontinuity in acceleration leads to an impulsive jerk, which may excite vibration modes in the manipulator and reduce tracking accuracy.
Quantic Polynomial Trajectories: In this method, we have six constraints (one each for initial and final configurations, initial and final velocities, and initial and final accelerations). Therefore, we require a fifth order polynomial.
()
Figure (3.2): Typical joint space trajectory
26
For distance
(3.8)
Robot Trajectory & Motion Planning
Chapter 3
Linear Segment Method with Parabolic Blends or (LSPB): In this project (LSPB) method was used and found to be suitable and appropriate when a constant velocity is desired along a portion of the path. This method will be explained in details in the next section.
3.2 Linear segment method with parabolic blends (LSPB)
The LSPB trajectory is such that the velocity is initially “ramped up” to its desired va lue and then “ramped down” when it approaches the goal position. To achieve this, we specify the desired trajectory in three parts. The first part from time t 0 to time t b is a quadratic polynomial. This results in a linear “ramp” velocity. At time t b, called the blend time, the trajectory switches to a linear function. This corresponds to a constant velocity. Finally, at time tf − t b the trajectory switches once again, this time to a quadratic polynomial po lynomial so that the velocity is linear.
Figure (3.3): Blend times for LSPB trajectory
( ̇ ) () ̇ ()
We choose the blend time t b so that the position curve is symmetric as shown in Figure (3.3) for convenience suppose that t0=0 and
. Then between times 0 and t b b we have.
So that the velocity is
The constraints q(0)=q0 and
() ̇
() ̇
imply that
27
(3.9)
(3.10)
Robot Trajectory & Motion Planning
Chapter 3
(3.11)
(3.12)
At time t b we want the velocity to equal a given constant, say V. Thus, we have
() ̇
(3.13)
This implies that,
(3.14)
() () ̇ ̈
Therefore, the required trajectory between 0 and t b b with
is given as:
(3.15)
(3.16)
(3.17)
Where α denotes the acceleration. Now, between time tf and and tf - t b, the trajectory is a linear segment (corresponding to a constant velocity V)
()
(3.18)
Since by symmetry,
(3.19)
We have,
(3.20)
Which yields,
Since the two segments must “blend” at time t b we require
28
(3.21)
Robot Trajectory & Motion Planning
Chapter 3
(3.22)
Which gives upon solving for the blend b lend time t b b
Note that we have the constraint
(3.23)
. The leads to the inequality
() ()
(3.24)
The inequality can be written in another way
() ()
(3.25)
Thus, the specified velocity must be between these limits or the motion is not possible. The portion of the trajectory between tf -t -t b and tf is is now found by symmetry considerations. The complete LSPB trajectory is given by
()
(3.26)
Figure (3.4) shows such an LSPB trajectory q0= zero, qf = 40, and t0 =0, tf =1, where the maximum
velocity V = 60. In this case t b= . The velocity and acceleration curves are given in the same Figure.
29
Robot Trajectory & Motion Planning
Chapter 3
Figure (3.4): Trajectory using LSPB
3.3 ED-7220C motion planning
Main task of this project is to send the robot end-effector from an initial position to a goal position taking a minimum path while avoiding obstacle in the environment, env ironment, joint, and torque limits. To achieve this RRT (Rapidly-exploring Random Tree) method has been used; RRT method relies on a random or deterministic function to choose a sample from workspace of ED-7220C. This sample represents the position of the end-effector in xyz plane known as C-space or state space. RRT evaluates whether a sample is in free space (no obstacle) and then determine nearby previous free-space samples; and a simple local planner to try to connect to, or move toward, the new sample. This process builds up a graph or o r tree representing feasible motions of the robot. Sampling methods generally give up on the resolution-optimal solutions of a grid search in exchange for the ability to find satisficing solutions quickly in high-dimensional state spaces. The samples are chosen to form a roadmap or search tree that quickly approximates the free space using fewer samples than would typically be required by a fixed high-resolution grid, where the number of grid points increases exponentially with the dimension of the search space. Most sampling methods are probabilistically complete: the probability of finding a solution, when one exists, approaches 100% as the number of samples goes goe s to infinity. The RRT algorithm searches for a collision-free motion from an initial position of the endeffector to a goal position. The figure (3.6) demonstrates the RRT algorithm applied to ED-7220C and figure (3.7) demonstrates the exploring phase of basic RRT.
30
Robot Trajectory & Motion Planning
Chapter 3
An overview of how the ED-7220C end-effector start from initial position to the goal position taking the minimum path with avoiding obstacle and generation of a trajectory is shown in the figure (3.5)
Environment g eometry
Robot geometr geometry y
Collision detection/ Obstacle distance computation
Initial Initi al pos iti ition on of the end-effector
RRT Search Goal position of the end-effector S o l u t i o n a t h
LSPB Time
Trajectory Planner
qi joint variable variable
Robot
Figure (3.5): Trajectory generation and motion planning diagram
31
Robot Trajectory & Motion Planning
Initialize search tree (T) with initial position (Pinit ) of the end-effector
Whi le tree (T) (T) < the maximum size of the tree tree (Tmax)
No
FAILURE
Yes
Position sample (Psample) from workspace Look for the nearest positi position on (Pnearest) in tree (T) to Psample
Employ Employ a local planner to find a motion from Pnearest to the new position (Pnew)in the direction of P of Psample
No
The motion is collision-free ? Yes
add Pnew to tree (T) with an edge from Pnearest to Pnew
Yes
No
Return Return SUCCESS SUCC ESS and the motion to Pnew
Pnew is a goal ?
Figure (3.6): RRT flowchart
32
Chapter 3
Robot Trajectory & Motion Planning
Chapter 3
Figure (3.7): Basic RRT exploring phase of a planar 5 DOF robot arm
3.4 Conclusion
The goal of this chapter was choosing a suitable trajectory planning method for our robot arm which is LSPB (Linear segment method with parabolic blends) method and then we introduced the RRT (rapidly-exploring random trees) method and applied it on our robot to achieve desirable motion of the arm with avoiding obstacles.
33
RESULTS & DISCUSSION
Results & Discussion
Chapter 5
In this chapter, the details of the practical and simulation results for the hardware design, 3D modeling, Kinematics, and trajectory planning are shown are discussed. 4.1 Hardware implementation:
To achieve a control of a robot arm by using personal computer, we must make the connection between the robot and PC. This connection is called interface connection and it is done by using a microcontroller; however, the Arduino microcontroller is unable to drive the beefy six 24V motors of ED-7220C. For this reason, a power drive circuit have been designed and implemented to control the six motors. To be able to control the motors in both directions three H-bridges were used to control the six motors of ED-7220C. The first H-bridge used to control the base and gripper, the second controls the shoulder and elbow, and the third is reserved for the wrist. Also; three power supplies are designed to deliver 24V to the motors using variable power supply (LM-380). The digital controller Arduino Due was used to read the pulses from encoders’ channels through its interrupt pins. The interrupt pins are faster than the normal digital pins and able to detect the rising edges of the channel (no data is missed). The serial port of the Arduino was used to receive data of joints’ positions gotten from the graphical user interface (GUI) that is designed by Processing. Six PI digital controllers are programmed inside the Arduino to assert six control signals to the H Bridges using PWM pins (V ss(max)= 3.3v). A bridge of diodes was used to protect the the controller from the reversing current from the motors. The overall system is introduced in the following figures:
Figure (4.1): The overall system structure.
Results & Discussion
Chapter 5
4.2 Modeling the ED-7220C robot in ROS:
ED-7220C robot arms was 3D modeled in ROS using the Unified Robot Description Format (URDF), the URDF describes the robot as follows: •
Geometry and inertia of the links including the DH parameters.
•
Joint types (revolute, prismatic…); maximum velocity and their upper and lower limits.
•
Collision detection and 3D visualization.
The final 3D model is show in figure (4.2) and links’ colors identifier are shown in table (4.1).
Figure (4.2): The 3D model of ED-7220C in ROS. Table (4.1): Links’ colors reference.
link color
Base Shoulder Black1+blue1+Green Orange
Elbow Red
Wrist Black2
Gripper Blue2
4.3 Forward kinematics
Mathematical modeling and kinematic analysis of robot arm ED-7220C was carried out in this study. ED-7220C was mathematically modeled with Denavit Hartenberg (DH) method; the Forward Kinematics and Inverse Kinematics are applied. An initial configuration of robot arm is given in the table (4.2) this configuration was obtained by the Forward Kinematic Window (GUI) after moving the angle sliders. The GUI window is shown in figure (4.3)
Angles in degree
1
0.0
2
3
4
Table (4.2): Initial configuration of ED-7220C.
90.0
0.0
90.0
5
0.0
Results & Discussion
Chapter 5
Figure (4.3): Forward Kinematic window for the initial configuration.
The total transformation matrix of the initial configuration is of the form.
50
T =
− − 0 1 0 0
1 0 0 0
0 2.50 0 0. 00 1 96.0 0 1
(4.1)
This matrix gives the initial configuration and orientation of the robot arm. The initial configuration is shown in the figure (4.4).
Figure (4.4): Initial configuration of ED-7220C.
From the matrix (4.1), we find that the (x, y, and z) position of the end-effector is equal to (2.50, 0.00, and 96.0) (cm). Changing the joints positions in GUI window by moving the angle sliders to have a configuration rather than the initial one, the GUI of the selected joints positions is shown in Figure
Results & Discussion
Chapter 5
Figure (4.5): Forward Kinematic window for the selected configuration at position (0.03, -41.58, 19.19).
50
The total transformation matrix T between the base of the robot arm and the end-effectors is of the form.
50
T (fin (final al)) =
− − −− − 0 0.822 0.569 0
1 0 0 0
0.00 0.03 0.569 41.58 0.822 19.19 0 1
(4.2)
From the matrix (4.2), we can find the position of the end-effector equal (0.03, -41.58 and 19.19) (cm). The selected configuration after moving the angle sliders is shown in figure (4.6).
Results & Discussion
Chapter 5
50
The results of the total transformation matrix T were computed and compared with the ones. measured ones. The Table (4.3) summarizes the computed results and the measured ones.
Table (4.3): Computed and measured results.
Position values x y z
Values (cm) 0.03 -41.58 19.19
Measured Values (cm) 1.00 -42.5 20.5
Absolute Error (cm) 0.97 0.92 1.31
The measured (x, y, and z) coordinates of the end-effector position are compared with the computed results and listed in table (4.3). The error in z direction is larger than the error in x or y direction. This error is due to two factors: •
•
The weight of the arm which appears as external torque disturbance in the shoulder, elbow and wrist. The gearing system of the shoulder, elbow and wrist introduce backlash that affects the precision of the arm.
From the equation (2.17), it is noticed that the sum of these errors yield a large error in the height (z axes). By experiment; the dead zones are summarized in the following table: Table (4.4): Motors’ dead zone.
link shoulder elbow wrist
(°)
0.7 0.5 0.5
4.4 Control design:
In this work, we have opted for a Proportional plus Integral controller (PI) as cascade compensation. PI controllers are tuned to control the position of the each joint independently following this experimental iterative technique: 1. Set the Integral and derivative gains to zero. 2. Tune the Proportional gain (K p p ) to improve the transient response, neglecting the steady state error. 3. Adjust the integral gain (K I ) to remove the steady state error which may slightly deteriorate the transient performance 4. Redo the previous steps(2-4) until reaching an satisfactory response. The satisfactory performance is a zero steady state position error (to the limit of the systems’ characteristics) and small percentage overshoot (critically damped) response from the robot arm controllers. The figure (4.7) and (4.8) show the practical (actual) response of the BASE and WRIST joints acquired from Arduino.
Results & Discussion
•
Chapter 5
Setpoint =50°: (Kp =3; Ki=0.05)
Figure (4.7): The actual response of the base joint to 50° setpoint. •
Setpoint = 100°: (Kp = 2; Ki= 0.1)
Figure (4.8): The actual response of the wrist joint to 100° setpoint.
The shoulder and wrist PI controllers operate along aside torque disturbance estimator to reject the disturbance torque on the joints. The figure (4.9) and (4.10) illustrate the change of the disturbance in function of the link position.
Results & Discussion
Chapter 5
Figure (4.10): Shoulder torque rejecter (case1) from 110° to 90°.
Figure (4.11): Shoulder torque rejection (case 2) from 90° to 110°.
For the same operating region (90°-110°); (90°-110°); two estimator and torque rejecter are used since it depends also on direction of the movement: For
2
•
•
= 100°: In fig (4.10); the torque disturbance is found to be T 1 =0.3108 corresponding to (110° to 90°) direction. Where in fig (4.11); the torque disturbance is found to be T 2 =0.1471 corresponding to (90° to 100°) direction.
Cancelling the forces acting on the arm by introducing torque cancelation reduces the complexity of the controllers and classical PI controllers are sufficient. The T 1 and T 2 are normalized quantities.
Results & Discussion
Chapter 5
4.5 Trajectory generation
Straight-line motions are most common in the industrial applications; however, movement on a line is mostly obtained by specifying the discrete time joint displacements at a constant time rate. The velocity and acceleration of the points can be calculated from the numerical approximation of the time derivatives. Several methods were used to compress the describing data of the trajectories as cubic, quantic and LSPB trajectory.
The Linear Segment Parabolic Blend is appropriate when a constant velocity is desired along a portion of the path. The LSPB trajectory is such that the velocity is initially “ramped up” to its desired value and then “ramped down” when it approaches the goal position. The robot arm will be moved to the final position and when that happens we see the LSPB trajectory curves as shown in Figure (4.12). The figure (4.13) is divided into three parts (A, B, and C) and shows the relation between the angle, velocity and acceleration with time.
A) LSPB trajectory curve of the Base joint position .
B) LSPB trajectory curve of the Base joint velocity.
Results & Discussion
Chapter 5
C) LSPB trajectory curves of the Base joint acceleration. Figure (4.12): LSPB trajectory curves of the base
LSPB of the base joint is constrained as follows: •
The initial position q 0 = 0.0 rad correspond to t 0 =0.0 s.
•
The final position q f f = -1.6 rad at t f =2.77s.
•
V (maximum) = 1 rad /s.
•
The blend time is equal to t b =1.15 .
The acceleration in the ramp up is found to be:
α tVb 1115 2 ≤ ≤ �−− − 2 ≤≤ ≤≤ =
=
.
= 0.86 0.869 9 (
/ )
(4.3)
The complete LSPB trajectory of the base joint is given by:
( )=
0.4345 ² 2.18 2.185 5+ 4.93 4.933 3 + 2.40 2.407 7
0.4345
0 1.15 s 1.62 s
LSPB is applied in the other joint of the robotic arm as illustrated below.
1.15 1.62 s 2.77 s
(4.4)
Results & Discussion •
Shoulder joint
A) LSPB trajectory curve of the shoulder joint position.
B) The LSPB trajectory curve of the shoulder joint velocity.
C) LSPB trajectory curve of the shoulder joint acceleration.
Chapter 5
Results & Discussion •
Elbow joint
A) LSPB trajectory curve of the elbow joint position.
B) LSPB trajectory curve of the elbow joint velocity.
C) LSPB trajectory curve of the elbow joint acceleration.
Chapter 5
Results & Discussion
•
Wrist joint
A) LSPB trajectory curve of the Wrist joint position.
B) LSPB trajectory curve of the Wrist joint velocity.
C) LSPB trajectory curve of the Wrist joint acceleration.
Chapter 5
Results & Discussion
Chapter 5
From the graphs, the trajectories have the same: •
The final time t f = 2.77 s.
•
The blend time t b = 1.15 s.
The blend time is found in the joint that has large position error (initial angle –final angle), this can be noticed in the elbow joint where the error is found to be q f =1.745 =1.745 rad. The waypoints pointed in the joint trajectories graphs are summarized in the table below where they are important to execute the generated path in the real robot. Table (4.5): Generated trajectory Waypoints.
waypoints 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
Angles (rad)
Cartesian coordinates (cm) X Y Z
0,00
0,00
0,00
0,00
0,00
2,50
0,00
96,00
-0,07
-0,04
-0,08
-0,02
-0,07
8,21
-0,58
95,67
-0,14
-0,09
-0,16
-0,04
-0,14
13,81
-1,99
94,66
-0,22
-0,13
-0,24
-0,06
-0,22
19,00
-4,15
93,01
-0,29
-0,17
-0,32
-0,08
-0,29
23,75
-7,01
90,71
-0,36
-0,22
-0,40
-0,10
-0,36
27,84
-10,45
87,82
-0,43
-0,26
-0,48
-0,12
-0,43
31,14
-14,28
84,42
-0,50
-0,31
-0,56
-0,14
-0,50
33,64
-18,47
80,48
-0,57
-0,35
-0,63
-0,16
-0,57
35,20
-22,76
76,18
-0,65
-0,39
-0,71
-0,18
-0,65
35,88
-27,05
71,48
-0,72
-0,44
-0,79
-0,20
-0,72
35,67
-31,16
66,48
-0,79
-0,48
-0,87
-0,22
-0,79
34,60
-34,85
61,38
-0,86
-0,52
-0,95
-0,24
-0,86
32,74
-38,11
56,10
-0,93
-0,57
-1,03
-0,26
-0,93
30,19
-40,73
50,82
-1,01
-0,61
-1,11
-0,28
-1,01
27,09
-42,65
45,59
-1,08
-0,65
-1,19
-0,30
-1,08
23,57
-43,78
40,49
-1,15
-0,70
-1,27
-0,32
-1,15
19,82
-44,05
35,62
-1,22
-0,74
-1,35
-0,33
-1,22
15,92
-43,52
31,04
-1,29
-0,79
-1,43
-0,35
-1,29
12,07
-42,15
26,82
-1,36
-0,83
-1,51
-0,37
-1,36
8,39
-40,00
23,04
-1,44
-0,87
-1,59
-0,39
-1,44
5,04
-37,18
19,72
-1,51
-0,92
-1,67
-0,41
-1,51
2,16
-33,80
16,96
-1,58
-0,96
-1,75
-0,43
-1,58
-0,25
-29,90
14,71
Using the robot total transformation (forward kinematic); the waypoints are mapped from the joint space ( ) to the Cartesian space (XYZ) to visualize in 3D. Figure (4.16) shows the resulting path of the arm end-effector.
Results & Discussion
Chapter 5
Figure (4.16): End effector generated path in XYZ plane.
The first point in the path corresponds to the initial position of the robotic arm (X=2.5 cm, Y= 0.0 cm and Z=96 cm) and the final position is ( X=0.0cm, Y= 29.9 cm and Z=14.71cm). The point to point trajectory execution in the real robot with large number of waypoint reduces the deviation error from the generated generate d path. 5.6 Motion planning
In this part; the Robotic operating system, Rviz and planning package Moveit (ompl) are used to perform the motion planning task as follows: •
Obstacle description
ROS uses the Unified Robot Description Format (URDF) to introduce a 3D environment with obstacles. •
Rviz package
Rviz is simulator integrated in ROS, uses the description of the robot in the URDF to create the 3D model of the robot. Also, Rviz introduces physical parameters such as gravity and friction. •
Moveit package
Moveit is defined as planner package with different planning algorithms such as (RRT, RPM…). Moveit consider the robotic arm as one moving group from the base to the end-effector to avoid problem of link collision by defining collision matrix. Moveit has to be connected to Rviz to allow the selected planning algorithm to detect the obstacles and generate the path. The figure (4.17) shows the 3D model of the robotic arm in black color and white box is considered as an obstacle.
Results & Discussion
Chapter 5
Figure (4.17): Planned scene with obstacle.
Pick and place task without obstacle is launched in this section. In figure (4.18) the pick position visualized in (Green) and the place position is visualized in (Orange).
Figure (4.18): Pick and place position in Rviz.
Launching the planned path is shown in the figure (4.19) the black trials are the waypoints of the planning.
Results & Discussion
Chapter 5
In this pick and place application; as shown in figure (4.19); only the base joint is moving and the others remain fixed since the pick height is equal to the place height and no obstacles are introduced in the environment.
Figure (4.20): End-effector curve of the planned path.
pick and place task with obstacle
In the same pick and place configuration; a box of dimensions (x=46.0 cm, y=18.0 cm, z= 46 cm) is introduced as an obstacle in the position (X=-42 cm, Y= 0.0 cm, Z = 23 cm) from its center of mass. Using the RRT algorithm; the obstacle collision is detected and a minimum path is constructed from the pick position to the place position as shown in the figure (4.21) where the black trials are the waypoints of the RRT algorithm.
Figure (4.21): The RRT planned path with obstacle avoidance
Results & Discussion
Chapter 5
The waypoints generated by the RRT algorithm are used by the LSBP to generate the joint trajectories trajectorie s as described in the trajectory traject ory generation and motion planning diagram in figure (4.5). The end-effector generated path is visualized in 3D in figure (4.22).
Figure (4.22): End-effector curve of the planned path with obstacle avoidance.
The generated path is executed successfully in the real robot using point to point technique where the waypoints are sent serially through the serial port to the Arduino. Between two waypoints; a time delay (Td) is selected experimentally to get a smooth path and avoiding the discontinuous motion.
Conclusion In this project report the ED-7220C robot arm was studied. A complete mathematical model of ED-7220C robot is developed including complete Kinematics analyses of the ED-7220C robot arm. Forward and inverse kinematics equations were derived using Denavit-Hartenberg notation. A manipulator position control system was designed based on the independent joints control. Each joint control system consists of PI cascade compensator plus a feedforward disturbance cancellation controller whenever necessary. By using Robotic Operating System; ED-7220C robot was modeled in 3D. The 3D model was then used to perform path planning for the pick and place task, the paths were generated by sampling-based algorithms in environments with obstacles. These paths were then converted into smooth trajectories using Linear Segment Parabolic Blend.
The Graphical User Interface (GUI) was developed for testing motional characteristics of the Robot arm. A physical interface between the ED-7220C robot arm and the GUI was designed and built. A comparison between kinematics solutions of the virtual arm and the robot's arm physical motional behaviors were been accomplished. The execution of the planned trajectories in the real robot was also tested and follow the defined path between the pick and place position in the presence of the obstacles. A future work can be focused on different topics, like the development of different types of controllers such as coupled control by studying the dynamics of the ED-7220C.
References
[1] Mark W. Spong, Seth Hutchinson, M. Vidyasagar,“Robot Modeling and Control”, 1st edition, PP 187-203. [2] O. Bottema and B. Roth, Theoretical Kinematics. Dover Publications, 1990. [3] Mohammed Abu Qassem, Iyad Abuhadrous, Hatem Elaydi, “Modeling and Simulation of 5 DOF Educational Robot Arm”, The 2nd IEEE International Conference on Advanced Computer Control, Shenyang, 20. Dec. 2009. [4] J. J. Craig, “Introduction to robotics: Mechanics and control”, Prentice Hall, 3rd edition. [5] J. Angeles, “Fundamentals of Robotic Mechanical Systems: Theory, Methods, and Algorithms”, 2nd Edition, Springer, 2003. [6] R. Geraerts, “Sampling-based motion planning: Analysis and path quality,” Ph.D. dissertation, Utrecht University, 2006. [7] J. Kuffner, S. Kagami, M. Inaba, and H. Inoue, “Performance benchmarks for path planning in high dimensions,” in JSM Conference on Robotics and Mechatronics, June 2001. [8] G. Wilfong, “Motion panning in the presence of movable obstacles,” in Proc. ACM Symp. Computat. Geometry, 1988, pp. 279–288. [9] R. Alami, T. Simeon, and J.-P. Laumond, “A Geometrical Approach To Planning Manipulation Tasks”, in 5th Int. Symp. Robot. Res, 1989, pp. 113–119. [10] A. D’Souza, S. Vijayakumar, and S. Schaal, “Learning inverse kinematics,” in Proc. IEEE/RSJ Int’l Conf. on Intelligent Robots and Systems, 2001. [11] A.Verman, Vivek A. Deshpande, “End-effector Position Analysis of SCORBOT-ER Vplus Robot”, International Journal of Smart Home, Vol. 5, No. 1, January, 2011 [12] Z. Rymansaib, P. Iravani , M.N.Sahinkaya, “Exponential Trajectory Generation for Point to Point Motions”, 2013 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM) Wollongong, Australia, July 9-12, 2013 [13] Wolfgang A, Pointner, “Sampling-based Motion Planning in Theory and Practice”, Stanford university research report, May 28, 2013 [14] Byeong-Kyu AhnGazebo, “MoveIt!, ros_control을 을을을 을을을 을을을 을 을을”, 2nd Open Robotics Seminar December 22, 2014
54
[15] ROS installation: “http://wiki.ros.org/indigo/Installation/Ubuntu”, February-June 2015. [16] Gazebo website: “http://gazebosim.org/tutorials”, February-June 2015. [17] ROS Wiki website: “http://wiki.ros.org ”, February-June 2015. [18] MoveIt installation :http://moveit.ros.org/install”, ”, February-June 2015.
55
Appendix
APPENDIX A: ROBOT HARDWARE AND SOFTWARE A.1 Hardware A.1.1 H-bridge/Drive circuit
L298 H-bridge is an Integrated circuit used in electronic control of high current load around 4 amperes. These circuits are often used in robotics in robotics and other applications to allow DC motors to run forwards and backwards.
Figure (A.1): L298 IC H-bridge
Most IC H-bridges can control two motors its arrangement is generally used to reverse the polarity/direction of the motor, but can also be used to 'brake' the motor, where the motor comes to a sudden stop, as the motor's terminals are shorted, or to let the motor 'free run' to a stop, as the motor is effectively disconnected from the circuit. The following figure summarizes operation, with S1-S4 corresponding to the diagram.
Figure (A.2): H-bridge structure and its working principle
In our drive circuit three H-bridges were used to control the six motors of ED-7220C. The drive circuit was designed using Proteus software, and then has been printed in double side form cause of the complexity of the circuit. The figure (A.3) illustrates the circuit design in Proteus and its 3D PCB.
56
Appendix
Figure (A.3): The circuit design in proteus and its 3D PCB
A.1.2 Arduino Due Microcontroller
Microcontrollers are designed to interface and interact with electrical/electronic devices, sensors and actuators, and high-tech gadgets to automate systems. Microcontrollers are generally embedded directly into the product or process for automated decision-making. The choice of the Microcontroller is one of the important steps in this project. Arduino Due microcontroller was used and found to be very adequate.
57
Appendix
Figure (A.4): Arduino Due microcontroller
A.1.2.1 Arduino Due Summary
Microcontroller Operating Voltage Input Voltage (recommended) Input Voltage (limits) Digital I/O Pins Analog Input Pins Analog Outputs Pins Total DC Output Current on all I/O lines DC Current for 3.3V Pin DC Current for 5V Pin Flash Memory SRAM Clock Speed Length Width Weight
AT91SAM3X8E 3.3V 7-12V 6-16V 54 (of which 12 provide PWM output) 12 2 (DAC) 130 mA 800 mA 800 mA 512 KB all available for the user applications 96 KB (two banks: 64KB and 32KB) 84 MHz 101.52 mm 53.3 mm 36 g
A.1.3 ED-7220C Robot Arm
ED-7220C has six DC motors of type DME38B50G rated at 24V and equipped with a gearbox with a ratio of 50/1. The motors have quadrature Encoders which are used as feedback sensors to the Arduino to update the Controller Controller with current state “position”. The table (A.5) illustrates different reading of joints’ encoders when move from limit to limit. Table (A.5): Readings of joints’ encoders
Joint
Base
Shoulder
Elbow
Wrist
Gripper
Enc.counts
0-15000
0-3700
0-3400
0-4500 0 -4500
0-1500 0-150 0
58
Appendix The workspace of ED-7220C arm expresses its ability to reach a specific area. So it is defined by the space that the robot can reach. Given the information about the range of motion for each joint of the robot the workspace can be determined. The workspace of ED-7220C is illustrated in figure (A.5) and the range of motion in table (A.6).
Figure (A.5): a. Workspace in XY
b. Workspace in XY
c. Workspace in 3D
Table (A.6): ED-7220C range of motion
Base
Shoulder
Elbow
Wrist pitch
Wrist Roll
-150°/150°
35°/110°
-10°/150°
-130°/130°
-180°/180°
A.2 Software
Software environment can be divided into two parts: the Arduino microcontroller program and Processing Program. In Arduino program, we wrote a code to make the interfacing and control of ED-7220C arm. The Processing program consists of the Serial Communication code and the graphical user interface (GUI) that is used to enter commands. The GUI for ED-7220C robotic arm control was written in Processing program; Processing is a powerful software package that allows for interactive programs with 2D, 3D, or PDF output. The program consists of forward kinematic window (GUI) as shown in figure (A.7).
59
Appendix
Figure (A.7): Forward Kinematic Window
60
