Autonomous Lane Changing using Model Predictive Control

Abstract

Research Questions

Methods

Plan of Action

PID vs MPC
- Controller Schematics
- Reference Frames
Lateral Control using Equations of Motion
- Bicycle Model
- Ackerman Steering
- Longitudinal and Lateral Velocities
- Equations of Motion in Lateral Direction
- Understanding Centripetal Acceleration
- Acceleration in lateral direction
- Modelling the Front Wheel
State-Space and Linear Time Invariant (LTI)
- Equations of motion to State-space
- Adding Extra States
- Computing New States
- Linear Time Invariant (LTI)
- LTI Model with Small Angle Approximation
MPC of Rocket
- Thrust Levels
- Cost Function
- Noise
- Weights
- Horizon Period
- Kalman Filter
- Quadratic and Other Cost Functions
MPC of Autonomous Car
- Forward Euler Method
- Predicting Future States
- Deriving a Cost Function
- Regulating $https://latex.codecogs.com/svg.image?\delta$
- Advantages of Predicting
- Errors as State-space equations

Implementation

1. PID vs MPC

The problem statement is simple: we have an obstacle (a car at rest) on our lane, we want to achieve a lane changing maneuvre to avoid that obstacle. So how does this work? We assume we have a lidar system or a pseudo-lidar system using stereo cameras which will detect the obstacle and plan a trajectory for lane changing. The goal here is to compute $https://latex.codecogs.com/svg.image?\delta$ degrees which is the angle required for the front wheel to rotate in order to achieve this planned trajectory.

So we need to design a controller which will derive automatically which $https://latex.codecogs.com/svg.image?\delta$ degrees is optimal. To simplify our system, we will use the line diagram in the picture below shown on the right. $https://latex.codecogs.com/svg.image?\delta$ is the angle between the vertical axis and the local axis of the wheel.

We will need to make a couple of assumptions in order to simplify our system:

We will be moving at a constant velocity.
We already have a trajectory pre-planned - it can be the optimal solution from another system.
We assume the car is moving on a flat surface so that we can ignore the Z-axis.

From the assumptions made above, we have a 3 degree of freedom (dof) system. That is, we only require 3 variables in order to derive the position and orientation of the car - $https://latex.codecogs.com/png.image?\dpi{110}(x,y,\psi)$ where $https://latex.codecogs.com/png.image?\dpi{110}\psi$ is the angle in radians of the orientation of the vehicle w.r.t to a reference frame.

Our goal will be to overtake the obstacle, that is changing the lane, in 7 seconds. While this may seem simple, it is quite a challenging problem as we have to take into consideration the comfort of the passenger in the vehicle, the safety of the passengers in the vehicle and of other vehicles and also the efficiency of our system.

Scenario 1: Staight Line Trajectory

We can have a case where we can straight line trajectory for lane overtaking. Also, it can be an easy task computationally, it will not be a realistic case. In real-life, it is impossibe to have such sharp turns and suppose if we can achieve it then it will be quite uncomfortable for our passengers. Hence, we can scrape scenatio 1.

Scenario 2: Bézier Curve Trajectory

The reason we use a spline or more specifically a Bézier curve is in order to achieve a more realistic scenario which we would expect in real-life. We can be sure to achieve a smooth transition during overtaking which will allow for the comfort and safety of the passengers.

For now, we are assuming our obstacles are constant however, if we had a system which could determine the speed at which other vehicles are moving and if we could determine the length of the vehicles then we could determine this overtaking trajectory on the go instead of pre-planned.

1.1 Controller Schematics

We will start our controller schematics with an Open-Loop system. Our input will be $https://latex.codecogs.com/png.image?\dpi{110}\delta$ , that is the angle of rotation of the front wheels, and our output will be $https://latex.codecogs.com/png.image?\dpi{110}(x,y,\psi)$ .

Once we have $https://latex.codecogs.com/png.image?\dpi{110}x,y,\psi$ , we need to compare them to our reference values.

$https://latex.codecogs.com/png.image?\dpi{110}x,y,\psi$ : where we are right now
$https://latex.codecogs.com/png.image?\dpi{110}x_{R},y_{R},\psi _{R}$ : where we want to be

In the open loop system above, we already have our $https://latex.codecogs.com/png.image?\dpi{110}\delta$ as the input, so now we need to build the closed-loop system which will allow us to derive the $https://latex.codecogs.com/png.image?\dpi{110}\delta$ .

