hello everyone this is Mohammed and

welcome to another video tutorial on

model predictive control and then it's

implementa­tion that using Casali just a

quick note if you haven't seen at least

the first video lecture and this

playlist so please do so before watching

this video or otherwise this video will

be completely undone sense to you so I

have received many questions from those

who have seen already the first two

lectures about the implementa­tion of MPC

for a trajectory tracking problem and

how to do this in Cassidy so in this

video tutorial I'm going to show you how

to do this which turns out to be a very

straightfo­rward extension to the code we

already discussed before for the point

stabilizat­ion control problem so again

I'm going to use the mobile robot

example as an implementa­tion example so

let's go ahead and first recover or

recall the dynamics model of the system

we are considerin­g which is basically a

differenti­al Drive robot system that has

this kinematics model with three states

X Y and theta x and y are the position

states and theta is the orientatio­n or

the heading of the robot or the heading

angle and me remember that we talked

about two common control objectives in

control problems in general the first

one being the point stabilizat­ion

problem where the reference we would

like to stabilize our system to is a

fixed point in time so for example if

we're looking at a point stabilizat­ion

we set the reference to be these three

values for the exposition the white

position and the angular position theta

and these three the three values are

fixed for all the time steps on the

other hand if we have a trajectory

tracking problem this means that we have

or we would like our robot to follow a

certain trajectory that has different

states or or that has estates that

evolve with time so let's recap the

model collective control or the NPC

formulatio­n for a differenti­al try and

drive orbit so we said that we will use

kinematics model and we'll use this

discrete time model to predict the state

of the robot right and then we said well

our NPC controller running costs is

going to be a penalizati­on for the

deviation between the predicted state of

the robot and the reference state and

also the penalisati­on for the deviation

or the difference between the control

action we would like to apply and the

reference control action then we

formulated our optimal control problem

or OCP for our NPC controller where we

would like to minimize a cost function

that is by simply a summation over a

certain prediction horizon length and

for D running costs that we showed up

here then we would like to minimize this

cost function while the minimizati­on is

subject to these constraint­s of course

one of the most important constraint is

basically the dynamics model actually

this formulatio­n in this form is valid

for both for instable ization problem

and the trajectory tracking problem and

I as this poles down to the way we

you were like if xref and UF are fixed

with time so that's point stabilizat­ion

if x-ray pannier are changing with time

then it's a tracking problem so let's

stabilizat­ion problem so in a point

stabilizat­ion problem our robot starts

from an initial condition X 0 and we

reference position and orientatio­n as

you see here so let's say that this

reference position has the state 2 2 and

0 like we would like our would move 2

meters and X 2 meters and Y and

stabilize its orientatio­n to 0 degrees

or 0 radians so this actually this

vector is actually our x-ray vector all

right so what about Europe so Europe is

the control action that would stabilize

the system there so if we'd like our

robot to be there we simply set it the

speeds to 0 right so let's look now at

our optimal control problem we saw on

the previous slide and let's expand the

prediction rise in length and off trees

let's say that N equals 2/3 and expand

this JN so if we do so and substitute

with the values for extract and since

this is a point stabilizat­ion problem

you will see that the three three terms

for the running costs has basically

like 2 to 0 here and that's repeated for

all the time steps or all the prediction

steps and also your F is fixed and

equals to 0 for all the prediction steps

so now let's look at the trajectory

tracking problem so here's our over

again and this time we would like our

over to like track trajectory instead of

just being stabilized at as a particular

trajectory that is parallel to the

x-axis with this equation so the x

position or the difference x position

that we would like to check is it

changing with time and with a constant Y

position and a constant orientatio­n so

this reference means that we would like

our robot to go from left to right along

this line while facing Y or while the

robot is facing the positive x direction

so what the first thing you would not

hear about this reference that this

reference now is a function of time so

the reference is not affected anymore as

the point stabilizat­ion case so for

example when the time T here is zero

extra becomes zero one zero and that's

the first point along my trajectory and

when T equals to 24 for example the

reference vector becomes twelve one and

two and let's take this as the endpoint

of our trajectory so this is X Ref so

far so what about Europe so we said that

here is the reference control action

that would make my robot go along this

trajectory alright so how to get this

actually we should have a look at our

kinematics equation in order to be able

to get a UF so we know that Europe has

two components V and Omega so looking at

the kinematics equation if we Square and

add the first two equations we will end

up having this equation X dot square

plus y dot square equals to v square

because if you squared this term and add

it to the square of this term you would

have sine square plus cosine square and

this big this is basically 1 multiplied

v multiplied by v square so now having

this formula if we find the first

derivative of the exposition or the

reference exposition and the first

and the reference position and why we

will end up having that V equals two

point half because if you go up here so

that's this is X of T so it's derivative

is 0.5 and this is y of T and it's

derivative is basically zero so this

equation for this boils down to V

squared equals two X dot squared over V

equals to zero point five meters per

second now by looking at the third

equation here we simply have Omega

equals two theta dot or the derivative

of the orientatio­n so going up to our

reference extra-extr­a and then going

orientatio­n which is zero and this means

that it's derivative is also zero and

this means that the reference for Omega

or or the angular speed is zero radians

per second actually these two control

actions or these two values for my

Europe all the reference control actions

so we have a point five meters per

second reference control for V and the

zero radians per second reference

control or the angular speed actually

this mu ref is also called the feed

forward control action so these are the

control actions that if my robot is

starting from the right position at the

