Motion Control of 3 Degree-of-Freedom Direct-Drive Robot. Rutchanee Gullayanon

Transcription

1 Motion Control of 3 Degree-of-Freedom Direct-Drive Robot A Thesis Presented to The Academic Faculty by Rutchanee Gullayanon In Partial Fulfillment of the Requirements for the Degree Master of Engineering School of Electrical and Computer Engineering Georgia Institute of Technology May 005

2 Motion Control of 3 Degree-of-Freedom Direct-Drive Robot Approved by: Dr. David G. Taylor, Advisor School of Electrical and Computer Engineering Georgia Institute of Technology Dr. Bonnie Heck School of Electrical and Computer Engineering Georgia Institute of Technology Dr. George Vachtsevanos School of Electrical and Computer Engineering Georgia Institute of Technology Date Approved: January, 005

3 ACKNOWLEDGEMENTS I am very grateful to my research advisor, Dr. David Taylor, my parents, and my friends who had given me encouragement and support throughout my graduate studies. Dr. Taylor had given me countless advise. When I almost gave up, he was there to encourage me to keep working. My parents, who I have not seen for several years, always called and made sure that I had the support I needed. To all my friends who gave me their companionship when I needed the most. iii

4 TABLE OF CONTENTS ACKNOWLEDGEMENTS iii LIST OF TABLES vi LIST OF FIGURES vii SUMMARY viii I INTRODUCTION II DYNAMICS Homogeneous Transformation Potential Energy Kinetic Energy Equation of Motion III TRAJECTORY PLANNING AND INVERSE KINEMATICS Trajectory Planning Fifth-Order Polynomials Interpolating Polynomials with Continuous Accelerations at Path Points (Spline) Inverse Kinematics Simulations IV MOTION CONTROL: THE THEORY PD Control With Position and Velocity Reference PD Control With Gravity Compensation PD Control With Full Dynamics Feedforward Computed Torque Control V MOTION CONTROL: THE SIMULATION Scenario 1: Perfect Condition Scenario : Pick and Place A Payload VI CONCLUSION APPENDIX A MODELING OF MANIPULATOR iv

5 REFERENCES v

6 LIST OF TABLES 1 Trigonometric Shorthand Manipulator Component Descriptions and Reference Frames Parameters for Transformation Matrices Inertia Matrix with Respect to Base Frame (kg mm ) vi

7 LIST OF FIGURES 1 Rear Isometric View of the Manipulator with Main Components Labeled. 4 Front, Rear, Top View of Coordinate Frames 0, 1,, and Detailed Location of Coordinate Frames 0 and Block Diagram of Inverse Kinematics Algorithm Using Inverse Jacobian Square: Cartesian Trajectory Square: Inverse Kinematics Solution Circle: Cartesian Trajectory Circle: Inverse Kinematics Solution PD Control: End Effector Position in Square Trajectory PD Control: End Effector Position Error in Square Trajectory PD Control with Gravity Compensation: End Effector Position Error in Square Trajectory PD Control with Full Dynamics Feedforward: End Effector Position Error in Square Trajectory Computed Torque Control: End Effector Position Error in Square Trajectory Performance Score Summary for All Controllers in Perfect Condition Performance Score Summary for All Controllers in All Scenarios vii

8 SUMMARY Modern motion controllers of robot manipulators require knowledge of the system s dynamics in order to intelligently predict the torque command. The main objective for this thesis is to apply various motion controllers on a parallel direct drive robot in simulations and verify if one can take advantage of the model knowledge to improve performance of controllers. The controllers used in this thesis varied from simple PD control with position and velocity reference only applied independently at each joint to more advanced PD control with full dynamic feedforward term and computed torque control, which incorporate full dynamic knowledge of the manipulator. In the first part, a thorough study of deriving dynamic equation using Lagrange formulation has been presented as well as the actual derivation of dynamic equations for MINGUS000. Next, in order to prepare proper sets of inputs for the simulations, detailed discussions of end effector trajectory path planning and inverse kinematics determination have been presented. Finally, background theories of various controllers used in this thesis have been presented and their simulation results on the closed-chain direct drive robot have been compared for verification purposes. viii

9 CHAPTER I INTRODUCTION Robot manipulators have been used widely in large number of industries. They are primarily used for repetitive tasks and in hazardous environment. Typical repetitive tasks are usually those involved in assembly line in manufacturing processes. space are some examples of tasks in hazardous environment. Operations in deep sea or No matter what the task is, performance of a robot is usually measured from the quality and quantity of work the robot can do in a given amount of time. Ultimately, the main objective for building and controlling any robot manipulator is to have the end effector moving in designed path with highest accuracy, precision, and speed. Motion controllers need to be designed with most accuracy in commanded torque predicted. In doing so, the question arises whether the mechanical dynamics knowledge offers any possibilities for improvement of control system. The answer is yes. Model based controllers such as computed torque and PD control with full dynamic feedforward term can generate torque commands more intelligently and accurately than simple non-model based controllers such as PD control with position and velocity reference only. Hence, the need for studying dynamics of robot manipulator as well as having a good understanding of various basic motion controller theories are important in designing and controlling motion of robot manipulator to achieve the highest quality and quantity of work. In the initial states of motion controller development, computer simulation is a key to verify the performance of any controllers. Hence, developing an accurate mathematical model of the robot is an important key in performing computer simulation. In many researches and publications regarding motion control of robot manipulators, many assumptions have to be made in order to match the behavior of the physical manipulator to the mathematical model. With the presence of high gear ratio and substantial joint friction in most of the commercial robot manipulators, the mathematical model can be made ideal 1