$https://latex.codecogs.com/png.image?\dpi{110}\begin{bmatrix}x_{R}\\y_{R}\\\psi _{R}\\\end{bmatrix}-\begin{bmatrix}x \\y \\\psi \end{bmatrix}=\begin{bmatrix}e_{x}\\e_{y}\\e_{\psi}\\\end{bmatrix}$

The error vector will be the input of our controller and its job, its output, will be to find the best steering wheel angle, $https://latex.codecogs.com/png.image?\dpi{110}\delta$ , so that the errors go to 0. The errors go to zero when $https://latex.codecogs.com/png.image?\dpi{110}x_{R} = x$ , $https://latex.codecogs.com/png.image?\dpi{110}y_{R} = y$ and $https://latex.codecogs.com/png.image?\dpi{110}\psi _{R} = \psi$ .

Suppose we are moving at 20 m/s then in 7 s, our distance covered in the y-direction will be 140 m whereas the distance covered in the x-direction will be only 4 m. An approximate of the yaw-angle is arctan(4/140) which is 1.64 degrees which is very small. Hence, we deduce $https://latex.codecogs.com/png.image?\dpi{110}\psi$ depends only on the y-direction and we can ignore the x-direction.

Note: We do not need to control the Forward Velocity and we will simplify our system to make is 2 dof such that we now only need to variables: y and $https://latex.codecogs.com/png.image?\dpi{110}\psi$ :

The reason we will be using a MPC instead of a PID is becasue a PID is a Single Input Single Output (SISO) whereas a MPS is a Multiple Input Multiple Output (MIMO). We can still use a PID controller but we have have to tune 6 constants - 3 constants $https://latex.codecogs.com/png.image?\dpi{110}(K_{P},K_{D},K_{I})$ for each PID. Figure below shows the schematic if we had used PID controllers:

1.2 Reference Frames

We will now setup our reference frames: Inertial Reference Frame and a Body Frame. The reason we have 2 reference frames is because our trajectory is defined in the Inertial Frame and therefore we need to translate our Body Frame's velocity into our Global Coordinate System.

Recall that we have a constant longitudinal velocity of 20 m/s.

From the image below we have $https://latex.codecogs.com/png.image?\dpi{110}\dot{x}$ and $https://latex.codecogs.com/png.image?\dpi{110}\dot{y}$ in our Body Frame and we need to transform it into $https://latex.codecogs.com/png.image?\dpi{110}\dot{X}$ and $https://latex.codecogs.com/png.image?\dpi{110}\dot{Y}$ respectively in the Global Reference Frame.

We start by finding the components of $https://latex.codecogs.com/png.image?\dpi{110}V_{x}$ and $https://latex.codecogs.com/png.image?\dpi{110}V_{y}$ in the Global Reference frame then compute the resultant velocities in the X and Y direction.

$https://latex.codecogs.com/png.image?\dpi{110}\dot{X}=V_{x_{x}}-V_{y_{x}}=V_{x}\cdot cos(\frac{\pi}{6})-V_{y}\cdot sin(\frac{\pi}{6})$

$https://latex.codecogs.com/png.image?\dpi{110}\dot{Y}=V_{x_{y}}+V_{y_{y}}=V_{x}\cdot sin(\frac{\pi}{6})+V_{y}\cdot cos(\frac{\pi}{6})$

From the two computations above, we transform the Body Frame's velocities to velocities in the Global Refrence Frame.

2. Lateral Control using Equations of Motion

We will now design our plant to describe motion in the lateral direction. We will use that model/plant to later design our controller (MPC). The system will try to relate the system's input, $https://latex.codecogs.com/png.image?\dpi{110}\delta$ to the system's output $https://latex.codecogs.com/png.image?\dpi{110}Y,\psi$ . so we will have a function that describes $https://latex.codecogs.com/png.image?\dpi{110}Y,\psi$ based on $https://latex.codecogs.com/png.image?\dpi{110}\delta$ - $https://latex.codecogs.com/png.image?\dpi{110}f(\delta )\mapsto Y,\psi$ .

2.1 Bicycle Model

Earlier we simplified our model with the line diagram, now we will further simplify our model with one of a Bicycle. We use the Bicycle model as it represents the car's dynamics well enough and allow the design of the controller reliably.

Scenario 1: Two instanteneous points with old cars model

Long time ago, cars used to turn their entire front axle and not only the wheels to make a turn. However, turning the entire axle is not very efficient. When two wheels are locked on an axle so that they are not free to turn separately, one or the other has to slide.

In the figure below, we have two instances where the axle is turn at 15 degrees (green) and another at 30 degrees (red). If we draw two projection lines perpendicular to the inner front and back wheels, then these two lines intersect at the Instanteneous Center Point.

