[轨迹规划实操] 横向优化算法+纵向DP算法的python复现（2）

原创已于 2024-06-04 23:30:40 修改 · 1.6k 阅读

24 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#算法 #python #开发语言

于 2024-06-04 19:30:04 首次发布

轨迹规划实操专栏收录该内容

3 篇文章

订阅专栏

文章目录

前言
纵向速度规划

前言

本文采用基于优化的横向规划方法和基于动态规划的纵向规划方法
横向优化控制详见第一篇文章 https://blog.csdn.net/AAAndrea/article/details/139444710

纵向速度规划

本文采用基于动态规划（DP）的速度规划方法。
首先需要了解下动态规划的原理：
在这里插入图片描述

在应用动态规划处理特定的优化问题时，可以将整个问题解决过程依据特定顺序（例如时间、空间或其他逻辑顺序）划分成多个相互依赖的「阶段」。接着，依次对每个阶段进行「决策」，这些决策不仅决定了当前阶段的效益，也影响了下一阶段的起始条件。完成所有阶段的决策后，便形成了针对整个问题的决策序列。

1. 对路程和时间进行采样

首先对路程和时间进行采样。
按照Apollo的做法是对比较近的距离以较小间隔采样；对比较远的距离以较大间隔采样。
但是这个时候会出现

如果采样点过多，之后计算的cost就越多，耗时就越长。所以在选择采样点的时候就需要删选。

maxsA = prev_maxs + prev_maxv * self.delta_t + 0.5 * self.a_max * self.delta_t ** 2
minsA = prev_mins + prev_minv * self.delta_t + 0.5 * self.a_min * self.delta_t ** 2

maxs = min(maxs, maxsA)    # maxs为最大前探距离
mins = max(mins, minsA)      # mins为自车起始S0

如果循环点的s不满足 mins <= s <= maxs,后续不会进行cost 计算，直接给cost = 10000（极大值）。

2. 计算代价函数

代价函数主要分为三个部分:
① 障碍物代价
② 加速度和速度代价
③ 损失代价

1.障碍物代价

障碍物处理的部分比较复杂，所以放在下一章。

本章直接使用处理完的障碍物列表。

在这里插入图片描述

障碍物代价函数的计算公式为: （同Apollo）
在这里插入图片描述

将障碍物绘制在S-T图上可以看到，绿色点构成的是障碍物的超车安全距离；蓝色点构成的是障碍物的跟车安全距离。

在这里插入图片描述

2.加速度和速度代价

计算速度的时候需要运用到"父节点"，因此在这一部分至少需要两个循环。

一个对当前节点cur_node的循环，一个是对当前节点的父节点prev_node进行循环。

在计算加速度的时候，则需要父节点的父节点，可以用父节点的’parent’属性找到pprev_node。

        v = (cur_node['s'] - prev_node['s']) / (cur_node['t'] - prev_node['t'])
        cost = (v - cv) ** 2 / 100
        # 如果前一个节点有父节点
        if prev_node['parent'] is not None:
            # 获取前一个节点所在层的上一层的所有节点
            pprev_nodes = st_node_tab[lvlnum - 1]
            # 根据前一个节点的父节点索引获取前一个节点
            pprev_node = pprev_nodes[prev_node['parent']]
            # 计算前一个节点与前前一个节点之间的速度
            prev_v = (prev_node['s'] - pprev_node['s']) / self.delta_t
            a = (v - prev_v) / (cur_node['t'] - prev_node['t'])
            cost += math.fabs(a) ** 2 / 100

3.损失代价
损失代价的设置目的是为了自车能够更快地达到最大前探距离（目的地），避免过于保守的驾驶行为。
在这里插入图片描述