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

Accumulate time in prediction horizon

master
matssteinweg 2019-12-06 10:45:20 +01:00
parent c16b14f3cf
commit d8d596bb14
3 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
MPC.py
View File

@ -90,14 +90,14 @@ class MPC:
kappa_r = current_waypoint.kappa kappa_r = current_waypoint.kappa
# Compute LTV matrices # Compute LTV matrices
A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s) f, A_lin, B_lin = self.model.linearize(v, kappa_r, delta_s)
A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin A[nx + n * nx:n * nx + 2 * nx, n * nx:n * nx + nx] = A_lin
B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin
# Set kappa_r to reference for input signal # Set kappa_r to reference for input signal
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 - f
lb, ub = self.model.reference_path.update_bounds( lb, ub = self.model.reference_path.update_bounds(
self.model.wp_id + n, self.model.safety_margin[1]) self.model.wp_id + n, self.model.safety_margin[1])
xmin_dyn[nx * n] = lb xmin_dyn[nx * n] = lb
@ -211,7 +211,7 @@ class MPC:
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, :])
print(spatial_state_prediction[n, 2])
#print('delta: ', u) #print('delta: ', u)
#print('e_y: ', spatial_state_prediction[n, 0]) #print('e_y: ', spatial_state_prediction[n, 0])
#print('e_psi: ', spatial_state_prediction[n, 1]) #print('e_psi: ', spatial_state_prediction[n, 1])

2
simulation.py
View File

@ -77,7 +77,7 @@ if __name__ == '__main__':
N = 30 N = 30
Q = sparse.diags([1.0, 0.0, 0.0]) Q = sparse.diags([1.0, 0.0, 0.0])
R = sparse.diags([0.01]) R = sparse.diags([0.01])
QN = Q QN = sparse.diags([0.0, 0.0, 1.0])
InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]), InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]),
'umax': np.array([np.tan(0.66)/car.l])} 'umax': np.array([np.tan(0.66)/car.l])}
StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]), StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]),

6
spatial_bicycle_models.py
View File

@ -434,13 +434,15 @@ class BicycleModel(SpatialBicycleModel):
# Construct Jacobian Matrix # Construct Jacobian Matrix
a_1 = np.array([1, delta_s, 0]) a_1 = np.array([1, delta_s, 0])
a_2 = np.array([-kappa_r**2*delta_s, 1, 0]) a_2 = np.array([-kappa_r**2*delta_s, 1, 0])
a_3 = np.array([-kappa_r/v*delta_s, 0, 0]) a_3 = np.array([-kappa_r/v*delta_s, 0, 1])
b_1 = np.array([0, ]) b_1 = np.array([0, ])
b_2 = np.array([delta_s, ]) b_2 = np.array([delta_s, ])
b_3 = np.array([0, ]) b_3 = np.array([0, ])
f = np.array([0.0, 0.0, 1/v*delta_s])
A = np.stack((a_1, a_2, a_3), axis=0) A = np.stack((a_1, a_2, a_3), axis=0)
B = np.stack((b_1, b_2, b_3), axis=0) B = np.stack((b_1, b_2, b_3), axis=0)
return A, B return f, A, B