一种基于数值优化的自动驾驶路径规划方法

本发明设计自动驾驶路径规划领域，设计一种基于数值优化的自动驾驶路径规划方法，在该规划方法下，驾驶车可以生成一条平滑的躲避障碍物的路线，进而实现自动驾驶。

背景技术：

1、自动驾驶技术的发展始于20世纪中期，但真正的突破出现在21世纪初。最早的自动驾驶汽车原型多由研究机构和大学开发，主要依赖于规则驱动的系统和简单的传感器技术。随着谷歌、特斯拉、uber等科技巨头的加入，自动驾驶技术得到了迅速发展。自动驾驶被认为能够提高交通安全性、减少交通事故、优化交通流量、降低能源消耗和环境污染，同时也可能改变未来的城市规划和生活方式。

2、自动驾驶技术相对于人类驾驶员来书有诸多优势。首先，自动驾驶汽车能够高度高效地感知周围环境，迅速获取行驶路线所需的信息，这种感知效率优于人类。其次，自动驾驶汽车能够根据预测的交通状况合理规划行驶路线，并在行驶中选择最佳路径，从而有效避开拥堵和繁忙路段。最后，自动驾驶汽车还能够预测周围车辆的行驶方向，并通过调整自身的行驶轨迹，提前避免潜在的风险和危险。

3、近年来，对于车辆路径规划的研究主要有基于搜索的方法、基于采样的方法、基于数值优化的路径方法。其中，基于搜索的局部路径规划方法是把规划问题向图搜索问题进行转化，对求解空间进行网格划分，建立成图，然后进行起始点到目标点的最优路径的搜索求解。基于搜索的规划方法通过不断迭代搜索，可以得到全局层面的最优解，但其搜索效率与规划问题状态维数相关，当维数较高时，搜索过程比较耗时，因此只适用于较低维数的规划问题的求解。基于采样的路径规划方法更适用于高维空间中的路径搜索问题。该方法通过在搜索空间中随机或规则地采样生成候选路径，然后在这些候选路径中选择一条最优路径来引导车辆从起点到达终点。但是，随着采样速度分辨率的提高，其求解时间将会大幅度增加，从而需要更强大的算力。在变换场景时，代价函数选择不合理也将会导致轨迹规划的稳定性较差。对于自动驾驶汽车来说，规划路径足够平滑对于跟踪控制至关重要，而通过搜索或者采样方法得到的结果很难满足车辆运动学和动力学要求。基于数值优化的规划方法能通过多项式曲线来表示规划结果，即将路径规划问题转化成曲线参数的求解问题，在这个过程中，可以添加车辆行驶规划中所需要的约束条件，并最终将路径规划问题抽象成一个有约束优化问题。基于该方法的算法目前得到了学术界和工业界的认可和深度的研究。

4、因此，本发明设计了一种基于数值优化的路径规划方法，通过构建一系列路径点，将环境中的所有避障约束转化为求代价函数最小值的约束，消除最优控制问题规模与环境复杂程度之间的相关性，通过凸多边形覆盖采样点可行域，减少冗余区域，提高空间利用率，有效简化自动驾驶汽车的避障约束，简化最优控制问题数学模型，提高规划路线的求解速度。

技术实现思路

1、本发明主要目的在于设计了一种基于数值优化的自动驾驶路径规划方法，以克服现有技术中的不足，为自动驾驶车辆提供更安全、更舒适、更平滑的驾驶轨迹，并解决了其他自动驾驶方法存在的不满足车辆动力学模型约束和车辆运动学模型约束的问题。具体实现步骤如下：

2、步骤一，计算车辆在高精度地图中以全局路径规划生成的路线作为待平滑的参考线，离散参考线上的每一个点得到一系列离散点，然后对相邻的离散点进行平滑处理得到平滑后的参考线；

3、步骤二，根据当前车辆的位置在cartesian(笛卡尔)坐标系下的坐标信息和所述步骤一中参考线上的离散点在cartesian坐标系下的信息，找到与车辆位置相邻最近的离散点记为匹配点，根据匹配点在cartesian坐标系下信息，找到自车位置在frenet坐标系中的理想投影点；

4、步骤三，根据所述步骤二中理想投影点的信息，计算车辆在frenet坐标系下的坐标信息。同理，获取障碍物在cartesian坐标系的坐标信息，并计算出障碍物在frenet坐标下的坐标信息；