10 under a lot of assumptions, i.e., under slow operating movement. Direct-drive robot manipulators are increasingly overcoming some of the performance limitations of conventional highly geared robots []. The manipulator dynamics are made ideal by the reduction of joint friction, backlash effects, and the control of joint torque is more feasible. In deriving mathematical model of this type of robot, only a few assumptions have to be made. As a result, direct drive robot is a good candidate for a test bed both for simulation and experiment for many types of controllers especially those that are model based. In performance comparison among any non-model based and model based controllers, simulation can represent a fair comparison since the full knowledge of the manipulator can be assumed and both types of controllers can be operating under optimal conditions. As part of this thesis, two main categories of motion controllers are implemented and tested for performance comparison on a direct drive manipulator; PD and computed-torque controllers. In addition, several sub-categories of PD controllers varied from simple PD control with reference position and velocity only to more intelligent PD control with full dynamic feedforward terms are studied and applied. The kinematics and dynamics of a five-bar parallel robot manipulator will be extensively studied and derived at the beginning part of the thesis. Using the trajectory planning and inverse kinematic algorithm suggested by Sciavicco and Siciliano, two geometric shapes are planned for the manipulator to draw in workspace and the corresponding desired joint angles are obtained and stored off-line. Finally, motion simulations with all controllers are performed, then the performance results obtained are compared.

11 CHAPTER II DYNAMICS Derivation of the dynamic model of any robot manipulator plays an important role in simulation, analysis of manipulator structures, and implementation of control algorithms. Simulation needs the mathematical model to represent the physical system to verify its behavior before any real experiments can be done. These dynamics can also be used to verify any limitations in which the robot can perform a certain task, such as the maximum joint acceleration that can still produce accurate precision and possible posture for end effector. In more advanced control algorithms, dynamic model of the system is incorporated into the controller to improve its performance. In this thesis, Mingus000 is used in motion simulation. This manipulator is a 3 degreeof-freedom manipulator custom designed and built by Courtney James in 1995 [4]. In this chapter, the dynamic model of this manipulator is generated using Lagrange formulation. The most important advantage of using Lagrange formulation in deriving dynamic model is that all the equations of motion can be derived in a systematic way independently of the reference coordinate frame. The isometric view of this manipulator is presented in Figure 1. It employs a parallel drive mechanism with closed-loop kinematic chain introducing some advantages over general serial-link manipulator. One advantage for this type of closed-loop kinematic chain design is that all the heavy motors are placed at the base frame and the drive torque is transmitted through the parallel links. This type of drive mechanism reduces arm weight significantly and hence improves the dynamic performances. Another advantage for using the direct-drive approach is that an electrical drive with no gear reducer is used. Hence the direct-drive approach is known to eliminate mechanical gearing completely, which means that the backlash is completely removed and friction is reduced significantly. In analyzing robotic manipulator, simple trigonometric expressions appear repeatedly. To simplify and shorten these expressions, some short hand notations are used as shown in 3

12 Table 1: Trigonometric Shorthand Notation Meaning C i cos(θ i ) S i sin(θ i ) C ij cos(θ i + θ j ) S ij sin(θ i + θ j ) C i j cos(θ i θ j ) S i j sin(θ i θ j ) Figure 1: Rear Isometric View of the Manipulator with Main Components Labeled Table 1..1 Homogeneous Transformation In all real life applications using robot manipulators, it is important to be able to describe the position of the end effector in one global coordinates. In translating the coordinate of the end effector from the local to the global (base) frame, the manipulator can be represented as a series of kinematic chains of rigid links. Each link can be related by defining a proper transformation matrix relating the positioning of the current link to the previous link. In 4

13 the following section, derivation and definition used in defining the transformation matrices is presented. In deriving the transformation matrices, proper frames have to be assigned to each link first. Since assigning frames to local links are not unique, a specific set of framing system is used in this thesis in order to generate the transformation matrices. Figure, 3, and Table summarize each component of the manipulator, the center of gravity, and the local reference frame for each link respectively. The transformation matrices relate coordinates from local frame to one global coordinate frame. This is for the convenience of describing the location and orientation of the end effector for any given task in its workspace. Throughout this thesis, the transformation matrix of frame i with respect to the previous frame i 1 is given in the following form T i i 1(q i ) = C θi S θi C αi S θi S αi o i i 1 (x) S θi C θi C αi C θi S αi o i i 1 (y) 0 S αi C αi o i i 1 (z) (1) where the numerical data for each link can be found in Table 3. Although the local coordinate frames for this manipulator are set up according to the Denavit-Hartenberg convention, parallel links require an extra step to generate the correct transformation matrix (i.e., Link ).. Potential Energy The general potential energy for each link can be expressed in the following form n U = (U li + U mi ) () i=1 In previous work during the construction of the robot manipulator [4], numerical data for the motor and the link for each section was considered as one rigid body. Hence, for the simplicity of the robot dynamics derivation, the mass of motor is included in link mass. Hence the potential energy of the system only involves the potential energy of the links, which becomes U li. For rigid link manipulator as in this case, the general form of U i can 5

