MODELLING AND CONTROL OF A DELTA ROBOT

Transcription

1 MODELLING AND CONTROL OF A DELTA ROBOT ABSTRACT The goal of this project was to develop a comprehensive controller for a generic delta robotic manipulator. The controller provides a variety of functions including hold position, point to point motion, and trajectory tracking. We also explored different techniques to develop variations of the controller and compare their performance. Lastly we performed tests to analyze the performance of the controller with respect to stability, disturbance rejection, accuracy, and precision. Testing and analysis was done in simulation, and variability and disturbance was injected using a random number generating function. Nick Panzarino James Kuszmaul Guled Elmi RBE 502

2 Contents Intro/Background... 2 Methods... 2 Kinematics... 3 Inverse Kinematics... 3 Forward Kinematics... 4 Dynamics... 5 Simplified Dynamics... 6 Simulation... 6 Design... 6 Testing... 8 Results... 9 Kinematics... 9 PID Control... 9 Performance Computed Torque Control Performance Robust Computed Torque Control Performance Adaptive Control Performance Conclusion Future Work Appendix... 19

3 Intro/Background The goal of our project is to develop a controller for a delta robotic manipulator. Delta manipulators are a form of parallel robotic manipulators which can have varying degrees of freedom (DOF). Parallel robotic manipulators can provide numerous benefits over serial manipulators in a variety of circumstances 1. Modelling and control of these systems can often be more complex than that of serial manipulators. We chose the delta manipulator because it is commonly used in industry and offers benefits such as high speed pick and place operations. Furthermore, from a controls standpoint, it is possible to control only translation (3-DOF version) or to control translation and rotation independently (6-DOF version). While much research has been done in modelling and control of the 3-DOF version of delta manipulators, there is far less information regarding the 6-DOF version, making this an interesting field to explore during this project. For the modeling and control of a 3-DOF Delta arm, one application chose to simply calculate trajectories for each joint variable and have separate controllers for each joint. 2 This seems to have functioned, but in looking for something a bit more substantial, we also found a conference paper title Robust Control for a Delta Arm, 3 which, in addition to deriving kinematics and dynamics of the (3-DOF) Delta arm, also describes an actual control algorithm. They used what is essentially a computed torque control law to control the arm; we will investigate this as well, although the paper in question is lacking in actual implementation details and we had troubles understanding and implementing their methods. In addition to computed torque, we will also explore robust and adaptive control methods. This will allow us to compare and contrast the performance of each controller and ultimately determine which would be most ideal for use on a physical robot. Methods Most calculations for the kinematics and dynamics are done relative to the base frame. The base frame has its origin in the center of the base of the robot, with the z-axis pointing upwards. We use the same coordinate frame positioning as Figure 1: Delta Robot Geometry, from (Andre Olsson Modeling and control of a Delta-3 robot ). The only difference is that we use positive θ values to indicate the joints moving up rather than down: 1 For an overview of Parallel Robotics and particular common types and manipulators, see: Merlet, J. (2006). Parallel Robotics. Springer. 2 For the paper in question, see: Olsson, Andre. Modeling and control of a Delta-3 robot, Department of Automatic Control Lund University February Hui-Hung Lin, Chih-Chin Wen, Shi-Wei Lin, Yuan-Hung Tai and Chao-Shu Liu, "Robust control for a delta robot," SICE Annual Conference (SICE), 2012 Proceedings of, Akita, 2012, pp

4 Figure 1: Delta Robot Geometry Kinematics Overall, the kinematics are calculated using the geometric constraints of the robot. This is roughly in line with how the inverse and forwards kinematics were calculated for a 3-DOF version in Robust Control for a Delta Robot 4. Inverse Kinematics We were able to find analytic solutions to the inverse kinematics of both the 3-DOF and 6-DOF delta manipulators. To do this, we first solved the inverse kinematics of each link separately. For each link, the inverse kinematic solution was simply to find the angle, theta, caused the end of the upper link to have a distance from the attachment point on the travelling plate equal to the length of the forearm. The steps used to solve for theta using these constraints are as follows: 1. Solve for the end position: This is a point in 3D space which can be directly computed from the end effector position and orientation. First, we express the end-effector connection points in terms of the end-effector frame as c i e. For the 6-DOF version, these will be the points on a hexagon, relative to an origin in the center of the hexagon; for the 3-DOF, these are the points on a triangle, also relative to the center of the triangle. Given a goal end-effector pose as described by a transformation matrix, H, we can get the connection points for each link, i, in the base frame as: c i 0 = Hc i e 4 Hui-Hung Lin, Chih-Chin Wen, Shi-Wei Lin, Yuan-Hung Tai and Chao-Shu Liu, "Robust control for a delta robot," SICE Annual Conference (SICE), 2012 Proceedings of, Akita, 2012, pp