5、步骤四，离散的在将要行驶的路径上采样一些点，进行动态规划，正向寻优，逆向求解；正向计算从规划起点到每一纵向采样间隔的最小代价，同时记录该结点的前一个结点的横向位置信息，假如从起点到第j个纵向采样间隔的最小代价为a，从第j个到j+1个的最小代价为b，则从起点到j+1个的最小代价为a+b，以此类推，直到终点；从终点逆向寻找它的每一个父节点从而得到最短路径，形成一个可行解区域记为凸空间；

6、步骤五，根据动态规划开辟的凸空间，二次规划在凸空间中找到最优解，以生成车辆在frenet坐标下的最优路径；

7、步骤六，对路径规划得到得路径点作进一步的速度方面的动态规划和二次规划。最终生成一条安全、平滑、舒适的避障路线；

8、步骤七、将避障路线上的坐标点从frenet坐标系转换为cartesian坐标系，发送到控制模块进行执行。

9、进一步，所述步骤一中，还包括：

10、其一，不妨将参考线上相邻的三个离散点按纵向顺序分别记为p1r、p2r和p3r，对应的平滑后的三个点分别记为p1、p2和p3，若p1r、p2r和p3r三个点不共线，则必然可以支成一个平行四边形，记平行四边形的对角线的模长为|p2rp0|，该模长越小，则参考线越平滑；其二，点p1r到点p1的模长|p1rp1|加上点p2r到点p2的模长|p2rp2|再加上点p3r到点p3的模长|p3rp3|的总和越小，则平滑后的路径越接近原几何路径；其三，对于平滑后的点，p1点到p2点的模长的平方|p1p2|2加上p2点到p3点的模长的平方|p2p3|2越小，则平滑后的点越紧凑越均匀。总的平滑的代价函数公式为：

11、

12、

13、其中，xi为对应的未平滑的点xir平滑后的横坐标，yi为对应的未平滑的点yir平滑后的纵坐标，此坐标是在cartesian坐标系下的坐标，w1为平滑后的参考线上的点与原路径点的相似代价，w2为平滑代价，w3为紧凑代价。

14、上述代价函数最终可以化简成二次规划的函数形式，最终的化简公式如下：

15、

16、其中，a1为相似代价的系数矩阵，a2为平滑代价的系数矩阵，a3为紧凑代价的系数矩阵，该系统矩阵都是己知的；最终代价函数可以简化成二次规划的标准形式，

17、

18、lb≤x≤ub

19、利用二次规划求解器找到使代价函数最小值的最优解x。

20、进一步，所述步骤二中，还包括：

21、从所述参考线上的起始点开始遍历参考线上的每一个离散点，根据所述参考线上的每一个离散点到车辆当前位置的距离，从所述参考线上的离散点中找到距离车辆当前位置距离最小的点记为最小匹配点pm，从cartesian坐标系原点到pm的位矢记为从点pm到车辆当前位置的位矢记为点pm处的方向向量记为则当前车辆在cartesian坐标系下的理想投影点的位矢为：

22、

23、根据最小匹配点pm处的曲率km和倾斜角θm，θm为该点的切线方向与cartesian坐标系下x轴的夹角，可以计算出理想投影点处的曲率kr和倾斜角θr，当离散点之间的距离不太大时，可近似认为km等于kr，理想投影点的倾斜角θr的计算公式为：

24、

25、至此，cartesian坐标系下代表车辆位置的点在参考线上的的投影点的信息：横坐标、纵坐标、倾斜角和曲率均己求得，障碍物在参考线上的理想投影点的计算公式同理。

26、进一步，所述步骤三中，还包括：

27、根据所述步骤二中理想投影点的信息，计算车辆在frenet坐标系下的坐标信息。记车辆在frenet坐标系下的纵坐标为s，车辆在frenet坐标系下的纵坐标对时间的一阶导数为车辆在frenet坐标系下的纵坐标对时间的二阶导数为车辆在frenet坐标系下的横坐标为l，车辆在frenet坐标下的横坐标对弧长ds的导数为l′，车辆在frenet坐标下的横坐标对弧长ds的二阶导数为l″，车辆在frenet坐标下的横坐标对时间的导数为车辆在frenet坐标下的横坐标对时间的二阶导数为其计算公式分别如下：

28、s＝sr

29、