14 Figure : Front, Rear, Top View of Coordinate Frames 0, 1,, and 3 Figure 3: Detailed Location of Coordinate Frames 0 and 1 6

15 Table : Manipulator Component Descriptions and Reference Frames 7

16 Table 3: Parameters for Transformation Matrices o i i 1 (mm) Frame i α i θ i o i i 1 (x) oi i 1 (y) oi i 1 (z) 1 0 θ θ θ 1 θ (θ 1 θ ) (θ 1 θ ) then be written as U i = m i g T p 0 l i (3) [ ] T where g 0 is the gravity acceleration vector in the base frame (i.e., g 0 = 0 0 g ) and the vector p li is the distance from the reference frame to the center of mass for link i. Numerical data for each link s center of mass with respect to its local coordinate frame, P ci, can be found in Table. Notice that p li 1 = T0 i P ci 1 (4) and P ci consists of P ci (x), P ci (y), and P ci (z) representing the x, y, and z components of the center of mass location. Hence the potential energy of each link can be easily calculated using the homogeneous transformation matrices from previous section. For the simplification of the system, the reference plane for the potential energy is chosen to be the horizontal plane that is parallel to the xy plane of frame 0 and 1 and through the center of gravity of link 0. Based on this reference frame, the potential energy for link 0 is zero. Hence, the final form of the potential energy is as follows: U = [ ] m 1 g [P c1 (x)s P c0 (z)] + m g P c (x)s 1 + o 3 (x)s P c0 (z) [ ] +m 3 g P c3 (x)s P c3 (y)c + o 4 3(x)S 1 + o 3 (x)s P c0 (z) [ ] +m 4 g P c4 (x)s 1 o 5 4(x)S + o 4 3(x)S 1 + o 3 (x)s P c0 (z) [ ] +M e g P ce (x)s P ce (y)c + o 4 3(x)S 1 + o 3 (x)s P c0 (z) (5) 8

17 .3 Kinetic Energy The general form for the kenetic energy for each link is in the following form: n T = (T li + T mi ) (6) i=1 Similar to the calculation of potential energy, the link and motor for each section are considered as one. Hence the kinetic energy for each link can be expressed in the form T i = 1 m i q T J (l T i) (l P J i ) P q + 1 qt J (l T i) O Ri Il i i Ri T J (l i) P q (7) where R i is the rotational matrix of frame i. In another words, R i is the 3 3 submatrix in the upper left portion of the transformation matrix T i. From the definition of geometric Jacobian, ṗ J P = ω J O q (8) Note that both ṗ and ω are with respect to the local coordinate frames. There also exists a relationship between the inertia tensor relative to the center of mass of Link i expressed in local frame, I i l i, and one expressed in the base frame, I li, as follows: I li = R i I i l i R T i (9) The inertia for each link with respect to the base frame is a symmetric matrix which is expressed in a form I li = I li xx I li xy I li xz I li yy I li yz I li zz (10) Table 4 summarizes numerical values for each link s inertia property. Finally, the kinetic energy equation for each link becomes T i = 1 m iṗ T ṗ + 1 ω it I li ω i (11) where ṗ is the time derivative of P ci found in the previous section and ω i is the angular velocity of link i with respect to the base frame. 9

18 Table 4: Inertia Matrix with Respect to Base Frame (kg mm ) Link i I li xx I li yy I li zz I li xy I li yz I li xz Equation of Motion There are several approaches which can lead to the dynamic model of the manipulator. The Lagrange formulation is used in this thesis. The most important advantage of the Lagrange formulation is that the equation of motion for the system can be derived systematically and independently from the reference coordinate frame. The function of generalized coordinates is L = T U (1) The Lagrange s equations are expressed by d L L = ξ i i = 1,..., n (13) dt q i q i where ξ i is the generalized force associated with link i. From the total potential and kinetic energy calculated previously, the equation of motion for each link can be derived using the Lagrange s equation. The final dynamic model of the manipulator is M(θ) θ 1 θ θ 3 + B(θ) θ 1 θ θ 1 θ 3 θ θ 3 + C(θ) θ 1 θ θ 3 + G(θ) = τ 1 τ τ 3 (14) where M(θ) is a 3 3 symmetric mass matrix of the manipulator, B(θ) is a 3 3 matrix of Coriolis coefficients, C(θ) is a 3 3 matrix of centrifugal coefficients, and G(θ) is an 3 1 vector of gravity terms. Detailed expressions for each matrix is shown in Appendix A. 10

19 Although the full dynamic modeling representation for any robot manipulators can be written as M(q) q + V (q, q) q + F v q + F s sgn( q) + G(q) = τ (15) where V (q, q) q = B(θ) θ 1 θ θ 1 θ 3 θ θ 3 + C(θ) θ 1 θ θ 3 (16) Both the static and dynamic friction terms in the robot model are ignored in this thesis because of the lack of knowledge of friction coefficients. Hence, the simplified dynamic equation that is used for the rest of the thesis is in the form M(q) q + V (q, q) q + G(q) = τ (17) 11

