Updated LATEX equations in notebook
parent
0cebd24dec
commit
b3593ab100
|
@ -0,0 +1,50 @@
|
||||||
|
# mpc_python
|
||||||
|
|
||||||
|
Python implementation of mpc controller for path tracking.
|
||||||
|
|
||||||
|
## About
|
||||||
|
|
||||||
|
The MPC is a model predictive path following controller which does follow a predefined reference path Xref and Yref by solving an optimization problem. The resulting optimization problem is shown in the following equation:
|
||||||
|
|
||||||
|
MIN $ J(x(t),U) = \sum^{t+T-1}_{j=t} (x_{j,ref} - x_{j})^T_{j}Q(x_{j,ref} - x_{j}) + u^T_{j}Ru_{j} $
|
||||||
|
|
||||||
|
s.t.
|
||||||
|
|
||||||
|
$ x(0) = x0 $
|
||||||
|
|
||||||
|
$ x_{j+1} = Ax_{j}+Bu_{j})$ for $t< j <t+T-1 $
|
||||||
|
|
||||||
|
$ U_{MIN} < u_{j} < U_{MAX} $ for $t< j <t+T-1 $
|
||||||
|
|
||||||
|
The vehicle dynamics are described by the differential drive model:
|
||||||
|
|
||||||
|
* $\dot{x} = v\cos{\theta}$
|
||||||
|
* $\dot{y} = v\sin{\theta}$
|
||||||
|
* $\dot{\theta} = w$
|
||||||
|
|
||||||
|
The state variables of the model are:
|
||||||
|
|
||||||
|
* $x$ coordinate of the robot
|
||||||
|
* $y$ coordinate of the robot
|
||||||
|
* $\theta$ heading of the robot
|
||||||
|
|
||||||
|
The inputs of the model are:
|
||||||
|
|
||||||
|
* $v$ linear velocity of the robot
|
||||||
|
* $w$ angular velocity of the robot
|
||||||
|
|
||||||
|
## Demo
|
||||||
|
|
||||||
|
![](img/demo.gif)
|
||||||
|
|
||||||
|
To run the demo:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 mpc_demo/main.py
|
||||||
|
```
|
||||||
|
|
||||||
|
## Requirements
|
||||||
|
|
||||||
|
```bash
|
||||||
|
pip3 install --user --requirement requirements.txt
|
||||||
|
```
|
|
@ -6,7 +6,7 @@ Python implementation of mpc controller for path tracking.
|
||||||
|
|
||||||
The MPC is a model predictive path following controller which does follow a predefined reference path Xref and Yref by solving an optimization problem. The resulting optimization problem is shown in the following equation:
|
The MPC is a model predictive path following controller which does follow a predefined reference path Xref and Yref by solving an optimization problem. The resulting optimization problem is shown in the following equation:
|
||||||
|
|
||||||
MIN $ J(x(t),U) = \sum^{t+T-1}_{j=t} (x_{j,ref} - x_{j})^T_{j}Q(x_{j,ref} - x_{j}) + u^T_{j}Ru_{j} $
|
min $ J(x(t),U) = \sum^{t+T-1}_{j=t} (x_{j,ref} - x_{j})^T_{j}Q(x_{j,ref} - x_{j}) + u^T_{j}Ru_{j} $
|
||||||
|
|
||||||
s.t.
|
s.t.
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,184 @@
|
||||||
|
#! /usr/bin/env python
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
from matplotlib import animation
|
||||||
|
|
||||||
|
from utils import compute_path_from_wp
|
||||||
|
from cvxpy_mpc import optimize
|
||||||
|
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
# Robot Starting position
|
||||||
|
SIM_START_X=0
|
||||||
|
SIM_START_Y=2
|
||||||
|
SIM_START_H=0
|
||||||
|
|
||||||
|
# Classes
|
||||||
|
class MPC():
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
|
||||||
|
# State for the robot mathematical model [x,y,heading]
|
||||||
|
self.state = [SIM_START_X, SIM_START_Y, SIM_START_H]
|
||||||
|
# Sim step
|
||||||
|
self.dt = 0.25
|
||||||
|
|
||||||
|
# starting guess output
|
||||||
|
N = 5 #number of state variables
|
||||||
|
M = 2 #number of control variables
|
||||||
|
T = 20 #Prediction Horizon
|
||||||
|
self.opt_u = np.zeros((M,T))
|
||||||
|
self.opt_u[0,:] = 1 #m/s
|
||||||
|
self.opt_u[1,:] = np.radians(0) #rad/s
|
||||||
|
|
||||||
|
# Interpolated Path to follow given waypoints
|
||||||
|
self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
|
||||||
|
|
||||||
|
# Sim help vars
|
||||||
|
self.sim_time=0
|
||||||
|
self.x_history=[]
|
||||||
|
self.y_history=[]
|
||||||
|
self.h_history=[]
|
||||||
|
self.v_history=[]
|
||||||
|
self.w_history=[]
|
||||||
|
|
||||||
|
#Initialise plot
|
||||||
|
plt.style.use("ggplot")
|
||||||
|
self.fig = plt.figure()
|
||||||
|
plt.ion()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
def run(self):
|
||||||
|
'''
|
||||||
|
'''
|
||||||
|
|
||||||
|
while 1:
|
||||||
|
if self.state is not None:
|
||||||
|
|
||||||
|
if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<1:
|
||||||
|
print("Success! Goal Reached")
|
||||||
|
return
|
||||||
|
|
||||||
|
#optimization loop
|
||||||
|
start=time.time()
|
||||||
|
self.opt_u = optimize(self.state,
|
||||||
|
self.opt_u,
|
||||||
|
self.path)
|
||||||
|
|
||||||
|
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
|
||||||
|
|
||||||
|
self.update_sim(self.opt_u[0,1],self.opt_u[1,1])
|
||||||
|
|
||||||
|
self.plot_sim()
|
||||||
|
|
||||||
|
def update_sim(self,lin_v,ang_v):
|
||||||
|
'''
|
||||||
|
Updates state.
|
||||||
|
|
||||||
|
:param lin_v: float
|
||||||
|
:param ang_v: float
|
||||||
|
'''
|
||||||
|
|
||||||
|
self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*self.dt
|
||||||
|
self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*self.dt
|
||||||
|
self.state[2] = self.state[2] +ang_v*self.dt
|
||||||
|
|
||||||
|
def plot_sim(self):
|
||||||
|
|
||||||
|
self.sim_time = self.sim_time+self.dt
|
||||||
|
self.x_history.append(self.state[0])
|
||||||
|
self.y_history.append(self.state[1])
|
||||||
|
self.h_history.append(self.state[2])
|
||||||
|
self.v_history.append(self.opt_u[0,1])
|
||||||
|
self.w_history.append(self.opt_u[1,1])
|
||||||
|
|
||||||
|
plt.clf()
|
||||||
|
|
||||||
|
grid = plt.GridSpec(2, 3)
|
||||||
|
|
||||||
|
plt.subplot(grid[0:2, 0:2])
|
||||||
|
plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time))
|
||||||
|
|
||||||
|
plt.plot(self.path[0,:],self.path[1,:], c='tab:orange',
|
||||||
|
marker=".",
|
||||||
|
label="reference track")
|
||||||
|
|
||||||
|
plt.plot(self.x_history, self.y_history, c='tab:blue',
|
||||||
|
marker=".",
|
||||||
|
alpha=0.5,
|
||||||
|
label="vehicle trajectory")
|
||||||
|
|
||||||
|
# plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue',
|
||||||
|
# marker=".",
|
||||||
|
# markersize=12,
|
||||||
|
# label="vehicle position")
|
||||||
|
# plt.arrow(self.x_history[-1],
|
||||||
|
# self.y_history[-1],
|
||||||
|
# np.cos(self.h_history[-1]),
|
||||||
|
# np.sin(self.h_history[-1]),
|
||||||
|
# color='tab:blue',
|
||||||
|
# width=0.2,
|
||||||
|
# head_length=0.5,
|
||||||
|
# label="heading")
|
||||||
|
|
||||||
|
plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1])
|
||||||
|
|
||||||
|
plt.ylabel('map y')
|
||||||
|
plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0))
|
||||||
|
plt.xlabel('map x')
|
||||||
|
plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0))
|
||||||
|
#plt.legend()
|
||||||
|
|
||||||
|
plt.subplot(grid[0, 2])
|
||||||
|
#plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
|
||||||
|
plt.plot(self.v_history,c='tab:orange')
|
||||||
|
locs, _ = plt.xticks()
|
||||||
|
plt.xticks(locs[1:], locs[1:]*self.dt)
|
||||||
|
plt.ylabel('v(t) [m/s]')
|
||||||
|
plt.xlabel('t [s]')
|
||||||
|
|
||||||
|
plt.subplot(grid[1, 2])
|
||||||
|
#plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
|
||||||
|
plt.plot(np.degrees(self.w_history),c='tab:orange')
|
||||||
|
plt.ylabel('w(t) [deg/s]')
|
||||||
|
locs, _ = plt.xticks()
|
||||||
|
plt.xticks(locs[1:], locs[1:]*self.dt)
|
||||||
|
plt.xlabel('t [s]')
|
||||||
|
|
||||||
|
plt.tight_layout()
|
||||||
|
|
||||||
|
plt.draw()
|
||||||
|
plt.pause(0.1)
|
||||||
|
|
||||||
|
|
||||||
|
def plot_car(x, y, yaw):
|
||||||
|
LENGTH = 1.0 # [m]
|
||||||
|
WIDTH = 0.5 # [m]
|
||||||
|
OFFSET = LENGTH/2 # [m]
|
||||||
|
|
||||||
|
outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],
|
||||||
|
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
|
||||||
|
|
||||||
|
Rotm = np.array([[np.cos(yaw), np.sin(yaw)],
|
||||||
|
[-np.sin(yaw), np.cos(yaw)]])
|
||||||
|
|
||||||
|
outline = (outline.T.dot(Rotm)).T
|
||||||
|
|
||||||
|
outline[0, :] += x
|
||||||
|
outline[1, :] += y
|
||||||
|
|
||||||
|
plt.plot(np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), 'tab:blue')
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def do_sim():
|
||||||
|
sim=MPC()
|
||||||
|
try:
|
||||||
|
sim.run()
|
||||||
|
except Exception as e:
|
||||||
|
sys.exit(e)
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
do_sim()
|
|
@ -822,23 +822,31 @@
|
||||||
"\n",
|
"\n",
|
||||||
"The objective function used minimize (drive the state to 0) is:\n",
|
"The objective function used minimize (drive the state to 0) is:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$J(x(t),U) = \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
|
"$\n",
|
||||||
"\n",
|
"\\begin{equation}\n",
|
||||||
"s.t.\n",
|
"\\begin{aligned}\n",
|
||||||
"\n",
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n",
|
||||||
"$ x(0) = x0$\n",
|
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
|
||||||
"\n",
|
" & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} t<j<t+T-1 \\\\\n",
|
||||||
"$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$ for $t<j<t+T-1 $\n",
|
"\\end{aligned}\n",
|
||||||
|
"\\end{equation}\n",
|
||||||
|
"$\n",
|
||||||
"\n",
|
"\n",
|
||||||
"Other linear constrains may be applied,for instance on the control variable:\n",
|
"Other linear constrains may be applied,for instance on the control variable:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$ U_{MIN} < u_{j|t} < U_{MAX} $ for $t<j<t+T-1 $\n",
|
"$ U_{MIN} < u_{j|t} < U_{MAX} \\quad \\textrm{for} t<j<t+T-1 $\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
|
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
|
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$J(x(t),U) = \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
|
"$\n",
|
||||||
|
"\\begin{equation}\n",
|
||||||
|
"\\begin{aligned}\n",
|
||||||
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
|
||||||
|
"\\end{aligned}\n",
|
||||||
|
"\\end{equation}\n",
|
||||||
|
"$\n",
|
||||||
"\n",
|
"\n",
|
||||||
"where the error w.r.t desired state is accounted for:\n",
|
"where the error w.r.t desired state is accounted for:\n",
|
||||||
"\n",
|
"\n",
|
||||||
|
@ -848,11 +856,15 @@
|
||||||
"\n",
|
"\n",
|
||||||
"In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
|
"In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$J(x(t),U) = \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})$\n",
|
"$\n",
|
||||||
"\n",
|
"\\begin{equation}\n",
|
||||||
"s.t.\n",
|
"\\begin{aligned}\n",
|
||||||
"\n",
|
"\\min_{} \\quad & \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
|
||||||
"$ x_{j+1|t} = f(x_{j|t},u_{j|t})$ for $t<j<t+T-1 $\n",
|
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
|
||||||
|
" & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad \\textrm{for} t<j<t+T-1 \\\\\n",
|
||||||
|
"\\end{aligned}\n",
|
||||||
|
"\\end{equation}\n",
|
||||||
|
"$\n",
|
||||||
"\n",
|
"\n",
|
||||||
"Other nonlinear constrains may be applied:\n",
|
"Other nonlinear constrains may be applied:\n",
|
||||||
"\n",
|
"\n",
|
||||||
|
|
|
@ -822,23 +822,31 @@
|
||||||
"\n",
|
"\n",
|
||||||
"The objective function used minimize (drive the state to 0) is:\n",
|
"The objective function used minimize (drive the state to 0) is:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$J(x(t),U) = \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
|
"$\n",
|
||||||
"\n",
|
"\\begin{equation}\n",
|
||||||
"s.t.\n",
|
"\\begin{aligned}\n",
|
||||||
"\n",
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n",
|
||||||
"$ x(0) = x0$\n",
|
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
|
||||||
"\n",
|
" & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} t<j<t+T-1 \\\\\n",
|
||||||
"$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$ for $t<j<t+T-1 $\n",
|
"\\end{aligned}\n",
|
||||||
|
"\\end{equation}\n",
|
||||||
|
"$\n",
|
||||||
"\n",
|
"\n",
|
||||||
"Other linear constrains may be applied,for instance on the control variable:\n",
|
"Other linear constrains may be applied,for instance on the control variable:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$ U_{MIN} < u_{j|t} < U_{MAX} $ for $t<j<t+T-1 $\n",
|
"$ U_{MIN} < u_{j|t} < U_{MAX} \\quad \\textrm{for} t<j<t+T-1 $\n",
|
||||||
"\n",
|
"\n",
|
||||||
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
|
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
|
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$J(x(t),U) = \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
|
"$\n",
|
||||||
|
"\\begin{equation}\n",
|
||||||
|
"\\begin{aligned}\n",
|
||||||
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
|
||||||
|
"\\end{aligned}\n",
|
||||||
|
"\\end{equation}\n",
|
||||||
|
"$\n",
|
||||||
"\n",
|
"\n",
|
||||||
"where the error w.r.t desired state is accounted for:\n",
|
"where the error w.r.t desired state is accounted for:\n",
|
||||||
"\n",
|
"\n",
|
||||||
|
@ -848,11 +856,15 @@
|
||||||
"\n",
|
"\n",
|
||||||
"In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
|
"In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
|
||||||
"\n",
|
"\n",
|
||||||
"$J(x(t),U) = \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})$\n",
|
"$\n",
|
||||||
"\n",
|
"\\begin{equation}\n",
|
||||||
"s.t.\n",
|
"\\begin{aligned}\n",
|
||||||
"\n",
|
"\\min_{} \\quad & \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
|
||||||
"$ x_{j+1|t} = f(x_{j|t},u_{j|t})$ for $t<j<t+T-1 $\n",
|
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
|
||||||
|
" & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad \\textrm{for} t<j<t+T-1 \\\\\n",
|
||||||
|
"\\end{aligned}\n",
|
||||||
|
"\\end{equation}\n",
|
||||||
|
"$\n",
|
||||||
"\n",
|
"\n",
|
||||||
"Other nonlinear constrains may be applied:\n",
|
"Other nonlinear constrains may be applied:\n",
|
||||||
"\n",
|
"\n",
|
||||||
|
|
|
@ -723,9 +723,9 @@
|
||||||
"name": "python",
|
"name": "python",
|
||||||
"nbconvert_exporter": "python",
|
"nbconvert_exporter": "python",
|
||||||
"pygments_lexer": "ipython3",
|
"pygments_lexer": "ipython3",
|
||||||
"version": "3.5.2"
|
"version": "3.6.9"
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
"nbformat": 4,
|
"nbformat": 4,
|
||||||
"nbformat_minor": 2
|
"nbformat_minor": 4
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue