我要一步一步往上爬，等待阳光静静看着它的脸。小小的天有大大的梦想，重重的壳挂着轻轻的仰望。
—《蜗牛》- 周杰伦
🏰代码及环境配置：请参考 环境配置和代码运行!
基于状态空间的采样运动规划是一种在机器人或自动驾驶系统中常用的技术，用于在复杂的环境中寻找一条从起始状态到目标状态的安全有效路径。这种方法通过将状态空间划分为离散的网格或采样点，并在这些网格或采样点中搜索最佳路径。
3.4.1 基本概念
状态空间：描述机器人或车辆所有可能状态（如位置、速度、姿态等）的集合。在三维空间中，状态空间可以包括位置坐标、速度向量等参数。状态空间是连续的，但为了进行采样和计算，通常需要将其离散化或划分成有限数量的状态点。 采样方法：在状态空间中，采样是指随机或系统性地选择一系列状态点作为代表，以便后续的路径搜索和优化。采样方法可以分为无偏采样和有偏采样两种： 无偏采样：每个状态点被选中的概率是相等的，不依赖于任何先验知识或偏好。无偏采样可以确保对整个状态空间的均匀覆盖，但可能在某些复杂环境中效率较低。 有偏采样：根据特定的偏好或先验知识来选择状态点。例如，可以倾向于在障碍物附近或狭窄通道中进行采样，以提高在这些区域找到可行路径的可能性。有偏采样可以提高采样效率，但也可能引入偏差。 path的生成：基于起始状态和终点状态，使用各类曲线（例如五次多项式等）拟合出一条path。
3.4.2 优缺点
优点 高效性：采样方法可以快速生成大量候选路径，提高搜索效率。 灵活性：适用于各种复杂环境和约束条件。 缺点 采样效率：在某些复杂环境中，可能需要大量的采样点才能找到可行路径。 路径质量：采样方法可能无法保证找到最优路径，尤其是在有偏采样的情况下。 计算复杂度：路径搜索和优化过程可能具有较高的计算复杂度，需要高效的算法和硬件支持。
3.4.3 应用实例
对于自动驾驶车辆而言，其常用的坐标系为Frenet坐标系（在1.5节中已经详细介绍，此处不再赘述），以生成Path为例：具体步骤如下：
在Frenet坐标系下采样一系列的点和点，并得到起始点和终点的。 基于五次多项式拟合曲线： 。
此时即可得到一系列车辆可能行驶的路径，之后再基于设置合适的cost选择最优的一条路径。
3.4.4 拓展
在确定性采样中，无论基于控制空间采样还是基于状态空间采样，其只是提供了一种采样方式，接下来我们还需要设置合适的cost（例如safety cost，smoothness cost等）来评价路径的优劣，最后使用合适的搜索算法（例如DP，A star）等来寻找最优路径。
3.4.c 确定性采样:基于采样空间采样代码解析
本节提供了基于控制空间采样的代码测试，其中代码是以1.1节中的自行车模型为基础进行实现的：
python3 tests/sampling_based_planning/state_based_sampler_test.py
3.4.c.1 基于状态空间采样的代码实现
在tests/sampling_based_planning/state_based_sampler_test.py 中定义了轨迹的生成方式，定义了起始点和终点的信息后，使用五次多项式生成轨迹，其中：
start_p ：轨迹的起点
trajectory_length ：轨迹的纵向长度（沿Frenet坐标系的轴）
max_l ：采样最大的横向
min_l ：采样最小的横向
delta_l ：的分辨率
delta_s：生成轨迹点的分辨率
def generate_trajectories(start_p, trajectory_length, max_l, min_l, delta_l, delta_s):
trajectories = []
for end_l in np.arange(min_l, max_l + delta_l, delta_l):
# Generate path with quintic polynomial.
curve = QuinticPolynomial(
start_p.l, start_p.dl, start_p.ddl, end_l, 0.0, 0.0, trajectory_length
)
points = [start_p]
for s in np.arange(0, trajectory_length + delta_s, delta_s):
new_l = curve.calc_point(s)
new_dl = curve.calc_first_derivative(s)
new_ddl = curve.calc_second_derivative(s)
new_p = FrenetFramePoint(start_p.s + s, new_l, new_dl, new_ddl)
points.append(new_p)
trajectories.append(points)
return trajectories
3.3.c.2 基于状态空间采样的代码测试
在测试中，分为两个阶段，第一个阶段从起始点开始，按照不同的横向位置（）采终点状态，并用五次多项式连接成轨迹（效果图中的洋红色曲线），第二阶段，以第一阶段轨迹的最后一个点为起点，然后继续按照不同的横向位置（）采终点状态，也用五次多项式连接成轨迹（效果图中的灰色曲线）；然后设置不同的cost来评估每一条曲线，最后基于cost选出最优轨迹（为了测试，代码中的cost没有填充，只是随机选择了一条轨迹，效果图中的红色曲线）。
从效果图也可以看出，基于状态空间的采样方式可以生成平滑的轨迹曲线，但不一定满足动力学约束，同时由于可以直接限制终点的状态，因此可以最大限度的保证轨迹在可行驶区域内。
def plot_trajectories(trajectories, color="m"):
for points in trajectories:
plt.plot([p.s for p in points], [p.l for p in points], color=color)
def select_optimal_path(paths, random_select=True):
if random_select:
return paths[np.random.randint(len(paths))]
# 这里可以添加更复杂的评估逻辑来选择最佳路径
return None
def main():
trajectory_length = 10.0
max_l = 3.0
min_l = -3.0
delta_l = 1.0
delta_s = 0.2
start_point = FrenetFramePoint()
# First phase.
fig = plt.figure()
first_phase_trajectories = generate_trajectories(
start_point, trajectory_length, max_l, min_l, delta_l, delta_s
)
plot_trajectories(first_phase_trajectories)
# Second phase.
second_phase_paths = []
for curve in first_phase_trajectories:
second_phase_trajectories = generate_trajectories(
curve[-1], trajectory_length, max_l, min_l, delta_l, delta_s
)
plot_trajectories(second_phase_trajectories, color="grey")
for path in second_phase_trajectories:
# Connect the trajectories of first phase and second phase.
path = curve[:-1] + path
second_phase_paths.append(path)
# Select a optimal path: we can set many cost to evaluate the path, such as:
# 1. Distance cost.
# 2. Speed cost.
# 3. Steer cost.
# 4. Acceleration cost.
# 5. Jerk cost.
# ...
# Here, we randomly select a path from the curve_list.
selected_curve = select_optimal_path(second_phase_paths)
# Translate to the xy point for visualization.
selected_curve_xy = [
Point(x=point.s, y=point.l, theta=point.dl) for point in selected_curve
]
plt.title("StateBasedSampler")
animation_car(
fig,
selected_curve_xy,
save_path=get_gif_path(
pathlib.Path(__file__), str(pathlib.Path(__file__).stem)
),
)
if __name__ == "__main__":
main()
推荐阅读:
🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn
