Integration Project Systems and Control 2013 Group 3
Group Members
Name:  Student id:  Email: 
Joep Alleleijn  0760626  j.h.h.m.alleleijn@student.tue.nl 
E. Romero Sahagun  0827538  e.romero.sahagun@student.tue.nl 
M. Kabacinski  0789360  m.j.kabacinski@student.tue.nl 
N. Kontaras  0827208  n.kontaras@student.tue.nl 
A. Simon  0676675  a.s.simon@student.tue.nl 
Planning
Week:  Activities:  

Feb 18  Feb 24  
 Make a list for the requirements of the controller  
 Prepare and conduct tests for coupling/decoupling(JA,NK,MK) 3.5 hours  
 Prepare and conduct tests for non linearity (JA,NK,MK) 3.5 hours  
 Prepare and conduct tests for joint identification and create FRF models of the joints (JA,NK,MK) 5 hours  
 Set up DH transfer matrices (AS) 5 hours  
 Generate Matlab Simulation of the kinematic chain (ER) 7 Hours  
Feb 25  Mar 3  
 Finish tests for moving direction and create FRF models of the moving direction, moving direction is (JA,NK,MK) 5 hours  
 Design PID feedback controller, add feedforward and test on the robot for each axis of freedom (JA,NK,MK) 11 hours  
 Prepare testing procedure, for following of the trajectory and checking of the requirements (JA,NK,MK) 1.5 hours  
 Matlab code for trajectory generation (TG) (ER) 5 hours  
Mar 4  Mar 11  
 Test controllers on the robot (JA,NK,MK) 2 hours  
 Test trajectory generation on the simulation (ER) 3 hours  
 Integrate Inverse Kinematics with Controllers (AS,ER) 2 hours  
Mar 12  Mar 19  
 Consider different controller design methods (JA,NK,MK,AS,ER) 10 hours  
 Work on report (JA,NK,MK,AS,ER) 2 hours  
Mar 20  Mar 27  
 Test routine generation on the robot (JA,NK,MK,AS,ER) 2 hours  
 Select optimal routine (JA,NK,MK,AS,ER) 4 hours  
 Problem solving (JA,NK,MK,AS,ER) 10 hours  
 Consider different controller design methods (JA,NK,MK,AS,ER) 10 hours  
 Test different controllers (JA,NK,MK,AS,ER) 5 hours  
 Work on report (JA,NK,MK,AS,ER) 2 hours  
Mar 28  Apr 4  
 Work on report (JA,NK,MK,AS,ER) 15 hours  
 Prepare presentation (JA,NK,MK,AS,ER) 5 hours 
Progress
Week 1: Feb 18  Feb 24
List of the requirements and design assumptions for the controller  
General controller requirements:  
 Each of the four inputs of the robot is required to follow the third order trajectory, thereby the resulting trajectory of the fork will be appropriate to safely move the three pizzas form there initial positions to the final positions.  
 The error between the reference trajectory and the actual trajectory of the fork has to be smaller then 0.5 cm during the whole time of operation.  
 The settling time of the controller needs to be less 0.5 seconds.  
 The acceleration applied to the platform has to be as high as possible without a pizza falling of the platform while following the trajectory.  
 The total time of transport of the three pizzas should be less 15 seconds.  
 For the controllers the following margins will be applied:  
 Phase margin: >45 degrees  
 Gain margin: >6dB  
 Modulus margin: <6dB  
Rotation axis:  
 The error between the reference trajectory for the rotation and the actual trajectory for the rotation of the robot has to be smaller then 0.25 degree.  
 The amount of overshoot should be less than 5%.  
 Bandwidth of the system (plant + control) should be high enough to minimize the nonlinear friction static error, but low enough in order to not amplify noise. A bandwidth of between 5Hz and 10Hz seems appropriate.  
 The rotation of the robot from one rack to the other rack should take less then 1.5 seconds.  
Vertical axis:  
 The error between the reference trajectory for the vertical movement and the actual trajectory for the vertical movement of the fork has to be smaller then 0.5 cm.  
 The amount of overshoot should be less than 5%.  
 Bandwidth of the system (plant + control) should be high enough to minimize the nonlinear friction static error, but low enough in order to not amplify noise. A bandwidth of between 5Hz and 10Hz seems appropriate.  
Horizontal axis:  
 The error between the reference trajectory for the horizontal movement and the actual trajectory for the horizontal movement of the fork has to be smaller then 2 cm.  
 The amount of overshoot should be less than 5%.  
 Bandwidth of the system (plant + control) should be high enough to minimize the nonlinear friction static error, but low enough in order to not amplify noise. A bandwidth of between 5Hz and 10Hz seems appropriate.  