20 CHAPTER III TRAJECTORY PLANNING AND INVERSE KINEMATICS Before any simulation can be done using the proposed controllers, a set of desired trajectories should be planned to prevent any sudden changes of torque required to control the manipulator. It is worth remarking that task specification, i.e. end effector motion, is usually carried out in operational space. However, control actions are performed in joint space. This fact naturally leads to consideration of two alternatives, transformation of desired operational space task into joint space coordinates or applying operational space control scheme. In this thesis, the joint space controllers are applied. Hence, the inverse kinematics is needed in order to transform the end effector coordinates from workspace to joint space. For demonstration purposes, two types of trajectories are planned for the manipulator to follow; square and circle. In Chapter, the direct kinematics problem is presented, the forward kinematics of the manipulator is carefully derived to determine the end-effector position given joint angles. Similarly, finding inverse kinematics solution involves solving sets of equations from forward kinematics and determining the joint angles given end effector position. The fact that this set of equations is generally nonlinear, sometimes there might be an admissible solution (considering the geometry of the manipulator), and sometimes there might be more than one solution, makes the inverse kinematics problem very complicated. In the second part of this chapter, an inverse kinematics algorithm is introduced to transform workspace coordinates into joint space coordinates without actually solving the forward kinematics equations. This method is then applied to desired workspace trajectories produced by trajectory planning in order to generate off-line sets of desired joint position, velocity, and acceleration profiles which can be used as inputs to all motion controllers. 1

21 3.1 Trajectory Planning In controlling the manipulator using any types of joint space controllers, any sudden changes in desired joint angle, velocity, or acceleration can result in sudden changes of the commanded torque. This can result in damages of the motors and the manipulator. Careful sets of pre-planned paths are required in order to prevent these hazardous conditions. As part of this thesis, all desired path patterns are divided into two categories and therefore two types of path planning schemes are introduced through this research to minimize the complexity of the calculation; fifth-order polynomials for straight lines (point-to-point motion) and interpolating polynomial for curvatures Fifth-Order Polynomials Fifth-order polynomials are chosen to generate end-effector trajectories of any straight lines in manipulator workspace (point-to-point motion). Assumption for producing this type of trajectory is that the user only has to specify a set of initial and final locations in workspace for the manipulator to move from and to. Desired end-effector path will be generated in such a way that it ensures zero initial and final velocity and acceleration as well as the smoothness of all three qualities. The path is completed within a given time interval T. This is a relatively mathematically simpler and computationally less demanding type of trajectory compared to spline, which is introduced in the next section for a more complex type of path planning. Let p(t), ṗ(t), and p(t) be the 3 1 matrices representing the x, y, and z components of position, velocity, and acceleration of the end-effector in workspace at any given time t. The general form of the end effector position, velocity, and acceleration profiles are as follows: p(t) = a 5 t 5 + a 4 t 4 + a 3 t 3 + a t + a 1 t + a 0 (18) ṗ(t) = 5a 5 t 4 + 4a 4 t 3 + 3a 3 t + a t + a 1 (19) p(t) = 0a 5 t 3 + 1a 4 t + 6a 3 t + a (0) The coefficients a 1, a,... and a 5 are 3 1 matrix coefficients for the polynomials. By setting 13

22 up the boundary condition equation to have zero initial and final velocity and acceleration, the following boundary condition equations can be applied: p(0) = p start (1) ṗ(0) = 0 () p(0) = 0 (3) p(t ) = p stop (4) ṗ(t ) = 0 (5) p(t ) = 0 (6) p start and p stop are the 3 1 matrices indicating the starting and ending position of the endeffector for any given section of straight line. By solving Equation 18 to 6, the coefficients of the polynomials can be solved as the following a 0 = p start (7) a 1 = ṗ start = 0 (8) a = p start = 0 (9) a 3 = 0p stop 8T ṗ stop + T p stop 3 p start T 1T ṗ start 0p start T 3 (30) = 10 p stop p start T 3 (31) a 4 = 4T p stop + 8ṗ stop + 6T p start + 3ṗ start 60 T (p stop p start ) 4T 3 (3) = 15(p start p stop ) T 4 (33) T a 5 = ( p stop p start ) 3T ṗ stop + 6p stop 3T ṗ start 6p start (34) T 5 = 6(p stop p start ) (35) T 5 Hence each section of a desired straight line end-effector path can be generated using the fifth-order polynomial as mentioned in Equation 18. In generating curvature trajectories, desired paths are usually described in terms of a sequence of points along the curves for the manipulator to follow. In this case, besides zero initial joint velocity and acceleration, it is also necessary to make sure that the end-effector 14

