12345qiupeng
/
Multi-Purpose-MPC
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add dynamic constraints on e_y via update_bounds method. 

Add show_prediction function to display prediction.
master
matssteinweg 2019-12-01 16:05:25 +01:00
parent 12329708a7
commit 296d1db030
1 changed files with 30 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
MPC.py
View File

@ -3,6 +3,10 @@ import cvxpy as cp
import osqp import osqp
import scipy as sp import scipy as sp
from scipy import sparse from scipy import sparse
import matplotlib.pyplot as plt
# Colors
PREDICTION = '#BA4A00'
################## ##################
# MPC Controller # # MPC Controller #
@ -110,8 +114,9 @@ class MPC:
self.current_prediction = self.update_prediction(self.x.value) self.current_prediction = self.update_prediction(self.x.value)
delta = np.arctan(self.u.value[0, 0] * self.model.l) delta = np.arctan(self.u.value[0, 0] * self.model.l)
u = np.array([v, delta])
return delta return u
def update_prediction(self, spatial_state_prediction): def update_prediction(self, spatial_state_prediction):
""" """
@ -200,6 +205,10 @@ class MPC_OSQP:
# Offset for equality constraint (due to B * (u - ur)) # Offset for equality constraint (due to B * (u - ur))
uq = np.zeros(self.N * nx) uq = np.zeros(self.N * nx)
# Dynamic state constraints
xmin_dyn = np.kron(np.ones(self.N + 1), xmin)
xmax_dyn = np.kron(np.ones(self.N + 1), xmax)
# Iterate over horizon # Iterate over horizon
for n in range(self.N): for n in range(self.N):
@ -220,6 +229,10 @@ class MPC_OSQP:
ur[n] = kappa_r ur[n] = kappa_r
# Compute equality constraint offset (B*ur) # Compute equality constraint offset (B*ur)
uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r
lb, ub = self.model.reference_path.update_bounds(self.model,
self.model.wp_id + n)
xmin_dyn[nx * n] = lb
xmax_dyn[nx * n] = ub
# Get equality matrix # Get equality matrix
Ax = sparse.kron(sparse.eye(self.N + 1), Ax = sparse.kron(sparse.eye(self.N + 1),
@ -232,10 +245,10 @@ class MPC_OSQP:
A = sparse.vstack([Aeq, Aineq], format='csc') A = sparse.vstack([Aeq, Aineq], format='csc')
# Get upper and lower bound vectors for equality constraints # Get upper and lower bound vectors for equality constraints
lineq = np.hstack([np.kron(np.ones(self.N + 1), xmin), lineq = np.hstack([xmin_dyn,
np.kron(np.ones(self.N), umin)]) np.kron(np.ones(self.N), umin)])
uineq = np.hstack([np.kron(np.ones(self.N + 1), xmax), uineq = np.hstack([xmax_dyn,
np.kron(np.ones(self.N), umax)]) np.kron(np.ones(self.N), umax)])
# Get upper and lower bound vectors for inequality constraints # Get upper and lower bound vectors for inequality constraints
x0 = np.array(self.model.spatial_state[:]) x0 = np.array(self.model.spatial_state[:])
leq = np.hstack([-x0, uq]) leq = np.hstack([-x0, uq])
@ -270,8 +283,9 @@ class MPC_OSQP:
# Solve optimization problem # Solve optimization problem
dec = self.optimizer.solve() dec = self.optimizer.solve()
x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx)) x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx))
u = np.arctan(dec.x[-self.N] * self.model.l) delta = np.arctan(dec.x[-self.N] * self.model.l)
self.current_prediction = self.update_prediction(u, x) self.current_prediction = self.update_prediction(delta, x)
u = np.array([v, delta])
return u return u
@ -289,7 +303,7 @@ class MPC_OSQP:
# get current waypoint ID # get current waypoint ID
print('#########################') print('#########################')
for n in range(self.N): for n in range(2, self.N):
associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n] associated_waypoint = self.model.reference_path.waypoints[self.model.wp_id+n]
predicted_temporal_state = self.model.s2t(associated_waypoint, predicted_temporal_state = self.model.s2t(associated_waypoint,
spatial_state_prediction[n, :]) spatial_state_prediction[n, :])
@ -304,3 +318,11 @@ class MPC_OSQP:
return x_pred, y_pred return x_pred, y_pred
def show_prediction(self):
"""
Display predicted car trajectory in current axis.
"""
plt.scatter(self.current_prediction[0], self.current_prediction[1],
c=PREDICTION, s=5)