The instanteneous centre of rotation/point is a point about which a rigid body in general plane motion can be considered to have purely rotational motion at that instant.

We see that a mere change of 15 degrees pushes the centre points further apart and does not allow the car for sharp turns. This can be problematic in practice specially if we need to have tight corners and paralell parking.

Scenario 2: Two instanteneous points with pivoting wheels

We will now see at an improvement of our car model whereby we no longer need to turn the entire axle to make a turn but only need to turn the wheels. We see that the problem with this kind of configuration that when turning atleast one wheel will be slipping.

So the question we should ask is: how can we turn the two wheels without turning the entire axle in such a way so as to have only one Instanteneous Center of Rotation?

Scenario 3: One instanteneous point with new cars model

The goal is to have two different angle of rotation of the front wheels with the inner wheel pivoted at a bigger angle than the outer wheel. With this solution we avoid turing an entire axle and avoid tire slipping.

However, the Bicycle model does not take this into account as it has only one front wheel. However, $https://latex.codecogs.com/png.image?\dpi{110}\alpha -\delta\approx 0$ , therefore, we can take $https://latex.codecogs.com/png.image?\dpi{110}\delta$ as the front wheel angle.

2.2 Ackerman Steering

Now, the question we should ask is that if we find $https://latex.codecogs.com/png.image?\dpi{110}\delta$ from the Bicycle model from the MPC then how do we apply it to the car model? To approximate the ideal steering ( $https://latex.codecogs.com/png.image?\dpi{110}\delta_{1},\delta_{2}$ where $https://latex.codecogs.com/png.image?\dpi{110}\delta_{1}\neq \delta_{2}$ ) that will cause one Centre of Instanteneous Rotation, we need to use Ackerman Steering.

The intention of Ackermann geometry is to avoid the need for tires to slip sideways when following the path around a curve. As the steering moved, the wheels turned according to Ackermann, with the inner wheel turning further.

When making a turn each tires' path has a different radius therefore, this means that each tire is rotating at a different rate. The wheels on the outside travel further distance so the angular velocities of the outer wheels are greater than the angular velocities of the inner wheels. But the wheels on the inside turn at a greater angle than those on the outside as shown below.

Note: Modern cars do not use pure Ackermann steering, partly because it ignores important dynamic and compliant effects, but the principle is sound for low-speed maneuvers.

2.3 Longitudinal and Lateral Velocities

Earlier we said that when the car performs a lane changing maneuvre with a constant longitudinal velocity of 20 m/s then the car will mostly drive in the Longitudinal direction and not in the Lateral direction. However, for our Bicycle model, we also have a velocity in the lateral direction which is small compared to the longitudinal velocity.

In the diagram above we see that we have a front velocity $https://latex.codecogs.com/png.image?\dpi{110}V_{F}$ but all along the body of our bicycle we have velocities V with angles $https://latex.codecogs.com/png.image?\dpi{110}\theta _{1}$ till $https://latex.codecogs.com/png.image?\dpi{110}\theta _{4}$ of magnitude less than $https://latex.codecogs.com/png.image?\dpi{110}V_{F}$ . And we can draw a perpendicular projection from all these velocities and all of them will intersect at the Centre of Instanteneous Rotation. The magnitude of the angles are such that:

$https://latex.codecogs.com/png.image?\dpi{110}\theta _{1}<\theta _{2}<\beta <\theta _{3}<\theta _{4}$

2.4 Equations of Motion in Lateral Direction

We will now derive the equations of motion which connect all forces to an acceleration and connect all moments to an angular acceleration.

$https://latex.codecogs.com/png.image?\dpi{110}\sum F = m\cdot a$

$https://latex.codecogs.com/png.image?\dpi{110}\sum M = I\cdot \ddot{\psi }$

Hence, from the equations above we derive the equations of motion in the lateral direction:

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{r}}+F_{y_{r}}=m\cdot a_{y}\xrightarrow[]{}1$

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{f}}\cdot l_{f}-F_{y_{r}}\cdot l_{r}=I\cdot \ddot{\psi }\xrightarrow[]{}2$

where

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{r}}$ and $https://latex.codecogs.com/png.image?\dpi{110}F_{y_{f}}$ are fritional forces.
$https://latex.codecogs.com/png.image?\dpi{110}\ddot{\psi }$ is the angular acceleration in $https://latex.codecogs.com/png.image?\dpi{110}rad \cdot s^{-2}$
I is the moment of inertia in $https://latex.codecogs.com/png.image?\dpi{110}Kg \cdot m^{2}$
m is the mass of the body in $https://latex.codecogs.com/png.image?\dpi{110}Kg$
$https://latex.codecogs.com/png.image?\dpi{110}a_{y}$ is the acceleration in the lateral direction
$https://latex.codecogs.com/png.image?\dpi{110}l_{r}$ and $https://latex.codecogs.com/png.image?\dpi{110}l_{f}$ are the perpendicular distance in m from the frictional forces to the Centre of gravity of the body, $https://latex.codecogs.com/png.image?\dpi{110}C_{g}$ .