Translation axis:  
 The error between the reference trajectory for the translation of the platform and the actual trajectory for the translation of the robot has to be smaller then 1 cm.  
 The amount of overshoot should be less than 5%.  
 Bandwidth of the controller should be high enough to minimize the nonlinear friction static error, but low enough in order to not amplify noise. A bandwidth of between 5Hz and 10Hz seems appropriate.
 
Design assumptions:  
 The controller works with the sample frequency of 500 Hz, the sampling frequency of the system is 500 Hz.  
 The acceleration exerted on to the platform has to be as high as possible without a pizza falling of the platform while following the trajectory.  
 FRF will be used to obtain a model of the system. This method can be used to obtain a model of a system without knowing all the parameters of the system.  
 The acceleration of the reference trajectories should be at least second order or higher to reduce the amount of overshoot  
 Position will be used as input reference for the pizza robot. The pizza robot provides its position as output, this information can be used to compare to the desired position in closed loop control.  
 Initially a PID controller will be used to follow the setpoint trajectory. A PID controller will be used because it is a simple controller and every team member is familiar with the concept. To increase performance this PID controller can be extended with feedforward block. When this controller functions according to the set requirements, different control methods will be explored to investigate if it would be possible to increase performance.
 
Joint identification and FRF models of the directions of motion (JA,NK,MK)  
We decided to estimate frequency response measurements of joints by nonparametric identification methods. We decided to try closedloop identification methods as they seemed most suitable for given task, e.i. the plant movement is restricted, we could easily design low bandwidth stabilizing controller and close the loop. Moreover these methods are know to us from Motion Control course.
Horizontal fork displacement: Rotation: Translation: The above results are not appealing. Despite trying different parameter configurations of tfestimate function, most of the frfs are not clear enough to be a base for controller design. We can hardly notice second order characteristics of the plants as well as resonances which we expect to be present, since they are typical for mechanical systems which consist of several elements. Low quality of the frfs is confirmed by coherence functions which are much lower than one. The only exception is the vertical movement frf, which gives much better results than the others and seems to be clear enough to be used to tune the controller. Concluding, frf measurement must be continued next week. We are going to try different measurement methods, as well as parametric identification of the plants.
 
Trajectory Planning (AS,ER)  
Given that the kinematic chain for the robot wasn't needed to be solved, all the work referred to robotic analysis (DH parameters, Inverse Kinematics, Simulation of the kinematic chain) was skipped and we started working right away into the functionality of the trajectory. For this the first step was the planning of the trajectory profiles and the strategy for programming the trajectory in Matlab. The following images have been used as a reference: 
From these images it was decided to program the movement of the pizzas in the following order: L2>R2>L3>R3>L4>R4, using the "out" position of these references as safe position. L1 and R1 are used as well as safe positions.
Third order (constant acceleration) and Quintic polynomial trajectory functions have been programmed, as well as a generic script for path generation using these functions. The functions allow to create the fastest trajectory between two points based on acceleration restrictions.The typical position, velocity and acceleration profiles generated by these functions are plotted below.
Week 2: Feb 25  Mar 3
Trajectory generation 
The trajectory was built with the LSBP path generator. The trajectory was structured in such a way that LSBP function could at any moment be changed with the quintic polynomial path generator. The trajectory was built using the the shortest possible path. The lowest pizza will be transported to the lowest position, the middle pizza to the middle position and the highest pizza to the highest position. The lowest pizza will be first transported, then the middle and then the highest pizza. The robot arm will move from point to point (see figure below), moving only in one direction at a time, except when rotating without pizza. It will also change height during that motion. 
Reference points for trajectory  
The following reference points have been taken to be used in the trajectory profiles (these references are based on the images of week1):

Open loop FRF measurement 
As our initial plan states, we scheduled to do our FRF measurements by closing the loop with negative feedback on each axis, thus stabilizing it (with a low bandwidth controller), and then estimating the frequency response by using the threepoint FRF method. We were able to get an acceptable FRF on the first axis (the vertical movement), however on the other three axes our results were unsatisfactory, with the main problem being the high noise making the gain and phase estimations unclear. 
In order to rectify these unclear readings, we attempted to perform openloop FRF measurements. These experiments were done with special care, as the axes should not be allowed to touch the airbags. The results were satisfactory to a high degree. 
Week 3: Mar 4  Mar 10
Improve the results of FRF measurements by applying new parametric identification methods 
In order to obtain high quality models for control design we proceeded to fit our frequency response data to discrete transfer functions using the System Identification Toolbox (using mainly the ARX and BoxJenkins models). These highorder models were used for building our controllers. The advantage of using models is being able to observe our estimated performance (such as step responses) as well and not just stability margins (gain margin, phase margin, modulus margin etc.).
Highorder models were used in order to catch all the dynamics and phase loss due to sampling the system shows. The first axis was fitted into a BoxJenkins model of 30th order (30 coefficients for model numerator and denominator each, and another 30 coefficients for the noise model). The 2nd and 3rd axes were fitted into ARX models of 120th order (120 coefficients for A(q) and another 120 coefficients for B(q)). The 4th axis was fitted into a BoxJenkins model of 60th order (60 coefficients for model numerator and denominator each, and another 60 coefficients for the noise model).

