From 7e7ff06029c4931b2eff1f8fc6f1d135b01d68b9 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Fri, 6 Dec 2019 21:32:36 +0100 Subject: [PATCH] velocity as mpc input --- MPC.py | 84 +++++++++++++++++++++------------------ reference_path.py | 60 +++++++++++++--------------- simulation.py | 37 +++++++---------- spatial_bicycle_models.py | 6 +-- 4 files changed, 91 insertions(+), 96 deletions(-) diff --git a/MPC.py b/MPC.py index 0a54163..61d548f 100644 --- a/MPC.py +++ b/MPC.py @@ -13,7 +13,8 @@ PREDICTION = '#BA4A00' class MPC: - def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): + def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints, + velocity_reference): """ Constructor for the Model Predictive Controller. :param model: bicycle model object to be controlled @@ -23,6 +24,7 @@ class MPC: :param QN: final state cost matrix :param StateConstraints: dictionary of state constraints :param InputConstraints: dictionary of input constraints + :param velocity_reference: reference value for velocity """ # Parameters @@ -34,10 +36,17 @@ class MPC: # Model self.model = model + # Dimensions + self.nx = self.model.n_states + self.nu = 2 + # Constraints self.state_constraints = StateConstraints self.input_constraints = InputConstraints + # Velocity reference + self.v_ref = velocity_reference + # Current control and prediction self.current_prediction = None @@ -45,20 +54,16 @@ class MPC: self.infeasibility_counter = 0 # Current control signals - self.current_control = None + self.current_control = np.ones((self.nu*self.N)) * velocity_reference # Initialize Optimization Problem self.optimizer = osqp.OSQP() - def _init_problem(self, v): + def _init_problem(self): """ Initialize optimization problem for current time step. """ - # Number of state and input variables - nx = self.model.n_states - nu = 1 - # Constraints umin = self.input_constraints['umin'] umax = self.input_constraints['umax'] @@ -66,14 +71,13 @@ class MPC: xmax = self.state_constraints['xmax'] # LTV System Matrices - A = np.zeros((nx * (self.N + 1), nx * (self.N + 1))) - B = np.zeros((nx * (self.N + 1), nu * (self.N))) + A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1))) + B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N))) # Reference vector for state and input variables - ur = np.zeros(self.N) - xr = np.array([0.0, 0.0, -1.0]) + ur = np.zeros(self.nu*self.N) + xr = np.array([0.0, 0.0, 0.0]) # Offset for equality constraint (due to B * (u - ur)) - uq = np.zeros(self.N * nx) - + uq = np.zeros(self.N * self.nx) # Dynamic state constraints xmin_dyn = np.kron(np.ones(self.N + 1), xmin) xmax_dyn = np.kron(np.ones(self.N + 1), xmax) @@ -82,34 +86,34 @@ class MPC: for n in range(self.N): # Get information about current waypoint - current_waypoint = self.model.reference_path.waypoints[ - self.model.wp_id + n] - next_waypoint = self.model.reference_path.waypoints[ - self.model.wp_id + n + 1] + current_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n) + next_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n + 1) delta_s = next_waypoint - current_waypoint - kappa_r = current_waypoint.kappa + kappa_ref = current_waypoint.kappa # Compute LTV matrices - 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 - B[nx + n * nx:n * nx + 2 * nx, n * nu:n * nu + nu] = B_lin + f, A_lin, B_lin = self.model.linearize(self.v_ref, kappa_ref, delta_s) + A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_lin + B[(n+1) * self.nx: (n+2)*self.nx, n * self.nu:(n+1)*self.nu] = B_lin - # Set kappa_r to reference for input signal - ur[n] = kappa_r + # Set reference for input signal + ur[n*self.nu:(n+1)*self.nu] = np.array([self.v_ref, kappa_ref]) # Compute equality constraint offset (B*ur) - uq[n * nx:n * nx + nx] = B_lin[:, 0] * kappa_r - f + uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array + ([self.v_ref, kappa_ref])) - f + # Compute dynamic constraints on e_y lb, ub = self.model.reference_path.update_bounds( self.model.wp_id + n, self.model.safety_margin[1]) - xmin_dyn[nx * n] = lb - xmax_dyn[nx * n] = ub + xmin_dyn[self.nx * n] = lb + xmax_dyn[self.nx * n] = ub # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), - -sparse.eye(nx)) + sparse.csc_matrix(A) + -sparse.eye(self.nx)) + sparse.csc_matrix(A) Bu = sparse.csc_matrix(B) Aeq = sparse.hstack([Ax, Bu]) # Get inequality matrix - Aineq = sparse.eye((self.N + 1) * nx + self.N * nu) + Aineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu) # Combine constraint matrices A = sparse.vstack([Aeq, Aineq], format='csc') @@ -131,13 +135,13 @@ class MPC: sparse.kron(sparse.eye(self.N), self.R)], format='csc') q = np.hstack( [np.kron(np.ones(self.N), -self.Q.dot(xr)), -self.QN.dot(xr), - -self.R.A[0, 0] * ur]) + -np.tile(np.array([self.R.A[0, 0], self.R.A[1, 1]]), self.N) * ur]) # Initialize optimizer self.optimizer = osqp.OSQP() self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False) - def get_control(self, v): + def get_control(self): """ Get control signal given the current position of the car. Solves a finite time optimization problem based on the linearized car model. @@ -145,6 +149,7 @@ class MPC: # Number of state variables nx = self.model.n_states + nu = 2 # Update current waypoint self.model.get_current_waypoint() @@ -153,21 +158,24 @@ class MPC: self.model.spatial_state = self.model.t2s() # Initialize optimization problem - self._init_problem(v) + self._init_problem() # Solve optimization problem dec = self.optimizer.solve() try: # Get control signals - deltas = np.arctan(dec.x[-self.N:] * self.model.l) - delta = deltas[0] + control_signals = np.array(dec.x[-self.N*nu:]) + control_signals[1::2] = np.arctan(control_signals[1::2] * self.model.l) + v = control_signals[0] + delta = control_signals[1] # Update control signals - self.current_control = deltas + self.current_control = control_signals # Get predicted spatial states x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx)) + # Update predicted temporal states self.current_prediction = self.update_prediction(delta, x) @@ -181,8 +189,8 @@ class MPC: print('Infeasible problem. Previously predicted' ' control signal used!') - u = np.array([v, self.current_control - [self.infeasibility_counter+1]]) + id = nu * (self.infeasibility_counter + 1) + u = np.array(self.current_control[id:id+2]) # increase infeasibility counter self.infeasibility_counter += 1 @@ -208,10 +216,10 @@ class MPC: #print('#########################') 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.get_waypoint(self.model.wp_id+n) predicted_temporal_state = self.model.s2t(associated_waypoint, spatial_state_prediction[n, :]) - print(spatial_state_prediction[n, 2]) + #print(spatial_state_prediction[n, 2]) #print('delta: ', u) #print('e_y: ', spatial_state_prediction[n, 0]) #print('e_psi: ', spatial_state_prediction[n, 1]) diff --git a/reference_path.py b/reference_path.py index 357df29..475bada 100644 --- a/reference_path.py +++ b/reference_path.py @@ -84,7 +84,7 @@ class Obstacle: class ReferencePath: def __init__(self, map, wp_x, wp_y, resolution, smoothing_distance, - max_width, n_extension, circular): + max_width, circular): """ Reference Path object. Create a reference trajectory from specified corner points with given resolution. Smoothing around corners can be @@ -97,8 +97,6 @@ class ReferencePath: :param smoothing_distance: number of waypoints used for smoothing the path by averaging neighborhood of waypoints :param max_width: maximum width of path to both sides in m - :param n_extension: number of samples used for path extension to allow - for MPC :param circular: True if path circular """ @@ -114,15 +112,15 @@ class ReferencePath: # Look ahead distance for path averaging self.smoothing_distance = smoothing_distance - # Number of waypoints used to extend path at the end - self.n_extension = n_extension - # Circular self.circular = circular # List of waypoint objects self.waypoints = self._construct_path(wp_x, wp_y) + # Number of waypoints + self.n_waypoints = len(self.waypoints) + # Length of path self.length, self.segment_lengths = self._compute_length() @@ -154,14 +152,6 @@ class ReferencePath: tolist() for i in range(len(wp_y) - 1)] wp_y = [wp for segment in wp_y for wp in segment] + [gp_y] - if self.n_extension is not None: - if self.circular: - wp_x += wp_x[:self.n_extension] - wp_y += wp_y[:self.n_extension] - else: - wp_x += wp_x[-self.n_extension:] - wp_y += wp_y[-self.n_extension:] - # Smooth path wp_xs = [] wp_ys = [] @@ -232,7 +222,7 @@ class ReferencePath: :return: length of center-line path in m """ segment_lengths = [0.0] + [self.waypoints[wp_id+1] - self.waypoints - [wp_id] for wp_id in range(len(self.waypoints)-self.n_extension-1)] + [wp_id] for wp_id in range(len(self.waypoints)-1)] s = sum(segment_lengths) return s, segment_lengths @@ -327,7 +317,7 @@ class ReferencePath: """ # Get reference waypoint - wp = self.waypoints[wp_id] + wp = self.get_waypoint(wp_id) # Get waypoint's border cells in map coordinates ub_p = self.map.w2m(wp.border_cells[0][0], wp.border_cells[0][1]) @@ -394,11 +384,6 @@ class ReferencePath: ub_ls = wp.x + ub * np.cos(angle_ub), wp.y + ub * np.sin(angle_ub) lb_ls = wp.x - lb * np.cos(angle_lb), wp.y - lb * np.sin(angle_lb) - # Update member variables of waypoint - #wp.ub = ub - #wp.lb = lb - #wp.border_cells = (ub_ls, lb_ls) - return lb, ub def add_obstacles(self, obstacles): @@ -444,14 +429,14 @@ class ReferencePath: vmax=1.0) # Get x and y coordinates for all waypoints - wp_x = np.array([wp.x for wp in self.waypoints][:-self.n_extension]) - wp_y = np.array([wp.y for wp in self.waypoints][:-self.n_extension]) + wp_x = np.array([wp.x for wp in self.waypoints]) + wp_y = np.array([wp.y for wp in self.waypoints]) # Get x and y locations of border cells for upper and lower bound - wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints][:-self.n_extension]) - wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints][:-self.n_extension]) - wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints][:-self.n_extension]) - wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints][:-self.n_extension]) + wp_ub_x = np.array([wp.border_cells[0][0] for wp in self.waypoints]) + wp_ub_y = np.array([wp.border_cells[0][1] for wp in self.waypoints]) + wp_lb_x = np.array([wp.border_cells[1][0] for wp in self.waypoints]) + wp_lb_y = np.array([wp.border_cells[1][1] for wp in self.waypoints]) # Plot waypoints plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=3) @@ -467,16 +452,16 @@ class ReferencePath: # Plot border of path bl_x = np.array([wp.border_cells[0][0] for wp in - self.waypoints][:-self.n_extension] + + self.waypoints] + [self.waypoints[0].border_cells[0][0]]) bl_y = np.array([wp.border_cells[0][1] for wp in - self.waypoints][:-self.n_extension] + + self.waypoints] + [self.waypoints[0].border_cells[0][1]]) br_x = np.array([wp.border_cells[1][0] for wp in - self.waypoints][:-self.n_extension] + + self.waypoints] + [self.waypoints[0].border_cells[1][0]]) br_y = np.array([wp.border_cells[1][1] for wp in - self.waypoints][:-self.n_extension] + + self.waypoints] + [self.waypoints[0].border_cells[1][1]]) # Smooth border # bl_x = savgol_filter(bl_x, 15, 9) @@ -499,11 +484,22 @@ class ReferencePath: for obstacle in self.obstacles: obstacle.show() + def get_waypoint(self, wp_id): + if wp_id >= self.n_waypoints and self.circular: + wp_id = np.mod(wp_id, self.n_waypoints) + elif wp_id >= self.n_waypoints and not self.circular: + print('Reached end of path!') + exit(1) + + return self.waypoints[wp_id] + + + if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Race' + path = 'Q' # Create Map if path == 'Race': diff --git a/simulation.py b/simulation.py index 510b318..4d36677 100644 --- a/simulation.py +++ b/simulation.py @@ -27,7 +27,7 @@ if __name__ == '__main__': # Create smoothed reference path reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=0.23, - n_extension=50, circular=True) + circular=True) elif sim_mode == 'Q': map = Map(file_path='map_floor2.png') wp_x = [-9.169, 11.9, 7.3, -6.95] @@ -37,7 +37,7 @@ if __name__ == '__main__': # Create smoothed reference path reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=1.50, - n_extension=50, circular=False) + circular=False) else: print('Invalid Simulation Mode!') map, wp_x, wp_y, path_resolution, reference_path \ @@ -65,7 +65,6 @@ if __name__ == '__main__': e_y_0 = 0.0 e_psi_0 = 0.0 t_0 = 0.0 - v = 1.0 car = BicycleModel(length=0.12, width=0.06, reference_path=reference_path, e_y=e_y_0, e_psi=e_psi_0, t=t_0) @@ -76,13 +75,15 @@ if __name__ == '__main__': N = 30 Q = sparse.diags([1.0, 0.0, 0.0]) - R = sparse.diags([0.01]) - QN = sparse.diags([0.0, 0.0, 1.0]) - InputConstraints = {'umin': np.array([-np.tan(0.66)/car.l]), - 'umax': np.array([np.tan(0.66)/car.l])} + R = sparse.diags([1.0, 0.0]) + QN = sparse.diags([0.0, 0.0, 0.0]) + InputConstraints = {'umin': np.array([0.0, -np.tan(0.66)/car.l]), + 'umax': np.array([2.5, np.tan(0.66)/car.l])} StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]), 'xmax': np.array([np.inf, np.inf, np.inf])} - mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) + velocity_reference = 1.5 # m/s + mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints, + velocity_reference) ######### # LiDAR # @@ -107,17 +108,11 @@ if __name__ == '__main__': while car.s < reference_path.length: # get control signals - start = time() - u = mpc.get_control(v) - end = time() - print('Control time: ', end-start) + u = mpc.get_control() # drive car car.drive(u) - # scan - scan = sensor.scan(car.temporal_state, map) - # log x_log.append(car.temporal_state.x) y_log.append(car.temporal_state.y) @@ -129,9 +124,6 @@ if __name__ == '__main__': # Plot path and drivable area reference_path.show() - # Plot scan - sensor.plot_scan(car.temporal_state) - # Plot MPC prediction mpc.show_prediction() @@ -140,11 +132,10 @@ if __name__ == '__main__': t += Ts - plt.title('MPC Simulation: Distance: {:.2f}m/{:.2f} m, Duration: ' + plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' '{:.2f} s'. - format(car.s, car.reference_path.length, t)) - if t == Ts: - plt.show() - plt.pause(0.0001) + format(u[0], u[1], t)) + + plt.pause(0.01) print('Final Time: {:.2f}'.format(t)) plt.close() diff --git a/spatial_bicycle_models.py b/spatial_bicycle_models.py index f133716..85be2cb 100644 --- a/spatial_bicycle_models.py +++ b/spatial_bicycle_models.py @@ -436,9 +436,9 @@ class BicycleModel(SpatialBicycleModel): a_2 = np.array([-kappa_r**2*delta_s, 1, 0]) a_3 = np.array([-kappa_r/v*delta_s, 0, 1]) - b_1 = np.array([0, ]) - b_2 = np.array([delta_s, ]) - b_3 = np.array([0, ]) + b_1 = np.array([0, 0]) + b_2 = np.array([0, delta_s]) + b_3 = np.array([-1/(v**2)*delta_s, 0]) f = np.array([0.0, 0.0, 1/v*delta_s])