Self-Driving Cars: Control (Nived Chebrolu)

welcome everyone welcome to today's lecture on control for self-driving cars so we'll look into different strategies for actually controlling a car along a desired trajectory uh we'll look into the concepts of feedback control and how to use this idea to actually smoothly drive our car autonomously along some some path that would be calculated say using a path planner we'll also look then into the modeling of a of a car so the kinematic modeling and to see how the controls affect these cars and use them within these controllers to exploit this knowledge and we'll also look at a control law which can be obtained as a result of an optimization problem and see what advantages that this kind of controllers give so broadly speaking we look at different strategies to control uh a car along a desired trajectory and the goal is to develop this control loss which obey certain rules and give us smooth driving experience for a self-driving car so this lecture is a part of the ongoing series of uh ongoing course on self driving cars so techniques for self-driving cars as it's called and this is being presented with my colleagues here as you can see here and it's my pleasure to uh to give today's lecture on the control aspect so i look into the different aspects involving the control of a self-driving car so let's get started so this is a typical scenario that you would see for a self-driving car say you're on the road or the car is on the road and this is the front wheel that you have now the car must decide what to do next right and this involves several steps this involves first for example understanding the world around you so understanding which parts of this world for example are traversable so you're only allowed to go say for example on these roads but not on the pathways here so this is one kind of understanding that is necessary and then once you say no what are what is good areas to go what is not good areas to go you also should understand the semantics for example there's a green light therefore you're allowed to go in front uh or if it was a red you would have to stop and so on so there are certain rules that this environment imposes on the car so once all this thing is done you also need to then figure out how should i go this depends of course where you want to go so say you have a planner which says you need to go to a particular position and this would determine how you put how you reach this particular goal and to do this you usually generate a trajectory and follow this trajectory as smoothly as possible so let's see how this strategy is usually done for a self-driving car so usually there are these different levels at which you need to control your car and at the top is what i was talking about perception so this would include say traversability analysis so this means that where or which parts of the environment are free which are occupied and not just in terms of let's say it's free or occupied but also if it's valid to drive in that particular area so for example you're not allowed to drive in the opposite direction or you're not allowed to drive in the lanes for the bicycles or so on so this kind of analysis is the traversability analysis and we also need to understand the semantics which is to understand where are the cars where are the people and other rules in the environment so this is dealt within the the block which is usually called as perception then once you kind of perceive your environment you need to then plan a trajectory now you know what are free spaces and you want to now plan a trajectory through this free space which would then eventually take you some place and with taking you this some place can be given by a plan which you would uh for example pre-compute say this is the output of something like a google maps or some other navigation system so these are blocks that we won't be considering in this lecture today but would be the topics of discussion of the future lectures what will primarily focus today is these two bottom blocks which are a control and actuation so what this means is that once you have a trajectory you want to follow how should you actually do the control along these two directions so here we say longitudinal we mean along the direction of the motion of the car and lateral meaning the steering so to say so as to reduce the error or the crosstrack error as it's called to our desired trajectory and finally to do this we actually need to apply some control some physical actuation is needed and this is usually done via the throttle or the brake in order to go faster or slower and of course to steer your car using the steering wheel so what will primarily focus today is these two aspects of control and actuation and the goal would be the following so if we are given a trajectory to follow like this one here in green which has been computed during a motion planning step the goal would be how do i actually now follow this particular trajectory here so when i say a trajectory it means couple of things one because it means the physical path that you see here but also it means something more in terms of the velocity you want to achieve while you travel on this particular path here so for each point let's say here on this trajectory we want to reach a particular velocity and usually this velocity is set in a way that the overall experience for the for the passenger so to say in sitting in the car is uh is is smooth and not jerky and of course we must follow the laws that are there so if this road has a maximum speed of 30 kilometers per hour it must also follow these kind of rules so today we'll look into different strategies on how to actually perform uh this kind of tracking uh given the fact that we have a trajectory already which we know how to try or which we need to track then the next question that comes immediately is if i want to track this particular trajectory then what is this trading angle that i should set or how much should i press on the gas and how much should i press on the brake in order to actually follow this uh trajectory efficiently so this is how humans would do it too so we would we would rotate the steering wheel and press on the gas or the brake to control the car of course in an autonomous car you need not physically do this but the idea is the same that you need to to steer your car or steer you steer your wheels so that you track your uh trajectory or your desired trajectory and so this is the basic question that we ask in this lecture what are the controls in order to move my car along a trajectory and we'll see different ways of doing this but before we actually look into the controls aspect we first need to understand the motion of the car itself and in this lecture we'll limit our motion analysis of motion modeling to the velocities so we'll model the motion for a car using its linear velocity v and its angular velocity omega and this so to save would completely define the the dynamics of the car as it moves or the kinematics of the car as it moves so we need to understand given certain commands what kind of velocity and angular velocity that the car gets and this is done through kinematic modeling so uh in the in the upcoming slides we will look at a simple model as to how the controls as well as the systems pass state themselves affect the motion so if you are moving at let's say 30 kilometers of an hour then your next position will depend on how fast you are moving in the previous position and where you were and of course it also depends on the control that you give so how much gas you push or how much throttle you give or the steering angle that you put and this would affect the overall linear velocity and the angular velocity for the car and for this will basically in this lecture limit ourselves to this simple model called as the 2d bicycle model so essentially instead of considering the whole car we have a simplified model with two wheels uh so of course our car has four wheels but in this case we uh uh approximate with this with an ideal wheel which sits in the back in the middle of the two axis uh with the wheels and a steerable front wheel on the in the real car this would then translate to the fact that the way you would steer this wheel would result in different steerings for the left and the right wheel but we'll consider this simplified model as it keeps the overall complexity uh in check and also is enough to understand and also model realistic situations so as i show here this is the bicycle model and the some of the terms that or some of the variables that we're interested here are shown here so this v is the linear velocity and since it's a rigid object all the points must move with this linear velocity and then we have the orientation of our car given by theta and we also have a delta which is the steering angle of the front wheel so by changing the delta we could then change the angular velocity and therefore make a curve for example right so this is how we actually model a car so at least the geometric parts of those model and then we have certain conditions which we want to respect so one of these conditions is the is that of rolling conditions for the wheel this means that the wheel always rolls and it never skids and one of the advantages of this is that the friction then on the wheel is limited and we obtain a smooth motion so in our modeling we'll always want to achieve a smooth motion of the tire so we always want the the wheel to roll and what this means kinematically is that if a wheel is moving or is having a linear velocity of v then it has an angular velocity which is given by the product of the r which is the radius of your wheel times the the angular velocity itself omega so we would always respect this law that the linear velocity is r times the angular velocity along which it rotates so if this is true for all points or on uh on our wheel then this ensures that we ro we roll smoothly on top of the uh on top of the ground so this is for each tire but in our bicycle model or in our model for the car you we have multiple tires right so the rolling condition must hold for each of the tires but also as a system it must roll smoothly and this brings us to the concept of the instant instantaneous center of curvature uh what what means what this means is that as a system as a for our car there exists one point this icc the instantaneous center about which we are rotating so you can think of our a bicycle model as a point mass which then is rotating about this icc with this particular omega and in order for this to happen there's a particular geometric con constraint that must be satisfied and this constraint is that the axis that passes through this wheels so here i call it the y-axis so this must intersect at a common point always so no matter with what velocity this the the car moves we should ensure that these accesses always meet at the same point and if they don't meet at the same point this means that we would be skidding in that case the rolling motion would no longer be uh be satisfied so with this uh with this in mind we always want to generate such commands so our controllers should generate such commands such that this icc condition is always up uphold right upheld and with this i move next to using these concepts here and the variables that i have introduced here so i've introduced a new variable here which is r which is this radius of the circle about which our car would be rotating and another term which would keep coming is this l uh which is the distance between the rear and the front wheel so this is so to say the length of the car so in a in this model so with this we can actually then derive the kinematic model for this uh bicycle uh a bicycle model which actually approximates a car and the kinematics are like this so how does my x position evolve how does my y position evolve and how does my orientation evolve so this is what the kinematics captures and in this particular model we consider the center of the rear axis as the point which we want to track this means that the controller that we will develop would actually say act on this point such that this point is as close to the trajectory the desired trajectory as possible so it's a matter of choice there are different ways in which this point could be chosen one could also say this is at the center of gravity of this car or one could even fix that at uh at the front wheel and we'll see that by changing these different reference values the properties of the controllers themselves often change and uh later on today we'll see a controller which actually does this change and see what advantages it has but for for this slide here and to in general to understand the kinematics let's fix this point we want to track here right then uh we have these following constraints so the the uh the motion in the x direction is basically the velocity that it has times the cosine so if x direction is this way and similarly the y direction is y times of sine of your orientation and finally we also obtain a constraint for the change in the orientation as v tan delta by l and this formula can be obtained using uh the using some some basic trigonometry in this triangle here and replacing for the fact that omega actually is the angular rotation which means the rate of change of our orientation so if we just compute the tan of delta here we have l over r and then substitute that in here we get the uh get the uh get the kinematics so to solve the motion for the change in uh yaw rate or the heading right so with this now we have some formula so a model of uh uh to predict so to say where we will end up given that we have a certain velocity v and a certain steering angle delta over the next time step so this helps us model the motion of a car using this bicycle model right so we can kind of put all things together in this slide here so in order to track this particular car or this model our state consists of the following variables the x and the y position of the car so in this case the rear axle position the orientation theta and the steering angle delta so this four variables would uh would explain completely a motion at any given time then we have our controls which is the linear velocity that we want to set to the car and the rate of change of the steering one could also argue that this could directly be the steering angle in this case we model it as the rate of steering because usually in practice we can't arbitrarily set a steering angle but rather we do this smoothly and therefore it it helps to actually control the rate of steering angle rather than the steering angle directly and then we have this particular condition that this delta so this steering angle must uphold this particular relation here which is given by tan inverse of l over r and this is necessary if you remember to uh to have a consistent icc point so if we take the tan of delta we would find that it's basically l over r and this would mean that by keeping the delta such that it's it holds this formula this means that we will always have our wheels intersecting at an icc point right and then we've seen the kinematics which are given by these three equations so this is the overall bicycle model which we'll use throughout this lecture to describe the motion of our car then we saw the kinematics but then there are also some constraints that are put onto our car for example there's a maximum velocity that our car can reach in this case we denote it by v max so no matter how much more throttle you give our car can't make more power in order to reach a higher velocity so this is some physical constraint that comes from the construction of the car from the engine that it has and so on and then we also have a limit on the on the steering angle so this means that we can only turn the the the steering wheel so much and if effectively we can only turn the wheel on the car so much so this is given by delta max so in this case this delta would be between let's say minus delta max to delta max so these are some physical constraints that come out of the construction of the car itself and whatever control law that we develop must also must also kind of follow these constraints in order for them to be effective okay so we saw basically the kinematics the constraints there are some things in this lecture that we won't really look into so what we said that by by controlling our brake and accelerator as well as the steering wheel we are able to obtain a particular velocity a linear and an angular velocity but in practice there are several forces which are actually acting on our car so as the car is moving there is certain air resistance some friction from the from the ground so for example if we are on on on an uh on a road on an asphalt road versus an outdoor road this kind of friction would vary then we have certain lateral forces so as we say on a curve we have some centripetal forces acting on our car and also say if we are going up or down a hill there is certain gradient forces or due to the gravity that is acting on our car but when we take this simplistic view of of being able to set a particular linear angular velocity we are in a way uh not considering all these forces that act on the car so uh if you if you want to be really precise we should also take this into account while developing a control law in this lecture we'll focus ourselves to this simple model and see that the concepts in a way propagate to the to the cases where you have these forces too but the equations are usually much larger and a bit more complicated so we make this simplifying assumption that we're able to set a particular linear and angular velocity uh and don't consider all the forces that are usually acting on the on the car this is a simplifying assumption that we make right so now we move on to the actuation part right so we saw how the uh how the car moves over time but now we want to understand what's the effect that uh that our actuators have on on the car or what is it that the actuators can actually can do and in this case we have two actuation mechanisms one is the steering wheel and by moving this steering wheel we can set the angle for our uh for our let's say the front wheel of our car or the front wheels of our car and in this case we also assume a very simple model it's a linear model so this means that if you rotate your steering wheel by delta s this would be directly proportional to the amount you would uh actually move the wheel so for simplifying in the simplest case if you consider ks as one this would mean that the wheel would move as much as your steering uh as you would move the steering wheel itself right and of course this has a constraint that you can only move your steering wheel so much so this is a very simple model as to how your steering affects the actual physical state of the of your car and in this case let's say our bicycle model how does the the steering angle change as i change my angle on the steering wheel right so you change your or you move your steering wheel this sets a different uh angle for your front wheel and this results in a different curvature that you can actually follow so in in reality again here we make certain simplifying assumptions usually when you make a particular turn it's not that you can directly set this angle to delta because there are certain lateral forces that are acting on this wheel and then it becomes also important to model how the tires respond to different forces that are there so again for this lecture we use the simple simplified linear model so the wheel moves as much as your or is proportional to your movement of the steering wheel okay the next part is the throttling part right so to throttling or the braking part so this is this sets your uh or this gives you velocity this gives you forward motion for your car and the way it does is that uh the mo so this delta t is is the is the amount you say press on your accelerator and delta b is the amount you press your brake and what we see say is that the amount you press your accelerator is directly proportional to the acceleration that you can generate right so if you press your uh if you if you give full throttle to your car it means that you can reach this maximum acceleration that your car is capable of and the same uh in the case of a brake but in the case of a brake it would be a deceleration instead of an acceleration so the way you the way this would work is that you would press your accelerator or the bake your car will gain certain acceleration and finally by applying this acceleration over a period of time it would either increase or decrease its linear velocity so this is the way you can actually control the velocity by physical interaction or basically by pushing these brakes and throttles on the car so we have seen basically kinematics for the car we have seen how to actually change these states so which means this change the velocity and the angular angular velocity for the car by through our actuation mechanisms so now we need to look into mechanisms of how to actually use this knowledge or to use this knowledge to come up with a control law which would help us finally let's say either reach a position or to follow a desired trajectory and for this i would like to introduce the concept of feedback control and this is a very common uh way of controlling systems to to bring it to your desired systems desired state and usually you see it as a let's say as opposites to either open loop system or feedback uh systems so open loop usually means that you have certain system in this case it's our car you give some certain a command so this is the controller which gives certain commands to the system and this reaches to a new position so if our systems were completely predictable and we know the exact modeling of how this system evolves then this is sufficient we know where we want to go we give a particular command and the system executes that perfectly but we know that this is not the case in most real systems including the car so even if we give a particular command depending on the speed you are depending on the road conditions etc you might not exactly end up where you plan for right and this is where the idea of feedback control comes where what you do is that you give a command to your system but then you measure that measure the state of the system using a sensor so this could be let's say in the case of a a car could be a gps uh sensor or could be a lidar scanner a camera or whatever using this sensor data you have an estimate of where you are and then basically you compute a difference between where you want to be so this x desired versus where you sense to be and then this difference is what goes to the controller and the idea here is that if the difference is large the controller should generate controls which are larger and if the if this difference is zero for example it should generate ideally no controls so that the system has already reached our desired goal so this is the concept of feedback control where we continuously measure the current state and compare it against the desired state in order to compute our controller against the open loop control where we only know where we want to go but we don't really know where we are at the current moment and just use this knowledge of where we want to go as well as the model that we have in order to do the control so in this lecture we'll primarily primarily be looking at schemes or strategies which are of this feedback control type right so just to make it a little bit more formal uh what we say is the controller gets in as input and error signal which is basically your desired position minus what you sense so in this case there's a little bit more than just sensing going on there usually the sensor either does not observe your complete state or you need some more processing or you need some modeling to get your uh yours uh get your state so to say from your sensor measurements right so there's something a little bit more going on in the sense that there's this function uh f here which actually looks into the into the measurements and then converts into your current state so this there are lots of techniques to do this lots of state estimation techniques you can think of a kalman filter or some other modeling which would actually give you uh your current state based on your based on your observations okay so let's say you know where you want to go you have an estimate of where you are you have this error signal then the goal is to is to compute this u which is a function of your error signal so we want such a signal which would then drive the system to reach our desired position right so the controller would generate ut which would then affect the system to move based on its kinematics and of course other noise that would for example be acting in the real world so for example here this part is the actual or the modeling that we have so this could be let's say the bicycle model for the car but in practice there's actually also some noise that's happening due to the lack of perfect commands or some external disturbances so this is let's say the overall setup of a feedback control and if we talk particularly of this feedback control for our self-driving car case this essentially means that the controller should generate my commands which here are the steering rate as well as how much should i press the gas in the throttle in order to uh to make my car or in order to drive my car to have a desired velocity and angular velocity desired linear and angular velocity so this would be the goal uh that in the next few slides we see using one particular kind of controller right okay so now we go on to the first kind of feedback controller that we see today and this is the popular pid controller where p i and d stands for proportional integral and derivative uh terms and then we'll actually see how we can use a pid controller to achieve our task of finally following a trajectory but before doing that we actually understand the concept of the pid controller and what these different terms do on a simpler task which is of position control what it means here is that we have a desired position uh x desired and we want to reach to this desired position right so in this case there's just one goal and not really a trajectory where this goal keeps changing so we do this so that we can see what the effects of these different terms are and it's a bit easier to analyze the question is always the same so in terms of control what's the control signal u such that i reach this desired position and for this example actually we would say that we are in a one dimension world and we only want to uh control our uh velocity through our throttle so that we reach this goal position here and we also assume uh for the sake of simplicity that our measurements said also give us directly the position of where my robot is so you could think of this as a very accurate gps sensor which always tells me where i am right and for modeling the kinematics uh we also take a simplified model not the bicycle model but even a simplified model where we consider a car as a point mass with a certain velocity shown here by x dot and its position is shown by x here so the way the system would evolve is that the new position let's say time t plus 1 would be your position at time t plus this velocity times the delta t that is that has happened in between right and just to make the task concrete let's say we start at x equals to zero and we want to reach the desired position at x just equals to one and uh basically at each time instant we apply a control ut so this would be pressing of your throttle for example in this case and the goal is how do i reach smoothly to this x desired position uh using this control right like i said so the system model or the way the system evolves so the kinematics of the system so to say is that the new position of the of the car would be the old position uh plus the velocity that you had in the previous time times the delta t so the delta t is the amount of time for which you actually apply this control so this would define the system uh kinematics and let's say we start at this initial state where our x position is at zero and uh what we see here are three different things so in red is shown the position of the robot and then in the in the slides to come we'll see also the x t which is basically the derivative or the velocity of the of the uh of the robot or the car in this case and then x desired which is basically this one so we want to reach to this position one as quickly as possible and on the lower side we will see the different components of the of the pid controller which contribute towards the final control so let's see how this works let's say we only use the proportional idea of the pid or the proportional term of the pid which means that our control law is some proportion is proportional to the error that you have so the error is nothing but your desired position minus your current position and the proportional control law says apply a control which is proportional to this error so this is let's say we are only using the p term of our pid controller what happens then so we start from x equals to zero and then we compute the difference to our desired position in this case it's one and then let's say kp if it's one just for example then we would apply a a control which is one minus zero which is one amount of control and let's say we apply it each time for one second this would then give a certain velocity to the car and it will start moving at some point we see that it does reach the position one but then it overshoots and this happens because uh the the the just this term is not able to account for the kinematics of um of our car in this case so as it reaches one it did reach one and it did make the the position error as zero but it reached there with certain velocity and therefore basically you can't then just immediately stop here you need certain time to actually come back and what this results is in this kind of an oscillation where you're always reaching your goal with certain velocity which is non-zero and you're not actually able to stop your car so for uh for the for the task of actually driving your uh car to a desired position just the proportional terms are not sufficient right and here also we then see that the controls also go left and right so this would this would correspond to your you steering to the left ones to the right ones to the left ones and so on and you're actually never constant or never reaching your desired position and one can actually fix this by considering the derivative parts as well so now we say that we have a control which not just depends on the on the distance to the destination but we also want to make sure that when we arrive to the destination the velocities are is is equal to the desired one so in this case to reach a position we want to reach there and stop so the desired velocity would be zero so we would like to arrive at one with a particular with a zero velocity so and when we have these two terms of both the proportional error and the derivative error then we actually see that we are able to arrive to the position one smoothly and then we are able to basically stop at one what you see on on the bottom here are the corresponding controls that were needed to actually achieve this particular performance and we basically see two components so this uh this pink part here and the light blue part here uh correspond to the proportional and the derivative components so this control here is coming due to the fact that you are away from your desired position and uh this blue part here is coming from the fact that you have a velocity that's different than the desired velocity and this dark green part here shows the combined control that's actually being sent to the car and as a combination of these two controls we are able to kind of achieve what we want okay uh of course now the question comes uh what to do with these gains so before i said we just assume them to be one but in practice we need to decide them and often it's important to define this or to set these gains properly for your system so for example in this case we have gains let's say we set gains which are higher than what is necessary and what this would mean is that for example we would overshoot overshoot our desired position and not stop there and then take some time to actually come back ideally we would like to avoid this kind of a a situation this this indicates that let's say our proportional error is higher than it's necessary right and therefore this is one of the effects that we will see if we have a much higher gains whereas if we do the opposite if we put two lower gains what this means that as a system takes much longer to actually reach our goal so there's always a trade-off between how fast you can reach your goal versus how much you would overshoot your desired position so these are the two things which we need to take into account when we decide these gains kp and kd while using a a proportional integrative or a pid controller in this case we only see the p and d parts then why do we need this ipad if everything is good this ipad comes in when we have certain systematic error so this means that let's say there's some external disturbance on your robot as it's moving or let's say you have wheels which are not exactly symmetric this would result in you always having some systematic bias so in this case for example even after you we have applied our p and d control we are always kind of a little bit away from our desired position and the idea to fix this kind of error is what the integral term does so this integral term what it says is basically take the difference between your desired position and your current position and add it up over your entire history so 0 to t and what this this does is uh basically it would add up this area and generate a control which is proportional to the the error or the area under this curve here and so we see this uh here where for example we start from zero and we reach a particular position and we have a certain uh certain uh uh error that's coming due to this biases and then what the integral term does it basically adds up this area under the curve and then uses this part to actually generate a command and finally lead it to the the uh the desired goal and once you reach the desired goal since there is no more any difference from your desired uh trajectory the component of the integral part would also decrease very soon so here again we see the three parts so the proportional part the derivative part and the integrated integrative part which would finally give this green thing here in order to uh to make our system reach our goal position and to make sure that if there are some external disturbances or some systematic bias this is also taken care of and so this gives us the overall formulation of our pid controller where we have the three terms as i discussed the proportional term multiplied by the proportional gain the derivative term multiplied by the derivative gain and the integral term which sums up this errors due to systematic bias also scaled by this gain here so usually for steady state systems or systems where you're already quite close to your trajectory this kind of scheme works quite well but one should be careful of the integral term because sometimes this could add up over time and could lead to dangerous effects which is also called as the wind-up effect so for example let's say you're moving and then you're slipping on the road due to some reason this means that you're not in the position that you expect to be and over a period of time this adds up to be a big error which could then result in a control that is not desirable right so uh apart from that part apart from this particular danger which actually you could also take care of by not always integrating or your entire time horizon but only for a short window so let's say you only look at the last five seconds and usually this is sufficient to ensure that you don't have uh very bad wind-up effects right so with this what we have seen is a controller which looks at all these different feedback errors so each one of this is a feedback error because it's a difference between what you desire and where you currently are so either the error directly in the position here or the velocities or the thing that you add up over time and so with this we have now a control lot to actually take our car to a desired location so just to give a quick summary we have three or three components the p i and the d most cases the p part is sufficient if there is no uh if you if you're moving slowly or the kinematics is quite slow and the proportional error is usually sufficient uh or in cases like in this case where we had a certain kinematics for the car a pd controller is recommended where you can also take into account the velocity of the the system and if we have some systematic bias we should also include the the integrator so this is in general uh uh or this is a general feedback controller uh this pid controller which can be used for several tasks not just of reaching a position but any kind of process where we want to bring our desired or bring our system to a desired uh position sort of or to a desired value right so now let's see how this uh extends to actually following a trajectory so what we have seen now is how to actually just reach a point but in practice uh when we are in the scenario for a self-driving car we don't just want to reach different positions but we want to track this uh this particular trajectory smoothly so this needs certain extensions and the way this is typically done for a self-driving car is to break this problem into a couple of parts so one part is what is called as the longitudinal control which means we need to make sure that our velocities are uh aligned so or are the ones that we want to reach and then we also want to make sure that if we are away from this uh or we are having some offset from this trajectory we want to correct that offset as fast as possible and so these are the two parts the second part is also called as the lateral control and so let's first look at the longitudinal control which means that uh let's say for now we are already aligned with the trajectory that we want to go so we have no error say in the heading or we have no offset but what we want to do is that at each point on this trajectory we want to achieve a particular velocity right and uh of course there are multiple ways where uh how one can come up with this velocity profile as it's called one is uh for example when you have a large curvature it's usually better to have a lower velocity as this would ensure smoother curve so there's not too much jerks whereas let's say when you have flatter flatter parts of the trajectory you could go faster this is one way you could come out with what velocity you want at each point but also there are certain uh rules that the environment imposes on you for example if the maximum speed of this road is let's say 30 kilometers per hour then you should ensure that you are not going behind or above 30 kilometers per hour so using this information we can actually uh or we want to use this information of this velocity profile to come out with a control which actually obtains these velocities at those particular times of point along the trajectory right so the first idea or the idea that we want to see here is how to then use let's say this pid controller that we just saw in order to obtain this uh this property right and in this case we slightly modify our pid controller in this case instead of reaching a particular position x desired we have a reference trajectory and what we do here as a proportional error we want to have our velocity which is the x dot here as close to the the the velocity we want to reach which was defined by the velocity profile so this would be our proportional error whereas the derivative error would now be the rate of change of this velocity so this means the acceleration so to say so we want to reach there with as little accelerations as possible and finally of course if there are certain biases that are there we want to also take care of them so for example if we set our car to go at 50 kilometers per hour but it always goes at 53 so this kind of errors would be for example taken care by this integral part okay and the output would be in a way acceleration that our throttle must be able to provide so with this pid we get a control which is in terms of an acceleration that we desire and the goal is that we either press the throttle or the brake such that we reach this particular desired acceleration so this is what we what what is shown here so we have a reference velocity coming from the the velocity profile we use this law here uh of course i didn't talk about the path that one has to actually set these uh kp kd and ki properly such that such that these gains allow us to have a performance in terms of the overshoot or reaching our desired velocity as fast as as possible and then this would translate to a certain amount of push on our brake or throttle to reach this position so this is how we can use a pid controller to achieve this longitudinal velocities along the trajectory assuming that we have already made our uh offset errors to zero right and uh this would result something like the following here so once we apply the pid control we would see that maybe initially we if we start from let's say zero and we want to reach to this uh vref so say 30 kilometers per hour initially maybe we need to give a full throttle here shown by just one so if it's one it's full throttle if it's zero it's no throttle and as as time goes by uh we need not put this entire uh acceleration as it as the car achieves certain velocity and then we probably need to give some constant acceleration so as to keep this velocity because as you move on the road there's some friction some other disturbances which need to be countered through the through the throttle so basically the pid controller would uh generate this particular uh profile here which would be the one that would be given to the throttle in the brake this is how we can achieve let's say the longitudinal control often there's a kind of a trick that's done or kind of an improvement that one can do on this basic pid scheme through what is called as the feed forward controller what this essentially means that if we kind of know what's the effect of our of our control so we have a mapping between a throttle and a velocity so if we set a particular or if we want a particular velocity what's the throttle that's required to obtain that velocity so using this information what you can do is that for example we we obtain this empirically beforehand for the car that we're interested in and then we can use this uh feed forward controller for which the reference for which the input is just vref so this means that say we want a velocity which is somewhere here we look up and we see okay this is the throttle let's require to obtain that velocity that's what this controller spits out and then the role of the pid control then would be to only do do the minor corrections so we want to reach vref my chart or my lookup table so to say says give this much throttle i give that much throttle and then this the role of the pid is just to give the small changes so as to uh to reach our desired positions often this kind of pid control controller which includes the fee forward term uh reaches its goal faster than if you were to track this error entirely from zero so this is one of the the ways to make a pid controller respond faster so this was all about the longitudinal control the next control that we see is the lateral one which essentially means that if we have certain offset with our trajectory we want to make this to zero so the term that's often used in uh in the literature is what is called as the cross track error so this is the error so to say perpendicular from your current heading or current position to your trajectory right so we want to develop a controller which would make this zero but we also want this controller to have some other properties such that once we make it to zero that it's also looking in the correct direction we not only want to go to the trajectory but we also want to set the correct heading along that trajectory so in this case for example our current heading is theta so once we say reduce our crosstalk error and go on to the on the trajectory we want it to have let's say the heading of the trajectory itself or the tangents here given by the theta f of the trajectory so these are the two goals usually one has when we're talking about a lateral control again we can use the idea of a pid control for obtaining this lateral error so what usually one does is uh basically the this error or this crosstrack error is taken as the thing that we want to bring to zero so that's why the the formulation looks slightly different because our desired crosstrack error is basically zero and let's say e c t e is our current uh um crosstalk error this means that basically it's zero minus the current crosstrack error therefore we basically just have the minus kp times uh e cte and the same all over uh here so this is very similar to our position control apart from the fact that our reference is always zero because we always want to make our crosstrack error zero and the way this would look for the car is that how should i move my steering so the output should here be the rate of change of my steering wheel or the steering angle such that i can drive this crosstrack error to zero and uh although it's not directly evident here in practice what happens by by by applying this control law is that you not only reach your desired position but you also reach with the correct heading which is kind of a consequence of the fact that as you reach closer to the trajectory you kind of also minimize your steering angle which so as to actually reach and not overshoot and even if you overshoot then you would compensate to align with the trajectory so although it's not guaranteed that our heading will be will be aligned with the trajectory in practice if we start close enough to trajectories and the trajectories are smooth then we obtain this property that we reduce the crosstalk error but also directly also align our orientation in the direction that we want so this then would look like something like this over time we would give some steering so maybe initially we need quite a bit of steering if you're far away so for example uh if i if our crosstaker is this high and then over time as the crosstrack error decreases also we see somehow that the steering angle that we need to compute also changes so very similar to our position control but in this case the commands are actually given by our uh our steering wheel here so in both these controllers there's one thing that we did was that we kind of separated the problem of uh controlling the the velocity or the linear velocity and the angular velocity so this is as two different pid problems and we solve them separately and this is how most systems actually do it because it's easy to design controllers for one task and pid controller is quite good at that and therefore this also makes the task for example of deciding the gains easier of course this might lead to the fact that you have competing interests in the sense that by decreasing your longitudinal error so the velocity error you might end up or let's say the other way around so by setting up by changing your steering wheel you not only affect the the uh the angular velocity but indirectly also affect the linear velocity of your of your car and so there is some competing interest of these two controllers but if the gains are suit are properly tuned and if if the range that we are uh hoping for is small enough then often this work quite well right so we then move on to the next kind of control especially for the steering which is called as the geometric steering control and we want to do this because in our pid controller that we developed let's say the lateral pid controller we actually didn't really exploit the kinematic model of uh of our car so in that sense the the output that the pid controller gives which is let's say the steering rate or to set a particular steering angle this means that maybe this actually does not uphold the the instantaneous center of curvature uh uh assumption that we made so this means that our car might be skidding instead of rolling smoothly since we didn't give any importance to the kinematics so to say so here we look at a couple of control laws which actually take into account this kinematics or at least look at the geometry of both your car as well as the trajectory to obtain a control law and the first one of these controllers is what's called as the pure pursuit controller so again we have a a particular trajectory we want to follow and in this case we only control again let's say the lateral aspect so we only control the steering wheel and there's one uh one term that's introduced with this controller is what is called as this ld which is the look ahead distance so instead of directly tracking to the closest point on the trajectory one of the ideas that this controller uses is to actually look up or look ahead at some point on the trajectory this often ensures a much smoother behavior than if you're always trying to go to the closest point on the on the trajectory with this uh we obtain the following geometry so the goal is the following so for the rear axle so again here we use this rear axle uh uh coordinate frame we want this rear axle point to track a particular arc such that it intersects with a trajectory at this look ahead distance point so we want such a control which takes us along this curve of course this curve keeps changing at every instance because let's say we apply this control we move a little bit forward our look ahead point on the trajectory would change and this the circle that we would be tracking would always be different but it would always ensure that we have this icc point so that we are always rolling with this we also uh get the following geometry such that uh we have these two uh uh two let's say parts of the circle or the the thing that forms the arc which has the equal length and we can do some geometry and come out with this controller so what we exploit here is the law of sines which essentially means that if you have a triangle like this the the sine of this angle divided by the up the length of the opposite uh opposite side of the triangle remains constant so here we say for example this ld divided by the sine of 2 alpha is the same as r or r divided by this angle here which is given by pi by 2 minus alpha so you could think of if this is alpha the whole thing is 90 so this thing is pi by 2 minus alpha and with this we come up with this first equation here and one with that we apply some high school trigonometry so we expand sine to alpha as 2 sine alpha cos alpha and sine of phi by 2 minus alpha is nothing but cos alpha and with some algebra basically we can come up with uh expression for our curvatures the radius of curvature which is shown here by kappa so radius of curvature is defined as nothing but 1 over r and by doing by manipulating the equation here we end up with the fact that kappa is nothing but 2 times sine of alpha divided by the local distance ld so we derive this and we'll see why this is useful so once we have this which is coming from the geometry that we saw here which uh then in turn make sure that we always have an icc results in the following control law in in the sense that we have this constraint that we got in the previous slide where kappa was 2 sine alpha over ld and if we remember from the bicycle model we said that we should always set the steering angle such that it's tan inverse of l over r and one over r is nothing but the kappa and then you just have to substitute this in here to obtain the control law so this pure pursuit control law so which says set my angle such that it's tan inverse of 2 l sine of alpha over ld and it's also quite nice in the sense that tan inverse has the property that it's a monotonically increasing function between minus pi by 2 and pi by 2 this means that we'll spit out a value between minus pi by 2 and pi by 2 for our steering angle this is quite nice right and we have obtained this from the fact that we are always scrolling while we are applying this particular uh particular control right but uh uh or what we want to see in the end actually is that the crosstrack error is driven to zero so the question that i ask is if i follow this particular control law do i actually make my crosstrack error zero and actually there's quite a nice relation that comes out here so again with some geometry so this point is or this length here is the crosstrack error which is basically nothing but the the place where oh this is our heading direction and the the direction perpendicular to it on our trajectory is this crosstrack error and if we look here we have a relation basically which says the sign of alpha would be uh e crosstalk error over ld so this part over the hypotenuse which is the look ahead distance and these are the two equations that we had from before and if we if we plug in the equation what we end up is that we see that uh the crosstalk error ecte is basically proportional to our curvature kappa and if we then put that here and then we we then see is that our steering angle is also directly proportional to our uh crosstalk error ecte and since tan inverse is a monotonically increasing function this means that as the crosstrack error increases our steering command also increases so in a way when we are farther away from our trajectory we have a more aggressive turning than when we are closer to the uh so this is in a way telling us that we are reaching towards our uh our trajectory by setting the delta as given here right and uh what we uh can also do is that still now we had set this look ahead distance as a constant so whatever three meters ahead four meters ahead also but often what this happens is that uh since we are not really controlling the forward velocity at this time this means that if we are going too fast this can result in a very fast maneuver to reach our trajectory so if i look ahead distances say not long enough then we want to reach this look at distance on our trajectory as fast as possible resulting in let's say some jerky behavior while uh reaching our trajectory and we want to avoid this and one way to actually do this is to link the look ahead distance uh to the velocity that we are actually moving at the forward velocity that we are having so if the lookout distance is proportional to the forward velocity we are having this means that if we are fast if we are moving faster the point on the trajectory we want to reach is also farther away resulting in the fact that we can reach there much uh smoothly than if the look ahead distance was slower again it also works if uh velocity is smaller then it's fine to actually apply a strong enough steering so that we reach a trajectory as fast as possible so by setting the look ahead distance as a function of our velocity we finally end up with this law which we can call our pure pursuit law so this is a lateral controller to compute the steering angle and the steering angle is given by the tan inverse of this formula here and now we have linked the look-ahead distance to the current velocity there's another popular lateral controller what is called as the stanley controller and it takes it names from the from the robot that it was used to control and it's a it's a really successful uh controller in the sense that it won or it helped in winning the darpa challenge where the car tracked several hundreds or even thousands of kilometers uh using this geometric controller again and the idea with this stanley controller is the following so in this case instead of just reducing the crosstalk error the stanley controller tries to reduce both the heading error and the crosstrack error explicitly so it says that give me my uh steering wheel command such that it's equivalent to phi and here phi is nothing but the angle of the uh the the angle of the difference between the current orientation and the orientation of the trajectory at that point so by setting delta equal to phi basically we are aligning ourselves to the trajectory and like before we also want to reduce the crosstrack error and in this case they actually propose this formula so to say empirically where they say set my steering angle as the tan inverse like we saw before and the error is also like we saw before in the pure pursuit controller proportional to the crosstrack error so this is quite intuitive because in the end the tasks of both the controller are the same and the fact that we see similar formulas uh it should not surprise us uh the one big change that happens here is that previously we were tracking the uh the center of the rear axle and in this case the uh the developers actually here track the center of the front axle although we won't really go into the error dynamics here they prove that by tracking the center of the front vehicle they have better aerodynamics in the sense that they're able to have an exponential decay in the in the crosstrack error especially when these errors acquire all these errors are small enough so they have these nice properties uh which are also independent of the forward velocity that you're going so either you're fast or slow you would end up or you would make your crosstrack error zero in the same amount of time because this leads to an error dynamics it's independent of the forward velocity and of course uh also make sure always that the delta is between our minimum and maximum ranges so by combining this the final stanley law is written like here so our the steering angle is my difference to the trajectory heading plus the tan inverse of this term which includes the crosstracker and of course make sure that the delta is between delta min and delta max and this would ensure that our car would uh track a trajectory similar in a way to the the pure pursuit controller uh but also with some other nice properties okay with that we move to the final kind of control strategy what is also called as the model predictive control so now we look at how to pose this control problem as an optimization problem and we'll see some of the nice properties that this controller which is also abbreviated as mpc controller gives us right so again let's say uh the main idea of using a model predictive control as the name suggests is that it actually uses the model of the system so how the system evolves over time in the process of actually choosing a best control so that's why the name model predictive so use your model to predict what will happen in the next time instances and by using this information can you then pick the correct uh controller right so here for example uh we uh consider this the the similar task of actually tracking this uh line at the center of the of the road by basically take the center line and ensure that we track this trajectory because here it's already on the center line so there's nothing to correct but if you were having an offset you want to correct this particular offset right uh then like i said so we have our controls so which are our brake and our accelerator and our steering wheel and if we know what happens when we apply these controls to our system so how does the velocity change when i apply my control or how does my yaw the heading change when i uh move my steering if i know how this work then i then i can use the predictions that of these actions to actually choose the best action that i want to do so essentially this means knowing the kinematics and the effect of the controls on the system and how it evolves right so by applying let's say different kind of controls over the next few time steps we might end up with let's say these three different uh different trajectories here and in the end we want to choose those controls which uh give me a trajectory which is the closest to the actual trajectory that i want to track here so here i want to track this green trajectory here and by choosing the controls which generated so to say these red uh dotted lines here is the control that i want to actually use right so we might ask we already know how to do this and what's the reason why we need an mpc control at the first place and one of the nice properties is that we are able to handle multiple inputs and outputs together right so in the sense that if we thought if we think of our pid controller we had a separate controller for making sure that we reached the desired velocity and we had a separate controller for making sure that we make the crosstrack error zero we make the headings zero and so on and this means that we have the same system which is the car in this case we want to control how we have multiple controllers to achieve the same task and with this also there's a complication that since there is certain uh or the dynamics is such that this control u1 would also affect the errors of the controller 2 and vice versa and this results in the fact that the tuning of the gains might become tricky and with the mpc idea the thing is that we can compress this whole part into one mpc controller which could for example both generate the the steering wheels as well as the accelerated braking commands in order to drive our system to a particular position right so this is one of the things that comes naturally in an mpc controller of course nobody stops us from using a single mpc like here for each of the control problem and also this is something that's that's done often to keep for example the computations uh smaller right the next reason why mpc is kind of desired is that it handles the constraints as a process of the optimization procedure itself so before for example using a pid controller if we wanted to limit our speed to let's say 50 kilometers per hour this would basically would mean just having a threshold and cutting off so we would put no or we wouldn't put any more uh throttle as soon as we see that we have reached 50. so it's in a very hard coded whereas in the mpc you can specify these controls as a part of the optimization problem so not just controls on the speed but also for example we might have maximum and minimum acceleration requirements to ensure that the drive is smooth for example and these kind of constraints can be put directly into it into an mpc based controller which is also nice uh what is also available so to say with an mpc is this idea of a preview which means that say you have a camera in here and you see the trajectory up to this part this means that the actions that we take next doesn't just depend on the closest point on the trajectory but actually looks ahead and therefore can use all this information to come out with the controller which may be much better than if we just always look at the closest point on our trajectory or even at a located point but we don't have this knowledge of how the trajectory is changing right so for example here if we want to make this turn smoothly along this corner if it were let's say a pid controller maybe we just realized this curve at this point and then we put the brake at the end point whereas with a mpc controller since we have this whole knowledge we can reduce the speed even earlier so as to make this uh the so as to make this turn smoother than if you didn't have this preview capability right so the idea of using mpc or in general the idea of using control as an optimization problem is done in multiple situations and the thing that is common to these situations of these different techniques is basically that you need to define a cost function which describes the objective that we want to minimize which means we want to reduce our crosstrack error we want to put little as little controls as possible all these things so these are then put into a objective function and then the goal of the optimization problem would be to minimize this objective function of course depending on the formulation of this optimization problem there are different techniques which can then be used to actually perform the optimization but the core idea of the mpc and other such algorithms is to have the control as an output of certain optimization problem and once we have that the question that one must ask is then what is a good cost function or a good objective function and uh some of the ideas that come straight away are that we want to minimize the error to the reference so this you could say as the crosstalk error or this would be for example the difference to the the the reference velocity that we want or this could also be that we can encode the the objective function in such a way that our controls are limited so to say so we don't want to steer a lot we would prefer to steer a little bit we would also prefer only to press the accelerator or the brake a little bit rather than having aggressive maneuvers which is easier on the driver but also on the passengers as well as the car itself right and if we actually uh right write that down what we have is that we will have some system which might be linear on or non-linear so this is let's say the kinematics of the system so how the system evolves over time and if it's linear it's good we have certain solutions which are even closed form and which is actually a particular form which is a particular controller which is called as the linear quadratic regulator or the lqr which which can be used but in this case we look at a more general case where our systems can be non-linear which is the case with our bicycle model or any other model which is realistic to to to model the motion of a car and then we basically have a quadratic cost function which uh takes in the error so this error could be the difference between our reference velocity and our current velocity this could be the error uh in terms of the crosstrack error so we could come out come up with some error vector which defines so to say the errors that we want to actually minimize and with that we can come up with this uh kind of cost function which is a weighted uh squared sum of your errors as well as a weighted squared sum of your controls so this way we have an objective that takes into account the error as well as uh controls and by minimizing this objective and adjusting the q and a correspondingly we can obtain the behavior that we like for our system and of course the goal always is to find the control uh which gives us the lowest cost once we define this cost function right uh and as we look at this scheme basically we see immediately that there are some pros which is that the cost matrix has this intuitive meaning of a trade-off between your error and the control and gives us different kinds of control that we would prefer in different situations however the the con on the other side is that uh this kind of controllers usually don't have a closed form solution unless in very special cases where the system equations or the system dynamics is linear and the constraints are linear or whatever in most cases this problem has no closed form solution which means we must compute the solution online as an optimization problem and in previously for the applications in slow moving processes maybe in a chemical factory or so on this was fine but for controlling a car where the car expects commands to be given it if not at 10 times a second or even 100 times a second this brings challenges in the sense that we have limited computing capability as well as limited time to actually compute this uh solution and therefore what one usually does is that instead of planning this or computing this optimization problem over the entire trajectory length you only do it for a small portion which is also called as the planning horizon so you do it maybe for the next two or three seconds or so right so when we talk about the requirements for mpc we require fast processing power which can be a challenge on uh on embedded systems which are usually what is found on in cars and we also need certain memory to to hold the different variables to perform the optimization but also some lookup tables if we need them so these are some requirements that we need for our mpc which you wouldn't need if you use a simple pid controller it's a static law once we decide the gains the formula basically does the whole control there is nothing that's changing online of course except the error signal itself in this case at each iteration we are actually solving an optimization problem and the solution of this optimization problem is the control that we actually give to our system so if you look at this thing for a self-driving car we would have a car model which in this case would be let's say the bicycle model that we introduced in the beginning and then this model will be used uh in conjunction with the optimizer to come out with certain controls and in our case a throttle and a steering control in order to drive our car along a particular trajectory right so at each point in time we basically use the predictions of our car model and then look at okay which one of these predictions actually looks like the best solution and if that is the best solution what controls actually lead me to this solution or is the control that i actually want to apply to the uh to the car okay so let's actually look at how this mpc works for a simplified problem so in this case we won't control both the forward velocity and the steering so in this case we'll just control the steering to actually bring this car along this green line but this doesn't mean that the mpc cannot be used for controlling both the linear and the angular velocity just for example in illustrations sake here we want to set the steering angle such that we track the center of the lane here as well as we can we can right so like i said so we have the uh the mpc which has the knowledge of the model and then now we want to set my steering angle such that i drive my car at the center of the lane which is the thing that you want to do most of the time so when you're driving in an urban environment you're in a lane and you want to make sure that you're at the center of the land so it's one of the important problems that one needs to solve and like i said before so the mpc has this stage of uh what is called as prediction or what is we can also call them simulation what this means is that i simulate certain controls in this case certain different steering angles and this would result in a certain trajectory of my car and so in this case i show one example so if i do these steerings in this particular order then i might end up with a trajectory which looks something like this right so what i did is that at this point i am at uh at time t and then by applying those controls which we saw before i might uh go along this trajectory as shown by here so this is a prediction using just the equations from our bicycle model so we use our car model and to say okay this is how i will end up if i apply these particular commands and the the thing is that we do this for multiple uh uh multiple of these controls right so there's multiple configurations possible whatever turn 10 degrees to the left uh turn 20 degrees to the left and a bit to the right and many such possibilities that that are there and each one of those control sequences would lead to different behaviors of the car so for example this would overshoot a bit this would reach uh later in overshoot and this guy probably does what we want right so again we see this in this graph here where if we are at time t each of these uh uh trajectories based on different controls lead us to different uh movements for the car all this is still in the prediction domain in the sense that none of these actions have actually been performed but this is how we expect if we uh perform certain controls then of all these choices that we have we must choose one choice which which makes most sense to us and that is defined here by the cost function that we have so we define a cost function which sums up these errors or the squared of these errors weighted by the corresponding q and r terms that we described before and here we show this we do this summation for p steps so this p here is 4 in the sense that we only do this optimization for the next four steps right and once we define such a control law then comes the optimization step which means we need to evaluate how good is this sequence of control how good is this other one and this other one right so here just as example i showed that the cost j for making this control is 65 so this cost is computed using this formula given up here we do the same for all our proposals here i show three proposals and like we see here this particular control sequence results in a cost which is the lowest one and this means that now we want to use this as the solution which would most likely take us to the desired trajectory in the desired manner and then the next thing that one does in an mpc control is actually then only apply the first of these controls although we compute these next few controls we actually apply just one of these controls uh in practice and we'll see why this is needed so we were at at this position at time t we chose one particular action which was given by this particular action here which resulted in my new position here at time t plus one and of course this new position that i reached may not be exactly the same as my model predicted maybe there was something some other disturbance that happened there maybe the command was not executed properly and in this case for example we expected to be here well we expected that we would end up here given the control but we ended up somewhere here and we then kind of repeat the entire optimization process considering this as a start of reference so now we look ahead few more steps so before we were looking till time t plus p now we look up to time t plus p plus one so we always have this kind of moving window of p where we do this optimization so now the whole optimization procedure which we did from time t equals to at time t equal to t we would repeat it again at time t equals to t plus one and uh we will again choose do the optimization choose that control which would then move us closer to the desired position and we keep repeating this again and again as the car moves and as we see that the the front end of so to say the horizon also keeps moving with us as the car moves and it's just a small animation here to kind of see what happens as you move the car this particular thing that we saw in gray is the is the horizon that moves and therefore in the literature sometimes it's also called as a receding horizon control or a sliding window control that we always look up so to say p steps ahead of us while we choose the next control action and the whole optimization problem is solved for those p steps that we have right so like all controllers also the mpc has certain design parameters so like when we talked about pid we had the gains which were the design parameters similarly for mpc it has its own design parameters and here i list some of the important ones and we'll see how we can choose them so that they give us the performance that we want so one of the most important one over is the prediction horizon which uh basically means this what is the p so how much how many steps ahead do we look into so if we look into a much farther this means that our uh or we instead of looking into let's say just next five seconds if we look into the next 30 seconds or next one minute this means that our optimization problem that we would solve would also need to solve for longer and which would take more uh more computational power so to say similarly we have something called as the control horizon which we'll see uh what's the relation with the prediction one and we have a sampling sampling time of the controller in the sense that the car expects us to give the commands at a particular frequency and we'll see how this also affects some of the design choices that we make and we'll also see how the constraints and weight affect the overall mpc controller so let's start with the sample time so this sample sampling time essentially means how often do i press my brakes or the throttle and how often do i make my steering commands and ideally we want them to be as high as possible so we want to give commands as as fast as possible so that this will ensure that if there is some disturbance i can uh i can counter these disciplines as fast as possible but also in general to track a trajectory it's always good to have the control at a high frequency but the cost is of course the computation if i need to make a a or if i need to compute a control command say 10 times every seconds i need to solve this optimization problem 10 times every second and instead if i now have to do it 100 times this means that the computation time itself or computation effort is increased by 10 terms 10 times and the and the trade-off we have for example is shown in the following example here so if we have a low sampling time so let's say we just apply our controls once a second and say we were following our trajectory but at some point there was some disturbance so you slipped or something happened there was a wind and you moved away from your trajectory if your sampling time is low this would mean that you would take longer to actually come back to your desired trajectory whereas if you had a high sampling time you would recognize this disturbance immediately and then you would try to correct for it also immediately ensuring in the end that you are not away from your desired trajectory for a long time so this is one of the uh the trade-offs that one needs to do between having too low a sampling time or too high a sampling time uh to a trade-off between your closeness to the trajectory versus the computation effort that's needed uh the other part is the prediction horizon so as we saw before whenever we if we have the system and we have an mpc controller essentially we are solving this uh problem over this horizon of p steps so the controller in a way only has knowledge of these p steps and nothing else and this means that the way we choose this horizon is also important for example let's say here we are going around along this trajectory and at some point there is this traffic signal which says you should stop and uh let's say our uh horizon prediction horizon is only of three seconds however our dynamics are the way uh actuation works is that we need at least five seconds to reach from our current velocity to zero this means that having a prediction horizon of three would not help we would always end up in the fact that we won't be able to stop and we'll only stop somewhere after the after the place where we are actually required to stop so while choosing the prediction horizon we should kind of take care that most of the dynamic range of our actuation system is covered so if we expect that our cars go at whatever 60 kilometers and we are expected to stop within the next three seconds or next five seconds uh we should ensure that this particular window is covered while we choose this prediction horizon whereas on the other hand if we take too long a prediction horizon this means for example say something unexpected happens much later in the trajectory since the the optimization problem is solved for the whole length of your prediction horizon all the calculations basically made in this part of the trajectory have to be thrown away because something changed about the trajectory and they are no longer valid so there's always a trade-off between having this prediction horizon again too short such that it doesn't cover the dynamics while being too long uh the fact that it's probably not useful when you actually apply those controls so these are two things that needs to be taken care of while deciding these prediction horizons of course also the fact that the larger the horizon the larger is the computational time this another constraint that we basically have so this next parameter or design parameter we have is what is the control horizon so uh and here this control horizon is shown by this part m here so this comes from the idea that typically after applying a few controls we are quite close to our desired positions or the desired trajectory and then the controls are usually either zero or very small and since we are actually solving an optimization problem online where each of these controls so to say are the unknowns the law the more number of this unknowns the more is complex is that optimization problem and one way to control it is that we say okay we have a prediction horizon of p but only for a small part of those prediction horizon do we actually compute some controls the other parts can either be set to zero or to some constant which was say for example computed in the last time instant this gives us a way to then minimize the computation time that we have the limit to this is of course that we can always uh just compute one so we can choose a compu a controlled horizon of one but in practice it's observed that by choosing such a short control horizon the uh the the the nice properties that we want from the mpc can't be obtained so you usually want to have a little bit more than just one right yeah so this is what i said so you mostly have these parts which are zero so basically we can gain by not considering those during the optimization problem okay finally we reach uh the next part which of the design parameters which are the constraints so like we saw before we might have constraints on our inputs which mean that we can only get so much acceleration from our from our car or we can only move so much so these are some constraints that this optimization problem must be aware of so it shouldn't generate a control which says move whatever 120 degrees steering which is not supported by the car then this means that uh these kind of constraints must be taken care while solving the optimization problem but then there are also some constraints on the output for example we want the car to have a maximum velocity or a minimum velocity uh given given the road rules and given our trajectory velocity profile so to say so typically uh it's it's not recommended to put both these constraints as hard constraints so you we have this distinction in the optimization procedure between what's hard constraint and what's a soft constraint usually what one does is for the input commands these constraints are hard in the sense that you actually can't put more control in therefore these constraints must be must be obtained or must be followed whereas with the output we are usually a bit more uh lenient in the sense that if my maximum velocity is 30 but if i go to 32 or 33 it's still okay so we do this in the fact that because if we keep both of these constraints as hard constraints it might end up that there is no valid solution which the optimizer can compute and this means that we have no control for that situation and since we want to avoid this we always want to have a control maybe it's not the control which satisfies all my constraints but at least uh does as well as it can given these constraints so this way we can choose between hard constraints and soft constraints while designing uh or while choosing the constraints for our system and uh finally we'll also look into the aspects of weights so basically how should i choose my q and r to obtain the desired uh performance so here for example uh i i show two situations in which one place where we apply let's say more controls and here we apply a fewer controls this results in two situations uh which are not entirely the same so in this case we are able to track the error more correctly by applying more control whereas in this in this example below we end up with some steady state error so to say but the amount of control that we put is lower so now it's up to us as the user to decide what do i prefer more do i prefer that i apply more aggressive controls but i'm very close to the desired trajectory or do i prefer that i apply lower controls and as a result i can accept a little bit of error to my uh to my desired trajectory and this basically can be set relatively using q and r and what one can also do is for example within queue itself for example this er computes the let's say the uh the difference between the current uh position of the car and the trajectory so this could be the x y and the heading by choosing let's say the elements of the diagonal matrix or of the diagonal elements of this matrix q such that for example we can give more importance to the fact that we want the xy position to be as close whereas i'm a little bit fine if my orientation or the heading is not exactly correct so it also allows a trade-off not just between the error and the controls but also between different kinds of errors and similarly between different kinds of controls if we have have them right so the uh ideas that we saw till now usually are applied in the case where we have a linearized system so this is done mainly from a computation point of view because if our system dynamics are linear and if uh uh if uh constraints so to say are also linear or inequality based and we have a quadratic uh cost function then there are very fast solvers that are available convex optimization methods which can solve these problems and if you actually want to exploit this for a situation where we actually have a non-linear system what one can do is to actually take a non-linear system and linearize them at the current operating point so if we are currently at this state although my system dynamics is non-linear i can actually linearize them so this is in some way similar to the idea of extended kalman filter where we also do this linearization while we track the state this is the control equivalent of that where we actually want to linearize our system and then what we can then do is uh basically in our uh model here we can then use the linearized model rather than the nonlinear model and then this allows us to then use a more effective optimization techniques that we have in our toolkit however if we actually have a a system which let's say is not so easy to linearize or we have constraints which are non-linear or a combination of both and for our application it's not sufficient that we do a linearization this then means that we must actually solve this non-linear optimization problem for this prediction time and in cases where we require let's say fast aggressive movements for our car it's in like in a racetrack or so then we might go for this situation where we actually solve a non-linear optimization problem every time but of course this still means that we need the computation power and some uh smart solvers so to say which can uh do this right uh and all i mean at each time the one thing that is kind of limiting us is that since it's an online optimization problem we would always want to run our optima or our controller as fast as we can possibly do so as to have a high sampling time and um basically uh what we can do is all these design parameters that we saw the prediction horizon the control horizon the sample time the number of constraints or the number of iterations for which i actually do the optimization i can decrease all of these i will suffer some something in terms of the performance so if i decrease for example the number of iterations maybe i don't end up at the actual minimum but just an approximation of the minimum and maybe given our application it's probably sufficient that we don't do let's say 100 iterations of optimization but only 10 of them and this way we can basically compromise so to send each of these parameters to make our mpc run faster so which is often the case if you want to run it on a car you want somewhere between 10 to 100 hertz and you might want to use each one of these uh tricks so to say to make it run faster there's also another kind of mpc uh which is called as explicit mpc or you can think of it as an offline mpc where where basically you compute all these controls in an offline manner so for uh for all the states that you want to achieve you say okay what should be the control and since we have in theory infinite computation power offline we compute it and then we basically store all these results on our memory so we basically bargain the the the processing time for memory by storing all the results that are previously computed say in a lookup table we can actually look them up when we need them of course the idea is quite straightforward but there's more to the implementation because you might need to actually discretize your space in a way that this lookup table makes sense or you might need to have some more smart strategies such that we can actually look up this table efficiently and come out with this u1 and u2 which is fast enough for our application but the general idea is that you can make this computation so you can make this control computations offline and then look them up which suits for this situation uh given your current state and the current reference trajectory okay with that we reach towards the end of this lecture and before i finish i just want to give a quick summary of what we covered so in the beginning we saw a kinematic model for the car so the bicycle model which lets us understand how the how the motion evolves over time and how controls affect the motion of a car then we saw the idea of a feedback control and how we can use the current state and so to say the error signal of the current state to the desired state to actually drive our controllers and a particular version of this feedback control is the popular pid controller and we saw how to use this pid controller to both attain a longitudinal controller as well as a lateral controller to drive let's say a car along the desired path and then we saw a couple of uh strategies for lateral control which are based on the geometry so we exploit the kinematics of the the the robot as well as the trajectory of the or the geometry of the trajectory to come out with some static loss here but which help us actually drive towards our trajectory in a smooth manner and finally we also looked at a dynamic control strategy strategy where basically this control is computed online every time as the situation comes up and we saw the particular idea or the implementation of mpc mr model predictive controller to achieve this task and if you are kind of interested in exploring more about let's say different strategies or even going more in depth at the strategies that we have looked today these are some of the resources that you might want to look at with that i would like to thank you for your attention and hope that you enjoyed the lecture

Channel: Cyrill Stachniss

Views: 5,627

Keywords: robotics, photogrammetry

Id: XmjjmnDcduU

Length: 99min 17sec (5957 seconds)

Published: Wed Nov 04 2020