23 will go through all points on the curve with continuous velocity and acceleration. In the following section, a second type of path planning scheme is introduced for curvature type paths Interpolating Polynomials with Continuous Accelerations at Path Points (Spline) Similar to the straight line type motion control, the assumption of zero initial and final velocity and acceleration is applied in the curvature case as well as some additional restrictions. In curvature motion planning, it is necessary to make sure that the end effector passes through a sequence of N points within a specific time interval T with continuous joint velocity and acceleration. Spline is the most suitable type of trajectory to use in this case since it guarantees that the trajectory passes through all N points and ensures the continuity of position, velocity, and acceleration between all path points along the curvature. Spline divides the desired end-effector path into N sections with corresponding time in which the end-effector should reach by. The end-effector should complete each section of path profile Π k at the time instant t k and the final generated path profile should be completed within a specific time T. The subscript k indicates the section number. Path position, velocity, and acceleration profiles in each section of the spline are represented by the third, second, and first-order polynomials Π k (t), Πk (t), and Π k (t) respectively, which are generalized in the following form: Π k (t) = a k t 3 + b k t + c k t + d k (36) Π k (t) = 3a k t + b k t + c k (37) Π k (t) = 6a k t + b k (38) In order to ensure the continuity in position, velocity, and acceleration at each end point, the following equations have to be satisfied: Π k 1 (t k ) = p k (39) Π k 1 (t k ) = Π k (t k ) (40) Π k 1 (t k ) = Π k (t k ) (41) 15

24 Π k 1 (t k ) = Π k (t k ) (4) The system for the N path points, including the initial and final points, consists of 4(N ) equations for the intermediate points and 6 equations for the boundary conditions, that is Π 0 (t 1 ) = p start, Π0 (t 1 ) = 0, Π 0 (t 1 ) = 0, Π N (t f ) = p stop, ΠN (t f ) = 0, and Π N (t f ) = 0. In general, one has 4N equations in 4(N 1) unknowns. In order to solve this system of equations, two virtual points are introduced for which continuity constraints on position, velocity, and acceleration can be imposed without specifying the actual position. The effective location of these virtual points is irrelevant, since their position constraints regard continuity only. Hence, the introduction of two virtual points implies the determination of N + 1 cubic polynomials. Consider N + time instants t k, where t and t N+1 refer to the two virtual points. The system of equations for determining the N +1 cubic polynomials can be found by taking the 4(N ) equations for the intermediate points, 6 equations for the initial and final points, and 6 equations for two virtual points. The overall system results in 4(N + 1) equations with 4(N + 1) unknowns which can be set up as the following: For k = 3, 4,..., N, written for the N + intermediate path points: Π k 1 (t k ) = p k (43) Π k 1 (t k ) = Π k (t k ) (44) Π k 1 (t k ) = Π k (t k ) (45) Π k 1 (t k ) = Π k (t k ) (46) For the initial and final points, 6 boundary equations have to be satisfied: Π 1 (t 1 ) = p start (47) Π 1 (t 1 ) = ṗ start = 0 (48) Π 1 (t 1 ) = p start = 0 (49) Π N+1 (t N+ ) = p stop (50) Π N+1 (t N+ ) = ṗ stop = 0 (51) Π N+1 (t N+ ) = p stop = 0 (5) 16

25 For the two virtual points, k =, N + 1, the following 6 equations have to be fulfilled: Π k 1 (t k ) = Π k (t k ) (53) Π k 1 (t k ) = Π k (t k ) (54) Π k 1 (t k ) = Π k (t k ) (55) Rearrange the 4(N + 1) linear equations to be in the form: A a 1 b 1 c 1 d 1 a. d N+1 = B (56) where present a nonsingular 4(N + 1) 4(N + 1) coefficient matrix A and a 4(N + 1) 1 vector of known values B. The coefficients of the N + 1 cubic polynomials can be solved and the position, velocity, and acceleration profiles of the curvatures can be generated using Equations 36 to Inverse Kinematics There are generally three different approaches to solve the inverse kinematics problem. The first approach is to solve the direct kinematics equations algebraically. The second approach is the geometric approach, where one can relate some important point of the structure to the end effector s position. The first two approaches are called the closed-form solution. The last approach is the numerical approach, in which the solution of the inverse kinematics problem is estimated, compared, and recalculated until the error falls below a certain threshold. This approach is generally computationally demanding. On the other hand, it can be applied to any manipulator structure and is guaranteed to have the same accuracy of the solution. In this research, an inverse kinematics algorithm introduced by Sciavicco and Siciliano is presented and applied to solve inverse kinematics. 17

26 There are two inverse kinematics algorithms suggested by Sciavicco and Siciliano, Jacobian (Pseudo-) Inverse and Jacobian Transpose kinematics algorithms. There are advantages and disadvantages for each type of algorithm. Transpose inverse kinematics is computationally less demanding. However, the algorithm can only guarantee asymptotic stability if the desired signal is not time varying. Jacobian inverse algorithm is computationally more demanding, but it guarantees asymptotic stability of the system even with a desired path that is time-varying. In this thesis, the Jacobian (Pseudo-) Inverse kinematics algorithm is discussed and applied to the desired end-effector motion generated in the previous section in order to obtain the corresponding desired joint angle profiles. Assume that the analytical Jacobian matrix J A is square and nonsingular, the choice q = J 1 (q)(ṗ + Ke) (57) A d and let error signal e be defined as e = p d p = p d k(q) (58) where k(q) is the forward kinematics function of the manipulator. Hence system in Equation 57 can lead to the equivalent linear system ė + Ke = 0 (59) If K is a positive definite matrix, the system in Equation 59 can be proved to be asymptotically stable. The error tends to zero along the trajectory with a convergence rate that depends on the eigenvalues of matrix K. The larger the eigenvalues, the faster the convergence. Depending on the sampling time, there is a limit for the maximum eigenvalue of K under which asymptotic stability of the error system is guaranteed. The block scheme corresponding to the Jacobian Inverse kinematics algorithm can be shown as in Figure 4. For a constant reference (ṗ = 0), the algorithm guarantees a null steady-state error. Furthermore, the feedforward action provided by ṗ for a time-varying reference ensures that the error is kept to zero (in the case e(0) = 0) along the whole trajectory regardless of the type of desired reference signal. 18