2.5 Understanding Centripetal Acceleration

We will further derive our equations of motion but before that we need to explore what is centripetal acceleration.

Centripetal acceleration is the the acceleration of a body traversing a circular path. Because velocity is a vector quantity (that is, it has both a magnitude and a direction), when a body travels on a circular path, its direction constantly changes and thus its velocity changes, producing an acceleration. The acceleration is directed radially toward the centre of the circle.

The figure below shows an object moving in a circular path at constant speed. The direction of the instantaneous velocity is shown at two points along the path. Acceleration is in the direction of the change in velocity, which points directly toward the center of rotation—the center of the circular path. We call the acceleration of an object moving in uniform circular motion—resulting from a net external force—the centripetal acceleration $https://latex.codecogs.com/png.image?\dpi{110}a_{c}$ ; centripetal means “toward the center” or “center seeking”.

In the image above, we have a body moving along the arc length x from point A to B. The equation relating x, r and $https://latex.codecogs.com/png.image?\dpi{110}\psi$ is:

$https://latex.codecogs.com/png.image?\dpi{110}x=r\cdot \psi$

It is important to note here that the linear velocity V is always perpendicular to the curved path - it always acts tangential to the circular path. We define velocity as the rate of change of displacement, therefore:

$https://latex.codecogs.com/png.image?\dpi{110}V = \frac{d(x)}{dt}=\dot{x}=\frac{d(r\cdot \psi )}{dt}=r\cdot \dot{\psi }$

where $https://latex.codecogs.com/png.image?\dpi{110}\dot{\psi }$ is the rate of change of angular displacement.

If we now differentiate the equation above a second time, we will get the acceleration which is defined as the rate of change of velocity:

$https://latex.codecogs.com/png.image?\dpi{110}a_{c}=\frac{d^{2}(x)}{dt^{2}}=\frac{d(V)}{dt}=r\cdot \ddot{\psi }$

We can also prove that the centripetal acceleration is equal to $https://latex.codecogs.com/png.image?\dpi{110}\frac{V^{2}}{r}$ thanks to this derivation from Wikipedia:

We can further derive the centripetal accelearion to get:

$https://latex.codecogs.com/svg.image?a_{c}=\frac{V^{2}}{r}=\frac{\dot{x}\cdot \dot{x}}{r}=\frac{(r\cdot \dot{\psi })\cdot \dot{x}}{r}=\dot{\psi} \cdot \dot{x}$

where:

$https://latex.codecogs.com/svg.image?\dot{x}$ : change of longitudinal velocity w.r.t time
$https://latex.codecogs.com/svg.image?\dot{\psi }$ : change of angular displacement w.r.t time

Note: In our case, our longitudinal velocity is constant but the direction of the velocity changes hence, we have an acceleration

2.6 Acceleration in lateral direction

In the lane changing project, the turns are so small and therefore the slip angle $https://latex.codecogs.com/png.image?\dpi{110}\beta$ is so small that we can neglect it and instead use $https://latex.codecogs.com/png.image?\dpi{110}\dot{x}$ instead.

Thus, from the theory explained above, we will then define our acceleration in the lateral direction as :

$https://latex.codecogs.com/png.image?\dpi{110}a_{y}=\ddot{y}+a_{c}=\ddot{y}+\dot{x}\cdot \dot{\psi }$

We can choose to either work with the Body Frame (Fixed to the vehicle) or work with the Inertial Frame (Fixed to the ground). When the vehicle rotates, the body frame rorates with it hence, for simplicity we will choose to work with the Body Frame.

From the image above, if we go from configuration 1 to configuration 2 and assuming $https://latex.codecogs.com/png.image?\dpi{110}\dot{y}$ remains constant then

$https://latex.codecogs.com/png.image?\dpi{110}\frac{d(\dot{y})}{dt}=0 ms^{-2}$

Why is that? Because in the Inertial Frame, $https://latex.codecogs.com/png.image?\dpi{110}\dot{y}$ changes direction but in the Body Frame the direction of $https://latex.codecogs.com/png.image?\dpi{110}\dot{y}$ remains constant. Hence, $https://latex.codecogs.com/png.image?\dpi{110}\dot{x}$ and $https://latex.codecogs.com/png.image?\dpi{110}\dot{y}$ rotate together with the Body Frame and $https://latex.codecogs.com/png.image?\dpi{110}\ddot{y}$ appears in the Body Frame only when the magnitude changes.

