feat: 添加注释
parent
ff06627956
commit
ef6e7c7589
|
|
@ -132,7 +132,8 @@ class LidarModel:
|
|||
if __name__ == '__main__':
|
||||
|
||||
# Create Map
|
||||
map = Map('real_map.png')
|
||||
map = Map(file_path='maps/real_map.png', origin=(-30.0, -24.0),
|
||||
resolution=0.06)
|
||||
plt.imshow(map.data, cmap='gray',
|
||||
extent=[map.origin[0], map.origin[0] +
|
||||
map.width * map.resolution,
|
||||
|
|
|
|||
57
src/map.py
57
src/map.py
|
|
@ -21,20 +21,20 @@ class Obstacle:
|
|||
:param cy: y coordinate of center of obstacle in world coordinates
|
||||
:param radius: radius of circular obstacle in m
|
||||
"""
|
||||
self.cx = cx
|
||||
self.cy = cy
|
||||
self.radius = radius
|
||||
self.cx = cx # 障碍物中心的 x 坐标（世界坐标系）
|
||||
self.cy = cy # 障碍物中心的 y 坐标（世界坐标系）
|
||||
self.radius = radius # 障碍物的半径（以米为单位）
|
||||
|
||||
def show(self):
|
||||
"""
|
||||
Display obstacle on current axis.
|
||||
"""
|
||||
|
||||
# 使用 matplotlib 在绘图中显示圆形障碍物
|
||||
# Draw circle
|
||||
circle = plt_patches.Circle(xy=(self.cx, self.cy), radius=
|
||||
self.radius, color=OBSTACLE, zorder=20)
|
||||
ax = plt.gca()
|
||||
ax.add_patch(circle)
|
||||
ax = plt.gca() # 获取当前轴
|
||||
ax.add_patch(circle) # 将障碍物添加到绘图中
|
||||
|
||||
|
||||
#######
|
||||
|
|
@ -55,22 +55,23 @@ class Map:
|
|||
"""
|
||||
|
||||
# Set binarization threshold
|
||||
self.threshold_occupied = threshold_occupied
|
||||
self.threshold_occupied = threshold_occupied # 设置二值化阈值
|
||||
|
||||
# Numpy array containing map data
|
||||
self.data = np.array(Image.open(file_path))[:, :, 0]
|
||||
self.data = np.array(Image.open(file_path))[:, :, 0] # 加载地图图像
|
||||
|
||||
# Process raw map image
|
||||
self.process_map()
|
||||
self.process_map() # 处理地图（包括二值化和小孔去除）
|
||||
|
||||
# Store meta information
|
||||
self.height = self.data.shape[0] # height of the map in px
|
||||
self.width = self.data.shape[1] # width of the map in px
|
||||
self.resolution = resolution # resolution of the map in m/px
|
||||
self.origin = origin # x and y coordinates of map origin
|
||||
self.height = self.data.shape[0] # 地图高度（像素）
|
||||
self.width = self.data.shape[1] # 地图宽度（像素）
|
||||
self.resolution = resolution # 地图分辨率（每像素代表的实际米数）
|
||||
self.origin = origin # 地图原点坐标（左下角）
|
||||
# (bottom-left corner) in m
|
||||
|
||||
# Containers for user-specified additional obstacles and boundaries
|
||||
# 初始化障碍物和边界的容器
|
||||
self.obstacles = list()
|
||||
self.boundaries = list()
|
||||
|
||||
|
|
@ -82,6 +83,7 @@ class Map:
|
|||
:param y: y coordinate in global coordinate system
|
||||
:return: discrete x and y coordinates in px
|
||||
"""
|
||||
# 从世界坐标系转换为地图坐标系
|
||||
dx = int(np.floor((x - self.origin[0]) / self.resolution))
|
||||
dy = int(np.floor((y - self.origin[1]) / self.resolution))
|
||||
|
||||
|
|
@ -95,6 +97,7 @@ class Map:
|
|||
:param dy: y coordinate in map coordinate system
|
||||
:return: x and y coordinates of cell center in global coordinate system
|
||||
"""
|
||||
# 从地图坐标系转换为世界坐标系
|
||||
x = (dx + 0.5) * self.resolution + self.origin[0]
|
||||
y = (dy + 0.5) * self.resolution + self.origin[1]
|
||||
|
||||
|
|
@ -107,11 +110,11 @@ class Map:
|
|||
|
||||
# Binarization using specified threshold
|
||||
# 1 corresponds to free, 0 to occupied
|
||||
self.data = np.where(self.data >= self.threshold_occupied, 1, 0)
|
||||
self.data = np.where(self.data >= self.threshold_occupied, 1, 0) # 二值化
|
||||
|
||||
# Remove small holes in map corresponding to spurious measurements
|
||||
self.data = remove_small_holes(self.data, area_threshold=5,
|
||||
connectivity=8).astype(np.int8)
|
||||
connectivity=8).astype(np.int8) # 去除小孔
|
||||
|
||||
def add_obstacles(self, obstacles):
|
||||
"""
|
||||
|
|
@ -126,11 +129,12 @@ class Map:
|
|||
for obstacle in obstacles:
|
||||
|
||||
# Compute radius of circular object in pixels
|
||||
radius_px = int(np.ceil(obstacle.radius / self.resolution))
|
||||
radius_px = int(np.ceil(obstacle.radius / self.resolution)) # 半径像素
|
||||
# Get center coordinates of obstacle in map coordinates
|
||||
cx_px, cy_px = self.w2m(obstacle.cx, obstacle.cy)
|
||||
cx_px, cy_px = self.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.data[cy_px-radius_px:cy_px+radius_px, cx_px-radius_px:
|
||||
|
|
@ -142,21 +146,24 @@ class Map:
|
|||
:param boundaries: list of tuples containing coordinates of boundaries'
|
||||
start and end points
|
||||
"""
|
||||
|
||||
# 向地图中添加线性边界
|
||||
# Extend list of boundaries
|
||||
self.boundaries.extend(boundaries)
|
||||
|
||||
# Iterate over list of boundaries
|
||||
# 遍历边界列表
|
||||
for boundary in boundaries:
|
||||
sx = self.w2m(boundary[0][0], boundary[0][1])
|
||||
gx = self.w2m(boundary[1][0], boundary[1][1])
|
||||
path_x, path_y, _ = line_aa(sx[0], sx[1], gx[0], gx[1])
|
||||
sx = self.w2m(boundary[0][0], boundary[0][1]) # 起点
|
||||
gx = self.w2m(boundary[1][0], boundary[1][1]) # 终点
|
||||
path_x, path_y, _ = line_aa(sx[0], sx[1], gx[0], gx[1]) # 绘制线条
|
||||
for x, y in zip(path_x, path_y):
|
||||
self.data[y, x] = 0
|
||||
self.data[y, x] = 0 # 标记边界
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
map = Map('maps/real_map.png')
|
||||
# map = Map('maps/sim_map.png')
|
||||
plt.imshow(np.flipud(map.data), cmap='gray')
|
||||
map = Map(file_path='maps/real_map.png', origin=(-30.0, -24.0),
|
||||
resolution=0.06)
|
||||
# map = Map(file_path='maps/sim_map.png', origin=[-1, -2],
|
||||
# resolution=0.005)
|
||||
plt.imshow(np.flipud(map.data), cmap='gray') # 显示地图
|
||||
plt.show()
|
||||
|
|
|
|||
|
|
@ -24,10 +24,11 @@ class Waypoint:
|
|||
orientation of waypoint psi and local curvature kappa. Waypoint further
|
||||
contains an associated reference velocity computed by the speed profile
|
||||
and a path width specified by upper and lower bounds.
|
||||
:param x: x position in global coordinate system | [m]
|
||||
:param y: y position in global coordinate system | [m]
|
||||
:param psi: orientation of waypoint | [rad]
|
||||
:param kappa: local curvature | [1 / m]
|
||||
路径点对象，用于存储全局坐标系下的位置、方向和局部曲率。路径点还包含由速度曲线计算的关联参考速度和由上下界指定的路径宽度。
|
||||
:param x: x position in global coordinate system | [m] 路径点的 x 坐标
|
||||
:param y: y position in global coordinate system | [m] 路径点的 y 坐标
|
||||
:param psi: orientation of waypoint | [rad] 路径点方向角（相对于全局坐标系）
|
||||
:param kappa: local curvature | [1 / m] 路径点的局部曲率
|
||||
"""
|
||||
self.x = x
|
||||
self.y = y
|
||||
|
|
@ -35,6 +36,7 @@ class Waypoint:
|
|||
self.kappa = kappa
|
||||
|
||||
# Reference velocity at this waypoint according to speed profile
|
||||
# 根据速度剖面计算的参考速度（此路径点的目标速度）
|
||||
self.v_ref = None
|
||||
|
||||
# Information about drivable area at waypoint
|
||||
|
|
@ -42,10 +44,15 @@ class Waypoint:
|
|||
# waypoint orientation.
|
||||
# Upper bound: free drivable area to the left of center-line in m
|
||||
# Lower bound: free drivable area to the right of center-line in m
|
||||
# 可驾驶区域的信息
|
||||
# 上边界：路径点中心线左侧的最大可驾驶距离（单位：米）
|
||||
# 下边界：路径点中心线右侧的最大可驾驶距离（单位：米）
|
||||
self.lb = None
|
||||
self.ub = None
|
||||
self.static_border_cells = None
|
||||
self.dynamic_border_cells = None
|
||||
|
||||
# 静态边界和动态边界的网格单元（存储额外信息）
|
||||
self.static_border_cells = None # 静态边界信息
|
||||
self.dynamic_border_cells = None # 动态边界信息
|
||||
|
||||
def __sub__(self, other):
|
||||
"""
|
||||
|
|
@ -54,6 +61,11 @@ class Waypoint:
|
|||
:param other: subtrahend
|
||||
:return: euclidean distance between two waypoints
|
||||
"""
|
||||
"""
|
||||
重载减法运算符。两个路径点之间的差定义为它们的欧几里得距离。
|
||||
:param other: 被减数（另一个路径点）
|
||||
:return: 两个路径点的欧几里得距离
|
||||
"""
|
||||
return ((self.x - other.x)**2 + (self.y - other.y)**2)**0.5
|
||||
|
||||
|
||||
|
|
@ -70,58 +82,63 @@ class ReferencePath:
|
|||
corner points with given resolution. Smoothing around corners can be
|
||||
applied. Waypoints represent center-line of the path with specified
|
||||
maximum width to both sides.
|
||||
:param map: map object on which path will be placed
|
||||
:param wp_x: x coordinates of corner points in global coordinates
|
||||
:param wp_y: y coordinates of corner points in global coordinates
|
||||
:param resolution: resolution of the path in m/wp
|
||||
:param smoothing_distance: number of waypoints used for smoothing the
|
||||
参考路径对象的构造函数，创建参考轨迹（trajectory）。参考轨迹由指定的角点和分辨率创建。可以对角点周围进行平滑处理。
|
||||
路径点代表路径的中心线，两侧有指定的最大宽度。
|
||||
:param map: map object on which path will be placed 地图对象，路径将基于此地图生成
|
||||
:param wp_x: x coordinates of corner points in global coordinates 关键点的 x 坐标（全局坐标系）
|
||||
:param wp_y: y coordinates of corner points in global coordinates 关键点的 y 坐标（全局坐标系）
|
||||
:param resolution: resolution of the path in m/wp 路径的分辨率（单位：米/路径点）
|
||||
: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 circular: True if path circular
|
||||
:param max_width: maximum width of path to both sides in m 路径两侧的最大宽度（单位：米）
|
||||
:param circular: True if path circular 如果路径是闭合的，则为 True
|
||||
"""
|
||||
|
||||
# Precision
|
||||
# 数值精度
|
||||
self.eps = 1e-12
|
||||
|
||||
# Map
|
||||
# 地图对象
|
||||
self.map = map
|
||||
|
||||
# Resolution of the path
|
||||
# 路径分辨率
|
||||
self.resolution = resolution
|
||||
|
||||
# Look ahead distance for path averaging
|
||||
# 路径平滑的滑动窗口大小
|
||||
self.smoothing_distance = smoothing_distance
|
||||
|
||||
# Circular flag
|
||||
# 是否为闭合路径
|
||||
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()
|
||||
|
||||
# Compute path width (attribute of each waypoint)
|
||||
# 计算路径宽度（每个路径点的属性）
|
||||
self._compute_width(max_width=max_width)
|
||||
|
||||
def _construct_path(self, wp_x, wp_y):
|
||||
"""
|
||||
Construct path from given waypoints.
|
||||
:param wp_x: x coordinates of waypoints in global coordinates
|
||||
:param wp_y: y coordinates of waypoints in global coordinates
|
||||
:return: list of waypoint objects
|
||||
根据关键点生成路径。
|
||||
:param wp_x: x coordinates of waypoints in global coordinates 关键点的 x 坐标
|
||||
:param wp_y: y coordinates of waypoints in global coordinates 关键点的 y 坐标
|
||||
:return: list of waypoint objects 路径点列表
|
||||
"""
|
||||
|
||||
# Number of waypoints
|
||||
# 计算关键点之间所需插值的路径点数量
|
||||
n_wp = [int(np.sqrt((wp_x[i + 1] - wp_x[i]) ** 2 +
|
||||
(wp_y[i + 1] - wp_y[i]) ** 2) /
|
||||
self.resolution) for i in range(len(wp_x) - 1)]
|
||||
|
||||
# Construct waypoints with specified resolution
|
||||
gp_x, gp_y = wp_x[-1], wp_y[-1]
|
||||
# 使用指定分辨率生成路径点
|
||||
gp_x, gp_y = wp_x[-1], wp_y[-1] # 最后一段路径的终点
|
||||
wp_x = [np.linspace(wp_x[i], wp_x[i+1], n_wp[i], endpoint=False).
|
||||
tolist() for i in range(len(wp_x)-1)]
|
||||
wp_x = [wp for segment in wp_x for wp in segment] + [gp_x]
|
||||
|
|
@ -129,7 +146,7 @@ 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]
|
||||
|
||||
# Smooth path
|
||||
# 使用滑动平均对路径进行平滑
|
||||
wp_xs = []
|
||||
wp_ys = []
|
||||
for wp_id in range(self.smoothing_distance, len(wp_x) -
|
||||
|
|
@ -139,7 +156,7 @@ class ReferencePath:
|
|||
wp_ys.append(np.mean(wp_y[wp_id - self.smoothing_distance:wp_id
|
||||
+ self.smoothing_distance + 1]))
|
||||
|
||||
# Construct list of waypoint objects
|
||||
# 将 (x, y) 坐标转化为路径点对象
|
||||
waypoints = list(zip(wp_xs, wp_ys))
|
||||
waypoints = self._construct_waypoints(waypoints)
|
||||
|
||||
|
|
@ -149,35 +166,45 @@ class ReferencePath:
|
|||
"""
|
||||
Reformulate conventional waypoints (x, y) coordinates into waypoint
|
||||
objects containing (x, y, psi, kappa, ub, lb)
|
||||
将普通的路径点 (x, y) 转换为包含方向角和曲率的路径点对象。
|
||||
:param waypoint_coordinates: list of (x, y) coordinates of waypoints in
|
||||
global coordinates
|
||||
:return: list of waypoint objects for entire reference path
|
||||
global coordinates 路径点的 (x, y) 坐标列表
|
||||
:return: list of waypoint objects for entire reference path 包含路径点对象的列表
|
||||
"""
|
||||
|
||||
# List containing waypoint objects
|
||||
# 路径点对象列表
|
||||
waypoints = []
|
||||
|
||||
# Iterate over all waypoints
|
||||
# 遍历所有路径点
|
||||
for wp_id in range(len(waypoint_coordinates) - 1):
|
||||
|
||||
# Get start and goal waypoints
|
||||
# 获取当前路径点和下一个路径点的坐标
|
||||
current_wp = np.array(waypoint_coordinates[wp_id])
|
||||
next_wp = np.array(waypoint_coordinates[wp_id + 1])
|
||||
|
||||
# Difference vector
|
||||
# 当前路径点到下一个路径点的向量
|
||||
dif_ahead = next_wp - current_wp
|
||||
|
||||
# Angle ahead
|
||||
# 当前路径点到下一个路径点的方向角
|
||||
psi = np.arctan2(dif_ahead[1], dif_ahead[0])
|
||||
|
||||
# Distance to next waypoint
|
||||
# 当前路径点到下一个路径点的距离
|
||||
dist_ahead = np.linalg.norm(dif_ahead, 2)
|
||||
|
||||
# Get x and y coordinates of current waypoint
|
||||
# 当前路径点的 x 和 y 坐标
|
||||
x, y = current_wp[0], current_wp[1]
|
||||
|
||||
# Compute local curvature at waypoint
|
||||
# 计算路径点的局部曲率
|
||||
# first waypoint
|
||||
# 第一个路径点
|
||||
if wp_id == 0:
|
||||
kappa = 0
|
||||
else:
|
||||
|
|
@ -196,7 +223,8 @@ class ReferencePath:
|
|||
"""
|
||||
Compute length of center-line path as sum of euclidean distance between
|
||||
waypoints.
|
||||
:return: length of center-line path in m
|
||||
计算路径的长度，即所有路径点之间的欧几里得距离之和。
|
||||
: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)-1)]
|
||||
|
|
@ -207,16 +235,21 @@ class ReferencePath:
|
|||
"""
|
||||
Compute the width of the path by checking the maximum free space to
|
||||
the left and right of the center-line.
|
||||
:param max_width: maximum width of the path.
|
||||
计算路径的宽度，即路径中心线两侧的最大可行驶距禂。
|
||||
:param max_width: maximum width of the path. 路径的最大宽度
|
||||
"""
|
||||
|
||||
# Iterate over all waypoints
|
||||
# 遍历所有路径点
|
||||
for wp_id, wp in enumerate(self.waypoints):
|
||||
# List containing information for current waypoint
|
||||
# 当前路径点的信息列表
|
||||
width_info = []
|
||||
# Check width left and right of the center-line
|
||||
# 检查路径中心线左右两侧的宽度
|
||||
for i, dir in enumerate(['left', 'right']):
|
||||
# Get angle orthogonal to path in current direction
|
||||
# 获取当前方向上与路径正交的角度
|
||||
if dir == 'left':
|
||||
angle = np.mod(wp.psi + math.pi / 2 + math.pi,
|
||||
2 * math.pi) - math.pi
|
||||
|
|
@ -224,19 +257,25 @@ class ReferencePath:
|
|||
angle = np.mod(wp.psi - math.pi / 2 + math.pi,
|
||||
2 * math.pi) - math.pi
|
||||
# Get closest cell to orthogonal vector
|
||||
# 获取与正交向量最近的网格单元
|
||||
t_x, t_y = self.map.w2m(wp.x + max_width * np.cos(angle), wp.y
|
||||
+ max_width * np.sin(angle))
|
||||
# Compute distance to orthogonal cell on path border
|
||||
# 计算到路径边界上正交网格单元的距离
|
||||
b_value, b_cell = self._get_min_width(wp, t_x, t_y, max_width)
|
||||
# Add information to list for current waypoint
|
||||
# 将当前路径点的信息添加到列表中
|
||||
width_info.append(b_value)
|
||||
width_info.append(b_cell)
|
||||
|
||||
# Set waypoint attributes with width to the left and right
|
||||
# 设置路径点的左右两侧宽度
|
||||
wp.ub = width_info[0]
|
||||
wp.lb = -1 * width_info[2] # minus can be assumed as waypoints
|
||||
wp.lb = -1 * width_info[2] # minus can be assumed as waypoints 负号可以认为是路径点
|
||||
# represent center-line of the path
|
||||
# 表示路径的中心线
|
||||
# Set border cells of waypoint
|
||||
# 设置路径点的边界网格单元
|
||||
wp.static_border_cells = (width_info[1], width_info[3])
|
||||
wp.dynamic_border_cells = (width_info[1], width_info[3])
|
||||
|
||||
|
|
@ -244,15 +283,17 @@ class ReferencePath:
|
|||
"""
|
||||
Compute the minimum distance between the current waypoint and the
|
||||
orthogonal cell on the border of the path
|
||||
:param wp: current waypoint
|
||||
:param t_x: x coordinate of border cell in map coordinates
|
||||
:param t_y: y coordinate of border cell in map coordinates
|
||||
:param max_width: maximum path width in m
|
||||
:return: min_width to border and corresponding cell
|
||||
计算当前路径点与路径边界上正交网格单元之间的最小距离
|
||||
:param wp: current waypoint object 当前路径点对象
|
||||
:param t_x: x coordinate of border cell in map coordinates 路径边界网格单元的 x 坐标
|
||||
:param t_y: y coordinate of border cell in map coordinates 路径边界网格单元的 y 坐标
|
||||
:param max_width: maximum path width in m 路径的最大宽度
|
||||
:return: min_width to border and corresponding cell 最小距离和对应的网格单元
|
||||
"""
|
||||
|
||||
# Get neighboring cells of orthogonal cell (account for
|
||||
# discretization inaccuracy)
|
||||
# 获取正交网格单元的相邻网格单元（考虑离散化不准确性）
|
||||
tn_x, tn_y = [], []
|
||||
for i in range(-1, 2, 1):
|
||||
for j in range(-1, 2, 1):
|
||||
|
|
@ -260,24 +301,30 @@ class ReferencePath:
|
|||
tn_y.append(t_y+j)
|
||||
|
||||
# Get pixel coordinates of waypoint
|
||||
# 获取路径点的像素坐标
|
||||
wp_x, wp_y = self.map.w2m(wp.x, wp.y)
|
||||
|
||||
# Get Bresenham paths to all possible cells
|
||||
# 获取到所有可能网格单元的 Bresenham 路径
|
||||
paths = []
|
||||
for t_x, t_y in zip(tn_x, tn_y):
|
||||
x_list, y_list, _ = line_aa(wp_x, wp_y, t_x, t_y)
|
||||
paths.append(zip(x_list, y_list))
|
||||
|
||||
# Compute minimum distance to border cell
|
||||
# 计算到边界网格单元的最小距离
|
||||
min_width = max_width
|
||||
# map inspected cell to world coordinates
|
||||
# 将检查的网格单元映射到世界坐标系
|
||||
min_cell = self.map.m2w(t_x, t_y)
|
||||
for path in paths:
|
||||
for cell in path:
|
||||
t_x, t_y = cell[0], cell[1]
|
||||
# If path goes through occupied cell
|
||||
# 如果路径穿过占用的网格单元
|
||||
if self.map.data[t_y, t_x] == 0:
|
||||
# Get world coordinates
|
||||
# 获取世界坐标
|
||||
c_x, c_y = self.map.m2w(t_x, t_y)
|
||||
cell_dist = np.sqrt((wp.x - c_x) ** 2 + (wp.y - c_y) ** 2)
|
||||
if cell_dist < min_width:
|
||||
|
|
@ -290,65 +337,82 @@ class ReferencePath:
|
|||
"""
|
||||
Compute a speed profile for the path. Assign a reference velocity
|
||||
to each waypoint based on its curvature.
|
||||
:param Constraints: constraints on acceleration and velocity
|
||||
为路径计算速度剖面。根据路径点的曲率为每个路径点分配一个参考速度。
|
||||
:param Constraints: constraints on acceleration and velocity 路线上加速度、速度、曲率的约束条件
|
||||
curvature of the path
|
||||
"""
|
||||
|
||||
# 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
|
||||
|
|
@ -356,14 +420,17 @@ class ReferencePath:
|
|||
def get_waypoint(self, wp_id):
|
||||
"""
|
||||
Get waypoint corresponding to wp_id. Circular indexing supported.
|
||||
:param wp_id: unique waypoint ID
|
||||
:return: waypoint object
|
||||
获取与路径点 ID 对应的路径点。支持循环索引。
|
||||
:param wp_id: unique waypoint ID 唯一路径点 ID
|
||||
:return: waypoint object corresponding to wp_id 与路径点 ID 对应的路径点对象
|
||||
"""
|
||||
|
||||
# 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)
|
||||
|
|
@ -373,8 +440,9 @@ class ReferencePath:
|
|||
def show(self, display_drivable_area=True):
|
||||
"""
|
||||
Display path object on current figure.
|
||||
在当前图中显示路径对象。
|
||||
:param display_drivable_area: If True, display arrows indicating width
|
||||
of drivable area
|
||||
of drivable area at waypoints. 如果为 True，则在路径点处显示指示可行驶区域宽度的箭头。
|
||||
"""
|
||||
|
||||
# Clear figure
|
||||
|
|
@ -395,10 +463,12 @@ class ReferencePath:
|
|||
vmax=1.0)
|
||||
|
||||
# Get x and y coordinates for all waypoints
|
||||
# 获取所有路径点的 x 和 y 坐标
|
||||
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
|
||||
# 获取上下界的边界网格单元的 x 和 y 坐标
|
||||
wp_ub_x = np.array([wp.static_border_cells[0][0] for wp in self.waypoints])
|
||||
wp_ub_y = np.array([wp.static_border_cells[0][1] for wp in self.waypoints])
|
||||
wp_lb_x = np.array([wp.static_border_cells[1][0] for wp in self.waypoints])
|
||||
|
|
@ -409,6 +479,7 @@ class ReferencePath:
|
|||
plt.scatter(wp_x, wp_y, c=WAYPOINTS, s=10)
|
||||
|
||||
# Plot arrows indicating drivable area
|
||||
# 在路径点处显示指示可行驶区域的箭头
|
||||
if display_drivable_area:
|
||||
plt.quiver(wp_x, wp_y, wp_ub_x - wp_x, wp_ub_y - wp_y, scale=1,
|
||||
units='xy', width=0.2*self.resolution, color=DRIVABLE_AREA,
|
||||
|
|
@ -418,6 +489,7 @@ class ReferencePath:
|
|||
headwidth=1, headlength=0)
|
||||
|
||||
# Plot border of path
|
||||
# 绘制路径的边界
|
||||
bl_x = np.array([wp.static_border_cells[0][0] for wp in
|
||||
self.waypoints] +
|
||||
[self.waypoints[0].static_border_cells[0][0]])
|
||||
|
|
@ -432,10 +504,12 @@ class ReferencePath:
|
|||
[self.waypoints[0].static_border_cells[1][1]])
|
||||
|
||||
# If circular path, connect start and end point
|
||||
# 如果路径是闭合的，则连接起点和终点
|
||||
if self.circular:
|
||||
plt.plot(bl_x, bl_y, color='#5E5E5E')
|
||||
plt.plot(br_x, br_y, color='#5E5E5E')
|
||||
# If not circular, close path at start and end
|
||||
# 如果路径不是闭合的，则在起点和终点处关闭路径
|
||||
else:
|
||||
plt.plot(bl_x[:-1], bl_y[:-1], color=OBSTACLE)
|
||||
plt.plot(br_x[:-1], br_y[:-1], color=OBSTACLE)
|
||||
|
|
@ -443,7 +517,9 @@ class ReferencePath:
|
|||
plt.plot((bl_x[0], br_x[0]), (bl_y[0], br_y[0]), color=OBSTACLE)
|
||||
|
||||
# Plot dynamic path constraints
|
||||
# 绘制动态路径约束
|
||||
# Get x and y locations of border cells for upper and lower bound
|
||||
# 获取上下界的边界网格单元的 x 和 y 坐标
|
||||
wp_ub_x = np.array(
|
||||
[wp.dynamic_border_cells[0][0] for wp in self.waypoints]+
|
||||
[self.waypoints[0].static_border_cells[0][0]])
|
||||
|
|
@ -460,57 +536,72 @@ class ReferencePath:
|
|||
plt.plot(wp_lb_x, wp_lb_y, c=PATH_CONSTRAINTS)
|
||||
|
||||
# Plot obstacles
|
||||
# 绘制障碍物
|
||||
for obstacle in self.map.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)
|
||||
计算自由路径段。
|
||||
:param wp: waypoint object 路径点对象
|
||||
:param min_width: minimum width of valid segment in m 有效路径段的最小宽度（单位：米）
|
||||
: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.static_border_cells[0][0],
|
||||
wp.static_border_cells[0][1])
|
||||
lb_p = self.map.w2m(wp.static_border_cells[1][0],
|
||||
wp.static_border_cells[1][1])
|
||||
|
||||
# Compute path from left border cell to right border cell
|
||||
# 计算从左边界网格单元到右边界网格单元的路径
|
||||
x_list, y_list, _ = line_aa(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:
|
||||
|
|
@ -523,24 +614,30 @@ class ReferencePath:
|
|||
"""
|
||||
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 = []
|
||||
border_cells_hor_sm = []
|
||||
|
||||
# Iterate over horizon
|
||||
# 遍历优化范围
|
||||
for n in range(N):
|
||||
|
||||
# get corresponding waypoint
|
||||
# 获取对应的路径点
|
||||
wp = self.get_waypoint(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]
|
||||
|
|
@ -550,10 +647,12 @@ class ReferencePath:
|
|||
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.get_waypoint(wp_id+n-1)
|
||||
delta_s = wp_prev - wp
|
||||
ub_pw[0] += delta_s * np.cos(wp_prev.psi)
|
||||
|
|
@ -562,39 +661,47 @@ class ReferencePath:
|
|||
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
|
||||
# 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)
|
||||
|
|
@ -603,33 +710,40 @@ class ReferencePath:
|
|||
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
|
||||
# 如果上下界为 0，则表示路径不可行
|
||||
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 和 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_sm = (ub_ls, lb_ls)
|
||||
# Compute cell on bound for computed distance ub and lb
|
||||
# 计算边界网格单元，用于计算距离 ub 和 lb
|
||||
ub_ls = wp.x + (ub + safety_margin) * np.cos(angle_ub), wp.y + (ub + safety_margin) * np.sin(
|
||||
angle_ub)
|
||||
lb_ls = wp.x - (lb - safety_margin) * np.cos(angle_lb), wp.y - (lb - safety_margin) * np.sin(
|
||||
|
|
@ -637,12 +751,14 @@ class ReferencePath:
|
|||
bound_cells = (ub_ls, lb_ls)
|
||||
|
||||
# Append results
|
||||
# 添加结果
|
||||
ub_hor.append(ub)
|
||||
lb_hor.append(lb)
|
||||
border_cells_hor.append(list(bound_cells))
|
||||
border_cells_hor_sm.append(list(bound_cells_sm))
|
||||
|
||||
# Assign dynamic border cells to waypoints
|
||||
# 为路径点分配动态边界网格单元
|
||||
wp.dynamic_border_cells = bound_cells_sm
|
||||
|
||||
return np.array(ub_hor), np.array(lb_hor), border_cells_hor_sm
|
||||
|
|
|
|||
Loading…
Reference in New Issue