27 Figure 4: Block Diagram of Inverse Kinematics Algorithm Using Inverse Jacobian In order to avoid noises and other unwanted signals, solution for joint velocities and accelerations cannot be calculated directly by taking the first and second derivatives of the joint position profiles produced by inverse kinematics algorithm. Recall that the translational velocity of the end-effector frame can be expressed as ṗ = J P (q) q (60) Given the joint position profiles, the corresponding joint velocity can be calculated using the following formula: q = J 1 (q)ṗ (61) P The relationship between the joint acceleration and the end-effector acceleration can be found by taking derivative with respect to time in Equation 60. Hence the solution to the joint acceleration can be calculated using Equation 6: 3.3 Simulations q = J 1 P (q)( p J P (q) q) (6) To demonstrate trajectory planning and inverse kinematics theories discussed above and to prepare a set of suitable input signals for motion controllers, several types of geometric shapes are designed within the manipulator workspace. These trajectories are generated 19

28 Figure 5: Square: Cartesian Trajectory using fifth-order polynomials, spline, or combination of both theories. Consider the geometric shape of a square path in manipulator s workspace as shown in Figure 5, it only consists of segments of straight lines. Hence, in developing the trajectory of a square for the manipulator, only fifth-order polynomials are used. The trajectory is started by holding the end effector in the home position, p home = [ ] T, for 3 seconds. This can be considered as the initialization phase where the motion controller is first turned on. This first 3 seconds should give the controller enough time to go through the transient state and settle down. For the next 3 seconds, the end effector is asked to move from the home position to the starting point of the square, p 1 = [ ] T. To complete the square path, the end effector is planned to move along the path and stop at all four corners, p 1 = [ ] T, p = [ ] T, p 3 = [ ] T, and p 4 = [ ] T. Each section of the square should be complete within 3 seconds. Finally, the end effector is moved back to the home position and held there for 3 seconds so that the controller will have to finish up before it is turned off. During the path planning for this square trajectory, each segment of straight line movement is developed using fifth-order polynomials and the final collections of trajectory in workspace is obtained. The solutions to the inverse kinematics problem are programed and calculated in Matlab 0

29 Simulink with the maximum allowable error of 10 9 meters. The resulting joint positions are then used to calculate corresponding joint velocities and accelerations. These results are plotted in Figure 6. The joint velocity and acceleration can go up to approximately 50 deg/s and 600 deg/s which occur during the movement from home position to the starting position of the square. These values can vary depending on the time interval allowed for the trajectory to complete a given segment of the path. Note that there are no sudden changes, in another words, there are no glitches in any of the inverse kinematics solutions which provide the desired pattern of the solution. Another example of a trajectory that consists of both straight line segments and curvatures is a circular trajectory. The end effector path is shown in Figure 7. Using the same starting state as in the square trajectory case, the end effector is held at the home position for 3 seconds. Then it is moved to the starting coordinate of the circle, p 1 = [ ] T, in the same amount of time. This is a straight line segment same as in the square trajectory case, which will be planned using the fifth-order polynomials. Next the end effector is moved along the circular path with the radius of 0.54 meters and stopped when it reaches the starting point again. This section of the path is designed to be completed in 1 seconds and is planned using spline. Finally, the end effector is returned back to the home position and held in position there for 3 seconds giving the controller enough time to settle down any transient response. Using the same method of solving for inverse kinematics problem as in the square case, the corresponding solution to the circular trajectory can be shown as displayed in Figure 8. The peak joint velocity and acceleration for the circular trajectory is the same as in the square case since the peaks occur during the movement of end effector from home position to the start point. 1

30 Figure 6: Square: Inverse Kinematics Solution

31 Figure 7: Circle: Cartesian Trajectory 3

32 Figure 8: Circle: Inverse Kinematics Solution 4

33 CHAPTER IV MOTION CONTROL: THE THEORY In previous chapters, theoretical work regarding the robot manipulator s dynamics is established. Trajectory planning techniques have been presented in order to generate sets of proper reference inputs to the motion control system. Here, several flavors of PD controller theories are presented. Some of them are non-model based controllers. They represent simple motion controllers without any system modeling knowledge. Next, more advanced PD controllers incorporating some modeling knowledge, and computed torque controllers, are studied and used. These controllers represent more intelligent model based motion controllers. The purpose here is to investigate performance of all controllers mentioned in this chapter to see what advantage modeling knowledge can introduce to these controllers. If it does give any advantages, another purpose is to see if only partial or full dynamic knowledge is needed for the best performance controller. In this chapter, control theories used in this thesis are introduced to identify any advantages modeling knowledge can give. In many cases, partial or full dynamic knowledge of the robot manipulator is involved. Recall that the simplified dynamics of any robot manipulator is written as M(q) q + V (q, q) q + G(q) = τ (63) which is derived in the earlier chapter. Partial or full dynamic information from Equation 63 is used in developing those controllers that are model-based. 4.1 PD Control With Position and Velocity Reference The simplest and most common form of robot control is independent joint PD control, described by τ = K D ( q d q) + K P (q d q) (64) 5

