12345qiupeng
/
Multi-Purpose-MPC
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 给refer_path添加注释

master
邱棚 2024-11-28 17:35:35 +08:00
parent ef6e7c7589
commit 03d49805a4
1 changed files with 32 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/reference_path.py
View File

@ -257,11 +257,11 @@ class ReferencePath:
angle = np.mod(wp.psi - math.pi / 2 + math.pi, angle = np.mod(wp.psi - math.pi / 2 + math.pi,
2 * math.pi) - math.pi 2 * math.pi) - math.pi
# Get closest cell to orthogonal vector # Get closest cell to orthogonal vector
# 获取与正交向量最近的网格单元 # 获取与正交向量最近的网格单元，将方向向量，转换为两个离散的单元点，用于下面的最短距离查找
t_x, t_y = self.map.w2m(wp.x + max_width * np.cos(angle), wp.y t_x, t_y = self.map.w2m(wp.x + max_width * np.cos(angle), wp.y
+ max_width * np.sin(angle)) + max_width * np.sin(angle))
# Compute distance to orthogonal cell on path border # Compute distance to orthogonal cell on path border
# 计算到路径边界上正交网格单元的距离 # 计算到路径边界上正交网格单元的距离，使用bresenham算法离散查找最近的障碍物点（也可能是边界点）
b_value, b_cell = self._get_min_width(wp, t_x, t_y, max_width) b_value, b_cell = self._get_min_width(wp, t_x, t_y, max_width)
# Add information to list for current waypoint # Add information to list for current waypoint
# 将当前路径点的信息添加到列表中 # 将当前路径点的信息添加到列表中
@ -271,7 +271,7 @@ class ReferencePath:
# Set waypoint attributes with width to the left and right # Set waypoint attributes with width to the left and right
# 设置路径点的左右两侧宽度 # 设置路径点的左右两侧宽度
wp.ub = width_info[0] 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 # represent center-line of the path
# 表示路径的中心线 # 表示路径的中心线
# Set border cells of waypoint # Set border cells of waypoint
@ -543,7 +543,7 @@ class ReferencePath:
def _compute_free_segments(self, wp, min_width): def _compute_free_segments(self, wp, min_width):
""" """
Compute free path segments. Compute free path segments.
计算自由路径段。 计算自由路径段。要求是左右边界之间上的路段是空闲的，且宽度大于最小宽度。在grid网格地图上进行搜索
:param wp: waypoint object 路径点对象 :param wp: waypoint object 路径点对象
:param min_width: minimum width of valid segment in m 有效路径段的最小宽度（单位：米） :param min_width: minimum width of valid segment in m 有效路径段的最小宽度（单位：米）
:return: segment candidates as list of tuples (ub_cell, lb_cell) 作为元组列表的路径段候选项（上界网格单元，下界网格单元） :return: segment candidates as list of tuples (ub_cell, lb_cell) 作为元组列表的路径段候选项（上界网格单元，下界网格单元）
@ -555,6 +555,7 @@ class ReferencePath:
# Get waypoint's border cells in map coordinates # Get waypoint's border cells in map coordinates
# 获取路径点的边界网格单元（地图坐标系） # 获取路径点的边界网格单元（地图坐标系）
# 计算出参考点上的左右边界，地图的边界点坐标ud_p(x，y)表示左边界，lb_p(x，y)表示右边界
ub_p = self.map.w2m(wp.static_border_cells[0][0], ub_p = self.map.w2m(wp.static_border_cells[0][0],
wp.static_border_cells[0][1]) wp.static_border_cells[0][1])
lb_p = self.map.w2m(wp.static_border_cells[1][0], lb_p = self.map.w2m(wp.static_border_cells[1][0],
@ -701,7 +702,7 @@ class ReferencePath:
ub_ls, lb_ls = (wp.x, wp.y), (wp.x, wp.y) ub_ls, lb_ls = (wp.x, wp.y), (wp.x, wp.y)
# Check sign of upper and lower bound # Check sign of upper and lower bound
# 检查上下界的符号 # 检查上下界的符号，根据边界点到路径点的方向角，用于判断方向
angle_ub = np.mod(np.arctan2(ub_ls[1] - wp.y, ub_ls[0] - wp.x) 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 - 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) angle_lb = np.mod(np.arctan2(lb_ls[1] - wp.y, lb_ls[0] - wp.x)
@ -710,7 +711,7 @@ class ReferencePath:
sign_lb = np.sign(angle_lb) sign_lb = np.sign(angle_lb)
# Compute upper and lower bound of largest drivable area # Compute upper and lower bound of largest drivable area
# 计算最大可行驶区域的上下界 # 计算最大可行驶区域的上下界 根据上面的方向以及距离，计算上下届的数值
ub = sign_ub * np.sqrt( ub = sign_ub * np.sqrt(
(ub_ls[0] - wp.x) ** 2 + (ub_ls[1] - wp.y) ** 2) (ub_ls[0] - wp.x) ** 2 + (ub_ls[1] - wp.y) ** 2)
lb = sign_lb * np.sqrt( lb = sign_lb * np.sqrt(
@ -743,7 +744,7 @@ class ReferencePath:
angle_lb) angle_lb)
bound_cells_sm = (ub_ls, lb_ls) bound_cells_sm = (ub_ls, lb_ls)
# Compute cell on bound for computed distance ub and lb # 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( ub_ls = wp.x + (ub + safety_margin) * np.cos(angle_ub), wp.y + (ub + safety_margin) * np.sin(
angle_ub) angle_ub)
lb_ls = wp.x - (lb - safety_margin) * np.cos(angle_lb), wp.y - (lb - safety_margin) * np.sin( lb_ls = wp.x - (lb - safety_margin) * np.cos(angle_lb), wp.y - (lb - safety_margin) * np.sin(
@ -752,13 +753,13 @@ class ReferencePath:
# Append results # Append results
# 添加结果 # 添加结果
ub_hor.append(ub) ub_hor.append(ub) # 上界，含义是距离
lb_hor.append(lb) lb_hor.append(lb) # 下界，含义是距离
border_cells_hor.append(list(bound_cells)) border_cells_hor.append(list(bound_cells)) # 不带安全距离的边界网格单元
border_cells_hor_sm.append(list(bound_cells_sm)) border_cells_hor_sm.append(list(bound_cells_sm)) # 带安全距离的边界网格单元
# Assign dynamic border cells to waypoints # Assign dynamic border cells to waypoints
# 为路径点分配动态边界网格单元 # 将带安全距离的边界网格单元分配给路径点的动态边界网格单元
wp.dynamic_border_cells = bound_cells_sm wp.dynamic_border_cells = bound_cells_sm
return np.array(ub_hor), np.array(lb_hor), border_cells_hor_sm return np.array(ub_hor), np.array(lb_hor), border_cells_hor_sm
@ -772,23 +773,28 @@ if __name__ == '__main__':
if path == 'Sim_Track': if path == 'Sim_Track':
# Load map file # Load map file
# 加载地图文件
map = Map(file_path='maps/sim_map.png', origin=[-1, -2], resolution=0.005) map = Map(file_path='maps/sim_map.png', origin=[-1, -2], resolution=0.005)
# Specify waypoints # Specify waypoints
# 指定路径点
wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25, wp_x = [-0.75, -0.25, -0.25, 0.25, 0.25, 1.25, 1.25, 0.75, 0.75, 1.25,
1.25, -0.75, -0.75, -0.25] 1.25, -0.75, -0.75, -0.25]
wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0, wp_y = [-1.5, -1.5, -0.5, -0.5, -1.5, -1.5, -1, -1, -0.5, -0.5, 0, 0,
-1.5, -1.5] -1.5, -1.5]
# Specify path resolution # Specify path resolution
# 指定路径分辨率
path_resolution = 0.05 # m / wp path_resolution = 0.05 # m / wp
# Create reference path # Create reference path
# 根据参考点创建参考路径，这里计算了路径点相对于地图上的边界之间的关系，得到的参考路径的上下界，上界表示中心线左侧的距离，下界表示中心线右侧的距离（负数）
reference_path = ReferencePath(map, wp_x, wp_y, path_resolution, reference_path = ReferencePath(map, wp_x, wp_y, path_resolution,
smoothing_distance=5, max_width=0.15, smoothing_distance=5, max_width=0.15,
circular=True) circular=False)
# Add obstacles # Add obstacles
# 添加障碍物
obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05) obs1 = Obstacle(cx=0.0, cy=0.0, radius=0.05)
obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08) obs2 = Obstacle(cx=-0.8, cy=-0.5, radius=0.08)
obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05) obs3 = Obstacle(cx=-0.7, cy=-1.5, radius=0.05)
@ -843,16 +849,29 @@ if __name__ == '__main__':
print('Invalid path!') print('Invalid path!')
exit(1) exit(1)
# Compute width of path
# 计算路径的宽度，计算各点的上下界距离，以及边界网格单元，同时将带安全距离的网格单元赋给动态边界dynamic_border_cells
ub, lb, border_cells = \ ub, lb, border_cells = \
reference_path.update_path_constraints(0, reference_path.n_waypoints, reference_path.update_path_constraints(0, reference_path.n_waypoints,
0.1, 0.01) 0.1, 0.01)
# 设置速度约束
SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5, SpeedProfileConstraints = {'a_min': -0.1, 'a_max': 0.5,
'v_min': 0, 'v_max': 1.0, 'ay_max': 4.0} 'v_min': 0, 'v_max': 1.0, 'ay_max': 4.0}
# Compute speed profile
# 计算速度剖面，根据纵向加速度和侧向加速度，最大速度等条件，通过二次规划得到路径上各点的参考速度
reference_path.compute_speed_profile(SpeedProfileConstraints) reference_path.compute_speed_profile(SpeedProfileConstraints)
# Get x and y locations of border cells for upper and lower bound # Get x and y locations of border cells for upper and lower bound
# 获取上下界的边界网格单元的 x 和 y 坐标
# 此处似乎是多余的，在上面的代码中已经赋值，这又重复赋值
for wp_id in range(reference_path.n_waypoints): for wp_id in range(reference_path.n_waypoints):
reference_path.waypoints[wp_id].dynamic_border_cells = border_cells[wp_id] reference_path.waypoints[wp_id].dynamic_border_cells = border_cells[wp_id]
# Display reference path
# 显示参考路径
reference_path.show() reference_path.show()
plt.show() plt.show()