Below we see a figure of the impulse response of the 4th Axis. We notice there is a 4 sample delay on our system. 
Week 4: Mar 11  Mar 17
Controller Synthesis
Using our previous system identification findings, we proceeded to our controller synthesis procedure. The parametric models allowed us to nicely observe the step response of our plant(s), and thus determine various performance and stability margins for our controllers (like Gain Margin, Phase Margin, Modulus Margin, Bandwidth, Settling time, Rise Time and Overshoot). The main tools we used were the loopshaping tool Shapeit and the PID tuning tool of Matlab. The complexity of the controllers was kept low, since our plants were not extremely complicated (resembled the expected double integrator transfer function quite substantially). 
Below we see screenshots of the tools we used to synthesize our controllers. The PID tuning tool of Matlab offers automated synthesis, given the type, phase margin and bandwidth of the controller one chooses to design, but at the expense of limited controller order. Below we see ShapeIt, which while not automated, offers greater controller complexity to experiment with. 
Below we can see our controllers' stability margins and performance properties. 
Controller testing
Having designed first controllers, we implemented them in the simulink model and tested their performance on the trajectory designed in the previous weeks. We started from "slow" trajectory, which took about one minute in total. Robot was moving without pizzas at this point. These tests gave us good overview on controller behavior as well as further trajectory improvements. Of course robot behavior on the trajectory is one of the most important specs of the controller design. Therefore controller synthesis and controller testing on the trajectory became an iterative procedure. By shortening the trajectory time (which made it more difficult to follow) we could give higher demands to the controllers.
Below we presents four plots which were used to precisely observe behavior of four robot joints while following the trajectory. The plots correspond to vertical fork displacement, horizontal fork displacement, rotation and translation (from upleft to downright). On each plot we can see: given trajectory, trajectory followed by the robot joint, error (difference of the first two plots), controller output (from the top to the bottom).
Trajectory improvements
The trajectory built in the previous weeks was changed. During initial testing it could be seen that the rotation of the robot took more time than changing height or translating. Thus changing the height during the rotation from the left to the right position did not result in a slower trajectory. The order of placing the pizza's was then changed, to create more space when putting down the pizza's. The trajectory was adjusted to place the pizzas in the following order: L2>R4>L3>R3>L4>R2. This resulted in more movement space when putting down the pizza's.
Week 5: Mar 18  Mar 24
Further trajectory improvements 
After testing the new trajectory, the results were satisfactory. We were now able to pick and place pizza's succesfully at a low speed. The accelerations of the different movements were increased until the error would become too big. To further improve the trajectory, unnecessary motions were deleted. The retracting of the arm after putting down a pizza was deleted, because there was enough movement space inbetween the two shelves. This resulted in a considerable time gain. 
Feedforward details 
Feedforward has been tuned for the rotation since a higher accuracy is needed and it is the slowest movement in the whole trajectory. A higher accuracy has been decided due to the amplification of small errors in the rotation once the arm of the robot is extended. There was no need to tune feedforward for the other movements since the required precision had been already achieved. The following image shows the trajectory used for tuning the parameters and the results: 
Max. original error: 0.4001
Max. error after FF: 0.04687
Trajectory Parameters (Ref3)
t0 = 15, x0 = 2.4, xe = 4, vel = 1.8, acc = 4, jerk = 1000, t0_2 = 16.8
PID Controller Parameters
kP = 290 , kI = 0 , kD = 19
Feedforward Parameters
Kfc = 1.3 , Kfv = 0.17 , Kfa = 0.17
Week 6: Mar 25  Mar 31
Further trajectory improvements 
The next improvement for the trajectory was the overlap of movements. Until now, the movement in different directions was seperated as much as possible. However, it was noticed that this was not necesarry. For example, when putting down a pizza, instead of moving the fork out, then going down and then retracting the fork, these movements were combined. We achieved as much as 50% overlap while still placing the pizza's succesfully. This resulted in considerable trajection time reduction. The final trajectory is shown below. 
Week 7: Apr 1  Apr 5
Final results 
This last week has been used for revision of the project, final adjustments and to test the overall result on the right robot. Up to this week all the tests have been performed on the left robot. With the controllers we have used until now, we achieved a good performance on the right robot as well. Final results for both robots can be observed in the following plots. As stated before, the plots correspond to vertical fork displacement, horizontal fork displacement, rotation and translation (from upleft to downright). On each plot we can see: given trajectory, trajectory followed by the robot joint, error (difference of the first two plots), controller output (from the top to the bottom).

Left Robot 
Right Robot