34 where q d and q d are the desired joint velocities and positions, and K D and K P are 3 3 diagonal matrices of velocity and position gains. This is the most common controller used in industrial robot control, partly because the controller is easy to implement and it doesn t require any knowledge of the robot parameters. Although this type of controller is suitable for real-time control since it has very few computations compared to the complicated nonlinear dynamic equations, there are a few downsides to this controller. It needs high update rate to achieve reasonable accuracy. Using local PD feedback law at each joint independently does not consider the couplings of dynamics between robot links. As a result, this controller can cause the motor to over-work compared to other controllers presented below. 4. PD Control With Gravity Compensation This is a slightly more sophisticated version of PD control with a gravitational feedforward term. Consider the case when a constant equilibrium posture is assigned for the system as the reference input vector q d. It is desired to find the structure of the controller which ensures global asymptotic stability of the above posture. The determination of the control input which stabilizes the system around the equilibrium posture is based on the Lyapunov direct method. Take the vector [ˆq T q T ] T as the system state, where ˆq = q d q (65) represents the error between the desired and the actual posture. Choose the following positive definite form as Lyapunov function candidate: W ( q, ˆq) = 1 qt M(q) q + 1 ˆqT K P ˆq > 0 q, ˆq 0 (66) where K P is 3 3 symmetric positive definite matrix. An energy-based interpretation of Equation 66 reveals a first term expressing the system kinetic energy and a second term expressing the potential energy stored in the system of equivalent stiffness K P provided by the 3 position feedback loops. 6

35 Differentiating Equation 66 with respect to time, and recalling that q d is constant, yields Ẇ = q T M(q) q + 1 qt Ṁ(q) q q T K P ˆq (67) Solving Equation 63 for M(q) q and substitute it in Equation 67 gives Ẇ = 1 qt (Ṁ(q) V (q, q)) q qt (τ G(q) K P ˆq) (68) The first term on the right hand side is null since the matrix N = property q T N(q, q) q = 0. The choice Ṁ V satisfies the τ = G(q) + K P ˆq (69) describing a controller with compensation of gravitational terms and a proportional action, leads to a negative semi-definite V since V = 0 (70) This result motivates the control law τ = G(q d ) + K D ( q d q) + K P (q d q) (71) with K D positive definite, corresponding to a nonlinear compensation action of gravitational terms with a linear proportional-derivative action. Although this controller seems to be able to guarantee a perfect controlling result, it only is proven with constant reference trajectories. With varying desired trajectories, this type of controller cannot guarantee perfect tracking performance. Hence, more dynamic modeling information is needed to incorporate into the controller. 4.3 PD Control With Full Dynamics Feedforward This next type of controller assumes the full knowledge of the robot parameters. The key idea for this type of controller is that if the full dynamics is correct, then the resulting torque generated by the controller will also be perfect. The controller is in the form τ = M(q d ) q d + V (q d, q d ) q d + G(q d ) + K D ( q d q) + K P (q d q) (7) 7

36 If the dynamic knowledge of the manipulator is accurate, and the position and velocity error terms are initially zero, then the applied torque τ = M(q d ) q d + V (q d, q d ) q d + G(q d ) (73) is sufficient to maintain zero tracking error during motion. This controller is very similar to the computed torque controller, which is presented next. The difference between these two controllers is the location of the position and velocity correction terms. This controller is less sensitive to any mass changes in the system. For example, if the robot picks up a heavy load in the middle of its operation, this controller is likely to respond to this change slower compared to computed torque controller. 4.4 Computed Torque Control Computed-torque control is a well known motion controller that takes advantage of the system s dynamic knowledge. The controller computes the dynamics on-line, using the sampled joint position and velocity data. The key idea is to find an input vector, τ, as a function of the system states, which is capable to realize an input/output relationship of linear type. It is desired to perform not a local linearization but a global linearization of system dynamics obtained by means of a nonlinear state feedback. Taking the control τ as a function of the manipulator state in the form τ = M(q)y + V (q, q) q + G(q) (74) leads to the system described by q = y (75) where y is a new input vector whose expression is to be determined. With the choice of τ in Equation 74, the manipulator control problem is reduced to that of finding a stabilizing control law y. To this purpose, the choice y = K P q K D q + r (76) leads to the system of second-order equations q + K D q + K P q = r (77) 8

37 which, on the assumption of positive definite matrices K P and K D, is asymptotically stable. Given any desired trajectory q d (t), tracking of this trajectory for the output q(t) is ensured by choosing r = q d + K D q d + K P q d (78) Substituting Equation 78 into 77 gives the homogeneous second-order differential equation ˆq + KD ˆq + KP ˆq = 0 (79) expressing the dynamics of position error Equation 65 while tracking the given trajectory. The computed torque controller consists of two feedback loops; an inner loop based on the manipulator dynamic model and an outer loop operating on the tracking error. The function of the inner loop is to obtain a linear and decoupled input/output relationship, whereas the outer loop is required to stabilize the overall system. Notice that the implementation of this controller requires computation of the robot dynamic on-line. If the manipulator dynamic is correct, the error signals become zero, then the controller becomes τ = M(q d ) q d + B(q d, q d ) q d + G(q d ) (80) In real life applications or computer simulations, disturbances from many sources can contribute to worse than theoretical performance result. Although computer simulation should have much fewer disturbances compared to real experiments, factors such as the integration estimation and sampling rate can cause the controllers to behave differently than the mathematical prediction. Hence, computer simulation is the first step to verify the performance of all the controllers above. 9

