MASTER INTERNSHIP REPORTjprevot/Documents/...MASTER 1 INTERNSHIP REPORT TRAJECTORY PLANNING FOR ROBOTS From May 4th to August 21th, 2015 Student: Julie PREVOT- IFIE 2016 Tutor: Alessandro

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


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


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


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


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


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


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


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


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


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


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



- 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.


- 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


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


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


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



- 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


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


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)


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


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


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


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


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


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


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


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


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


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

http://nuva.uniud.it/scon/ateneo-in-cifre/informazioni-generali

https://en.wikipedia.org/wiki/University_of_Udine

http://www.uniud.it/it/ateneo-uniud/ateneo-uniud-menu/piano-strategico/piano-strategico-2015-2019/piano-strategico-pdf/view

http://www.uniud.it/it/ateneo-uniud/ateneo-uniud-menu/piano-strategico/piano-strategico-2015-2019/piano-strategico-pdf/view

http://www.brnt.eu/phd/node11.html

https://fr.wikipedia.org/wiki/International_Standard_Book_Number

https://fr.wikipedia.org/wiki/Sp%C3%A9cial:Ouvrages_de_r%C3%A9f%C3%A9rence/978-0-387-30303-1

28


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


ANNEXES

30


2.1 Gantt planning of my internship

31


32


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


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


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


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;


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,:));



end

end

%%%%%%%%%%%%%%%

% figure;

% sp(1)=subplot(3,1,1);plot(T,Y(3,:))



% 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


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


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));


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));


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


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));


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


[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;


[Q,dQ,d2Q,d3Q,T]=doubleS(Traj(k,4),Traj(k,5),0,0,Param(i),Param(i+1),Param(i+2));


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


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).';


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;


% 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


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;



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


6.1 Excel file Machine.xlsx

41


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).';


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


B=[B

zeros(4,1)];



Beq=[Beq

zeros(4,1)];

C=1;


Param0=[Param0



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)


B=[B

zeros(3,1)];



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


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));


i=i+4;


[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


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


% 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;


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


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));


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


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');