30、其中，sr为车辆在参考线上的理想投影点到规划起点的距离；为车辆在cartesian坐标系下的位置相对于坐标原点的位矢；为车辆在cartesian坐标系下的速度向量；为车辆在cartesian坐标系下的加速度向量；为投影位矢；kr为投影位矢相对于cartesian坐标系下在道路几何上的曲率；为投影位矢相对于cartesian坐标系下在道路几何上的切线向量；为投影位矢相对于cartesian坐标系下在道路几何上的法线向量；障碍物在cartesian坐标系下的坐标与frenet坐标系下的坐标转换与之同理。

31、根据静态障碍物在frenet坐标下的纵坐标s和横坐标l，建立静态障碍物的s-l图；根据动态障碍物在frenet坐标系下的纵坐标s和横坐标l以及纵坐标对时间的导数和横坐标对时间的导数建立动态障碍物的s-t图。

32、进一步，所述步骤四中，还包括：

33、对于相邻的采样点之间的路径使用五次多项式生成的轨迹连接，在frenet坐标系下的五次多项式公式如下：

34、l＝a0+a1s+a2s2+a3s3+a4s4+a5s5

35、l′＝a1+2a2s+3a3s2+4a4s3+5a5s4

36、l″＝2a2+6a3s+12a4s2+20a5s3

37、以frenet坐标轴的纵向距离s作为五次多项式的自变量，当相邻两结点的横向状态，如l、l′、l″均己知时，可以求得五次多项式的系数矩阵，当系数矩阵求得时，则相邻两结点随着纵向s连续变化的横向状态也均可求得，即两个相邻结点的连接轨迹，假设位于start_s处的横向状态分别为start_l、start_dl、startddl，位于end_s处的横向状态分别为end_l、end_dl、end_ddl，则五次多项式的自变量矩阵a和己知的状态矩阵b分别为：

38、

39、b＝[start_l，start_dl，start_ddl，end_l，end_dl，end_ddl]t

40、令待求解的系数矩阵为coff，则coff的形式如下：

41、coff＝[a0，a1，a2，a3，a4，a5]t

42、则五次多项式的矩阵表达形式为：

43、b＝a×coff

44、根据车辆在frenet坐标系中的运动初始状态和运动结束状态，即b矩阵和a矩阵是己知的，可求得五次多项式的系数矩阵，根据完备的五次多项式，可以求得随着纵向采样频率的变化，对应的横向状态end_l、end_dl、end_ddl。

45、当相邻结点的轨迹己知时，需要评价该轨迹的代价，响应的代价函数分别有轨迹平滑代价、障碍物碰撞代价和参考线的距离代价，分别记为costsmooth、costcollision和costref，其计算公式分别如下：

46、costsmooth＝wdl·(l′t·l′)+wddl·(l″t·l″)+wdddl·(l″′t·l″′)

47、

48、costref＝w_ref·(lt·l)

49、其中，l′为frenet坐标系下的横坐标对纵坐标的一阶导数，l″为frenet坐标系下的横坐标对纵坐标的二阶导数，l″′为frenet坐标系下的横坐标对纵坐标的三阶导数，wdl、wddl和wdddl分别为其对应的权重系数；minl为车辆到障碍物的距离，wcollision为碰撞代价权重系数；l为frenet坐标系下的横坐标，w_ref为与参考线距离的代价函数权重系数。总的代价函数为三者之和，即：

50、cost＝costsmooth+costcollison+costref

51、经过相邻结点之间的代价函数的计算，得到相邻结点之间的代价，随后从采样起点到采样终点进行动态规划。

52、己知各结点之间的代价，找到一条从采样起点到采样终点代价之和的最小的路径，作为动态规划的结果。动态规划的步骤如下：

53、首先，遍历第一列结点，找到一条从起点到第一列结点的最短代价路径，并记录最短路径上当前结点的父结点；其次，外层循环遍历第二列结点，内层循环遍历前一列结点，找到从起点到第二列的最短代价路径，并记录当前结点的父结点；依次类推，直到最后遍历到最后一列结点，找到从起点到最后一列结点的最短代价路径，并依次索引父结点，找到中间路径结点。

54、相邻结点之间的轨迹用五次多项式连接，动态规划的结果对于相邻结点而言有一个直线连接，但是对于整体的轨迹而言并不平滑。因此，动态规划的结果仅能作为最优轨迹的一个粗解，最优轨迹需要在动态规划的基础上进行二次规划得到。