38 CHAPTER V MOTION CONTROL: THE SIMULATION In real life applications, various instrument inaccuracy and environmental disturbances can cause the controllers to behave differently from the theory prediction. To investigate each controller s performance, computer simulation is used in this thesis. The robot dynamic model, which was developed earlier, is constructed. The reference signals for different types of motion are generated and stored off-line. The simulation is an ideal way of comparing performance of various motion controllers. Although it can contain some errors in mathematical approximation, most of disturbances can be ignored and full knowledge of the manipulator dynamics can be assumed. Hence, in all motion controllers especially those that are model-based, the optimal performance of each controller can be obtained and compared. In this thesis, the main purpose for developing the motion controllers is to obtain a good trajectory tracking capability. Hence a performance score is defined to compare the tracking capability of each controller. Let the tracking performance score, P, be defined as the RMS values of the end effector error. The end effector error is defined as E xyz = e x + e y + e z (81) where e x, e y, and e z are the position errors in x-, y-, and z-axis given in manipulator s workspace coordinates. To properly measure the size of this error signal over time, the L norm function is introduced. Hence the tracking performance score can be defined as P = e L = (E xyz ) = (e x + e y + e z ) (8) The simulation in this thesis is carried out in Matlab using the Dormand-Prince algorithm to perform the integration. The system is sampled in the rate of 1 khz. During the simulation, both static and dynamic frictions are neglected. The nominal position and velocity gains are adjusted to achieve high stiffness and critically damped characteristics. 30

39 It can be shown that in order to obtaine the critically damped response, the diagonal position and velocity gain choices of K P = ω n (83) K D = ω n (84) are used, where ω n is the undamped natural frequency, which is the only value that is adjusted in order to changed the position and velocity gains. In performing the simulation in this chapter, several scenarios are considered giving all the controllers a fair chance to compete with each other. Hence, two different scenarios are shown in this thesis to give a bigger picture of the advantages and disadvantages, if any, each controller presents. Some of these scenarios below involved picking and placing a payload. To simplify the calculation, the payload here is defined to be a point mass (1 kg) payload that is attached to the tip of end effector. 5.1 Scenario 1: Perfect Condition In this scenario, the manipulator is given a task to move along pre-planned trajectories without any external disturbances as well as no interaction with environment is required. In this case, those controllers that are model-based can take the full advantage of the dynamic model knowledge. Each type of desired trajectory is simulated using all motion controllers presented in the previous chapter and the tracking performance score, P, is obtained to be compared. Because of the similarities in the results from different types of trajectories, only some are shown here to be discussed. Consider the case of a square trajectory as discussed previously, using PD controller with only position and velocity references, the actual end effector position and desired end effector position can be plotted as shown in Figure 9. The tracking error cannot be seen from the naked eyes in this plot. To magnifying the error signals, a separate plot is generated to show end effector positioning error in each axis as shown in Figure 10. As it can be seen from the plot, the minimum and the maximum tracking error in x-, y-, and z-axis are m and m, m and m, and m 31

40 Figure 9: PD Control: End Effector Position in Square Trajectory and m respectively. The tracking performance score, P, for PD controller with only position and velocity reference is Next, PD controller with gravity compensation is applied to the same desired trajectory, the magnified trajectory tracking error in all three axes are plotted in Figure 11. The tracking ability of this type of controller is significantly better, almost ten times better, than the simple PD controller without any modeling knowledge. The tracking performance score is obtained to be Finally, PD controller with full dynamics feedforward terms and computed torque controller are applied to the same desired trajectory and the trajectory tracking errors are all plotted in Figure 1 and 13 respectively. The tracking performance scores are obtained to be and respectively. As one can observe from the trajectory tracking error plots for both cases, they are about equally accurate. This is not surprising, since the same model is used in both controllers. The main difference in the two controllers is in their feedback controller mechanisms, and in the absence of large perturbations, the 3

41 Figure 10: PD Control: End Effector Position Error in Square Trajectory Figure 11: PD Control with Gravity Compensation: End Effector Position Error in Square Trajectory 33

42 Figure 1: PD Control with Full Dynamics Feedforward: End Effector Position Error in Square Trajectory differences in the feedback controller may not be significant for trajectory following accuracy [1]. Hence more simulation scenario is needed to give a broader picture of what advantages each controller can present with imperfect dynamic modeling knowledge. In summary, for this scenario, the overall performance score for all controllers can be plotted into the same graph as shown in Figure 14. The results show that both partial and full dynamics compensation can improve the tracking ability significantly when compared to simple PD controller with only position and velocity reference. Both PD controller with dynamic feedforward and computed torque controller perform similarly. Hence, more simulation scenario is considered in the next section. 34