12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
README.md
View File

@ -1,2 +1,50 @@
# 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
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.6 MiB

BIN
mpc_demo/__pycache__/cvxpy_mpc.cpython-36.pyc Normal file
View File

Binary file not shown.

BIN
mpc_demo/__pycache__/utils.cpython-36.pyc Normal file
View File

Binary file not shown.

2
mpc_demo/cvxpy_mpc.py
View File

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

69
mpc_demo/main.py
View File

@ -34,7 +34,7 @@ class MPC():
self.opt_u[1,:] = np.radians(0) #rad/s
# 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
self.sim_time=0
@ -57,13 +57,17 @@ class MPC():
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))
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
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.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)
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.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.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('sample')
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]')
plt.xlabel('sample')
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:

2
mpc_demo/utils.py
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):
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)
fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)

72
notebooks/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb → notebook/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb
View File

@ -122,13 +122,13 @@
"\\quad\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",
"$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
"\n",
"So:\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",
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\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",
"metadata": {},
@ -757,6 +802,7 @@
"metadata": {},
"source": [
"![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)"
]
},
@ -778,9 +824,19 @@
"\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",
"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",
"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",
"$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",
@ -796,11 +852,11 @@
"\n",
"s.t.\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",
"Other nonlinear constrains may be applied:\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",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.5.2"
"version": "3.6.9"
}
},
"nbformat": 4,
"nbformat_minor": 2
"nbformat_minor": 4
}

72
notebooks/MPC_cvxpy.ipynb → notebook/MPC_cvxpy.ipynb
View File

@ -122,13 +122,13 @@
"\\quad\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",
"$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
"\n",
"So:\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",
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\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",
"metadata": {},
@ -757,6 +802,7 @@
"metadata": {},
"source": [
"![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)"
]
},
@ -778,9 +824,19 @@
"\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",
"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",
"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",
"$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",
@ -796,11 +852,11 @@
"\n",
"s.t.\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",
"Other nonlinear constrains may be applied:\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",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.5.2"
"version": "3.6.9"
}
},
"nbformat": 4,
"nbformat_minor": 2
"nbformat_minor": 4
}

0
notebooks/MPC_scipy.ipynb → notebook/MPC_scipy.ipynb
View File

0
notebooks/img/mpc_block_diagram.png → notebook/img/mpc_block_diagram.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 38 KiB

After

Width:  |  Height:  |  Size: 38 KiB

0
notebooks/img/mpc_t.png → notebook/img/mpc_t.png
View File

Side by Side Swipe Overlay

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