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

---

前言

横向轨迹规划:
[轨迹规划实操] 横向优化算法+纵向DP算法的python复现（1）
纵向速度规划:
[轨迹规划实操] 横向优化算法+纵向DP算法的python复现（2）
本章主要详解对障碍物的处理

---

一、在纵向速度规划时利用横向信息

处理纵向轨迹:
首先是 起始轨迹点的位置
本文主要分为三大类:

refleft : 表示车辆刚开始的时候就位于车道线的左侧
ref : 表示车辆刚开始的时候就位于车道线附近
refright : 表示车辆刚开始的时候就位于车道线的右侧

self.first_tag 只有 横向轨迹的第一个值（车辆的起始点）决定

二、临界情况判断

利用 self.check_distance 进行临界情况判断 。

但是这种方式就会出现 如果参考线并不是直线 也就是说 参考线自行换道的情况，就不能采用固定值进行判断。

（临界情况的判断本身就是为了确定障碍物的持续时间，假设参考线自行换道，但是自车轨迹一直在参考线周围，那么self.tag 一直不会超过self.check_distance 那么当自车起始位置位于ref 的时候，自车一直只会考虑当前车道的障碍物，但是实际情况是自车在变道，必须考虑当前车道和其他车道的障碍物）

需要将判断条件 self.check_distance 改成 self.check_angle.
self.check_angle 定义为 根据横向轨迹计算出的航向角与参考线的航向角的差值，以此来判断参考线的走向和轨迹的偏移量。
通过临界点 self.edge_time来计算障碍物的延迟时间，具体如下图所示:

确定完障碍物的延迟时间，就可以在S-T图上绘制障碍物的占用情况，从而计算代价函数，利用动态规划进行速度规划。