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

Add dynamic speed constraint based on predicted path curvature

master
matssteinweg 2019-12-07 10:16:07 +01:00
parent 4e9959a1b1
commit 7abc906af5
1 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
MPC.py
View File

@ -42,6 +42,9 @@ class MPC:
self.state_constraints = StateConstraints self.state_constraints = StateConstraints
self.input_constraints = InputConstraints self.input_constraints = InputConstraints
# Maximum lateral acceleration
self.ay_max = 5.0
# Current control and prediction # Current control and prediction
self.current_prediction = None self.current_prediction = None
@ -76,6 +79,11 @@ class MPC:
# Dynamic state constraints # Dynamic state constraints
xmin_dyn = np.kron(np.ones(self.N + 1), xmin) xmin_dyn = np.kron(np.ones(self.N + 1), xmin)
xmax_dyn = np.kron(np.ones(self.N + 1), xmax) xmax_dyn = np.kron(np.ones(self.N + 1), xmax)
# Dynamic input constraints
umax_dyn = np.kron(np.ones(self.N), umax)
# Get curvature predictions from previous control signals
kappa_pred = np.tan(np.array(self.current_control[3::] +
self.current_control[-1:])) / self.model.l
# Iterate over horizon # Iterate over horizon
for n in range(self.N): for n in range(self.N):
@ -102,6 +110,10 @@ class MPC:
self.model.wp_id + n, self.model.safety_margin[1]) self.model.wp_id + n, self.model.safety_margin[1])
xmin_dyn[self.nx * n] = lb xmin_dyn[self.nx * n] = lb
xmax_dyn[self.nx * n] = ub xmax_dyn[self.nx * n] = ub
# Constrain maximum speed based on predicted car curvature
vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n]) + 1e-12))
if vmax_dyn < umax_dyn[self.nu*n]:
umax_dyn[self.nu*n] = vmax_dyn
# Get equality matrix # Get equality matrix
Ax = sparse.kron(sparse.eye(self.N + 1), Ax = sparse.kron(sparse.eye(self.N + 1),
@ -116,8 +128,7 @@ class MPC:
# Get upper and lower bound vectors for equality constraints # Get upper and lower bound vectors for equality constraints
lineq = np.hstack([xmin_dyn, lineq = np.hstack([xmin_dyn,
np.kron(np.ones(self.N), umin)]) np.kron(np.ones(self.N), umin)])
uineq = np.hstack([xmax_dyn, uineq = np.hstack([xmax_dyn, umax_dyn])
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])