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

---

前言

本文采用基于优化的横向规划方法，具体流程如下:

1. 根据障碍物信息，确定横向轨迹的边界条件；
2. 根据车辆运动学设置约束条件；
3. 定义目标函数；
4. 用 ipopt 求解优化问题。

本文采用动态规划(DP)进行纵向规划，具体流程如下:

1. 处理障碍物信息；
2. 计算出障碍物在ST图上占据的空间；
3. 计算自车的纵向边界信息，生成图节点；
4. 分别计算层节点的cost值；
5. 从末层cost最小值的节点开始根据"parent"值进行回溯；
6. 通过回溯得到的每一个时刻的s（路程），计算出v（速度）和a（加速度）。

---

一、横向优化算法

1. 横向轨迹的边界计算（基于SL图）

代码模块 LateralTrajectoryCalculator.calcu_choosed_bound()
简单地说，边界的生成主要与道路边界和障碍物有关:（见图1中的红线部分）
需要满足以下条件:
① 上下边界不超过道路边界
② 如果出现障碍物在参考线上面部分，则上边界修改为障碍物边界；
同理，如果障碍物出现在参考线下面部分，则下边界修改为障碍物边界；

上述定义方式只适合在极少障碍物的情况下，如果出现连续障碍物或者障碍物出现在参考线上，这种简单的设置方式就不是非常合理了。

因此本文在设置障碍物边界之前，定义一种障碍物边界决策方案。
通过生成采样点，计算采样点的势能值，根据势能值小的采样点的分布（以参考线为中心线）判断绕行障碍物的方向，从而缩小横向约束边界。

        # 生成采样点
        Sample = GenSampling(self.currentlanewidth,self.leftlanewidth,self.rightlanewidth, self.leftlanenum, self.rightlanenum, self.num_samples, self.backdistance, self.frontdistance, self.reference_list)
        # 随机采样
        #sampled_points = GenSampling.random_sampling(self)
        # 均匀采样
        # sampled_points = Sample.uniform_sampling()
        #极简采样
        sampled_points = Sample.quick_sampling()
        print("Sample Finished")
        print("Time taken for sampling: {:.4f} seconds".format(time.time() - start_time))

        start_time = time.time()  # 记录开始时间
        # 计算势能值
        Potential = CalPotential(self.ds, self.oblist, self.obstacle_length, self.obstacle_width,self.currentlanewidth,self.leftlanewidth,self.rightlanewidth, self.leftlanenum, self.rightlanenum,self.vehicle_width,
                     sampled_points, self.vehicle_length,self.vehicle_speed,self.obstacle_speed,self.reference_list,self.obstacle_heading,self.vehicle_heading)
        points_potential = Potential.sum_potential()
        print(points_potential)
        print("Calculate Potential Finished")

        # 每一个s值 保留potential最小的5个点 根据点的分布情况 确定行驶区域
        choose_points,x_keep = Potential.process_coordinates_and_potential(sampled_points,points_potential)
        print(choose_points)

在进行完障碍物边界决策后，每一个障碍物都会对应一个’TAG’.
如果出现 TAG==‘left’ ，就意味着对于这一个障碍物，自车采取从左边进行绕行，也就是意味着要修改下边界条件。
同理，如果出现 TAG ==‘right’，就意味着自车采取从右边绕行该障碍物的方式，需要修改上边界条件。
如果出现势能值小的采样点在参考线左右分布相同，本文结合国内的驾驶习惯，从左边进行超车，因而修改下边界条件。

2.根据车辆运动学设置约束条件

如果不设置车辆运动学约束就会出现轨迹曲率突变等情况。
因此在优化求解之前，加入车辆运动学的约束条件:

            for i in range(self.length - 1):   # 车辆运动学   
                opti.subject_to(l[i+1] - l[i] - dl[i] * self.ds - 1/3 * ddl[i] * self.ds**2 -
                                 1/6 * ddl[i+1] * self.ds **2 == 0)
                opti.subject_to(dl[i+1] - dl[i] - 1/2 * ddl[i] * self.ds - 1/2 * ddl[i+1] * self.ds == 0)
                opti.subject_to(ddl[i+1] - ddl[i] <= 0.5 * self.ds)    # 正常 0.4-1.0
                opti.subject_to(ddl[i+1] - ddl[i] >= -0.5 * self.ds)

3.定义目标函数

本文一开始选择采用横向偏移量最小作为目标函数。
进行测试时，发现自车在避障障碍物的时候先会往左拐，再往右避障障碍物。
经过分析，可能是由于目标函数的设置只考虑了横向偏移量，自车会通过先往左再往右这种方式来减小整体偏移量，但这并不符合实际情况。
因此，本文在横向偏移量最小的前提下考虑了航向角的选取，需要接近参考线的航向。

在适当调整目标函数的权重后，就可以比较理想的横向轨迹了。