【技术实现步骤摘要】
本申请涉及自动驾驶的路径规划，具体涉及一种路径规划算法、存储介质和电子设备。
技术介绍
1、自动驾驶的局部路径规划算法是在车辆行驶过程中，根据车辆周围的实时环境信息生成短期行驶路径的关键技术。该算法需要考虑动态障碍物、道路曲率、速度限制等因素，以确保车辆能够安全、平稳地行驶。局部路径规划算法是自动驾驶系统中保证车辆安全、平稳行驶的核心技术之一。ilqr(iterative linear quadratic regulator，迭代线性二次调节器)算法虽然计算速度快，可以处理非线性的车辆动力学约束，但是ilqr只能处理等式约束，对于不等式约束的处理效果不够好；al-ilqr(augmented lagrangianiterative linear quadratic regulator，增广拉格朗日的轨迹规划)算法虽然在ilqr算法的基础上加入了对于不等式约束的处理，但是在求解的末期往往由于极大的惩罚参数会造成病态解，导致行驶轨迹的约束有时候会无法满足。
技术实现思路
1、本专利技术要解决的技术问题是现有技术中的al-ilqr局部路径规划算法在某些情况下生成病态解的情况，进而提供一种路径规划算法、存储介质和电子设备。
2、第一方面，本申请技术方案提供一种路径规划算法，包括：
3、构建车辆行驶过程中的运动学模型，所述运动学模型用于限定相邻两时刻的行驶状态向量的关系；
4、构建增广拉格朗日轨迹规划模型，所述增广拉格朗日轨迹规划模型包括行驶代价函数子模型
5、根据所述运动学模型和所述增广拉格朗日轨迹规划模型得到行驶路径的初始平滑结果；
6、对所述初始平滑结果进行二次平滑处理得到最终的平滑路径。
7、一些方案中所述的路径规划算法，所述对所述初始平滑结果进行二次平滑处理得到最终的平滑路径，包括：
8、将所述初始平滑结果按照步长切分为若干轨迹段；
9、获取车辆行驶过程中的路径约束条件；
10、获取每一步长对应的轨迹段的扰动量；
11、根据所述路径约束条件对每一所述轨迹段的扰动量进行修正；
12、每一修正后的轨迹段重新连接后得到最终的平滑路径。
13、一些方案中所述的路径规划算法，所述获取车辆行驶过程中的路径约束条件，包括：
14、车辆行驶路径的舒适度在设定舒适度范围内；
15、车辆行驶路径的平缓度在设定平缓度范围内且车辆行驶路径对应的控制向量趋于设定下限值；
16、车辆行驶路径与参考基准线的偏移程度小于设定偏移；
17、车辆行驶路径对应的控制向量的变化率在设定变化率范围内；
18、车辆行驶路径与道路障碍物之间的距离大于安全距离。
19、一些方案中所述的路径规划算法，所述构建增广拉格朗日轨迹规划模型，所述增广拉格朗日轨迹规划模型包括行驶代价函数子模型和拉格朗日子模型中：
20、所述增广拉格朗日轨迹优化模型表示为：
21、
22、其中，j表示代价函数，表示行驶状态向量，表示控制向量，xi表示行驶状态序列，ui表示控制序列；λ表示拉格朗日乘法算子，μ为惩罚系数，c(xi)表示约束条件；所述约束条件包括等式约束条件和不等式约束条件。
23、一些方案中所述的路径规划算法，所述构建增广拉格朗日轨迹规划模型，所述增广拉格朗日轨迹规划模型包括行驶代价函数子模型和拉格朗日子模型，包括：
24、构建舒适度代价函数：
25、jcomfort＝0.5wacc(v2κ)2+0.5wjerk(2vaκ+v3κ′)2；
26、构建平缓度代价函数：
27、jshape＝0.5(wκκ2+wκ′κ′2+wuκ″2)；0
28、构建路径偏移代价函数：
29、jref＝0.5wref((x-xref)2+(y-yref)2)2；
30、构建控制向量偏差代价函数：
31、ju＝0.5(wclbc2(κ″min-κ″)+wcubc2(κ″-κ″max))；
32、构建安全距离代价函数：
33、jsafe＝0.5wsafec2(lsafe-l)；
34、构建总代价函数作为行驶代价函数子模型：
35、j＝jcomfort+jshape+jref+ju+jsafe；
36、其中：
37、wacc、wjek、wκ、wκ′、wu、wref、wclb、wcub、wsafe分别表示权重值，v表示车辆行驶速度，κ表示行驶路径曲率，κ′表示行驶路径曲率的一阶导数，κ″表示行驶路径曲率的二阶导数，a表示加速度，xref和yref表示参考基准线上参考轨迹点的坐标，x和y表示车辆行驶路径上轨迹点的坐标，κmin表示行驶路径曲率的最小值，κmax表示行驶路径曲率的最大值，lsafe表示安全距离；
38、c(x)表示取值分段函数，其取值结果为：
39、
40、一些方案中所述的路径规划算法，所述获取每一步长对应的轨迹段的扰动量，包括：
41、获取所述初始平滑结果的总扰动量，其表示为矩阵δz且满足如下关系：
42、
43、其中，z表示所述初始平滑结果中每一轨迹点对应的行驶状态向量和控制向量拼接得到的矩阵，t表示转置，h矩阵是行驶代价的黑塞矩阵，d矩阵是所述初始平滑结果中尚未满足所述路径约束条件约束的雅可比矩阵，d表示所述路径约束条件的具体值；
44、将所述总扰动量对应的所述关系拆分为对应于每一步长的轨迹段的扰动量函数关系：
45、
46、其中，i表示步长编号；
47、所述根据所述路径约束条件对每一所述轨迹段的扰动量进行修正，包括：
48、将每一步长的轨迹段的扰动量函数转换为增广拉格朗日方程，求解所述增广拉格朗日方程的kkt条件的最优解作为所述轨迹段的最优扰动量δzi；
49、所述每一修正后的轨迹段重新连接后得到最终的平滑路径，包括：
50、根据每一所述轨迹段的所述最优扰动量计算得到该轨迹段对应的最优矩阵zi；每一所述轨迹段的最优矩阵zi拼接得到所述平滑路径对应的最优矩阵。
51、一些方案中所述的路径规划算法，所述根据每一所述轨迹段的所述最优扰动量计算得到该轨迹段对应的最优矩阵zi，包括：
52、构建最优矩阵计算模型：
53、zi＝z+αδzi，0≤α≤1；
54、利用线性搜索法确定所述最优矩阵计算模型中的α的最优解。
55、第二方面，本申请技术方案提供一种计算机可读存储介质，其上存储有计算机程序/指令，该计算机程序/指令被处理器执行时实现第一方面任一项所述路径规划算法的步骤。
56、第三方面，本申请技术方案提供一种电子设备，包括存储器本文档来自技高网...
【技术保护点】
1.一种路径规划算法，其特征在于，包括：
2.根据权利要求1所述的路径规划算法，其特征在于，所述对所述初始平滑结果进行二次平滑处理得到最终的平滑路径，包括：
3.根据权利要求2所述的路径规划算法，其特征在于，所述获取车辆行驶过程中的路径约束条件，包括：
4.根据权利要求3所述的路径规划算法，其特征在于，所述构建增广拉格朗日轨迹规划模型，所述增广拉格朗日轨迹规划模型包括行驶代价函数子模型和拉格朗日子模型中：
5.根据权利要求4所述的路径规划算法，其特征在于，所述构建增广拉格朗日轨迹规划模型，所述增广拉格朗日轨迹规划模型包括行驶代价函数子模型和拉格朗日子模型，包括：
6.根据权利要求4或5所述的路径规划算法，其特征在于，所述获取每一步长对应的轨迹段的扰动量，包括：
7.根据权利要求6所述的路径规划算法，其特征在于，所述根据每一所述轨迹段的所述最优扰动量计算得到该轨迹段对应的最优矩阵Zi，包括：
8.一种计算机可读存储介质，其上存储有计算机程序/指令，其特征在于，该计算机程序/指令被处理器执行时实现权利
9.一种电子设备，包括存储器、处理器及存储在存储器上的计算机程序，其特征在于，所述处理器执行所述计算机程序以实现权利要求1-7任一项所述路径规划算法的步骤。
10.一种计算机程序产品，包括计算机程序/指令，其特征在于，该计算机程序/指令被处理器执行时实现权利要求1-7任一项所述路径规划算法的步骤。
...【技术特征摘要】
1.一种路径规划算法，其特征在于，包括：
2.根据权利要求1所述的路径规划算法，其特征在于，所述对所述初始平滑结果进行二次平滑处理得到最终的平滑路径，包括：
3.根据权利要求2所述的路径规划算法，其特征在于，所述获取车辆行驶过程中的路径约束条件，包括：
4.根据权利要求3所述的路径规划算法，其特征在于，所述构建增广拉格朗日轨迹规划模型，所述增广拉格朗日轨迹规划模型包括行驶代价函数子模型和拉格朗日子模型中：
5.根据权利要求4所述的路径规划算法，其特征在于，所述构建增广拉格朗日轨迹规划模型，所述增广拉格朗日轨迹规划模型包括行驶代价函数子模型和拉格朗日子模型，包括：
6.根据权利要求4或5所述的路径规划算法，其特征在...
【专利技术属性】
技术研发人员：廖田志浩，
申请(专利权)人：云控智行科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。