From 4e9959a1b11277c3536c8904fc0cac4e6455d110 Mon Sep 17 00:00:00 2001 From: matssteinweg Date: Sat, 7 Dec 2019 00:29:55 +0100 Subject: [PATCH] Add speed profile to reference path. Include speed tracking in MPC --- MPC.py | 18 +++---- map.py | 1 + reference_path.py | 128 ++++++++++++++++++++++++++++++++++++---------- simulation.py | 32 ++++++------ 4 files changed, 126 insertions(+), 53 deletions(-) diff --git a/MPC.py b/MPC.py index 61d548f..2f28426 100644 --- a/MPC.py +++ b/MPC.py @@ -13,8 +13,7 @@ PREDICTION = '#BA4A00' class MPC: - def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints, - velocity_reference): + def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints): """ Constructor for the Model Predictive Controller. :param model: bicycle model object to be controlled @@ -24,7 +23,6 @@ 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 @@ -44,9 +42,6 @@ class MPC: self.state_constraints = StateConstraints self.input_constraints = InputConstraints - # Velocity reference - self.v_ref = velocity_reference - # Current control and prediction self.current_prediction = None @@ -54,7 +49,7 @@ class MPC: self.infeasibility_counter = 0 # Current control signals - self.current_control = np.ones((self.nu*self.N)) * velocity_reference + self.current_control = np.ones((self.nu*self.N)) # Initialize Optimization Problem self.optimizer = osqp.OSQP() @@ -90,19 +85,20 @@ class MPC: next_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n + 1) delta_s = next_waypoint - current_waypoint kappa_ref = current_waypoint.kappa + v_ref = current_waypoint.v_ref # Compute LTV matrices - f, A_lin, B_lin = self.model.linearize(self.v_ref, kappa_ref, delta_s) + f, A_lin, B_lin = self.model.linearize(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 reference for input signal - ur[n*self.nu:(n+1)*self.nu] = np.array([self.v_ref, kappa_ref]) + ur[n*self.nu:(n+1)*self.nu] = np.array([v_ref, kappa_ref]) # Compute equality constraint offset (B*ur) uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array - ([self.v_ref, kappa_ref])) - f + ([v_ref, kappa_ref])) - f # Compute dynamic constraints on e_y - lb, ub = self.model.reference_path.update_bounds( + lb, ub, cells = self.model.reference_path.update_bounds( self.model.wp_id + n, self.model.safety_margin[1]) xmin_dyn[self.nx * n] = lb xmax_dyn[self.nx * n] = ub diff --git a/map.py b/map.py index bbb5c24..55bae67 100644 --- a/map.py +++ b/map.py @@ -56,6 +56,7 @@ class Map: + if __name__ == '__main__': map = Map('map_floor2.png') plt.imshow(map.data, cmap='gray') diff --git a/reference_path.py b/reference_path.py index 475bada..3ea4208 100644 --- a/reference_path.py +++ b/reference_path.py @@ -4,7 +4,8 @@ from map import Map from skimage.draw import line import matplotlib.pyplot as plt import matplotlib.patches as plt_patches -from scipy.signal import savgol_filter +from scipy import sparse +import osqp # Colors DRIVABLE_AREA = '#BDC3C7' @@ -31,7 +32,10 @@ class Waypoint: self.psi = psi self.kappa = kappa - # information about drivable area at waypoint + # Reference velocity at this waypoint according to speed profile + self.v_ref = None + + # Information about drivable area at waypoint # upper and lower bound of drivable area orthogonal to # waypoint orientation self.lb = None @@ -112,7 +116,7 @@ class ReferencePath: # Look ahead distance for path averaging self.smoothing_distance = smoothing_distance - # Circular + # Circular flag self.circular = circular # List of waypoint objects @@ -308,6 +312,88 @@ class ReferencePath: return min_width, min_cell + def compute_speed_profile(self, Constraints): + """ + Compute a speed profile for the path. Assign a reference velocity + to each waypoint based on its curvature. + """ + + # Set optimization horizon + N = self.n_waypoints - 1 + + # Constraints + a_min = np.ones(N-1) * Constraints['a_min'] + a_max = np.ones(N-1) * Constraints['a_max'] + v_min = np.ones(N) * Constraints['v_min'] + v_max = np.ones(N) * Constraints['v_max'] + + # Maximum lateral acceleration + ay_max = Constraints['ay_max'] + + # Inequality Matrix + D1 = np.zeros((N-1, N)) + + # Iterate over horizon + for i in range(N): + + # Get information about current waypoint + current_waypoint = self.get_waypoint(i) + next_waypoint = self.get_waypoint(i+1) + # distance between waypoints + li = next_waypoint - current_waypoint + # curvature of waypoint + ki = current_waypoint.kappa + + # Fill operator matrix + # dynamics of acceleration + if i < N-1: + D1[i, i:i+2] = np.array([-1/(2*li), 1/(2*li)]) + + # Compute dynamic constraint on velocity + v_max_dyn = np.sqrt(ay_max / (np.abs(ki) + self.eps)) + if v_max_dyn < v_max[i]: + v_max[i] = v_max_dyn + + # Construct inequality matrix + D1 = sparse.csc_matrix(D1) + D2 = sparse.eye(N) + D = sparse.vstack([D1, D2], format='csc') + + # Get upper and lower bound vectors for inequality constraints + l = np.hstack([a_min, v_min]) + u = np.hstack([a_max, v_max]) + + # Set cost matrices + P = sparse.eye(N, format='csc') + q = -1 * v_max + + # Solve optimization problem + problem = osqp.OSQP() + problem.setup(P=P, q=q, A=D, l=l, u=u, verbose=False) + speed_profile = problem.solve().x + + # Assign reference velocity to every waypoint + for i, wp in enumerate(self.waypoints[:-1]): + wp.v_ref = speed_profile[i] + self.waypoints[-1].v_ref = self.waypoints[-2].v_ref + + def get_waypoint(self, wp_id): + """ + Get waypoint corresponding to wp_id. Circular indexing supported. + :param wp_id: unique waypoint ID + :return: waypoint object + """ + + # Allow circular indexing if circular path + if wp_id >= self.n_waypoints and self.circular: + wp_id = np.mod(wp_id, self.n_waypoints) + # Terminate execution if end of path reached + elif wp_id >= self.n_waypoints and not self.circular: + print('Reached end of path!') + exit(1) + + return self.waypoints[wp_id] + def update_bounds(self, wp_id, safety_margin): """ Compute upper and lower bounds of the drivable area orthogonal to @@ -341,10 +427,10 @@ class ReferencePath: lb_o = (x, y) # If cell is occupied, end segment. Update best segment if current # segment is larger than previous best segment. Then, reset upper - # and lower bound to current cell. + # and lower bound to current cell if self.map.data[y, x] == 0 or (x, y) == lb_p: if np.sqrt((ub_o[0]-lb_o[0])**2+(ub_o[1]-lb_o[1])**2) > \ - np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2): + np.sqrt((ub_ls[0]-lb_ls[0])**2+(ub_ls[1]-lb_ls[1])**2): ub_ls = ub_o lb_ls = lb_o # Start new segment @@ -380,11 +466,13 @@ class ReferencePath: # Compute absolute angle of bound cell angle_ub = np.mod(math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi angle_lb = np.mod(-math.pi/2 + wp.psi + math.pi, 2 * math.pi) - math.pi + # Compute cell on bound for computed distance ub and lb 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) + border_cells = (ub_ls, lb_ls) - return lb, ub + return lb, ub, border_cells def add_obstacles(self, obstacles): """ @@ -397,8 +485,12 @@ class ReferencePath: # Iterate over list of obstacles for obstacle in obstacles: + # Compute radius of circular object in pixels radius_px = int(np.ceil(obstacle.radius / self.map.resolution)) + # Get center coordinates of obstacle in map coordinates cx_px, cy_px = self.map.w2m(obstacle.cx, obstacle.cy) + + # Add circular object to map y, x = np.ogrid[-radius_px: radius_px, -radius_px: radius_px] index = x ** 2 + y ** 2 <= radius_px ** 2 self.map.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px: @@ -439,7 +531,7 @@ class ReferencePath: 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) + plt.scatter(wp_x, wp_y, color=WAYPOINTS, s=10) # Plot arrows indicating drivable area if display_drivable_area: @@ -463,11 +555,6 @@ class ReferencePath: br_y = np.array([wp.border_cells[1][1] for wp in self.waypoints] + [self.waypoints[0].border_cells[1][1]]) - # Smooth border - # bl_x = savgol_filter(bl_x, 15, 9) - # bl_y = savgol_filter(bl_y, 15, 9) - # br_x = savgol_filter(br_x, 15, 9) - # br_y = savgol_filter(br_y, 15, 9) # If circular path, connect start and end point if self.circular: @@ -484,22 +571,11 @@ 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 = 'Q' + path = 'Race' # Create Map if path == 'Race': @@ -512,7 +588,7 @@ if __name__ == '__main__': # Specify path resolution path_resolution = 0.05 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, - smoothing_distance=5, max_width=0.22, n_extension=30, + smoothing_distance=5, max_width=0.15, circular=True) # Add obstacles obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) @@ -534,7 +610,7 @@ if __name__ == '__main__': path_resolution = 0.20 # m / wp reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, smoothing_distance=5, max_width=1.5, - n_extension=30, circular=False) + circular=False) obs1 = Obstacle(cx=-6.3, cy=-11.1, radius=0.20) obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) diff --git a/simulation.py b/simulation.py index 4d36677..33ef0a2 100644 --- a/simulation.py +++ b/simulation.py @@ -5,8 +5,6 @@ from spatial_bicycle_models import BicycleModel import matplotlib.pyplot as plt from MPC import MPC from scipy import sparse -from time import time -from lidar_model import LidarModel if __name__ == '__main__': @@ -65,6 +63,7 @@ if __name__ == '__main__': e_y_0 = 0.0 e_psi_0 = 0.0 t_0 = 0.0 + V_MAX = 2.5 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) @@ -78,18 +77,15 @@ if __name__ == '__main__': 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])} + 'umax': np.array([V_MAX, 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])} - velocity_reference = 1.5 # m/s - mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints, - velocity_reference) + mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) - ######### - # LiDAR # - ######### - - sensor = LidarModel(FoV=90, range=0.25, resolution=4.0) + # Compute speed profile + SpeedProfileConstraints = {'a_min': -2.0, 'a_max': 2.0, + 'v_min': 0, 'v_max': V_MAX, 'ay_max': 5.0} + car.reference_path.compute_speed_profile(SpeedProfileConstraints) ############## # Simulation # @@ -103,6 +99,7 @@ if __name__ == '__main__': # Logging containers x_log = [car.temporal_state.x] y_log = [car.temporal_state.y] + v_log = [0.0] # Until arrival at end of path while car.s < reference_path.length: @@ -116,6 +113,7 @@ if __name__ == '__main__': # log x_log.append(car.temporal_state.x) y_log.append(car.temporal_state.y) + v_log.append(u[0]) ################### # Plot Simulation # @@ -123,6 +121,8 @@ if __name__ == '__main__': # Plot path and drivable area reference_path.show() + plt.scatter(x_log, y_log, c=v_log, s=10) + plt.colorbar() # Plot MPC prediction mpc.show_prediction() @@ -130,12 +130,12 @@ if __name__ == '__main__': # Plot car car.show() + # Increase simulation time t += Ts + # Set figure title plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' - '{:.2f} s'. - format(u[0], u[1], t)) + '{:.2f} s'.format(u[0], u[1], t)) - plt.pause(0.01) - print('Final Time: {:.2f}'.format(t)) - plt.close() + plt.pause(0.0001) + plt.show()