5 2. Solve for start position as a function of theta: The start positions for each link chain is just a constant position vector, s i, which can be calculated geometrically as a vertex of a hexagon or triangle. We can now express the end of the first link (aka the elbow) (which is just on a normal rotational joint) as a function of the joint angle, again just looking at the geometry of the first joint: Origin θ Figure 2: Delta Upper Arm 3. Generate symbolic function: Because the second link in the chain is connected via unconstrained shoulder joints, the only constraint we have on the elbow position is that it must be a specific distance away from c i 0. In MATLab, we can express this as saying that norm(elbowpos(θ) c) == linklength. 4. Solve symbolic expression: the above expression can be solved symbolically using MATLab s solve function. 5. Solve inverse velocity kinematics: We can then use MATLab s diff() function to differentiate the inverse position kinematics with respect to time and get the inverse velocity kinematics as well. Forward Kinematics Unfortunately, the forward kinematics are actually more difficult than the inverse kinematics. This is due to the nature of the parallel robot, because in the inverse kinematics we are really just solving the inverse kinematics for 6 separate slightly complicate 2-link arms. However, for the forwards kinematics we have six separate joints which create 18 (6 joints times 3 dimensions) different nonlinear symbolic constraints that must be solved, and MATLab takes so long to finish solving it that we never actually saw it finish. As such, using the MATLab solve() function was not a realistic way to calculate forward kinematics. In the end, we were able to solve the forwards kinematics for the 3-DOF arm by directly solving it geometrically. Basically, because we know the joint angles, it is relatively easy to calculate the elbow positions (as they are just one link on a revolute joint). Now, because we know that the link chains must all connect to the end effector and that the link chains are a constant length long, we basically have to find the intersection of 3 spheres. It is possible to find the math for doing this geometry online, 5 which gives us two possible solutions to the forwards kinematics, of which we choose the lower one, where lower just means the solution with the lesser z-coordinate (the choice is largely arbitrary and is just a matter of how you physically put the robot together, but this choice better matches the one shown in the earlier picture of a delta robot). While this method may provide an avenue to calculate the forwards kinematics of the 6-DOF delta robot, we did not attempt it, as there were obvious additional complexities in handling both the extra 3 robot 5 Specifically, we found it online on the StackExchange forums (

6 links (the intersection of 6 spheres would be a bit harder to deal with) and handling the rotation of the endeffector (in the 3-DOF version, there is no rotation, simplifying things). For the 3-DOF version, we then calculate the Jacobian by taking the forward kinematics and using the MATLab symbolic toolbox to differentiate the forward kinematics with respect to the joint variables. Because there is no rotational velocity, there is no need to make any special efforts to account for the rotational velocities. Dynamics In order to calculate the dynamics, we initially tried to use the Euler-Lagrange equations to brute force a solution for the 3-DOF (we initially ignored the 6-DOF, as we wanted to solve the simpler problem first). At the time, we did not have usable forward kinematics for either the 3 or 6 DOF delta arm. However, we did have the inverse position and velocity kinematics. As such, we attempted to solve the dynamics in the workspace, which would allow us to avoid using the forward kinematics. 1. First, we calculate the kinetic and potential energy of each base link and the end-effector, ignoring the mass of the second, unpowered link. 6 2 a. K end_effector =.5 m end v end 2 2 b. K link_i =.5 m link r link ω i (for links 1-3) c. P end_effector = m end g z end d. P link_i = m link g z i (for links 1-3). e. Where: i. ω is the vector of joint velocities at the base ii. v end = Jacobian * omega iii. M end and m_link are the masses of the end effector and links iv. z end and z i are the z-positions of the end-effector and center of masses of the base links 2. Then, we calculate the Lagrangian as L = K - P 3. Then we call MATLab functions to calculate the Euler-Lagrange equations: a. tau = dl d( dl dω ) dθ dt 4. Giving us the dynamics of the system. However, not only did these calculations take a while for MATLab to complete, but once they did complete, actually running the calculations even once took well over a minute of run-time. 6 A similar simplifying assumption was made in Olsson, Andre. Modeling and control of a Delta-3 robot, Department of Automatic Control Lund University February 2009.

7 We decided that spending further time investigating other methods of doing the dynamics or of speeding up the existing dynamics would consume too much of our time and decided to instead create a simplified set of dynamics and focus primarily on the control laws. Simplified Dynamics In order to do the simplified dynamics, we decided to make the simplifying assumption that the mass and moment of inertias of the base links was much greater than that of the end effector and end links. If only the base links have mass, that means that each base link can be treated as an independent, single-link arm that does not see any coupling to the other links. Each of the 3 (or 6, in the case of the 6-DOF) base links was modeled as a basic, 1-R single link arm with the following dynamics and inverse dynamics: τ i = I i q i + B i q i + m i g r i sin(q) q i = tau B i q i + m i g r i sin(q) I i Where q i, q i, and q i are the joint position, velocity, and acceleration for the given joint, m i is the mass, r i is the position of the center of mass, I i is the moment of inertia about the axis of rotation, B i is a friction coefficient, and g is the acceleration due to gravity. This substantially simplified and sped up our dynamics, allowing us to actually test and simulate control laws. Simulation Design In order to test controllers as we were developing them, we had to create a method of simulating a delta robot under various circumstances. To do so, we developed a method of performing simulations that was very modular; each component of the simulation could be modified separately, and we could choose which conditions to run the simulation by interchanging these components. The four components were as follows: Name Description Inputs Outputs Desired Trajectory Describes the desired motion of the manipulator over time Current time Desired joint positions, velocities and accelerations Control Law Determines what forces/torques to apply to each joint Desired Trajectory and actual joint positions and velocities Joint forces/torques

8 System Response Determines how the system reacts to applied forces/torques. Similar to a differential equation. Current state of the system (position and velocity), and applied forces/torques Time derivative of the state of the system (velocities and accelerations) Noise/Disturbance Changes the response of the system to represent un-modeled error, uncertainty, or external forces/torques Current state of the system (position and velocity), and applied forces/torques Difference between expected system response and actual response The relationships between these components is shown in Figure 3: Simulation Process below. Figure 3: Simulation Process

9 Each of the components (shown in yellow) can be interchanged with functions of the same type which may behave differently, thereby modifying the simulation. While the forward dynamics was used as the response function, we were able to select various trajectories, controllers, and noise functions to run different tests. The blue step block above was used to determine the change in state of the system from one time step to the next. The equations used were: p f = p i + v i t + 1 a t2 2 v f = v i + a t Where p f and v f represent the state of the system in the next time step. Testing To compare the behaviors of different controllers, testing was performed under specific conditions: 1. The same trajectory was used when comparing controllers. The chosen trajectory was one which traced a circle in the workspace. This was then converted to a joint space trajectory using the inverse kinematic function, and controlled via one of several joint space control algorithms. The trajectory used is depicted in both the workspace and joint space in the Appendix, Figure 14 and Figure The same initial conditions were also used for each test case. The chosen conditions were for all joints to begin at the zero position, e.g. all joint positions and velocities have a value of zero. Since the trajectory starts with a non-zero position and velocity, this initial condition was a good way of observing the ability of the controller to cause the system to converge to the desired state. 3. Different Noise/Disturbance functions were used to test the behavior of the controllers under different circumstances. Three different types of noise functions were used: a. Zero noise (Ideal). This simulates behavior when the model of the system exactly matches actual system response. b. Gaussian noise with constant bias: This noise function consists of a normally distributed random noise with fixed mean and standard deviation. This simulates a real world environment in which there can be random variations, and assumes that the model is mostly correct, except there is some slight error in one or more constants. c. Joint Coupling: This noise function simulates coupling effects between joint variables not accounted for in the dynamic model, but which may affect the behavior of the system. For our purposes, we chose a non-realistic coupling effect, in which the response of each link is linearly affected by the velocity of another link. While this is not an accurate representation of the coupling effects on a delta robot, it was a simple way to test the effect of such forces on different control algorithms. 4. Each simulation was run under the specified conditions for 10 seconds, and the positions and velocities of each joint variable over that time were reported. For simplicity (and brevity) only the positions are discussed in the results

10 We did the testing with both the 3 and 6-DOF version of the robots; all the graphs in our results are for the 3- DOF version, but are equivalent to what happened when we ran the same trajectory with the 6-DOF version. We did not do substantial testing of trajectories involving rotations of the 6-DOF end-effector. Results Kinematics The kinematics generally worked well; we did tune the functions over time to make them faster and easier to use, but overall they were functional and we were able to create nice plots of the delta robot like Figure 4, where we show the 6-DOF version of the robot with the end-effector being translated and rotated from the origin. PID Control Figure 4: 6-DOF Delta Visualization PID controllers are simple, and can provide decent performance for many systems. The fundamental theory is to determine input torques based on errors in position, velocity, and integral of position. Each of these errors is scaled by some constant and the sum is used as a control signal. Formally, the controller can be written as: t τ = K p (p d p a ) + K d (v d v a ) + K i (p d p a )dt 0 The largest downside to PID control is that it does not account for the model of the system, and therefore it can take long to converge, have high oscillations, and have poor trajectory tracking.

11 Performance Figure 5, below, shows the behavior of the system under PID control with zero noise in the system. By using fairly large gains we were able to achieve decent trajectory tracking, at the expense of large oscillations in the beginning of the simulation. Reducing the gains led to reduced oscillations, at the expense of poor trajectory tracking. Additionally, modifying the trajectory or initial conditions also greatly changed the performance of the controller, often requiring different gains to be used to achieve desired performance Computed Torque Control Figure 5: PID Control Computed torque control was used to incorporate the robot s dynamic model into the control law. This controller utilizes the inverse dynamics function we calculated for the delta robot to estimate the torques required to move the manipulator in the desired way at the current state. The controller functions by modifying desired acceleration using position and velocity error (modified by some gains). Formally: Where τ = InverseDynamics (p a, v a, a d + K p (p d p a ) + K d (v d v a )) InverseDynamics = f(q, q, q ) = M(q) q + V(q, q ) + G(q) Performance

12 Under ideal conditions (model exactly matches actual response), computed torque control works very well. Starting with a large difference between desired and actual state, the system quickly converges to the desired trajectory and tracks very well. This behavior is shown in Figure 6. Figure 6: Computed Torque, No Noise When adding Gaussian noise with a constant bias, the computed torque controller begins to fail. While it still approaches the desired trajectory and tracks fairly well, the constant bias results in the controller being unable to fully converge to desired trajectory. This behavior is shown in Figure 7 Figure 7: Computed Torque, Gaussian Noise

13 When testing this simple computed torque controller with coupling effects, the result was a highly unstable system. This is caused by that fact that the computed torque controller acts only using the information of the model it is given. It does not update the model as the simulation is running even if the actual behavior deviates from the expected behavior. Robust Computed Torque Control The objective of robust computed torque control is to reduce undesirable tracking or positional errors resulting from the model mismatch and parametric uncertainty. Robust Computed Torque Control (RCTC) depends on the priori specified uncertainty bound from previous data estimation, nominal values and etc. The error dynamics of the system is bounded by the specified known bound function, which means the system is stabilized within the known bounded envelope. Passivity-based RCTC is developed to make bound specification more intuitive and exploit the linearity parameters in the system dynamics. In passive robust control, the uncertainty bound depend on the inertia parameters of the robot. In this section of the report, description of non-adaptive passivity-based robust computed torque control scheme for delta parallel manipulator robot is presented. Using MATLab, trajectory to be tracked is generated and control algorithm reducing servo errors between the desired trajectory and actual system trajectory is established. RCTC uses Partitioning technique in which both the known system dynamics parameters and error driven dynamic model of the system are combined in the control law. The simplified dynamics of the system treats the 6-DOF delta parallel manipulator robot as 6 independent one-link arms. RCTC controls the motion of each arm independently. In order to achieve uniform ultimate boundedness conservative estimation of the bound is needed to make the system practically stable. Nominal Values: m = 0.12; r = 0.11; B = 0.15; Actual values: m = 0.15; r = 0.15; B = 0.1; Robust control values: ε = 0.001; ρ =.004; Control Law Parameterize system as: τ = Y(q, dq, a, v) π Where: r = q error + Λ q error, v = q desired Λ q error, a = v

14 π = set of inertial parameters Control: U = { ρyt r norm(y T r) if norm(yy r) > ε ρyt r ϵ if norm(y Y r) ε τ = Y T (π nominal + U) K r In order to compute epsilon and rho (which are constants specified by us), we roughly estimated them based on the magnitude of our parameter uncertainties and then tuned them based on what made the system perform well. Performance The below plot demonstrates how robust computed torque control affects the system when there is no noise, model mismatch and parametric uncertainty. In this case, RCTC has the same effect as the CTC algorithm. Figure 8: Robust Control, No Noise As show below, when constant bias and Gaussian noise are added to the system, RCTC compensates for the disturbance and the actual trajectory converges to the desired trajectory.

15 Figure 9: Robust Control, Gaussian Noise RCTC was developed to yield a uniform ultimate boundedness of the tracking error when there is suitable estimate uncertainty for the system dynamics. RCTC handles the noise, joint coupling disturbance, model mismatch and parameter uncertainty. As shown in the above picture, the actual system trajectory is tracking the desired trajectory within a bounded offset. RCTC cannot adapt to unspecified model parametric uncertainty and this paves the way for the adaptive control algorithm to do its work. Figure 10: Robust Control (Inaccurate Model), Gaussian Noise

16 Adaptive Control The goal of the adaptive control is to account for longer term variations in the system parameters. The method of adaptive control used here is as described in the textbook, section 8.5.4, Adaptive Control 7. Essentially, in order to develop adaptive control, we change the dynamics to be of the form: τ = Y(q, q, q ) π At simplest, we could use Y = τ = Mq + Cq + G and set π = 1. If for some reason the overall joint torques were then off by a constant multiplier (say, if the motors had 10% more torque than expected), then the adaptive control would adjust π until π = 1.1 and the dynamics were properly modelled. However, if there were instead some unexpected dynamics which affected only a part of the system (if say, the first joint was slightly heavier than expected), then the adaptive control would attempt to adjust until it is as close as possible, but it will never be able to perfectly account for variation that cannot be modelled by changing π. For the control law itself, there are two steps: 1. Update π to account for unexpected behavior in the system. a. σ = q error + Δ q error b. π = K π transpose(y) σ 2. Update the control input to account for transient disturbances a. τ = Y π + K d σ Where Δ, K π, and K d are positive definite gain matrices (K π must also be symmetric). As demonstrated in the textbook, this system is stable in the sense of Lyapunov so long as the dynamics are of the form τ = Mq + Cq + G with the provided control law 8, with σ and q error globally asymptotically converging to zero. For our implementation of adaptive control, we started with an extremely simple parameterization: Making Y a 3x2 matrix with the following elements Y1 = M q i + C q i + G = the first column of Y Y2 = 1 = the second column of Y Where our nominal values for π are: π1 = 1, π2 = 0 This allows us to account for any linear disturbances in the torque, be it a constant disturbance (by updating π2) or something that scales linearly with tau (by updating π1). This system is still stable because we could express the dynamics in the M q i + C q i + G form by updating G to include some constant offset to correspond to Y2. 7 Siciliano, Bruno, Lorenzo Sciavicco, Luigi Villani, and Giuseppe Oriolo.Robotics: modelling, planning and control. Springer Science & Business Media, Ibid.

17 We then added in the coupling disturbance described earlier. In order to make the adaptive control properly handle this coupling, we had to add a term to Y such that π could be updated appropriately. In this case, we added three more columns to Y, essentially appending a 3x3 diagonal matrix to Y, where along the diagonal we put the three joint velocities. Because the coupling involves one joint velocity affecting the next joint s torque, the diagonal looks like: [dq_ dq_ dq_2] This adds three more parameters to π, which we nominally say are zero. The adaptive control must then update the parameters until the match whatever the simulation is run with (or, if the simulation is run with the parameters as zero, it shouldn t erroneously update the parameters). Performance With a constant disturbance and Gaussian noise, the adaptive control handles essentially as well as without noise (see Appendix Figure 16), eliminating the bias and updating its parameters to get rid of the constant disturbance, as seen in Figure 11 Figure 11: Adaptive Control, Gaussian Noise Next, we consider what happens when we add in the expected coupling the controller expects that there will be the coupling, but doesn t know how much to expect. This is the same coupling that caused the computed torque controller to become extremely unstable, and results in some consistent tracking error in the robust controller. Here, the adaptive controller takes slightly longer to start tracking fully, but once it gets tracking, it is essentially indistinguishable from the no-noise curve, as seen in Figure 12.

18 Figure 12: Adaptive Control, Coupling For some video of the adaptive controller running with the modeled coupling, see Finally, we must consider what happens when the coupling is not expected. In the real Delta robot, there may be couplings that we are unable to predict, even after tuning the adaptive controller. As such, it is important to see how the adaptive controller responds when it can t resolve all the tracking error just be adjusting the parameters. The unexpected coupling is similar to the expected coupling, but instead of joint 1 s velocity affecting joint 2, joint 2 affect joint 3, and joint 3 affect joint 1, we reverse it and have joint 1 affect joint 3, 2 affect 1 and 3 affect 2. This results in the adaptive controller not being able to perfectly track the trajectory, although it avoids going way off target, Figure 13. Figure 13: Adaptive Control, Un-modelled Coupling

19 Conclusion Overall, the different controllers behaved as follows: PID Controller: Simple, and it can handle most basic disturbances, but it struggles to precisely follow a trajectory, instead introducing slight oscillations. Computed Torque Controller: Better for tracking trajectories, but when substantial disturbances are introduced, it can introduce high torques and potential instabilities. Robust Controller: Does a solid job of handling simple disturbances, although it struggles a bit more with unexpected disturbances and with constant biases that result in a constant error. Adaptive Controller: Does a good job when there is a constant bias in the dynamics parameters. It does a slightly worse job of handling variance that is not tied to any of the model parameters (although no worse than any other controller). Of particular interest is the distinction between the adaptive and robust controller. They both work off of very similar principals (both use the Y * π setup and modify the π parameters to account for variance in the model). As such, they can both only really handle changes to the parameters in π, and if there are changes to anything unanticipated, then the controller may or may not handle it. The difference comes in how the two actually go about updating π. In the robust controller, at any given time step, π is updated solely based on the current state of the system it has now knowledge of whether the parameters have been off in the past or whether there is any sort of consistent bias. For the adaptive controller, it actually changes π at every timestep, taking the previous estimate of π and updating based on the current model errors. What this means is that the adaptive controller will do a better job of adaptive to consistent biases in the model such as if the mass of a joint has been mis-estimated, whereas the robust controller will just always be off be a constant amount (albeit less than a raw computed torque controller would be). However, if the errors in the parameter estimates is not consistent, then the robust controller will respond quicker than the adaptive controller and the adaptive controller may end up updating the model parameters and then having to spend even longer bringing the model parameters back to their new values. This means that the adaptive controller is likely best for our application. In general, any errors in our model are going to be consistent errors and not purely transient ones, and even though we do have to know the nature of those errors, that is more a matter of spending time with and tuning the model for the robot rather than a fundamental flaw in the controller. Although we would have to tune the adaptive controller to work with a real robot, it would likely require less and faster tuning than the other controllers, as it is more a matter of trying out different types of parameters and then seeing what the adaptive controller ends up with as model parameters (which could then be used as the nominal parameters for later controllers). Future Work The most obvious avenue for future work is to actually apply these controllers to a real Delta robot and tune the controller to make them actually work. Baring work on a true robot, we would suggest reinvestigating the creation of a good dynamic model for both the 3 and 6 DOF robots; even if it was not possible to create dynamics good enough to run in real-time on the robot, having dynamics available for simulation would allow someone to tune the adaptive controller so that it could properly handle the real robot.

20 Appendix Code available in public online repository at: Figure 14: Workspace Trajectory

21 Figure 15: Joint Space Trajectory Figure 16: Adaptive Control, No Noise