$https://latex.codecogs.com/png.image?\dpi{110}a_{c}=\dot{x}\cdot \dot{\psi }$ appears because the Body Frame is rotating w.r.t the Inertial Frame. If the Body Frame did not rotate then the second term in $https://latex.codecogs.com/png.image?\dpi{110}a_{y}$ would be 0 because $https://latex.codecogs.com/png.image?\dpi{110}\psi$ would be 0.

Therefore:

We update our equations of motion as we have expanded our terms in $https://latex.codecogs.com/png.image?\dpi{110}a_{y}$ :

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{f}}+F_{y_{r}}=m(\ddot{y}+\dot{x}\cdot \dot{\psi })\xrightarrow[]{}1$

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{f}}\cdot l_{f}-F_{y_{r}}\cdot l_{r}=I\cdot \ddot{\psi }\xrightarrow[]{}2$

2.7 Modelling the Front Wheel

Before we assumed that the velocity of each wheel was in the direction of tha twheel. However, this is only true when the vehicle is moving at low speed.

When the vehicle is moving at high speed, it experiences tyre deformation. How this happens? The tyre are made of rubber and the friction between the tyres and the road under lateral load causes deformation. So our contact patch is not going to be facing the exact same direction as the rest of the tyre which is not experiencing friction.

The velocity fo the wheel will be between the longitudinal axis and the axis of the wheel - the velocity will be faicng the same direction as the direction of the contact patch. From the diagram above, we can therefore deduce the following equations:

$https://latex.codecogs.com/png.image?\dpi{110}\alpha _{f}=\delta -\theta _{V_{f}}$

$https://latex.codecogs.com/png.image?\dpi{110}\alpha _{r}=0 -\theta _{V_{r}}=-\theta _{V_{r}}$

The typical relation between the lateral force and the slip angle can be observed in the figure below. It is shown in expriments that slip angles are proportional to the lateral forces.

The tire model describes a linear relationship between tire lateral force and slip angle. Note this is valid only for small slip angles as shown on the graph above - for slip angles < 8 deg.

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{f}} \propto\alpha _{f}$

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{r}} \propto\alpha _{r}$

We can then express our lateral forces in terms of cornering stiffness constants C:

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{f}} = 2\cdot C_{\alpha _{f}}\cdot \alpha _{f}$

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{r}} = 2\cdot C_{\alpha _{r}}\cdot \alpha _{r}$

We already know the equation for our slip angles $https://latex.codecogs.com/png.image?\dpi{110}\alpha _{f}$ and $https://latex.codecogs.com/png.image?\dpi{110}\alpha _{r}$ :

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{f}} = 2\cdot C_{\alpha _{f}}\cdot (\delta -\theta _{V_{f}})$

$https://latex.codecogs.com/png.image?\dpi{110}F_{y_{r}} = 2\cdot C_{\alpha _{r}}\cdot (-\theta _{V_{r}})$

Now, we need to figure out how can we write $https://latex.codecogs.com/png.image?\dpi{110}\theta _{V_{f}}$ and $https://latex.codecogs.com/png.image?\dpi{110}\theta _{V_{r}}$ in terms of our known variables: $https://latex.codecogs.com/png.image?\dpi{110}\dot{x},\dot{y},l_{f},l_{r},\dot{\psi }$

2.7.1 Front Wheel's Slip Angle

We can find $https://latex.codecogs.com/png.image?\dpi{110}\theta _{V_{f}}$ using tan. $https://latex.codecogs.com/png.image?\dpi{110}V_{L}$ can be seen as the resultant velocity in the lateral direction.

$https://latex.codecogs.com/png.image?\dpi{110}tan(\theta _{V_{f}})=\frac{V_{L}}{\dot{x}}$

Similar to finding the acceleration in the lateral direction, we have two scenarios. First, we can have lateral motion of the centre of mass as shown below in the configuration 1 and 2. Our lateral velocity is then

$https://latex.codecogs.com/png.image?\dpi{110}V_{L} = \dot{y}$

Or, we can have rotation about the centre of mass describing a circular path of radius $https://latex.codecogs.com/png.image?\dpi{110}l_{f}$ which is the length from the centre of mass till the front of the wheel. In this configuration 3, we have

$https://latex.codecogs.com/png.image?\dpi{110}V_{L}=\dot{\psi }\cdot l_{f}$