55、进一步，所述步骤五中，还包括：

56、二次规划对动态规划得到的l、l′、l″进行进一步优化，其主要的代价函数如下：

57、

58、其中，wref为车辆所在的frenet坐标系下横坐标位置到参考线距离的代价权重，wdl为车辆所在的frenet坐标系下横坐标的一阶导数的代价权重，wddl为车辆所在的frenet坐标系下横坐标的二阶导数的代价权重，wdddl为车辆所在的frenet坐标系下横坐标的三阶导数的代价权重，wmid为车辆所在凸空间中间的代价权重。代价函数的值越小，说明车辆的轨迹越平滑，且车辆位置越靠近凸空间中央。

59、二次规划的代价函数最终可转化为二次规划的标准形式，其二次规划的标准形式如下：

60、xthx+ftx

61、其中，需要满足三种约束，分别是二阶连续性约束、碰撞约束和规划起点约束，函数形式分别如下：

62、aeqx＝beq

63、ax≤b

64、lb≤x≤ub

65、aeq为二阶连续性约束矩阵，a为碰撞约束矩阵，lb和ub为凸空间的边界范围，最后求解的x矩阵为：

66、x＝[l1，l′1，l″1，...，ln，l′n，l″n]t

67、x矩阵即为二次规划优化后的车辆在frenet坐标系下的横坐标，横坐标对纵坐标的一阶导数以及横坐标对纵坐标的二阶导数。

68、进一步，所述步骤六中，还包括：

69、获取从路径规划模块己经计算好的参考路径，这条路径是车辆在当前环境下应当行驶的理想路线。将参考路径按照一定的间隔进行采样，以生成多个离散的路径点，这些路径点将用于速度规划。

70、计算障碍物的s-t图(时空图)，时空图是一种以时间(t)为横轴，距离(s)为纵轴的二维图表，图中的每个点表示在某一时间点上，车辆可以处于某一距离位置。将环境中的静态和动态障碍物投影到时空图上，静态障碍物通常在某一个位置上形成一条竖直线，而动态障碍物则根据其具有的速度在时空图上形成一个平行四边形区域。

71、设定目标路径点速度，根据参考路径以及交通规则(例如限速等)和障碍物的位置和速度，动态调整目标速度曲线。例如，如果前方有车辆减速，则需要调整本车的速度曲线以避免相撞。

72、设计代价函数，用于评估每一条可能的速度曲线，其代价函数主要包含车辆与障碍物的距离代价、加速度代价、推荐速度代价和舒适度代价。通过动态规划从所有可能的速度曲线中选择一条代价成本最低的一条作为最优速度曲线。其动态规划流程与步骤四相似。

73、通过二次规划对初步生成的速度曲线进行平滑处理，以减少突变或不连续的速度变化。在平滑过程中，确保曲线满足一定的约束条件，如加速度、加加速度不能超出车辆的物理极限。

74、进一步，所述步骤七中，还包括：

75、经过速度规划找到平滑轨迹的最优解，此时己经得到frenet坐标系下的行驶轨迹点的横坐标l和纵坐标s的相关信息，要想发送到控制模块控制执行规划的轨迹，还需将frenet坐标系下的轨迹点转化到cartesian坐标系，其转换公式如下：

76、xx＝xr-l sinθr

77、yx＝yr-l cosθr

78、

79、其中，xx和yx为从frenet坐标系转换到cartesian坐标系下的横坐标和纵坐标；θx为车辆在cartesian坐标系下的速度方向与x轴的夹角；vx为车辆在cartesian坐标系下的线速度；ax为车辆在cartesian坐标系下的加速度；kx为车辆在cartesian坐标系下的曲率；θr为frenet坐标系下理想投影点的切线向量与cartesian坐标轴的x轴的夹角。路径规划的结果是一系列离散点，每个点具有以上所述的信息，这一系列离散点组成的路径即为路径规划得到的理想路线。

80、最终，将速度曲线转化为控制命令，传递给车辆的执行模块，控制车辆的油门、刹车等。随着车辆的移动和环境的变化，速度规划也需要不断更新，以应对新出现的障碍物或变化的道路状况。

81、通过这些步骤，可以为自动驾驶车辆生成一条安全、平稳且高效的规划曲线，确保车辆在各种环境下能够正常行驶。