diff --git a/MPC.py b/MPC.py index 129cb63..1eafe2b 100644 --- a/MPC.py +++ b/MPC.py @@ -105,16 +105,18 @@ class MPC: # Compute equality constraint offset (B*ur) uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array ([v_ref, kappa_ref])) - f - # Compute dynamic constraints on e_y - 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 + # 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 + # Compute dynamic constraints on e_y + ub, lb, cells = self.model.reference_path.update_path_constraints( + self.model.wp_id, self.N+1, 2*self.model.safety_margin[1], 0.05) + xmin_dyn[::self.nx] = lb + xmax_dyn[::self.nx] = ub + # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), -sparse.eye(self.nx)) + sparse.csc_matrix(A) diff --git a/map.py b/map.py index 55bae67..bbb5c24 100644 --- a/map.py +++ b/map.py @@ -56,7 +56,6 @@ 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 3ea4208..03abfa4 100644 --- a/reference_path.py +++ b/reference_path.py @@ -336,6 +336,8 @@ class ReferencePath: # Iterate over horizon for i in range(N): + look_ahead = 30 + # Get information about current waypoint current_waypoint = self.get_waypoint(i) next_waypoint = self.get_waypoint(i+1) @@ -343,6 +345,9 @@ class ReferencePath: li = next_waypoint - current_waypoint # curvature of waypoint ki = current_waypoint.kappa + if np.abs(ki) <= 0.1: + kis = [wp.kappa for wp in self.waypoints[i:i+look_ahead]] + ki = np.mean(kis) # Fill operator matrix # dynamics of acceleration @@ -511,8 +516,8 @@ class ReferencePath: plt.yticks([]) # Plot map in gray-scale and set extent to match world coordinates - canvas = np.ones(self.map.data.shape) - # canvas = np.flipud(self.map.data) + #canvas = np.ones(self.map.data.shape) + canvas = np.flipud(self.map.data) plt.imshow(canvas, cmap='gray', extent=[self.map.origin[0], self.map.origin[0] + self.map.width * self.map.resolution, @@ -571,11 +576,184 @@ class ReferencePath: for obstacle in self.obstacles: obstacle.show() + def _compute_free_segments(self, wp, min_width): + """ + Compute free path segments. + :param wp: waypoint object + :param min_width: minimum width of valid segment + :return: segment candidates as list of tuples (ub_cell, lb_cell) + """ + + # Candidate segments + free_segments = [] + + # Get waypoint's border cells in map coordinates + ub_p = self.map.w2m(wp.border_cells[0][0], + wp.border_cells[0][1]) + lb_p = self.map.w2m(wp.border_cells[1][0], + wp.border_cells[1][1]) + + # Compute path from left border cell to right border cell + x_list, y_list = line(ub_p[0], ub_p[1], lb_p[0], lb_p[1]) + + # Initialize upper and lower bound of drivable area to + # upper bound of path + ub_o, lb_o = ub_p, ub_p + + # Assume occupied path + free_cells = False + + # Iterate over path from left border to right border + for x, y in zip(x_list[1:], y_list[1:]): + # If cell is free, update lower bound + if self.map.data[y, x] == 1: + # Free cell detected + free_cells = True + lb_o = (x, y) + # If cell is occupied or end of path, end segment. Add segment + # to list of candidates. Then, reset upper and lower bound to + # current cell. + if (self.map.data[y, x] == 0 or (x, y) == lb_p) and free_cells: + # Set lower bound to border cell of segment + lb_o = (x, y) + # Transform upper and lower bound cells to world coordinates + ub_o = self.map.m2w(ub_o[0], ub_o[1]) + lb_o = self.map.m2w(lb_o[0], lb_o[1]) + # If segment larger than threshold, add to candidates + if np.sqrt((ub_o[0]-lb_o[0])**2 + (ub_o[1]-lb_o[1])**2) > \ + min_width: + free_segments.append((ub_o, lb_o)) + # Start new segment + ub_o = (x, y) + free_cells = False + elif self.map.data[y, x] == 0 and not free_cells: + ub_o = (x, y) + lb_o = (x, y) + + return free_segments + + def update_path_constraints(self, wp_id, N, min_width, safety_margin): + """ + Compute upper and lower bounds of the drivable area orthogonal to + the given waypoint. + """ + + # container for constraints and border cells + ub_hor = [] + lb_hor = [] + border_cells_hor = [] + + # Iterate over horizon + for n in range(N): + + # get corresponding waypoint + wp = self.waypoints[wp_id+n] + + # Get list of free segments + free_segments = self._compute_free_segments(wp, min_width) + + # First waypoint in horizon uses largest segment + if n == 0: + segment_lengths = [np.sqrt((seg[0][0]-seg[1][0])**2 + + (seg[0][1]-seg[1][1])**2) for seg in free_segments] + ls_id = segment_lengths.index(max(segment_lengths)) + ub_ls, lb_ls = free_segments[ls_id] + + else: + + # Get border cells of selected segment at previous waypoint + ub_pw, lb_pw = border_cells_hor[n-1] + ub_pw, lb_pw = list(ub_pw), list(lb_pw) + + # Project border cells onto new waypoint in path direction + wp_prev = self.waypoints[wp_id+n-1] + delta_s = wp_prev - wp + ub_pw[0] += delta_s * np.cos(wp_prev.psi) + ub_pw[1] += delta_s * np.cos(wp_prev.psi) + lb_pw[0] += delta_s * np.sin(wp_prev.psi) + lb_pw[1] += delta_s * np.sin(wp_prev.psi) + + # Iterate over free segments for current waypoint + if len(free_segments) >= 2: + + # container for overlap of segments with projection + segment_offsets = [] + + for free_segment in free_segments: + + # Get border cells of segment + ub_fs, lb_fs = free_segment + + # distance between upper bounds and lower bounds + d_ub = np.sqrt((ub_fs[0]-ub_pw[0])**2 + (ub_fs[1]-ub_pw[1])**2) + d_lb = np.sqrt((lb_fs[0]-lb_pw[0])**2 + (lb_fs[1]-lb_pw[1])**2) + mean_dist = (d_ub + d_lb) / 2 + + # Append offset to projected previous segment + segment_offsets.append(mean_dist) + + # Select segment with minimum offset to projected previous + # segment + ls_id = segment_offsets.index(min(segment_offsets)) + ub_ls, lb_ls = free_segments[ls_id] + + # Select free segment in case of only one candidate + elif len(free_segments) == 1: + ub_ls, lb_ls = free_segments[0] + + # Set waypoint coordinates as bound cells if no feasible + # segment available + else: + ub_ls, lb_ls = (wp.x, wp.y), (wp.x, wp.y) + + # Check sign of upper and lower bound + angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x) + - wp.psi + math.pi, 2 * math.pi) - math.pi + angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x) + - wp.psi + math.pi, 2 * math.pi) - math.pi + sign_ub = np.sign(angle_ub) + sign_lb = np.sign(angle_lb) + + # Compute upper and lower bound of largest drivable area + ub = sign_ub * np.sqrt( + (ub_ls[0] - wp.x) ** 2 + (ub_ls[1] - wp.y) ** 2) + lb = sign_lb * np.sqrt( + (lb_ls[0] - wp.x) ** 2 + (lb_ls[1] - wp.y) ** 2) + + # Subtract safety margin + ub -= safety_margin + lb += safety_margin + + # Check feasibility of the path + if ub < lb: + # Upper and lower bound of 0 indicate an infeasible path + # given the specified safety margin + ub, lb = 0.0, 0.0 + + # 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) + bound_cells = (ub_ls, lb_ls) + + # Append results + ub_hor.append(ub) + lb_hor.append(lb) + border_cells_hor.append(list(bound_cells)) + + return np.array(ub_hor), np.array(lb_hor), border_cells_hor + if __name__ == '__main__': # Select Path | 'Race' or 'Q' - path = 'Race' + path = 'Q' # Create Map if path == 'Race': @@ -615,16 +793,27 @@ if __name__ == '__main__': obs2 = Obstacle(cx=-2.2, cy=-6.8, radius=0.25) obs3 = Obstacle(cx=1.7, cy=-1.0, radius=0.15) obs4 = Obstacle(cx=2.0, cy=-1.2, radius=0.25) - reference_path.add_obstacles([obs1, obs2, obs3, obs4]) + obs5 = Obstacle(cx=2.2, cy=-0.86, radius=0.1) + obs6 = Obstacle(cx=2.33, cy=-0.7, radius=0.1) + obs7 = Obstacle(cx=2.67, cy=-0.73, radius=0.1) + obs8 = Obstacle(cx=6.42, cy=3.97, radius=0.3) + obs9 = Obstacle(cx=7.42, cy=4.97, radius=0.3) + obs10 = Obstacle(cx=7.14, cy=5.7, radius=0.1) + reference_path.add_obstacles([obs1, obs2, obs3, obs4, obs5, obs6, obs7, obs8, obs9, obs10]) else: reference_path = None print('Invalid path!') exit(1) - [reference_path.update_bounds(wp_id=wp_id, safety_margin=0.02) - for wp_id in range(len(reference_path.waypoints))] + ub, lb, border_cells = reference_path.update_path_constraints(0, reference_path.n_waypoints, 0.60, 0.1) + # Get x and y locations of border cells for upper and lower bound + for wp_id in range(reference_path.n_waypoints): + if ub[wp_id] > 0.0 and lb[wp_id] > 0.0: + print(wp_id) + reference_path.waypoints[wp_id].border_cells = border_cells[wp_id] reference_path.show() + plt.show() diff --git a/simulation.py b/simulation.py index 33ef0a2..c7062ee 100644 --- a/simulation.py +++ b/simulation.py @@ -10,7 +10,7 @@ from scipy import sparse if __name__ == '__main__': # Select Simulation Mode | 'Race' or 'Q' - sim_mode = 'Race' + sim_mode = 'Q' # Create Map if sim_mode == 'Race': @@ -83,8 +83,8 @@ if __name__ == '__main__': mpc = MPC(car, N, Q, R, QN, StateConstraints, InputConstraints) # Compute speed profile - SpeedProfileConstraints = {'a_min': -2.0, 'a_max': 2.0, - 'v_min': 0, 'v_max': V_MAX, 'ay_max': 5.0} + SpeedProfileConstraints = {'a_min': -1.0, 'a_max': 1.0, + 'v_min': 0, 'v_max': V_MAX, 'ay_max': 1.0} car.reference_path.compute_speed_profile(SpeedProfileConstraints) ############## @@ -92,7 +92,7 @@ if __name__ == '__main__': ############## # Sampling time - Ts = 0.05 + Ts = 0.20 t = 0 car.set_sampling_time(Ts) @@ -137,5 +137,6 @@ if __name__ == '__main__': plt.title('MPC Simulation: v(t): {:.2f}, delta(t): {:.2f}, Duration: ' '{:.2f} s'.format(u[0], u[1], t)) - plt.pause(0.0001) + plt.pause(0.00001) + print(min(v_log)) plt.show()