right time on this trajectory and then

we apply these control actions or feed

forward control actions the robot would

follow exactly this trajectory so now

that we know our X rap and u F of the

trajectory we would like to follow or

track so let's expand the objective

function of our optimal control problem

for n equal to three similar to what we

did and the point stabilisat­ion case so

in order to do so we have to get the

first three reference points along this

trajectory so let's say so for T equals

to zero the first reference along this

come 0 1 and 0 and then let's say that

our sampling time is 0.5 seconds so this

means that the next reference point

along this trajectory is obtained by

substituti­ng T equals to 0.5 so if we'd

go to extract and substitute T equals to

0.5 we end up having this reference

posture and then the following time step

for for T equals to 1 this becomes our

reference for the state vector for the

three cases and actually for all the

points along this trajectory Europe is

constant at 0.5 for V and U for Omega so

let's now expand JN so as you can see

here J and is expanded for N equals to 3

this means that we are adding up 3

running costs so as you can see extract

is it changing for every time step and

Europe is constant here but it's not

stabilizat­ion case I just would like to

let you know that this is a very simple

trajectory tracking problem or it's a

very simple trajectory to follow but in

other cases we can have T appears

everywhere in x-ray and Europe so I'll

show you how to program the an MPC

controller for trajectory tracking

problem for this x-ray and Europe and

this is or can be generalize­d for any

type of reference trajectori­es too so

let's look at the matlab implementa­tion

for the trajectory tracking problem it

turns out that we will use mostly the

code we used for the point stabilizat­ion

case and we will have just a few changes

to implement the trajectory tracking the

first change is with the definition of

where we previously had this vector of

size n states plus n states so the first

n states elements and this vector where

my initial condition and the other

elements where my reference state or

x-ray and the trajectory tracking case

we have been have the first n states

element but we have more elements for to

define the reference trajectory so what

we have extra this time is we have the

number of states and number of controls

times n parameters these parameters are

prediction rising steps and so now we're

implementa­tion we will stop again at the

definition of the objective function or

the variable option so previously in the

point stabilizat­ion case we had the

penalisati­on of the deviation between

the predicted state and the reference

state and as you notice or as you can

notice here the reference state which

was a specific portion of beef was the

same for all the predictor for all the

prediction steps but for the trajectory

tracking case which we are considerin­g

in this video we are like for every step

along the prediction we are penalizing

the deviation between the state and a

different reference because for each

step along the prediction the reference

is exchanged and the same with the

control so this what we have from here

is the reference state and what we have

from the parameters vector or P here is

the reference control so in the point

stabilizat­ion case as also as you can

also notice we were directly panel

the control action because the reference

at that time was just zero so we simply

penalize the control action by Aleksey

and this means that we are penalizing

the variation between the control action

and simply zero so now moving forward

with the implementa­tion everything

stabilizat­ion case until we stop again

at the simulation loop where we fill the

variable Erick's that fee by the initial

condition and we reference for all the

controls and for all the states

all right so previously in the point

stabilizat­ion case we were filling this

vector by just the initial condition and

the reference condition which was fixed

but now as the reference is a changing

for every prediction step we will fill

the eggs the D vector differentl­y so

again the first three elements and the

arms that we are going to be the initial

condition for the robot and inside this

for loop we will fill the rest of our

eggs would be here we are filling eggs

would be by the reference state xref

wider phaedra and here in this line we

film aired the B by the reference for

the control actions which are the EU f

which is VF and Omega F right okay so

I'm defining a new variable inside this

loop which is called the current time

and this current time gives gives us or

tells us the the time that correspond­s

to the current MPC hydration and it

basically equals to M facilitati­on times

descent and time T right so going back

again to this for loop where we define

our reference we calculate the time that

correspond­s to that predicted step

right and this predicted time is

basically my current time plus their

predicted step times the sampling time

right so knowing this T product we can

now calculate X rap because x-ray is

while F and theta s remains the same

because we are considerin­g that straight

line reference trajectory we also

defined Ryu ref and Omega R which are my

reference for the linear speed and the

reference for the angular speed of the

robot right and we drive these two

values in the flight this if condition

is simply a stopping condition that

marks the end of the directory so we say

that if X rap that keeps updating up

here if this reaches the value of 12

then our robot should stop at this

position so let's run the code now so

I'll go to editor and then run so we'll

get this closed loop performanc­e so as

you can see this is our reference

trajectory and you can see that the

robot converts to that trajectory and

kept tracking it and as the robot

reaches the end of the trajectory the

reference becomes the same point and the

robot stops there so when the simulation

ends you will see a graph for the

closed-loo­p control actions for V and

Omega so if we have a closer look at our

control action so you will see that

during the transient part where my robot

was like converging to the directory the

control action was just hitting the

limit of the maximal speed which is 0.6

in this case and this and then sorry it

compares it and converged it down to 0.5

which is the reference control action

that we set right and as the robot

reaches the end point of the trajectory

the reference for the linear speed

becomes zero again because now our

problem or the problem is a trajectory

tracking sorry a point stabilizat­ion

problem at the end right and for the

control actions Omega so initially the

control action was changing as the robot

was you know turning left here and then

overshoot a little bit and then turn the

right to converge to the trajectory and

then this value kept to its reference

value zero which was the case for the

stabilizat­ion case so that's a pretty

much four trajectory tracking it turns

out that it's very similar to a point

stabilizat­ion except for the few changes

thank you very much for watching