Updated README.md

master
mcarfagno 2020-03-04 12:32:29 +00:00
parent 3c9152ffab
commit e114ca9f87
13 changed files with 244 additions and 27 deletions

View File

@ -1,2 +1,50 @@
# mpc_python # mpc_python
Python implementation of mpc controller for path tracking
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
```

BIN
img/demo.gif Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.6 MiB

Binary file not shown.

Binary file not shown.

View File

@ -1,4 +1,6 @@
import numpy as np import numpy as np
np.seterr(divide='ignore', invalid='ignore')
from scipy.integrate import odeint from scipy.integrate import odeint
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
import cvxpy as cp import cvxpy as cp

View File

@ -34,7 +34,7 @@ class MPC():
self.opt_u[1,:] = np.radians(0) #rad/s self.opt_u[1,:] = np.radians(0) #rad/s
# Interpolated Path to follow given waypoints # Interpolated Path to follow given waypoints
self.path = compute_path_from_wp([0,20,30,30],[0,0,10,20]) self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
# Sim help vars # Sim help vars
self.sim_time=0 self.sim_time=0
@ -57,13 +57,17 @@ class MPC():
while 1: while 1:
if self.state is not None: 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 #optimization loop
start=time.time() start=time.time()
self.opt_u = optimize(self.state, self.opt_u = optimize(self.state,
self.opt_u, self.opt_u,
self.path) self.path)
print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
self.update_sim(self.opt_u[0,1],self.opt_u[1,1]) self.update_sim(self.opt_u[0,1],self.opt_u[1,1])
@ -97,31 +101,78 @@ class MPC():
plt.subplot(grid[0:2, 0:2]) plt.subplot(grid[0:2, 0:2])
plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time)) 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.path[0,:],self.path[1,:], c='tab:orange',
plt.plot(self.x_history, self.y_history, c='tab:blue', marker=".", alpha=0.5, label="vehicle trajectory") marker=".",
plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue', marker=".", markersize=12, label="vehicle position") label="reference track")
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)
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.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.xlabel('map x')
plt.legend() 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.subplot(grid[0, 2])
#plt.title("Linear Velocity {} m/s".format(self.v_history[-1])) #plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
plt.plot(self.v_history,c='tab:orange') 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.ylabel('v(t) [m/s]')
plt.xlabel('sample') plt.xlabel('t [s]')
plt.subplot(grid[1, 2]) plt.subplot(grid[1, 2])
#plt.title("Angular Velocity {} m/s".format(self.w_history[-1])) #plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
plt.plot(np.degrees(self.w_history),c='tab:orange') plt.plot(np.degrees(self.w_history),c='tab:orange')
plt.ylabel('w(t) [deg/s]') plt.ylabel('w(t) [deg/s]')
plt.xlabel('sample') locs, _ = plt.xticks()
plt.xticks(locs[1:], locs[1:]*self.dt)
plt.xlabel('t [s]')
plt.tight_layout() plt.tight_layout()
plt.draw() plt.draw()
plt.pause(0.1) 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(): def do_sim():
sim=MPC() sim=MPC()
try: try:

View File

@ -18,7 +18,7 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
for idx in range(len(start_xp)-1): for idx in range(len(start_xp)-1):
section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2))) section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))
interp_range = np.linspace(0,1,section_len/delta) interp_range = np.linspace(0,1,int(section_len/delta))
fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1) fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1) fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)

View File

@ -122,13 +122,13 @@
"\\quad\n", "\\quad\n",
"$\n", "$\n",
"\n", "\n",
"This State Space Model is not linear (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n", "This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
"\n", "\n",
"$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n", "$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
"\n", "\n",
"So:\n", "So:\n",
"\n", "\n",
"$ x{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u}) )dt $\n", "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x_t-\\bar{x}) + B(u_t-\\bar{u}) )dt $\n",
"\n", "\n",
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n", "$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
"\n", "\n",
@ -157,6 +157,51 @@
"-----------------" "-----------------"
] ]
}, },
{
"cell_type": "markdown",
"metadata": {},
"source": [
"-----------------\n",
"[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
"\n",
"In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
"\n",
"Typically it is possible to assume that the system is operating about some nominal\n",
"state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
"\n",
"Recall that the Taylor Series expansion of f(x) around the\n",
"point $\\bar{x}$ is given by:\n",
"\n",
"$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
"\n",
"For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
"\n",
"The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
"is given by:\n",
"\n",
"$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
"\n",
"Where:\n",
"\n",
"$ A =\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"$ and $ B = \\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
"\\end{bmatrix}\n",
"\\quad $\n",
"\n",
"Then:\n",
"\n",
"$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"-----------------"
]
},
{ {
"cell_type": "markdown", "cell_type": "markdown",
"metadata": {}, "metadata": {},
@ -757,6 +802,7 @@
"metadata": {}, "metadata": {},
"source": [ "source": [
"![mpc](img/mpc_block_diagram.png)\n", "![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)" "![mpc](img/mpc_t.png)"
] ]
}, },
@ -778,9 +824,19 @@
"\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", "$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",
"This 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", "s.t.\n",
"\n", "\n",
"Becouse the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n", "$ x(0) = x0$\n",
"\n",
"$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$ for $t<j<t+T-1 $\n",
"\n",
"Other linear constrains may be applied,for instance on the control variable:\n",
"\n",
"$ U_{MIN} < u_{j|t} < U_{MAX} $ for $t<j<t+T-1 $\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",
"\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", "$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", "\n",
@ -796,11 +852,11 @@
"\n", "\n",
"s.t.\n", "s.t.\n",
"\n", "\n",
"$ x_{j+1|t} = f(x_{j|t},{j|t}) t<j<t+T-1 $\n", "$ x_{j+1|t} = f(x_{j|t},u_{j|t})$ for $t<j<t+T-1 $\n",
"\n", "\n",
"Other nonlinear constrains may be applied:\n", "Other nonlinear constrains may be applied:\n",
"\n", "\n",
"$ g(x_{j|t},{j|t})<0 t<j<t+T-1 $" "$ g(x_{j|t},{j|t})<0$ for $t<j<t+T-1 $"
] ]
}, },
{ {
@ -1176,9 +1232,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
} }

View File

@ -122,13 +122,13 @@
"\\quad\n", "\\quad\n",
"$\n", "$\n",
"\n", "\n",
"This State Space Model is not linear (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n", "This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
"\n", "\n",
"$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n", "$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
"\n", "\n",
"So:\n", "So:\n",
"\n", "\n",
"$ x{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u}) )dt $\n", "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x_t-\\bar{x}) + B(u_t-\\bar{u}) )dt $\n",
"\n", "\n",
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n", "$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
"\n", "\n",
@ -157,6 +157,51 @@
"-----------------" "-----------------"
] ]
}, },
{
"cell_type": "markdown",
"metadata": {},
"source": [
"-----------------\n",
"[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
"\n",
"In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
"\n",
"Typically it is possible to assume that the system is operating about some nominal\n",
"state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
"\n",
"Recall that the Taylor Series expansion of f(x) around the\n",
"point $\\bar{x}$ is given by:\n",
"\n",
"$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
"\n",
"For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
"\n",
"The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
"is given by:\n",
"\n",
"$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
"\n",
"Where:\n",
"\n",
"$ A =\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"$ and $ B = \\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
"\\end{bmatrix}\n",
"\\quad $\n",
"\n",
"Then:\n",
"\n",
"$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"-----------------"
]
},
{ {
"cell_type": "markdown", "cell_type": "markdown",
"metadata": {}, "metadata": {},
@ -757,6 +802,7 @@
"metadata": {}, "metadata": {},
"source": [ "source": [
"![mpc](img/mpc_block_diagram.png)\n", "![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)" "![mpc](img/mpc_t.png)"
] ]
}, },
@ -778,9 +824,19 @@
"\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", "$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",
"This 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", "s.t.\n",
"\n", "\n",
"Becouse the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n", "$ x(0) = x0$\n",
"\n",
"$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$ for $t<j<t+T-1 $\n",
"\n",
"Other linear constrains may be applied,for instance on the control variable:\n",
"\n",
"$ U_{MIN} < u_{j|t} < U_{MAX} $ for $t<j<t+T-1 $\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",
"\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", "$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", "\n",
@ -796,11 +852,11 @@
"\n", "\n",
"s.t.\n", "s.t.\n",
"\n", "\n",
"$ x_{j+1|t} = f(x_{j|t},{j|t}) t<j<t+T-1 $\n", "$ x_{j+1|t} = f(x_{j|t},u_{j|t})$ for $t<j<t+T-1 $\n",
"\n", "\n",
"Other nonlinear constrains may be applied:\n", "Other nonlinear constrains may be applied:\n",
"\n", "\n",
"$ g(x_{j|t},{j|t})<0 t<j<t+T-1 $" "$ g(x_{j|t},{j|t})<0$ for $t<j<t+T-1 $"
] ]
}, },
{ {
@ -1176,9 +1232,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
} }

View File

Before

Width:  |  Height:  |  Size: 38 KiB

After

Width:  |  Height:  |  Size: 38 KiB

View File

Before

Width:  |  Height:  |  Size: 32 KiB

After

Width:  |  Height:  |  Size: 32 KiB

4
requirements.txt Normal file
View File

@ -0,0 +1,4 @@
cvxpy==1.0.28
numpy==1.18.1
osqp==0.6.1
scipy==1.4.1