master internship reportjprevot/documents/...master 1 internship report trajectory planning for...
TRANSCRIPT
MASTER 1 INTERNSHIP REPORT
TRAJECTORY PLANNING FOR ROBOTS
From May 4th to August 21th, 2015
Student: Julie PREVOT- IFIE 2016 Tutor: Alessandro GASPARETTO Supervisor: Paolo BOSCARIOL School tutor: Emmanuel OTTON
2
Trajectory Planning for Robots JULIE PREVOT
French abstract / Résumé en français Ce stage d'assistant ingénieur à l'université des sciences d'Udine prend place au sein d'un
projet pour l'innovation de mon tuteur M. Gasparetto et de mon superviseur M. Boscariol, ayant
pour but de supprimer ou minimiser les pertes d'énergie dans les robots industriels. Pour ce
projet, plusieurs pistes sont explorées dans chacun des domaines qui constituent la mécatronique:
la mécanique, l'électronique et le contrôle et les logiciels. Mon travail à pour objectif d'optimiser
les trajectoires des robots dans le but de minimiser leur énergie globale consommée. On
distinguera, pour aboutir à la construction d'un outil de simulation, les quatre grandes étapes
suivantes:
- Transcrire et généraliser le modèle du Pr. Hansen [4.B], [4.C] sur Matlab.
- Constituer un catalogue de trajectoires applicables aux robots industriels.
- Créer et coder une procédure l'optimisation des trajectoires, adaptée aux modèles
étudiés.
- Mettre en place une interface utilisateur afin de faire de ces programmes un outil de
simulation intuitif.
C'est selon ces 4 axes que s'articule ce rapport. En effet le modèle développé par le Pr.
Hansen propose de partager la puissance de plusieurs DC-moteurs en série. Avec un système
d'inverter, un moteur pendant une phase de décélération devient générateur de puissance et réduit
les besoins en alimentation. Avec cette configuration, optimiser la trajectoire de chaque moteur
indépendamment ne constitue forcement pas une solution optimale pour l'ensemble. Afin
d'optimiser les paramètres de ses trajectoires, on en propose quatre types de la famille des
trajectoires trapézoïdales classiquement utilisée en mécatronique. Ce sont sur ces deux modèles
que repose la procédure d’optimisation que j’ai créée.
Ce stage fut d’autant plus enrichissant qu’il nécessite des connaissances avancées dans les
domaines : des mathématiques, de mécanique et de programmation. C’est donc un éventail de
compétences dans ces différents domaines que j’ai pu mettre en œuvre au service d’un projet
concret et utile. C’est également une fierté d’avoir pu faire aboutir mon travail par la réalisation
d’un outil prêt à l’emploi et ce de façon entièrement autonome. Enfin, cette expérience m’aura
appris la place et l’importance des recherches bibliographiques à mener pour mettre en route ce
type de projet. C’est en effet une grande chance de pouvoir bénéficier de résultats établis, qui
m’ont permis ici de me concentrer sur la mise en place de la procédure d’optimisation.
3
Trajectory Planning for Robots JULIE PREVOT
Table of contents French abstract / Résumé en français ........................................................................................................................ 2
Table of contents ................................................................................................................................................................. 3
Acknowledgements ............................................................................................................................................................ 4
I) INTRODUCTION ..................................................................................................... 5
II) ENVIRONMENT ................................................................................................... 6
1) The University [2.A] ............................................................................................................................ 6 2) Human environment ........................................................................................................................ 6
III) ROBOT MODEL AND COST FUNCTION [3.B], [3.C] ............................................................. 7
1) Model of robot .................................................................................................................................. 7 2) Translation motor ............................................................................................................................ 8 3) Rotation motor ................................................................................................................................ 9 4) From torque to energy ....................................................................................................................... 9 5) Conclusion..................................................................................................................................... 10
IV) TRAPEZOIDAL TRAJECTORY MODEL ...................................................................... 11
1) Trapezium ..................................................................................................................................... 11 2) 3rd order polynomial ....................................................................................................................... 12 3) B-spline [4.A].................................................................................................................................... 13 4) Double S [4.B] .................................................................................................................................. 15 5) Conclusion..................................................................................................................................... 17
V) OPTIMIZATION .................................................................................................... 18
1) Parameters .................................................................................................................................... 18 2) Algorithms .................................................................................................................................... 19 3) Method ......................................................................................................................................... 20 4) Conclusion..................................................................................................................................... 20
VI) USER INTERFACE .............................................................................................. 21
1) Why this interface? ......................................................................................................................... 21 2) User's manual ................................................................................................................................ 21 3) Results .......................................................................................................................................... 23 4) Conclusion..................................................................................................................................... 24
VII) CONCLUSION ................................................................................................... 25
Internship continuation ................................................................................................................................................ 26
Bibliography ...................................................................................................................................................................... 27
Table of illustrations ...................................................................................................................................................... 28
ANNEXES .............................................................................................................................................................................. 29
2.1 GANTT PLANNING OF MY INTERNSHIP ......................................................................... 30
3.1 FUNCTION POWERMOTOR.M ....................................................................................... 32
4.1 FUNCTION TRAPEZIUM.M ........................................................................................... 33
4.2 FUNCTION POLYNOMIAL3.M ....................................................................................... 33
4.3 FUNCTION GENTRAJ.M ............................................................................................. 34
4.4 FUNCTION DOUBLES.M ............................................................................................. 36
5.1 FUNCTION COSTFUNCTION.M ...................................................................................... 38
5.2 FUNCTION NONLINCON.M .......................................................................................... 39
6.1 EXCEL FILE MACHINE.XLSX ....................................................................................... 40
6.2 PROGRAM MACHINE.M ............................................................................................. 41
4
Trajectory Planning for Robots JULIE PREVOT
Acknowledgements It is my radiant sentiment to place on record my best regards and deepest sense of
gratitude to the department director Pr. Roberto Rinaldo and my tutor Pr. Alessandro
Gasparetto, who gave me a big milestone in my career development through this internship, and
Pr. Paolo Boscariol for his accompanying.
I express my deepest thanks to my academic tutor Mr. Emmanuel Otton from Ecole des
Mines d'Albi-Carmaux, for following my work and his encouragement during this assignment.
I’d like to say thanks to all the people at the Mechatronics Research Group working at the
University of Udine: Department of Electrical, Management and Mechanical Engineering,
researchers, PhD's and administrative workers who considered me as a co-worker and as a friend.
This department made my four months of training time worthwhile and mind-opening.
In the end, I also acknowledge with a deep sense of reverence and gratitude towards my
housemate Chiara and the students of the University, who always supported me morally and
made me discover Italian culture.
5
Trajectory Planning for Robots JULIE PREVOT
I) Introduction
The internship takes part of a collective wish about ecological issues. Each scientific field
is developing new technologies which tomorrow will be able to fill the increasing energy demand.
In this development, the importance of software has devalued against hardware race for
performance. Here we are only focused on software and how it can be included in an ecological
approach. Indeed, the objective is to make a program for robots trajectory in order to reduce their
energy consumption.
There are so many ways to do the same task that choosing an optimized trajectory which
could cut off 15% of the global consumption of the robot. A task is associated with a motor and is
defined by a trajectory. My main objective is to create an optimization routine to optimize the
trajectory. This objective is decomposed into four main tasks:
- Transcribe and generalize Pr. Hansen's [4.B], [4.C] model on Matlab.
- Propose a set of trajectory for robots
- Create and code algorithms to build an optimization procedure
- Set up an user interface to compute robot data
The first application will be for industrial robots. Indeed, for industrial companies, this solution
will save energy, money and often time.
My work consists in creating a program on Matlab with a predefined model of cost
function from M. Hansen's work. It calculates the energy and power consumption for a set of
motors which defines a machine. The program is a bridge between establishment of a model and
simulation. Indeed, the objective is using it to simulate the energy consumption of robots. To
make its utilization the most intuitive as possible, the user will enter machine and trajectory data
thanks to the interface. Then, the program will be ready to be used to search the best trajectories
to optimize energy consumption of robots.
6
Trajectory Planning for Robots JULIE PREVOT
II) Environment 1) THE UNIVERSITY [2.A]
The “Universita degli studi di Udine” was founded in 1978 in the city of Udine as part of
the reconstruction plan of Friuli after the earthquake in 1976. Its aim was to provide the Friuli
region community with an independent centre for advanced training in cultural and scientific
studies. This is an important centre for the studies of Friulian language. [2.B] Currently, the
University is actively involved in student and staff exchange projects with universities within the
European Union.
Here are the data of the University in September, 2014. [2.C]
Students 15 635 Doctoral students 329
Teachers 678 Administrative and technical supervisors 537
Training programs 70 Fields of study 14
Research Departments 14
In the science building, there are six research departments : I work in one of these:
Dipartimento di ingegneria elettrica, gestionale e meccanica – Dieg. It’s the 3rd bigger department in the
University in terms of number of employees.
Data for Department Electronic, Management and Mechanic Engineering:
Professors 39 Researchers 18
PhD 40 Research assistants 17 External students 13
Administrative workers 12
2) HUMAN ENVIRONMENT
This internship project is additional to the work of my supervisor M. Paolo Boscariol who
is assistant researcher in the Dieg department of the University, more specifically for
Mechatronics laboratory. He is also professor of mechanics applied to machinery, logistics and
production in the University of Bozen - Bolzano. In the engineering department of the university,
the Mechatronics laboratory is shared with others searcher teams. The environment is exclusively
composed of mans.
My work in this environment is autonomous. Indeed, my tutor M. Gasparetto and the
supervisor M. Boscariol only oversee and check it regularly. You will find in annex 2.1 the Gantt
planning of the internship where you can see all the steps of the work and meetings with my
supervisors. The autonomy working method is a method that allows the emergence of new ideas.
Thanks to this method, I can add a new and innovative point of view in this project not doing it
exactly as the supervisor would have done it. Even if the scientific models are given, the method
of realization of the programs and interface is free.
Hours and working places are not imposed. For instance, I could work in the library of
the university but most of the time I worked in the laboratory where a computer with Matlab and
Internet was made available to me.
7
Trajectory Planning for Robots JULIE PREVOT
III) Robot model and cost function [3.B], [3.C]
This part is inspired by the work of Pr. Hansen related in his scientific article with the aim
of computing his method. Indeed, he proposes a model of robots with two motors. I began by
copying it, then I generalized for N different motors, using his equations derived from dynamic
equations.
1) MODEL OF ROBOT
The model proposed to share the power of several DC-motors. A DC-motor converts
electrical energy to mechanical work. Indeed, the set of N motors has the same power supply and
each nth motor is installed with an inverter, all in series. Each motor consumes energy during
acceleration phases but with the inverter it can also provide power during deceleration phases.
During the deceleration phase the energy comes back to the DC-bus. Thus, the energy
recuperated is now available to supply other motors.
Figure 1: Electric circuit of the robot model
This system with N serial motors corresponds to industrial robot models which already
exist in factories, where each motor generates each axis movement as in figure 2. Pr. Hansen proposes the following model which is composed of two motors, the first in
rotation, and the second in translation. The two types of motors show the diversity of motors that
can be used in Mechatronics. On the next figure 3 you can find some notations used thereafter.
Figure 2:Drawing of industrial robot with six axes
8
Trajectory Planning for Robots JULIE PREVOT
Figure 3: Mechanic schema of the two motors model
Following this model, the user can simulate a machine with more than two motors
choosing its type and its kinematics constraints which are different, functions of its type.
2) TRANSLATION MOTOR
The translation motor is the number 2 on the schema, the slope of its linear axis differs by
angle from the horizontal. The most important part of its power losses comes from Coulomb
friction losses. is the vector of motor shaft position:
(1)
Where u is the motor gear transmission and r the belt pulley radius. The time derivatives, motor
shaft velocity , acceleration and jerk could be calculated analogously. Therefore, equations
of motion for the utilized mechanical set-up are formulated to calculate conservative dynamic
force dyn, with linear slide mass m.
(2)
(3)
M is the motor shaft torque and jadd is the additional inertia for mechanics losses. c and v are the
coefficients respectively of Coulomb friction and viscous damping.
9
Trajectory Planning for Robots JULIE PREVOT
3) ROTATION MOTOR
The motor on rotation is the one numbered 1 in the example. For this kind of motor, inertia is
important; the greater the inertia, the greater the power used is.
(4)
The dynamic force dyn is calculated with j rotational inertia.
(5)
(6)
With the same notations than the rotation motor.
From this model, we add the possibility of introducing a reduction gear between the rotational
motor and the load, like the one in the translation motor model. The equations (5) and (6)
become (5.bis) and (6.bis):
(5.bis)
(6.bis)
No gear is equivalent to u = 1
4) FROM TORQUE TO ENERGY
The torque of the motor corresponds to a motor current in the electric circuit and
consequently to an electrical motor power to whom electric losses are summed
(7)
With the motor current i, the motor voltage v, stator resistance R, stator inductance l,
motor constant km and back electromotive force constant kg.
(8)
(9)
(10)
10
Trajectory Planning for Robots JULIE PREVOT
The electrical motor power PM is defined in (9). The electrical servo inverter power is
defined in the equation 10 as the sum of the electric losses term with the coefficient of resistive
losses in the inverter conductor paths klss and the constant switching losses of the insulated-gate
bipolar transistor pigbt.
(11)
When the power consumed is negative, it takes the role of generator. The sum of all
powers is the power consumption of the entire machine. Here, the potential negative energy of
some motors offset the consumption of others. Then, to calculate the energy, the positive power
must be time-integered. This power is the one we pay, coming from our electrical power supply.
As for the negative power, it is lost.
(12)
The function ‘powermotor.m’ (In annex 3.1), calculates the power consumed by one
motor, depending on its type, its kinematics constants and the task it does.
5) CONCLUSION
The DC bus model is already used in industry because it’s easy to set up and can save
little energy. The issue lies in the complexity of optimizing energy with this sharing model.
Indeed, the trajectories chosen for each motor influence its own consumption, therefore the
global consumption of the machine. In fact, this problem is not only optimizing the consumption
of each motor, because a motor could restitute its power to supply another one.
11
Trajectory Planning for Robots JULIE PREVOT
IV) Trapezoidal trajectory model The word trapezoid characterizes the velocity profile. Its specificity is a stagnation level of
velocity during the natural domain definition. The profile matches efficiency and confort: during
the constant maximum speed phase the position is linear, and out of this domain the profile is
softened. That prevents shocks. These types of profiles are already used in many sectors, in
industrial robots but also automatic transports such as trams and subways. In Matlab, the trajectory is considered as a signal with an extrapolation time of 1ms. To
correspond to the industrial reality where robots do point to point movement, the velocity limit
constraints are fixed to zero.
1) TRAPEZIUM
In an ideal trapezium, the increasing and decreasing phases are constant. On this model,
one cans parameter the level of stagnation and the gradient/slope to optimize the trajectory. With
this ideal model, the management of lost energy is easier because it's directly linked to the
acceleration which is simply constant during the natural definition domain. The function
'trapezium.m' is in annex 4.1
(13)
(14)
(15)
Parameters: amax, maximum acceleration
Figure 4: Display of 'trapezium.m' trajectory
Natural definition domain
amax=2
12
Trajectory Planning for Robots JULIE PREVOT
Advantages Disadvantages
- It’s easy to situate power losses
because acceleration is constant
- The profile is defined in 3 parts
- It generates a jerk Dirac because of the
non continuous profile of acceleration,
which induces vibration in the
mechanical structure
NB: the jerk is the 3rd order derivate of the position.
Because of the vibration generation, the trapezoid trajectory isn’t actually used in
industry. That would generate damage on hardware and so waste of money regards to energy
save. Nevertheless this model can be used in simulation to observe the behavior of power and
compare to the approximation function presented hereafter.
2) 3RD ORDER POLYNOMIAL
With this function, the velocity is a 2nd order function. This is a parabola oriented in way
to have a similar profile to a trapeze. To be represented by a polynomial, the model needs a
minimum of 3rd degrees to consider the position and speed limit constraints. The function
'polynomial3.m' is in annex 4.2
(16)
The values of the coefficients are:
(17)
Parameters: None
Figure 5: Display of 'polynomial3.m' trajectory
13
Trajectory Planning for Robots JULIE PREVOT
Advantages Disadvantages
- One formula includes all time
segments
- Uniqueness of the generated trajectory:
no parameters for optimization
- Rough approximation for speed profile
Actually this model doesn’t propose a good approximation, that’s why one can think to
approximate the velocity by an n-order polynomial (n>3) like Fourier model in order to
approximate electrical signals.
Advantages Disadvantages
- Better approximation of trapezoidal
model when n -> +∞
- Can be used with more constraints (not
very useful for my project)
- The model oscillates
- With few constraints, there are too
many parameters to optimize and they
are not representative of function
profile
The approximation with an n-order polynomial is never used because of its complexity
and its oscillating behavior with is a source of wasted energy.
3) B-SPLINE [4.A]
A B-spline function is a linear combination of basic functions, d-order polynomials (d ℕ)
on their intervals, so as to be constant on a determined segment called "natural definition
domain". The next example is a 3rd degrees B-spline:
Figure 6: 3rd degrees B-spline
2·d basic functions are necessary to build the final spline, and the polynomial functions
need to verify the following properties:
- ∀i [-d+1;d], the d degrees polynomial Ni,d is defined on its interval [ki,ki+d]
- Positivity on its interval: ∀ (18)
- Boundary values: (19)
14
Trajectory Planning for Robots JULIE PREVOT
The B-spline Ni,d+1 of degree d (order d+1) with knots ki<...<ki+d+1 is defined recursively
with the following relation:
∀
(20)
∀
(21)
These properties are enough to guarantee the uniqueness of the polynomials. They
depend on the values of ki. The simplest way of using them is defining intervals of constant
length, like one can see on the next figure:
Figure 7: 3rd degree B-spline with intervals of constant length
To generate trajectories, B-splines method is used to represent the velocity, that’s what
Mr. Béarée program does using the matlab function “filter” which defines the function
recursively to the order Tj/dt (jerk time/ interpolation time). To parameter the trajectory, the user
chooses the jerk time and the snap time (snap is the 4th order derivate of position). The function
‘GenTraj.m’ is in annex 4.3 and written by R. Béarée, ENSAM CER Lille, France. I made few
changes in the code in order to make it fit better to my program.
Parameters: amax, vmax, Tj, Ts
Figure 8: Display of 'GenTraj.m' trajectory
15
Trajectory Planning for Robots JULIE PREVOT
Advantages: Disadvantages:
- Trapezoid profiles are well
approximated
- Complexity of the method
- No control on end time
This trajectory generation method seems to be well-developped and convenient to
robotics, to the point where Mr. Hansen uses this method to praise the results of his model.
Although, it should be noted that the meaning of the parameters Tj and Ts are difficult to be
interpreted by the user.
4) DOUBLE S [4.B]
This trajectory proposes a double trapezoid acceleration, and consequently the velocity is
approximated by a 2nd order polynomial defined by parts. The Equations of this model are:
Acceleration phase:
(22)
(23)
(24)
16
Trajectory Planning for Robots JULIE PREVOT
Constant velocity phase:
(25)
Deceleration phase:
(26)
(27)
(28)
The expressions of Tj1, Tj2, Ta and Td change depending upon the input. They are all listed
in the ‘doubleS.m’ matlab function in annex 4.4
Parameters: jmax, amax, vmax
Figure 9: Display of 'doubleS' trajectory
17
Trajectory Planning for Robots JULIE PREVOT
Advantages Disadvantages
- Very good trapezoid approximation
- Do not generate vibration
- (continuous acceleration)
- Tends towards trapezoid when
parameter jmax (maximum of jerk) is
close to the infinite
- Meanings of parameters easy to
understand
- 7 parts definition
- Condition for existence of the trajectory
- No control of end time
5) CONCLUSION
All these models have advantages and disadvantages for a programmer, but their
description are not enough to determine which one has the better profile to optimize the energy
consumption of a kind of robot, if there is one. For a chosen set of motors, the best parameters of
trajectories are determined by the optimization routine.
18
Trajectory Planning for Robots JULIE PREVOT
V) Optimization The goal is to determine the entire configuration for a set of motors. Not all parameters
can't be optimized by a routine procedure, for instance the type of trajectory. The routine
optimization is set up to reduce the number of parameters so the number of tests in the simulation
phase, optimizing the trajectory parameters (amax, jmax, Ts ...). The only parameters that the user
needs to choose will be: the boundary constraints, time and position, and the type of trajectory.
1) PARAMETERS
It is not possible to optimize the trajectory parameters of each motor independently
because they share their energy, so the optimization routine needs to consider the entire set of
motor simultaneously. To do this, a vector is created, whose coordinates are all parameters. It is
built like this:
(29)
In this structure, param.n represents the trajectory parameters of the nth motor, but
regarding to the computer it is only numbers that we need to separate. I introduced a pointer i to
make the difference between the parameters of each motor. When the program has considered
the motor n and has treated param.n, the pointer is incremented in order to indicate the location
of param.n+1. That's how the procedure calculates progressively the power of the set and adds up
to get the energy of the machine. This energy is what I want to minimize, it's the result of the cost
function. I called Param0 the initial vector of parameters to start the routine.
This structure is the core of the program; it is used in every step. Those steps are: the
construction of Param0, the cost function, the non linear constraints and the processing of the
solution. Thus, they are all built on the same template. They use the parameter vector and the
kinematic data of the machine to determine the consumed energy, either to calculate the cost
with the parameters we want to optimize or to calculate and display the results with the solution
parameters which stem from the optimization routine.
19
Trajectory Planning for Robots JULIE PREVOT
2) ALGORITHMS
In each step of the procedure, the instructions depend on the trajectory type, so I set up an
‘if’ loop to filter with the trajectory type. The instructions are adapted to the type, the numbers of
parameters and constraints tied to this type. For example, in the cost function, Param gives
successively the parameters of each trajectory so every loop "for" in the program calculates first
the trajectory then the power of the motor k to get the total power of the motors 1 to k.
Eventually, the positive total power is time-integered to get the energy paid.
costfunction.m in annex 5.1
costfunction(Param)=
i<=1
M <= machine data
For k=1 to N do
If trajectory_type = trapezium
(Q,T)<= trapezium ( t0, tf, q0, qf, Param(i) )
PM<= powermotor (M, Q)
i<= i + 1
Elseif trajectory_type = polynomial3
(Q,T)<= polynomial3 ( t0, tf, q0, qf)
PM<= powermotor (M, Q)
Elseif trajectory_type = GenTraj
(Q,T)<= GenTraj ( Param(i), Param(i+1), qf-q0, Param(i+2),Param(i+3))
Q<= Q + q0
T<= T+t0
PM<= powermotor (M, Q)
i<= i + 4
Else
(Q,T)= doubleS ( q0, qf, v0, vf, Param(i), Param(i+1), Param(i+2))
T<=T+t0
PM= powermotor (M, Q)
i<= i + 3
End
P= P + PM
End
E=0
For k= 0 to max (tf) do
E= E + max (0, P (k)) * dt
End
Param(i) means amax
Only positive power is added,
negative power is lost
GenTraj works considering
q0=0 and t0=0
20
Trajectory Planning for Robots JULIE PREVOT
3) METHOD
The optimization function used in the program is "fmincon". I chose it because it can
optimize multidimensional non linear problems with different types of constraints: equality or
inequality, linear or non linear constraints. Our problem is monodimensional: we want to
minimize the value of the energy E, but there are non linear constraints so another optimization
function wouldn't be able to handle it.
fmincon function has four resolution algorithms: by default it uses the "interior-point"
method. When there are non linear constraints like there are in this trajectory problem, the
procedure uses the "active-set" method. The active-set method consists in considering the
constraints like a set which is taken into account during the calculation of Lagrange multiplier
resolution [6.A]. Thus, the optimized parameters will be stored in the vector ParamSol:
ParamSol=fmincon('costfunction',Param0,A,B,Aeq,Beq,ParamMin,ParamMax,’nonlincon’)[6.B]
The matrices A and B, respectively Aeq and Beq, contain inequality linear constraints
coefficients, respectively equality. This way, calling x the parameters vector to optimize:
(30)
(31)
ParamMin and ParamMax are the lower and upper bounds of the vector parameter x:
(32)
Finally, 'nonlincon.m’ in annex 5.2 is the function that gives nonlinear constraints
residues, for equalities and inequalities. For functions 'GenTraj' and 'doubleS', I forced the final
time calculated by the function to match the final time specified by the user, as these functions
themselves do not take into account the time restrictions. If T is the execution time of the
trajectory calculated by the function, then the following constraint is added:
(33)
For the 'doubleS' function, the limit acceleration alim does not always reach the parameter
amax value. Consequently, this parameter can take a high value and become neutral in during
optimization. In that case, the solution given to amax is no longer representative of the optimized
path, that's why I added an additional constraint:
(34)
4) CONCLUSION
For this optimization procedure I wanted to create an algorithm not only correct but also
whose results are exploitable and representative of the problem addressed. When I wrote the
Matlab program, I was sensitive to the problem of data size used, which also has a positive
impact on execution time of the program. Indeed, the optimization algorithm can potentially
handle medium sized problems (hundreds of data) and become a cumbersome procedure.
21
Trajectory Planning for Robots JULIE PREVOT
VI) User interface 1) WHY THIS INTERFACE?
The set of machine and trajectory used in the model needs many parameters: kinematics
constant, constraints… That represents more than 25 inputs for each motor. I made this interface
to make it as easy and as intuitively as possible for the user to fill in all data.
The excel file ‘Machine.xlsx’ in annex 6.1 is a support tool for the Matlab program:
‘Machine.m’ in annex 6.2. This way, the user doesn’t have to change any parameter in this
program. By reading it, the program takes the data of the machine and the trajectories in the
sheet, optimizes the trajectories and simulates the energy consumption of the machine.
2) USER'S MANUAL
The machine: One needs to define the machine in the sheets on excel.
Indicate the number N of motors used, the program takes only information of the N first
columns. Here, only 10 motors are represented in this file but the program doesn’t have a limit so
one can put more than 10, as long as one fills all necessary data following the template. Here I
chose to do the example with three motors.
Figure 10: Picture from 'Machine.xlsx', number of motors
In the sheet, in brown, for each motors one has to fill the type of motor, put 0 if it's rotary
and 1 in translation. Then the kinematic constants which correspond with this chosen type must
be filled. If the user doesn't fill all the information about the type of chosen motor, the program
will fail, and if one puts too much information which corresponds to the other type, the program
functions and this information will not be used. In my example, the first and the second motor
are rotary so I put 0 in the type, the third is in translation so I put 1 and I inquired the necessary
data in the first three columns.
Figure 11: Picture from 'Machine.xlsx', kinematics constants
The trajectories: Each motor corresponds to a task, which is given by a trajectory. One chooses a type of
trajectory in the set defined before (trapezium, polynomial3, GenTraj or doubleS). The choice
can be different for any motor. The program computes only real numbers so the user has to use
this protocol:
- trapezium = 1
- polynomial3 = 2
- GenTraj = 3
- doubleS = 4
22
Trajectory Planning for Robots JULIE PREVOT
Then the trajectory must be defined by his initial and final time and the boundary values of
position.
Figure 12: Picture from 'Machine.xlsx', trajectories
All information to fill correctly the document is indicated in a legend in the bottom of the page.
Figure 13: Picture from 'Machine.xlsx', legend
Matlab:
When one has inquired the data of the machine and saved the document, the file
machine.m can be opened with the matlab software and computed. The run can take some time:
30 seconds for one motor but less than 10 minutes for 3 motors.
As output, the program displays the vector Paramsol which is the result of the optimization
routine. Then FVAL indicates the total energy consumed by the machine defined before. The
value in Joule is the minimum value of the cost function. Eventually, “EXITFLAG = 4” means
that the method found a solution with the active-set method.
Warnings: - numbers out of the area provided to this purpose must never be written - Standard scientific units must be used - The excel file must be saved before computing the data
The following tab sums up everything needed to run the optimization:
Files Software
- trapezium.m - polynomial3.m - GenTraj.m - doubleS.m - powermotor.m - costfunction.m - nonlincon.m - Machine.m - Machine.xlsx
- Matlab - Microsoft office excel 2007
23
Trajectory Planning for Robots JULIE PREVOT
3) RESULTS
The few results presented thereafter come from the execution of the program with the
input parameters presented in the last part "manual". The execution time is 6 min 35 sec.
Figure 14: Display results of 'Machine.m'
Titles and legends are computed in the same time as the post processing of the results, in
order for the display to be the most precise possible and for the results to be exploitable visually.
One can notice that the translation motor uses a far more significant power than the two other
rotation motors. This observation is valid for any task assigned to the motors. One can view also
how simple is the power profile coming from a « trapezium » trajectory, which is due to an
acceleration constant by parts during every phase of the path.
24
Trajectory Planning for Robots JULIE PREVOT
To observe all the variations of power for the rotation motors, it is necessary to zoom on
the display, like on the figure above:
Figure 15: Zoom on the display
One can notice than the power profile of motor 1 is similar to the velocity profile of its
trajectory, while the one of the motor 2 is more comparable to the acceleration profile of its
trajectory. This difference is due to the kinematic constraints which gives weight to certain terms
compared to others. It would be necessary to realize a series of tests to identify precisely the
effects and interaction of the kinematic constants of the motors on the power profile.
4) CONCLUSION
The interface was simple to build and make the simulation tool easier to use. The most
important work around the interface was adapted the optimization procedure for it collects the
data in the Excel file, while keeping in mind data size of the problems to avoid cluttering of
benefit the program.
25
Trajectory Planning for Robots JULIE PREVOT
VII) Conclusion
The optimization work shows that innovation and development towards future
technologies don't lie exclusively in the creation of new innovative devices. It is also in the
optimization and better utilization of systems that already exist. Furthermore, robot trajectories
optimization does not apply only in the environment of the big industry. It can be used in all
areas where robotics can be found: the medical sector, transport, advanced industry, household
appliances, personal assistance...
From a personal point of view, this internship was the opportunity to apply my skills in
Matlab, algorithms, program writing, optimization, electronics, English and Italian. Indeed the
area of Mechatronics is as complete as diversified, like my general engineering training I put in
the service of this project. Finally, it is satisfying to see the project taking shape in the successful
deliverables which are algorithms, programs and interface.
During those four months I learned one hand to work independently and also the
importance and place of literature searches in this type of project. Indeed, the reliance on the
work of other scientists helped me a lot to focus on the optimization part of the work and
complete this work on time. This project would take more than four months of work if I had to
redo the work already done in these researches.
26
Trajectory Planning for Robots JULIE PREVOT
Internship continuation It is possible to improve the overall deliverable. Several tracks are to be considered:
- Propose a larger choice of trajectories. For instance: fifteen segments trajectory, piecewise
polynomial trajectory or modified trapezoidal trajectory.
- Find a better way of displaying the results. More visual and exploitable faster
- Include a macro to the excel file in order to make it an interactive interface and not only a
support for the data.
- Use a meta-heuristic method for the search of the optimum, in order to reduce the
computing time for huge problems.
Without intending to improve either the program or the interface, the overall tool I
developed is already exploitable as simulation tool. Indeed, the purpose of my project is to find
the optimal path for a given machine. More generally, one can identify types of preferred
trajectories for machine families and then study the influence of the trajectory input data on final
energy by performing an experimental design with Taguchi for instance.
Moreover, it is conceivable to use it to study precisely the energy gain that can generate
the replacement of a piece with better characteristics. Indeed it’s not just replacing a factor in the
calculation of power. Optimal trajectories of the entire machine may change. Therefore, one must
simulate the energy consumption of the new machine to appreciate the total energy saving.
27
Trajectory Planning for Robots JULIE PREVOT
Bibliography
[2.A]Nucleo di valutazione Uniud, Informazioni generali (viewed on May 25th, 2015) link:
http://nuva.uniud.it/scon/ateneo-in-cifre/informazioni-generali
[2.B]Wikipedia, University of Udine (viewed on July 8th, 2015) link:
https://en.wikipedia.org/wiki/University_of_Udine
[2.C]Universita degli studi di Udine, Piano strategico 2015-2019, 2014 (download link:
http://www.uniud.it/it/ateneo-uniud/ateneo-uniud-menu/piano-strategico/piano-strategico-
2015-2019/piano-strategico-pdf/view)
[3.B]Enhanced Approach for Energy-Efficient Trajectory Generation of Industrial Robots,
Christian Hansen, Julian Oltjen, Davis Meike, and Tobias Ortmaier, 2012
[3.C]Experimental Validation of Advanced Minimum Energy Robot Trajectory Optimization,
Christian Hansen, Jens Kotlarski, and Tobias Ortmaier, 2013
[4.A]Parametric Models of Function, Ph.D. dissertation, November 2010 - Florent Brunet
(Webpage generated on July 2011, viewed on May 21th, 2015), link:
http://www.brnt.eu/phd/node11.html
[4.B]Trajectory Planning for Automatic Machines and Robots, Luigi Biagiotti, Claudio
Melchiorri, Springer Verlag Berlin Heibelberg, 2008
[5.A]Jorge Nocedal , Stephen J. Wright, Numerical Optimization, Springer, 2006, 2e éd. (ISBN
978-0-387-30303-1), p. 308
[5.B]Introduction à l'optimisation numérique, Jean-Louis Dirion, École des Mines d'Albi
Carmaux, 2014
28
Trajectory Planning for Robots JULIE PREVOT
Table of illustrations Figure 1: Electric circuit of the robot model ................................................................................................7
Figure 2:Drawing of industrial robot with six axes .....................................................................................7
Figure 3: Mechanic schema of the two motors model .................................................................................8
Figure 4: Display of 'trapezium.m' trajectory ............................................................................................ 11
Figure 5: Display of 'polynomial3.m' trajectory ........................................................................................ 12
Figure 6: 3rd degrees B-spline .................................................................................................................... 13
Figure 7: 3rd degree B-spline with intervals of constant length .................................................................. 14
Figure 8: Display of 'GenTraj.m' trajectory............................................................................................... 14
Figure 9: Display of 'doubleS' trajectory ................................................................................................... 16
Figure 10: Picture from 'Machine.xlsx', number of motors ........................................................................ 21
Figure 11: Picture from 'Machine.xlsx', kinematics constants ................................................................... 21
Figure 12: Picture from 'Machine.xlsx', trajectories .................................................................................. 22
Figure 13: Picture from 'Machine.xlsx', legend ......................................................................................... 22
Figure 14: Display results of 'Machine.m' ................................................................................................. 23
Figure 15: Zoom on the display ................................................................................................................ 24
29
Trajectory Planning for Robots JULIE PREVOT
ANNEXES
30
Trajectory Planning for Robots JULIE PREVOT
2.1 Gantt planning of my internship
31
Trajectory Planning for Robots JULIE PREVOT
32
Trajectory Planning for Robots JULIE PREVOT
3.1 function powermotor.m %Author: Julie PREVOT
function [Pi]= powermotor(M,Q,dQ,d2Q,T)
global dt;
g=9.81;
type=M(1);
jadd=M(2);
muc=M(3);
muv=M(4);
km=M(5);
klss=M(6);
pigbt=M(7);
R=M(8);
l=M(9);
kg=M(10);
u=M(12);
if (type==0)
%data needed for rotation motor
j=M(11);
%end data
phi=u*Q;
dphi=u*dQ;
d2phi=u*d2Q;
taudyn=j*d2Q;
tauM=(1/u)*taudyn+jadd*d2Q+muc*sign(dphi)+muv*d2phi;
elseif (type==1)
%data needed for translation motor
r=M(13);
m=M(14);
alpha=M(15);
%end data
phi=(u/r)*Q;
dphi=(u/r)*dQ;
d2phi=(u/r)*d2Q;
taudyn=m*d2Q+m*g*sin(alpha)*ones(1,length(T));
tauM=(r/u)*taudyn+jadd*d2Q+muc*sign(dphi)+muv*d2phi;
end
i=tauM/km;
di=diff(i)/dt;
% function diff make a vector of size N-1, that is a problem to addition
% next because you need, same size matrix. so i add a column of zero.
di=[di 0];
v=R*i+l*di+kg*dphi;
Pm=v.*i;
Pi=Pm+klss*i+pigbt*ones(1,length(T));
end
33
Trajectory Planning for Robots JULIE PREVOT
4.1 function trapezium.m % Author: Julie PREVOT
function [Q,dQ,d2Q,T] = trapeze(q0,qf,t0,tf,a)
global dt;
deltaT=real((tf-t0-sqrt((tf-t0)^2-4*(qf-q0)/a))/2);
t1=t0+deltaT;
t2=tf-deltaT;
T1=t0:dt:t1;
Q1=(a/2)*(T1-t0*ones(1, length(T1))).^2+q0*ones(1, length(T1));
dQ1=a*(T1-t0*ones(1, length(T1)));
d2Q1=a*ones(1,length(T1));
T2=t1:dt:t2;
Q2=a*deltaT*(T2-t1*ones(1, length(T2)))+((a/2)*deltaT^2+q0)*ones(1, length(T2));
dQ2=a*deltaT*ones(1,length(T2));
d2Q2=zeros(1, length(T2));
T3=t2:dt:tf;
Q3=-(a/2)*(T3-tf*ones(1, length(T3))).^2+qf*ones(1, length(T3));
dQ3=-a*(T3-tf*ones(1, length(T3)));
d2Q3=-a*ones(1,length(T3));
T=[T1 T2 T3];
Q=[Q1 Q2 Q3];
dQ=[dQ1 dQ2 dQ3];
d2Q=[d2Q1 d2Q2 d2Q3];
end
4.2 function polynomial3.m % Author: Julie PREVOT
function [Q,dQ,d2Q,T] = polynome3(q0,qf,t0,tf,dq0,dqf)
global dt;
a0=q0;
a1=dq0;
a2=(3*(qf-q0)-(tf-t0)*(dqf+2*dq0))/(tf-t0)^2;
a3=(-2*(qf-q0)+(tf-t0)*(dqf+dq0))/(tf-t0)^3;
T=t0:dt:tf;
Q=a3*(T-t0*ones(1, length(T))).^3+a2*(T-t0*ones(1, length(T))).^2+a1*(T-
t0*ones(1, length(T)))+a0*ones(1, length(T));
dQ=3*a3*(T-t0*ones(1, length(T))).^2+2*a2*(T-t0*ones(1,
length(T)))+a1*ones(1, length(T));
d2Q=6*a3*(T-t0*ones(1, length(T)))+2*a2*ones(1, length(T));
end
34
Trajectory Planning for Robots JULIE PREVOT
4.3 function GenTraj.m function [Y,T]=GenTraj(A,V,P,Tj,Ts)
%GenTraj Trajectory generation for point to point motion with velocity,
% acceleration, jerk and snap (second time derivative of acceleration)
% constraints
% Example:[Y,T]=GenTraj(A,V,P,Tj,Ts) returns the position, velocity
% and acceleration profiles for a snap controlled law from the specified
% constraints on maximum velocity V, maximum acceleration A, desired
% travelling distance P, Jerk time Tj and Snap time Ts.
% Y is a 3 row matrix containing the position, velocity and acceleration
% profile associated to the time vector T.
%
% If Tj and Ts are not given, Tj=Ts=0 is assumed. The resulting mouvement
is
% acceleration limited. If Ts is not given, Ts=0 and P contains the points
% of the corresponding jerk limited law
%
% R. Béarée, ENSAM CER Lille, France
%
% 2007-06-14
%--------------------------------------------------------------------------
if nargin<3
error('At Least Three Input Arguments are Required.')
end
if nargin==3
type=0;
Tj=0;
Ts=0;
elseif nargin==4
type=1;
Ts=0;
elseif nargin==5
type=2;
end
global dt;
Te=dt; % interpolation time
% Verification of the acceleration and velocity constraints
Ta=V/A; % Acceleration time
Tv=(P-A*Ta^2)/(V); % Constant velocity time
if P<=Ta*V % Triangular velocity profile
Tv=0;Ta=sqrt(P/A);
end
Tf=2*Ta+Tv+Tj+Ts; % Mouvement time
% Elaboration of the limited acceleration profile
T=0:Te:Tf;
t(1)=0;t(2)=Ta;t(3)=Ta+Tv;t(4)=2*Ta+Tv;
s(1)=1;s(2)=-1;s(3)=-1;s(4)=1;
P=zeros(3,length(T));
% Ech=zeros(4);
for k=1:3
u=zeros(1,k+1);u(1,1)=1;
for i=1:4
Ech = tf(1, u,'inputdelay',t(i));
law(i,:)=impulse(s(i)*A*(Ech),T);
end
35
Trajectory Planning for Robots JULIE PREVOT
Y(k,:)=sum(law);
end
if (type==1 || type==2)
% Average Filter for Jerk limitation
a = 1; % Filter coefficients
b = (1/(Tj/Te))*ones(1,fix(Tj/Te)); % Filter duration equal to jerk time
Y(3,:)= filter(b,a,Y(3,:));
Y(2,1:length(T)-1)=diff(Y(3,:),1)/Te;
Y(1,1:length(T)-1)=diff(Y(2,:),1)/Te;
if type==2
% Average Filter for snap limitation
a = 1; % Filter coefficients
b = (1/(Ts/Te))*ones(1,fix(Ts/Te)); % Filter duration equal to snap time
Y(3,:)= filter(b,a,Y(3,:));
Y(2,1:length(T)-1)=diff(Y(3,:),1)/Te;
Y(1,1:length(T)-1)=diff(Y(2,:),1)/Te;
end
end
%%%%%%%%%%%%%%%
% figure;
% sp(1)=subplot(3,1,1);plot(T,Y(3,:))
% sp(2)=subplot(3,1,2);plot(T,Y(2,:))
% sp(3)=subplot(3,1,3);plot(T,Y(1,:))
% linkaxes(sp,'x');
% ylabel(sp(1),'Position [m]');ylabel(sp(2),'Velocity
[m/s]');ylabel(sp(3),'Acceleration [m/s^2]');xlabel(sp(3),'Time [s]');
36
Trajectory Planning for Robots JULIE PREVOT
4.4 function doubleS.m % Author: Julie PREVOT
function [Q,dQ,d2Q,d3Q,T] = doubleS(q0,qf,v0,vf,jmax,amax,vmax)
global dt;
Tj=min(sqrt(abs(vf-v0)/jmax),amax/jmax);
% trajectory existence condition
if (Tj<(amax/jmax)&&(qf-q0)>Tj*(v0+vf))
B=1;
elseif (Tj==(amax/jmax)&&(qf-q0)>(1/2)*(v0+vf)*(Tj+abs(vf-v0)/amax))
B=1;
else
B=0;
end
% if amax is not reached
if ((vmax-v0)*jmax<amax^2)
Tj1=sqrt((vmax-v0)/jmax);
TA=2*Tj1;
else
Tj1=amax/jmax;
TA=Tj1+(vmax-v0)/amax;
end
% if amin is not reached
if((vmax-vf)*jmax<amax^2)
Tj2=sqrt((vmax-vf)/jmax);
TD=2*Tj2;
else
Tj2=amax/jmax;
TD=Tj2+(vmax-vf)/amax;
end
Tv=((qf-q0)/vmax)-(TA/2)*(1+(v0/vmax))-(TD/2)*(1+(vf/vmax));
% if Tv>0 the maximum velocity is reached, if tv<0, vlim is smaller than
% vmax, this is the seconde case:
if(Tv<0)
Tv=0;
Tj1=amax/jmax;
Tj2=amax/jmax;
delta=(amax^4/jmax^2)+2*(v0^2+vf^2)+amax*(4*(qf-q0)-2*(amax/jmax)*(v0+vf));
TA=((amax^2/jmax)-2*v0+sqrt(delta))/(2*amax);
TD=((amax^2/jmax)-2*vf+sqrt(delta))/(2*amax);
if (TA<0)
TA=0;
TD=2*(qf-q0)/(vf+v0);
Tj2=(jmax*(qf-q0)-sqrt(jmax*(jmax*(qf-q0)^2+(vf+v0)^2*(vf-v0))))/(jmax*(vf+v0));
end
if (TD<0)
TD=0;
TA=2*(qf-q0)/(vf+v0);
Tj1=(jmax*(qf-q0)-sqrt(jmax*(jmax*(qf-q0)^2+(vf+v0)^2*(vf-v0))))/(jmax*(vf+v0));
end
end
Ta=TA-2*Tj1;
Td=TD-2*Tj2;
t0=0;
t1=t0+Tj1;
t2=t1+Ta;
t3=t2+Tj1;
t4=t3+Tv;
t5=t4+Tj2;
t6=t5+Td;
t7=t6+Tj2;
if B
% if the trajectory could be generate => do it!
T1=t0:dt:t1;
Q1=q0*ones(1, length(T1))+v0*(T1-t0*ones(1,length(T1)))+(jmax/6)*(T1-t0*ones(1,
length(T1))).^3;
dQ1=v0*ones(1,length(T1))+(jmax/2)*(T1-t0*ones(1, length(T1))).^2;
d2Q1=jmax*(T1-t0*ones(1, length(T1)));
d3Q1=jmax*ones(1, length(T1));
37
Trajectory Planning for Robots JULIE PREVOT
T2=t1:dt:t2;
Q2=(q0+v0*Tj1+(jmax/6)*Tj1^3)*ones(1, length(T2))+(v0+(jmax/2)*Tj1^2)*(T2-
t1*ones(1,length(T2)))+(jmax/2)*Tj1*(T2-t1*ones(1, length(T2))).^2;
dQ2=(v0+(jmax/2)*Tj1^2)*ones(1,length(T2))+jmax*Tj1*(T2-t1*ones(1, length(T2)));
d2Q2=jmax*Tj1*ones(1,length(T2));
d3Q2=0*ones(1,length(T2));
T3=t2:dt:t3;
Q3=(q0+v0*(Tj1+Ta)+(jmax/6)*Tj1^3+(jmax/2)*Tj1^2*Ta+(jmax/2)*Tj1*Ta^2)*ones(1,
length(T3))+(v0+(jmax/2)*Tj1^2+jmax*Tj1*Ta)*(T3-t2*ones(1,length(T3)))+(jmax/2)*Tj1*(T3-
t2*ones(1,length(T3))).^2-(jmax/6)*(T3-t2*ones(1, length(T3))).^3;
dQ3=(v0+(jmax/2)*Tj1^2+jmax*Tj1*Ta)*ones(1, length(T3))+jmax*Tj1*(T3-
t2*ones(1,length(T3)))-(jmax/2)*(T3-t2*ones(1, length(T3))).^2;
d2Q3=jmax*Tj1*ones(1,length(T3))-jmax*(T3-t2*ones(1, length(T3)));
d3Q3=-jmax*ones(1, length(T3));
T4=t3:dt:t4;
Q4=(q0+v0*(2*Tj1+Ta)+(3*jmax/2)*Tj1^2*Ta+(jmax/2)*Tj1*Ta^2+jmax*Tj1^3)*ones(1,
length(T4))+(v0+jmax*Tj1^2+jmax*Tj1*Ta)*(T4-t3*ones(1, length(T4)));
dQ4=(v0+jmax*Tj1^2+jmax*Tj1*Ta)*ones(1, length(T4));
d2Q4=zeros(1,length(T4));
d3Q4=zeros(1,length(T4));
T5=t4:dt:t5;
Q5=(q0+v0*(2*Tj1+Ta+Tv)+(3*jmax/2)*Tj1^2*Ta+(jmax/2)*Tj1*Ta^2+jmax*Tj1^3+(jmax*Tj1^2+jmax*Tj1*
Ta)*Tv)*ones(1, length(T5))+(v0+jmax*Tj1^2+jmax*Tj1*Ta)*(T5-t4*ones(1, length(T5)))-
(jmax/6)*(T5-t4*ones(1,length(T5))).^3;
dQ5=(v0+jmax*Tj1^2+jmax*Tj1*Ta)*ones(1, length(T5))-(jmax/2)*(T5-
t4*ones(1,length(T5))).^2;
d2Q5=-jmax*(T5-t4*ones(1,length(T5)));
d3Q5=-jmax*ones(1, length(T5));
T6=t5:dt:t6;
Q6=(q0+v0*(2*Tj1+Ta+Tv+Tj2)+(3*jmax/2)*Tj1^2*Ta+(jmax/2)*Tj1*Ta^2+jmax*Tj1^3+(jmax*Tj1^2+jmax*
Tj1*Ta)*Tv+(jmax*Tj1^2+jmax*Tj1*Ta)*Tj2-(jmax/6)*Tj2^3)*ones(1,
length(T6))+(v0+jmax*Tj1^2+jmax*Tj1*Ta-(jmax/2)*Tj2^2)*(T6-t5*ones(1, length(T6)))-
(jmax/2)*Tj2*(T6-t5*ones(1, length(T6))).^2;
dQ6=(v0+jmax*Tj1^2+jmax*Tj1*Ta-(jmax/2)*Tj2^2)*ones(1, length(T6))-jmax*Tj2*(T6-t5*ones(1,
length(T6)));
d2Q6=-jmax*Tj2*ones(1, length(T6));
d3Q6=zeros(1,length(T6));
T7=t6:dt:t7;
Q7=(q0+v0*(2*Tj1+Ta+Tv+Tj2+Td)+(3*jmax/2)*Tj1^2*Ta+(jmax/2)*Tj1*Ta^2+jmax*Tj1^3+(jmax*Tj1^2+jm
ax*Tj1*Ta)*Tv+(jmax*Tj1^2+jmax*Tj1*Ta)*Tj2-(jmax/6)*Tj2^3+(jmax*Tj1^2+jmax*Tj1*Ta-
(jmax/2)*Tj2^2)*Td-(jmax/2)*Tj2*Td^2)*ones(1, length(T7))+(v0+jmax*Tj1^2+jmax*Tj1*Ta-
(jmax/2)*Tj2^2-jmax*Tj2*Td)*(T7-t6*ones(1, length(T7)))-(jmax/2)*Tj2*(T7-t6*ones(1,
length(T7))).^2+(jmax/6)*(T7-t6*ones(1, length(T7))).^3;
dQ7=(v0+jmax*Tj1^2+jmax*Tj1*Ta-(jmax/2)*Tj2^2-jmax*Tj2*Td)*ones(1, length(T7))-
jmax*Tj2*(T7-t6*ones(1, length(T7)))+(jmax/2)*(T7-t6*ones(1, length(T7))).^2;
d2Q7=-jmax*Tj2*ones(1, length(T7))+jmax*(T7-t6*ones(1, length(T7)));
d3Q7=jmax*ones(1, length(T7));
T=[T1 T2 T3 T4 T5 T6 T7];
Q=[Q1 Q2 Q3 Q4 Q5 Q6 Q7];
dQ=[dQ1 dQ2 dQ3 dQ4 dQ5 dQ6 dQ7];
d2Q=[d2Q1 d2Q2 d2Q3 d2Q4 d2Q5 d2Q6 d2Q7];
d3Q=[d3Q1 d3Q2 d3Q3 d3Q4 d3Q5 d3Q6 d3Q7];
else
% else, the trajectory couldn't be generated => it sets zero
T=t0:dt:t7;
Q=zeros(1,length(T));
dQ=zeros(1,length(T));
d2Q=zeros(1,length(T));
d3Q=zeros(1,length(T));
end
end
38
Trajectory Planning for Robots JULIE PREVOT
5.1 function costfunction.m % Author: Julie PREVOT
function E=costfunction(Param)
global dt Data;
dt=0.001;
Data=xlsread('Machine.xlsx');
% Traj represents data for trajectories, defined in the xls file.
Traj=zeros(Data(1,1),5);
% a necessary step is to define Tf, before any calculation of power
Tf=0;
i=1;
for k=1:1:Data(1,1)
Traj(k,:)=Data(22:26,k).';
if (Traj(k,1)==1) %trapezium
i=i+1;
elseif(Traj(k,1)==3) %GenTraj
[Y,T]=GenTraj(Param(i),Param(i+1),Traj(k,5)-Traj(k,4),Param(i+2),Param(i+3));
Traj(k,3)=T(length(T))+Traj(k,2);
i=i+4;
elseif (Traj(k,1)==4) %doubleS
[Q,dQ,d2Q,d3Q,T]=doubleS(Traj(k,4),Traj(k,5),0,0,Param(i),Param(i+1),Param(i+2));
Traj(k,3)=T(length(T))+Traj(k,2);
i=i+3;
end
if (Tf<Traj(k,3))
Tf=Traj(k,3);
end
end
P=zeros(Data(1,1),fix(Tf/dt));
i=1;
for k=1:1:Data(1,1)
% compute data to generate the trajectory
if (Traj(k,1)==1)
[Q,dQ,d2Q,T]=trapezium(Traj(k,4),Traj(k,5),Traj(k,2),Traj(k,3),Param(i));
i=i+1;
elseif (Traj(k,1)==2)
[Q,dQ,d2Q,T]=polynomial3(Traj(k,4),Traj(k,5),Traj(k,2),Traj(k,3),0,0);
% We impose v0=vf=0
elseif (Traj(k,1)==3)
[Y,T]=GenTraj(Param(i),Param(i+1),Traj(k,5)-Traj(k,4),Param(i+2),Param(i+3));
Q=Y(3,:);
dQ=Y(2,:);
d2Q=Y(1,:);
Q=Q+Traj(k,4)*ones(1,length(Q));
T=T+Traj(k,2)*ones(1,length(T));
i=i+4;
elseif (Traj(k,1)==4)
[Q,dQ,d2Q,d3Q,T]=doubleS(Traj(k,4),Traj(k,5),0,0,Param(i),Param(i+1),Param(i+2));
T=T+Traj(k,2)*ones(1,length(T));
i=i+3;
end
% then calculate the power
PM=powermotor(Data(5:20,k).',Q,dQ,d2Q,T);
for j=1:1:length(PM)
P(k,Traj(k,2)/dt+j)=PM(j);
end
end
Pt=sum(P,1);
E=0;
for j=1:1:Tf/dt
E=E+max(0,Pt(j))*dt;
end
end
39
Trajectory Planning for Robots JULIE PREVOT
5.2 function nonlincon.m
function [c,ceq]= nonlincon (Param)
global Data;
Traj=zeros(Data(1,1),5);
c=[];
ceq=[];
i=1;
for k=1:1:Data(1,1)
Traj(k,:)=Data(22:26,k).';
if (Traj(k,1)==1) %trapezium
i=i+1;
elseif (Traj(k,1)==3) %GenTraj
Ta=Param(i+1)/Param(i); % Acceleration time
Tv=((Traj(k,5)-Traj(k,4))-Param(i)*Ta^2)/(Param(i+1)); % Constant velocity time
if (Traj(k,5)-Traj(k,4))<=Ta*Param(i+1) % Triangular velocity profile
Tv=0;
Ta=sqrt((Traj(k,5)-Traj(k,4))/Param(i));
end
ceq=[ceq
Traj(k,3)-Traj(k,2)-(2*Ta+Tv+Param(i+2)+Param(i+3))];
i=i+4;
elseif (Traj(k,1)==4) %doubleS
% As v0=vf=0 the existence condition are always verified
if ((Param(i+2))*Param(i)<Param(i+1)^2)
Tj1=sqrt((Param(i+2))/Param(i));
TA=2*Tj1;
else
Tj1=Param(i+1)/Param(i);
TA=Tj1+(Param(i+2))/Param(i+1);
end
if((Param(i+2))*Param(i)<Param(i+1)^2)
Tj2=sqrt((Param(i+2))/Param(i));
TD=2*Tj2;
else
Tj2=Param(i+1)/Param(i);
TD=Tj2+(Param(i+2))/Param(i+1);
end
Tv=((Traj(k,5)-Traj(k,4))/Param(i+2))-(TA/2)-(TD/2);
if(Tv<0)
Tv=0;
Tj1=Param(i+1)/Param(i);
Tj2=Param(i+1)/Param(i);
delta=(Param(i+1)^4/Param(i)^2)+Param(i+1)*(4*(Traj(k,5)-Traj(k,4)));
TA=((Param(i+1)^2/Param(i))+sqrt(delta))/(2*Param(i+1));
TD=((Param(i+1)^2/Param(i))+sqrt(delta))/(2*Param(i+1));
if (TA<0) % Must never happen !?
TA=0;
TD=2*(Traj(k,5)-Traj(k,4))/(0+0);
Tj2=(Param(i)*(Traj(k,5)-Traj(k,4))-sqrt(Param(i)*(Param(i)*(Traj(k,5)-
Traj(k,4))^2)))/(Param(i)*(0+0));
end
if (TD<0)
TD=0;
TA=2*(Traj(k,5)-Traj(k,4))/(0+0);
Tj1=(Param(i)*(Traj(k,5)-Traj(k,4))-sqrt(Param(i)*(Param(i)*(Traj(k,5)-
Traj(k,4))^2)))/(Param(i)*(0+0));
end
end
ceq=[ceq
Traj(k,3)-Traj(k,2)-(TA+Tv+TD)
Param(i+1)-Param(i)*max(Tj1,Tj2)];
i=i+3;
end
end
end
40
Trajectory Planning for Robots JULIE PREVOT
6.1 Excel file Machine.xlsx
41
Trajectory Planning for Robots JULIE PREVOT
6.2 Program Machine.m % Author: Julie PREVOT
% Main program
% This program's aim is: First, to define the machine: number of motors and
% kinetic constants. Secondly, it defines and generates all trajectories.
% Eventually it calculates the power and the energy.
% There are 'xlswrite' in comments, so MS excel must be installed on the
% computer of the user, otherwise this function can't be used
clear all
global dt Data;
dt=0.001;
Data=xlsread('Machine.xlsx');
% N is the number of motors in the machine
N=Data(1,1);
% Traj represents data for trajectories
% Param0 is built, which is the initial parameters sent to the optimizing
% routine, same for ParamMin,ParamMax, and matrix for constraints.
Traj=zeros(N,5);
Param0=[];
ParamMin=[];
ParamMax=[];
A=[];
B=[];
Aeq=[];
Beq=[];
C=0;
for k=1:1:N
Traj(k,:)=Data(22:26,k).';
if (Traj(k,1)==1) %trapezium
Param0=[Param0
20*((Traj(k,5)-Traj(k,4))/(Traj(k,3)-Traj(k,2))^2)];
ParamMin=[ParamMin
0];
ParamMax=[ParamMax
500];
SA=size(A);
A=[A zeros(SA(1),1)
zeros(1,SA(2)) -1];
B=[B
-4*(Traj(k,5)-Traj(k,4))/(Traj(k,3)-Traj(k,2))^2];
Aeq=[Aeq zeros(SA(1),1)
zeros(1,SA(2)) 0];
Beq=[Beq
0];
C=1;
elseif (Traj(k,1)==3) %GenTraj
Param0=[Param0
20*((Traj(k,5)-Traj(k,4))/(Traj(k,3)-Traj(k,2))^2)
5*((Traj(k,5)-Traj(k,4))/(Traj(k,3)-Traj(k,2)))
1
3];
ParamMin=[ParamMin
0.1
0.1
0.001
0.001];
ParamMax=[ParamMax
500
500
500
500];
SA=size(A);
A=[A zeros(SA(1),4)
zeros(4,SA(2)) zeros(4,4)];
42
Trajectory Planning for Robots JULIE PREVOT
B=[B
zeros(4,1)];
Aeq=[Aeq zeros(SA(1),4)
zeros(4,SA(2)) zeros(4,4)];
Beq=[Beq
zeros(4,1)];
C=1;
elseif (Traj(k,1)==4) %doubleS
Param0=[Param0
20*((Traj(k,5)-Traj(k,4))/(Traj(k,3)-Traj(k,2))^3)
10*((Traj(k,5)-Traj(k,4))/(Traj(k,3)-Traj(k,2))^2)
1*((Traj(k,5)-Traj(k,4))/(Traj(k,3)-Traj(k,2)))];
ParamMin=[ParamMin
0.1
0.1
0.1];
ParamMax=[ParamMax
500
500
500];
SA=size(A);
A=[A zeros(SA(1),3)
zeros(3,SA(2)) zeros(3,3)];
B=[B
zeros(3,1)];
Aeq=[Aeq zeros(SA(1),3)
zeros(3,SA(2)) zeros(3,3)];
Beq=[Beq
zeros(3,1)];
C=1;
end
end
if C %There isn't only polynomial3 type of trajectory
[Paramsol,FVAL,EXITFLAG,output]=fmincon('costfunction',Param0,A,B,Aeq,Beq,ParamMin,
ParamMax,'nonlincon')
% Else the parameter vector is empty, the optimization cannot be used
% and is useless because there is not parameter to optimize.
end
% a necessary step is to define Tf, before any calculation of power
Tf=0;
i=1;
for k=1:1:N
if (Traj(k,1)==1) %trapezium
i=i+1;
elseif(Traj(k,1)==3) %GenTraj
[Y,T]=GenTraj(Paramsol(i),Paramsol(i+1),Traj(k,5)-
Traj(k,4),Paramsol(i+2),Paramsol(i+3));
Traj(k,3)=T(length(T))+Traj(k,2);
i=i+4;
elseif (Traj(k,1)==4) %doubleS
[Q,dQ,d2Q,d3Q,T]=doubleS(Traj(k,4),Traj(k,5),0,0,Paramsol(i),Paramsol(i+1),Paramsol
(i+2));
% v0=vf=0
Traj(k,3)=T(length(T))+Traj(k,2);
i=i+3;
end
if (Tf<Traj(k,3))
Tf=Traj(k,3);
end
end
%xlswrite('Machine.xlsx',Traj(:,3),'F27:O27');
P=zeros(N,fix(Tf/dt));
i=1;
for k=1:1:N
43
Trajectory Planning for Robots JULIE PREVOT
% compute data to generate the trajectory
if (Traj(k,1)==1)
type='trapezium';
[Q,dQ,d2Q,T]=trapezium(Traj(k,4),Traj(k,5),Traj(k,2),Traj(k,3),Paramsol(i));
i=i+1;
elseif (Traj(k,1)==2)
type='polynomial3';
[Q,dQ,d2Q,T]=polynomial3(Traj(k,4),Traj(k,5),Traj(k,2),Traj(k,3),0,0);
% We impose v0=vf=0
elseif (Traj(k,1)==3)
type='GenTraj';
[Y,T]=GenTraj(Paramsol(i),Paramsol(i+1),Traj(k,5)-
Traj(k,4),Paramsol(i+2),Paramsol(i+3));
Q=Y(3,:);
dQ=Y(2,:);
d2Q=Y(1,:);
Q=Q+Traj(k,4)*ones(1,length(Q));
T=T+Traj(k,2)*ones(1,length(T));
i=i+4;
else
type='doubleS';
[Q,dQ,d2Q,d3Q,T]=doubleS(Traj(k,4),Traj(k,5),0,0,Paramsol(i),Paramsol(i+1),Paramsol
(i+2));
%v0=vf=0
T=T+Traj(k,2)*ones(1,length(T));
i=i+3;
end
subplot(N+1,1,k)
plot(T,Q,T,dQ,T,d2Q)
title(['Trajectory for motor : ',num2str(k),', type=',type])
legend('position','velocity','acceleration')
xlabel('time(s)')
% then calculate the power
PM=powermotor(Data(5:20,k).',Q,dQ,d2Q,T);
for j=1:1:length(PM)
P(k,Traj(k,2)/dt+j)=PM(j);
end
LEG{k} = ['motor' int2str(k)];
end
T=0:dt:(length(P(1,:))-1)*dt;
Pt=sum(P,1);
LEG{N+1}='sum of all motors';
subplot(N+1,1,N+1)
plot (T,P,T,Pt)
legend(LEG)
xlabel('time(s)')
ylabel('power')
E=0;
for j=1:1:Tf/dt
E=E+max(0,P(j))*dt;
end
%xlswrite('Machine.xlsx',E,'N42');