def __init__(self, start, goal, start_heading, goal_heading, start_steering, mapsize, freeGrid_num, tolerance, car_length, car_width, wheelbase, obstacle_type): self.start = start self.goal = goal self.start_heading = start_heading self.goal_heading = goal_heading self.start_steering = start_steering self.freeGrid_num = freeGrid_num self.mapsize = mapsize self.tolerance = tolerance self.car_length = car_length self.car_width = car_width self.wheelbase = wheelbase self.obstacle_type = obstacle_type # the bezier spline # using the bezier as the referenece line bz = Bezier(start, goal, self.start_heading, self.goal_heading, self.start_steering, self.mapsize, self.car_length, self.car_width, self.wheelbase, self.mapsize * 3, "NoFound") self.bezier_spline = bz.calculation() #print("self.bezier_spline",self.bezier_spline) # the map and obstacle self.freeGrid_num, self.obstacle, self.danger_zone = mapGenerator( start, self.obstacle_type, self.mapsize)
def __init__(self, start, goal, start_heading, goal_heading, start_steering, mapsize, freeGrid_num, tolerance, car_length, car_width, wheelbase, OBS_type): self.start = start self.goal = goal self.start_heading = start_heading self.goal_heading = goal_heading self.start_steering = start_steering self.freeGrid_num = freeGrid_num self.mapsize = mapsize self.tolerance = tolerance self.car_length = car_length self.car_width = car_width self.wheelbase = wheelbase self.OBS_type = OBS_type bz = Bezier(start, goal, self.start_heading, self.goal_heading, self.start_steering, self.mapsize, self.car_length, self.car_width, self.wheelbase, self.mapsize * 3, "NoFound") self.bezier_spline = bz.calculation() self.freeGrid_num, self.obstacle, self.danger_zone = mapGenerator( start, self.OBS_type, self.mapsize)
def calculation(self): start = self.start goal = self.goal # the vehicle state in 1.5 second theta, theta_forward, displacement_rear, displacement_rear_forward, steering_step = vehicle_model( self.wheelbase, search_length=2.5, speed=5, dt=1.5) # the raw initialization x = start[0] y = start[1] x_forward = x y_forward = y x_prev = x y_prev = y heading_state = self.start_heading rotate_angle = heading_state # the raw initilization steering_state = self.start_steering found = 0 cost_list = [[0, [x, y], heading_state, steering_state]] #print("cost_list",cost_list) next_state = cost_list # the raw initilization path_discrete = list([]) # Initialize discrete path path = [] # Initialize continuous path tree_leaf = [[x, y]] # Initialize search tree leaf (search failed) path_tree = [] # Initialize continuous path for search trees search = 1 # the A_star search iterators step = 1 # the time step for MPC while found != 1: if search >= self.freeGrid_num: # the map has been searched all break cost_list.sort( key=lambda x: x[0] ) # sort by the x[0] element #([total_cost, candidates[i], heading_state[i], steering_step[i]]) # [0.5702793293265661, array([ 2.49501973, 24.86343515]), 0.10936082940740502, 0.08726646259971649] next_state = cost_list.pop( 0) # pop the first element/ the min cost path_discrete.append(np.round( next_state[1])) # round [x,y] of the candidate path.append(next_state[1]) # [x,y] of the candidate [x, y] = next_state[1] [x_forward, y_forward] = [x, y] # the raw initialization heading_state = next_state[2] steering_state = next_state[3] ############################################################################################################################# # step is the flag used for MPC control, the online rolling optimization if step > 1: [x_prev, y_prev] = path[step - 1] # not the very first step step += 1 rotate_angle = heading_state # the raw initilization # reach the goal position if sqrt( np.dot(np.subtract([x, y], goal), np.subtract( [x, y], goal))) <= self.tolerance: found = 1 rotate_matrix = [[np.cos(rotate_angle), -np.sin(rotate_angle)], [np.sin(rotate_angle), np.cos(rotate_angle)]] action = (np.dot( displacement_rear, rotate_matrix)).tolist() #dot()返回的是两个数组的点积(dot product) #print(" action ##############################################") #print( action ) action_forward = np.dot(displacement_rear_forward, rotate_matrix) candidates = np.add([x, y], action) #print(" candidates ###########################################") #print( candidates ) candidates_forward = np.add([x_forward, y_forward], action_forward) candidates_round = np.round(candidates).astype(int) heading_state = np.add(heading_state, theta) #print(" len(candidates_round) ###########################################") #print( len(candidates_round) ) invalid_ID = [ ((candidates_round[i] == path).all(1).any() | (candidates_round[i] == self.danger_zone).all(1).any() | (candidates_round[i] == tree_leaf).all(1).any()) for i in range(len(candidates_round)) ] remove_ID = np.unique( np.where((candidates < 0) | (candidates > self.mapsize))[0]) candidates = np.delete(candidates, remove_ID, axis=0) candidates_forward = np.delete(candidates_forward, remove_ID, axis=0) heading_state = np.delete(heading_state, remove_ID, axis=0) candidates = np.delete(candidates, np.where(invalid_ID), axis=0) candidates_forward = np.delete(candidates_forward, np.where(invalid_ID), axis=0) heading_state = np.delete(heading_state, np.where(invalid_ID), axis=0) ################################################################################################################################ # calculate the cost and add the candidates's cost to the cost_list #Due to the usage of Bezier spline, no expand grid or heuristic layer is pre-computed as in Min_cost, making it very efficient. if len(candidates) > 0: cost_list = [] #print(" len(candidates) ###########################################") #print( len(candidates) ) for i in range(len(candidates)): diff = np.square( candidates[i] - self.bezier_spline) # using the Bezier spline min_dis = min(np.sqrt(np.sum(diff, axis=1))) diff_forward = np.square( candidates_forward[i] - self.bezier_spline) # using the Bezier spline min_dis_forward = min(np.sqrt(np.sum(diff_forward, axis=1))) total_cost = min_dis + min_dis_forward cost_list.append([ total_cost, candidates[i], heading_state[i], steering_step[i] ]) else: # the very first step 1 / the raw initilaztion search += 1 if (next_state[1] == tree_leaf).all(1).any(): tree_leaf.append(np.round([x_prev, y_prev])) else: tree_leaf.append((np.round([x, y])).tolist()) x = start[0] y = start[1] x_forward = x y_forward = y x_prev = x y_prev = y heading_state = self.start_heading rotate_angle = heading_state steering_state = self.start_steering found = 0 cost_list = [[0, [x, y], heading_state, steering_state]] next_state = cost_list path_discrete = list([]) # Initialize discrete path path_tree.append(path) path = [] # Initialize continuous path step = 1 # for the last point bz_last = Bezier(next_state[1], goal, next_state[2], self.goal_heading, self.start_steering, self.mapsize, self.car_length, self.car_width, self.wheelbase, self.tolerance * 3, "Found") bz_last = bz_last.calculation() #print(" bz_last ###########################################") #print( bz_last ) path.append(bz_last) return path, path_tree
def calculation(self): start = self.start goal = self.goal theta, theta_future, displacement_rear, displacement_rear_future, steering_step = \ Dynamics(self.wheelbase, search_length=2.5, speed=5, dt=1) # bz = Bezier(start, goal, self.start_heading, self.goal_heading, self.start_steering, # self.mapsize, self.car_length, self.car_width, self.wheelbase, self.mapsize * 3, "NoFound") # bezier_spline = bz.calculation() x = start[0] y = start[1] x_future = x y_future = y x_prev = x y_prev = y heading_state = self.start_heading rotate_angle = heading_state steering_state = self.start_steering found = 0 cost_list = [[0, [x, y], heading_state, steering_state]] next_state = cost_list path_discrete = list([]) # Initialize discrete path global path global path_tree path = [] # Initialize continuous path tree_leaf = [[x, y]] # Initialize search tree leaf (search failed) search = 1 step = 1 path_tree = [] # Initialize continuous path for search trees while found != 1: if search >= self.freeGrid_num: break cost_list.sort(key=lambda x: x[0]) next_state = cost_list.pop(0) path_discrete.append(np.round(next_state[1])) path.append(next_state[1]) [x, y] = next_state[1] [x_future, y_future] = [x, y] heading_state = next_state[2] steering_state = next_state[3] if step > 1: [x_prev, y_prev] = path[step - 1] step += 1 rotate_angle = heading_state if sqrt( np.dot(np.subtract([x, y], goal), np.subtract( [x, y], goal))) <= self.tolerance: found = 1 rotate_matrix = [[np.cos(rotate_angle), -np.sin(rotate_angle)], [np.sin(rotate_angle), np.cos(rotate_angle)]] action = (np.dot(displacement_rear, rotate_matrix)).tolist() action_future = np.dot(displacement_rear_future, rotate_matrix) candidates = np.add([x, y], action) candidates_future = np.add([x_future, y_future], action_future) candidates_round = np.round(candidates).astype(int) heading_state = np.add(heading_state, theta) invalid_ID = [ ((candidates_round[i] == path).all(1).any() | (candidates_round[i] == self.danger_zone).all(1).any() | (candidates_round[i] == tree_leaf).all(1).any()) for i in range(len(candidates_round)) ] remove_ID = np.unique( np.where((candidates < 0) | (candidates > self.mapsize))[0]) candidates = np.delete(candidates, remove_ID, axis=0) candidates_future = np.delete(candidates_future, remove_ID, axis=0) heading_state = np.delete(heading_state, remove_ID, axis=0) candidates = np.delete(candidates, np.where(invalid_ID), axis=0) candidates_future = np.delete(candidates_future, np.where(invalid_ID), axis=0) heading_state = np.delete(heading_state, np.where(invalid_ID), axis=0) if len(candidates) > 0: cost_list = [] for i in range(len(candidates)): diff = np.square(candidates[i] - self.bezier_spline) min_dis = min(np.sqrt(np.sum(diff, axis=1))) diff_future = np.square(candidates_future[i] - self.bezier_spline) min_dis_future = min(np.sqrt(np.sum(diff_future, axis=1))) total_cost = min_dis + min_dis_future cost_list.append([ total_cost, candidates[i], heading_state[i], steering_step[i] ]) else: search += 1 if (next_state[1] == tree_leaf).all(1).any(): tree_leaf.append(np.round([x_prev, y_prev])) else: tree_leaf.append((np.round([x, y])).tolist()) x = start[0] y = start[1] x_future = x y_future = y x_prev = x y_prev = y heading_state = self.start_heading rotate_angle = heading_state steering_state = self.start_steering found = 0 cost_list = [[0, [x, y], heading_state, steering_state]] next_state = cost_list path_discrete = list([]) # Initialize discrete path path_tree.append(path) path = [] # Initialize continuous path step = 1 bz_last = Bezier(next_state[1], goal, next_state[2], self.goal_heading, self.start_steering, self.mapsize, self.car_length, self.car_width, self.wheelbase, self.tolerance * 3, "Found") bz_last = bz_last.calculation() path.append(bz_last) return path, path_tree
freeGrid_num, obstacle, occupy_grid = mapGenerator(start, obstacle_type, mapsize) # Vehicle dtnamics start_to_goal_distance = sqrt( (start[0] - goal[0])**2 + (start[1] - goal[1])**2) #distance from start point to goal point # the vehicle state in the forward 1 second theta, theta_forward, displacement_rear, displacement_rear_forward, steering_step = vehicle_model( wheelbase, search_length=2.5, speed=5, dt=1) # Bezier spline calculation bezier = Bezier(start, goal, start_heading, goal_heading, start_steering, mapsize, car_length, car_width, wheelbase, 3 * mapsize, "Nofound") bezier_spline = bezier.calculation() #bezier.plot() ################################################################################################################################################### # the main process p = Planner(start, goal, start_heading, goal_heading, start_steering, mapsize, freeGrid_num, tolerance, car_length, car_width, wheelbase, obstacle_type) path, path_tree = p.calculation() # the trajectory points x, y = np.vstack(path).T # 沿着竖直方向将矩阵堆叠起来 plt.figure(figsize=(10, 10)) # plot the obstacles plt.scatter(obstacle[:, 0], obstacle[:, 1], marker='*') # plot the trajectory