基于分段加加速度路径优化的两阶段路径规划方法及装置

本申请涉及一种基于分段加加速度路径优化的两阶段路径规划方法及装置。所述方法包括：生成第一阶段的初始路径；在初始路径的各个路径点上进行泰勒展开和曲率估计，得到第二阶段路径优化的曲率估计结果；构建第一阶段路径优化问题的目标函数，在所述第一阶段路径优化问题的目标函数的基础上将第二阶段路径优化的曲率估计结果进行二次型展开，将考虑曲率代价的二次代价矩阵和一次代价向量作为曲率代价加入到第一阶段路径优化问题的目标函数中，得到第二阶段的新优化目标函数，利用二次规划优化器对所述第二阶段的新优化目标函数进行优化求解，得到路径优化解。采用本方法能够生成平滑路径。