局部路径规划器的实现：有限状态机+conformal lattice
局部路径规划（Local Motion planning）
-
- 1. 路径生成 Path generation
- 2. 碰撞检测 Path collision checking
- 3. 行为规划 Behavioural planning
- 4. 速度变化 Velocity profile generation
基于车道追踪，避障，车辆跟随，停车标志几个场景，说明下local planner的基本实现原理
1. 路径生成 Path generation
- 目标点（goal point）: ego vehicle前面离视野距离（look- ahead distance)最近的路标点
def get_goal_index(self, waypoints, ego_state, closest_len, closest_index):
# Find the farthest point along the path that is within the
# lookahead distance of the ego vehicle.
# Take the distance from the ego vehicle to the closest waypoint into consideration.
arc_length = closest_len
wp_index = closest_index
# In this case, reaching the closest waypoint is already far enough for the planner.
# No need to check additional waypoints.
if arc_length > self._lookahead:
return wp_index
# We are already at the end of the path.
if wp_index == len(waypoints) - 1:
return wp_index
# Otherwise, find our next waypoint.
# TODO YOUR CODE HERE
while wp_index < len(waypoints) - 2:
arc_length += self.arc_length_between_2_points(waypoints[wp_index], waypoints[wp_index + 1])
if arc_length >= self._lookahead:
break
wp_index += 1
return wp_index
- 目标状态集(Goal state set): 沿着垂直于路面的方向径向横移目标点产生的集合
def get_goal_state_set(self, goal_index, goal_state, waypoints, ego_state):
# Compute the final heading based on the next index.
# If the goal index is the last in the set of waypoints, use
# the previous index instead.
# To do this, compute the delta_x and delta_y values between
# consecutive waypoints, then use the np.arctan2() function.
# TODO YOUR CODE HERE.
if goal_index == len(waypoints)-1:
p1 = goal_index - 1
p2 = goal_index
elif goal_index >= 0:
p1 = goal_index
p2 = goal_index + 1
delta = np.subtract(waypoints[p2],waypoints[p1])
heading = np.arctan2(delta[1],delta[0])
# Compute the center goal state in the local frame using
# the ego state. The following code will transform the input
# goal state to the ego vehicle's local frame.
# The goal state will be of the form (x, y, v).
goal_pos_local = copy.copy(goal_state)
# Translate so the ego state is at the origin in the new frame.
# This is done by subtracting the ego_state from the goal_state_local.
# TODO YOUR CODE HERE.
goal_pos_local[0] -= ego_state[0]
goal_pos_local[1] -= ego_state[1]
# Rotate such that the ego state has zero heading in the new frame.
# Recall that the general rotation matrix is [cos(theta) -sin(theta)
# sin(theta) cos(theta)]
# and that we are rotating by -ego_state[2] to ensure the ego vehicle's
# current yaw corresponds to theta = 0 in the new local frame.
# TODO YOUR CODE HERE
theta = -ego_state[2]
rotation_matrix = np.array([[cos(theta), -sin(theta)],
[sin(theta),cos(theta)]])
goal = rotation_matrix@np.transpose(goal_pos_local[0:2])
# Compute the goal yaw in the local frame by subtracting off the
# current ego yaw from the heading variable.
# TODO YOUR CODE HERE
goal_t = heading + theta
# Velocity is preserved after the transformation.
goal_v = goal_state[2]
# Keep the goal heading within [-pi, pi] so the optimizer behaves well.
if goal_t > pi:
goal_t -= 2*pi
elif goal_t < -pi:
goal_t += 2*pi
# Compute and apply the offset for each path such that
# all of the paths have the same heading of the goal state,
# but are laterally offset with respect to the goal heading.
goal_state_set = []
for i in range(self._num_paths):
offset = (i - self._num_paths // 2) * self._path_offset
# Compute the projection of the lateral offset along the x
# and y axis. To do this, multiply the offset by cos(goal_theta + pi/2)
# and sin(goal_theta + pi/2), respectively.
# TODO YOUR CODE HERE
x_offset = offset * cos(goal_t + pi/2)
y_offset = offset * sin(goal_t + pi/2)
goal_state_set.append([goal[0] + x_offset, goal[1] + y_offset, goal_t, goal_v])
return goal_state_set
- 坐标变换：将目标状态集从global frame 转换到ego vehicle frame,使得ego vehicle处于原点，并且heading angle = 0，便于后续计算
- Sprial Optimizer: 计算出从ego state到goal state set的多条待选择路径
def plan_paths(self, goal_state_set):
paths = []
path_validity = []
for goal_state in goal_state_set:
path = self._path_optimizer.optimize_spiral(goal_state[0], goal_state[1], goal_state[2])
if np.linalg.norm([path[0][-1] - goal_state[0], path[1][-1] - goal_state[1], path[2][-1] - goal_state[2]]) > 0.1:
path_validity.append(False)
else:
paths.append(path)
path_validity.append(True)
return paths, path_validity
2. 碰撞检测 Path collision checking
- Circle based collision checking: 假设汽车由几个均匀的圆组成，并且模拟这些圆经过生成的多条路径。如果在某路径（标红）中，有障碍物落在圆内则判定为collision，将此路径从待选路径中删去。
可以看作将此图中的长方形替换为圆形：
原因 1. 判定方式简单 只需比较障碍点到圆心的距离和半径r之间的关系 2. 保守 不会漏掉任何可能的碰撞
def collision_check(self, paths, obstacles):
collision_check_array = np.zeros(len(paths), dtype=bool)
for i in range(len(paths)):
collision_free = True
path = paths[i]
# Iterate over the points in the path.
for j in range(len(path[0])):
# Compute the circle locations along this point in the path.
# The circle offsets are given by self._circle_offsets.
# The circle offsets need to placed at each point along the path,
# with the offset rotated by the yaw of the vehicle.
# Each path is of the form [[x_values], [y_values], [theta_values]],
# where each of x_values, y_values, and theta_values are in sequential
# order.
# Thus, we need to compute:
# circle_x = point_x + circle_offset*cos(yaw)
# circle_y = point_y circle_offset*sin(yaw)
# for each point along the path.
# point_x is given by path[0][j], and point _y is given by path[1][j].
# TODO YOUR CODE HERE.
circle_locations = np.zeros((len(self._circle_offsets), 2))
for c in range(len(self._circle_offsets)):
circle_locations[c, 0] = path[0][j] + self._circle_offsets[c] * np.cos(path[2][j])
circle_locations[c, 1] = path[1][j] + self._circle_offsets[c] * np.sin(path[2][j])
# Assumes each obstacle is approximated by a collection of points
# of the form [x, y].
# Here, we will iterate through the obstacle points, and check if any of
# the obstacle points lies within any of our circles.
for k in range(len(obstacles)):
collision_dists = scipy.spatial.distance.cdist(obstacles[k], circle_locations)
collision_dists = np.subtract(collision_dists, self._circle_radii)
collision_free = collision_free and not np.any(collision_dists < 0)
if not collision_free:
break
if not collision_free:
break
collision_check_array[i] = collision_free
return collision_check_array
- Optimal path（black）：由cost function确定
在这里可以设为距离目标中心的距离，这样可以让车辆尽快回到车道内；前提是之前的circle based collision checking中要的circle半径需要大一点，给一定的余量，删去离obstacle太近的path，以免发生碰撞
def calculate_path_score(self, path, goal_state):
# So we have a path (multiple points) and a goal.
# At this point the on;y thing i can think off is to compare how close you are to the GOAL at the end of the path.
last_pt_idx = len(path[0]) - 1
last_pt = np.array([path[0][last_pt_idx],path[1][last_pt_idx]])
score = np.linalg.norm(np.subtract(last_pt, goal_state[:2]))
return score
3. 行为规划 Behavioural planning
这里只针对stop sign，所以有限状态机（Finite State Machine）只包含三个状态：匀速，减速，静止
在这个过程中确定目标点和目标速度
匀速：如果视野(Lookahead distance)中没有发现stop sign, 目标速度：V_goal = speed limit = min(Vref,Vlead)；目标点：P = P_cur + Dis_Lookahead。若发现stop sign，状态转为减速
def transition_state(self, waypoints, ego_state, closed_loop_speed):
# In this state, continue tracking the lane by finding the
# goal index in the waypoint list that is within the lookahead
# distance. Then, check to see if the waypoint path intersects
# with any stop lines. If it does, then ensure that the goal
# state enforces the car to be stopped before the stop line.
# You should use the get_closest_index(), get_goal_index(), and
# check_for_stop_signs() helper functions.
if self._state == FOLLOW_LANE:
# First, find the closest index to the ego vehicle.
# TODO YOUR CODE HERE
closest_len, closest_index = get_closest_index(waypoints, ego_state, self._previous_closest_index)
# Next, find the goal index that lies within the lookahead distance along the
# waypoints.
# TODO YOUR CODE HERE
goal_index = self.get_goal_index(waypoints, ego_state, closest_len, closest_index)
# Finally, check the index set between closest_index and goal_index for stop signs,
# and compute the goal state accordingly.
# TODO YOUR CODE HERE
goal_index, stop_sign_found = self.check_for_stop_signs(waypoints, closest_index, goal_index)
self._goal_index = goal_index
self._goal_state = self.get_goal_state(goal_index, waypoints)
self._previous_closest_index = closest_index
# If stop sign found, set the goal to zero speed, then transition to
# the deceleration state.
# TODO YOUR CODE HERE
if stop_sign_found:
self._goal_state[2] = 0.0
self._state = DECELERATE_TO_STOP
减速：发现stop sign, 开始减速以在它前面停下，预留出缓冲速度, 目标速度：V_goal = STOP_THRESHOLD_SPEED；目标点(goal point)：P = P_stop。
elif self._state == DECELERATE_TO_STOP:
# TODO YOUR CODE HERE
distance_to_stop_sign = np.linalg.norm(np.subtract(self._goal_state[:2], ego_state[:2]))
print('Goal state, Ego State, Distance: ', self._goal_state, ego_state, distance_to_stop_sign)
# if (closed_loop_speed <= STOP_THRESHOLD_SPEED) and (distance_to_stop_sign <= STOP_THRESHOLD_DISTANCE): # If Fully stopped we start the count
# self._state = STAY_STOPPED
# elif (closed_loop_speed <= STOP_THRESHOLD_SPEED):
# self._state = FOLLOW_LANE
if (closed_loop_speed <= STOP_THRESHOLD_SPEED):
self._state = STAY_STOPPED
静止：计时一定时间，在这段时间内保持静止V=0，到时间后，状态转换为匀速
elif self._state == STAY_STOPPED:
# We have stayed stopped for the required number of cycles.
# Allow the ego vehicle to leave the stop sign. Once it has
# passed the stop sign, return to lane following.
# You should use the get_closest_index(), get_goal_index(), and
# check_for_stop_signs() helper functions.
# TODO YOUR CODE HERE
if self._stop_count == STOP_COUNTS:
closest_len, closest_index = get_closest_index(waypoints, ego_state, self._previous_closest_index)
goal_index = self.get_goal_index(waypoints, ego_state, closest_len, closest_index)
# We've stopped for the required amount of time, so the new goal
# index for the stop line is not relevant. Use the goal index that
# is the lookahead distance away.
# TODO YOUR CODE HERE
_, stop_sign_found = self.check_for_stop_signs(waypoints, closest_index, goal_index)
self._goal_index = goal_index
self._goal_state = self.get_goal_state(goal_index, waypoints)
self._previous_closest_index = closest_index
# If the stop sign is no longer along our path, we can now transition
# back to our lane following state.
# TODO YOUR CODE HERE
if not stop_sign_found:
self._stop_count = 0
self._state = FOLLOW_LANE
# Otherwise, continue counting.
# TODO YOUR CODE HERE
else:
self._stop_count +=1
4. 速度变化 Velocity profile generation
这一部分确定ego vehicle行驶过程中的速度
- 确定目标速度
取决于ego vehicle的行为
decelerating to stop/stay stop: 目标速度为0
其他情况V_goal = speed limit = min(Vref,Vlead)
Vref: 道路的限速
Vlead: lead vehicle的速度 - 生成于最优路径相对应的Velocity profile
已知当前速度和位置，目标速度和位置，加速度, 计算处路径各点应有的速度（Velocity profile)
最终的效果图
具体的代码:
https://github.com/Z-SB/Self-Driving-Vehicle-Control-Using-Carla