piecewise jerk path optimizer Apollo二次规划算法解析 _二次规划算法

Apollo里面最重要的模块就是感知和规划模块，规划模块最重要的是路径规划和速度规划任务，对应ROS机器人里面的局部规划。Apollo的规划模块首先根据当前的情况进行多场景（scenario）调度，每个场景又包含一个或者多个阶段（stage），每个阶段又由多个具有独立功能的小模块任务（task）完成。这篇文章就主要解读一下路径规划piecewise jerk path optimizer这个任务。任务最终会生成轨迹（trajectory）：每个点的位姿和速度信息，进而输出给控制模块去控制车辆。
Content:

任务代码调用入口
二次规划定义
Apollo 中二次规划问题的构造
Apollo代码的实现
Python代码的实现
参考文献

任务代码调用入口【piecewise jerk path optimizer Apollo二次规划算法解析】比如通过测试用例可得到如下的堆栈：

apollo::planning::PiecewiseJerkPathOptimizer::Process(apollo::planning::PiecewiseJerkPathOptimizer * const this, const apollo::planning::SpeedData & speed_data, const apollo::planning::ReferenceLine & reference_line, const apollo::common::TrajectoryPoint & init_point, const bool path_reusable, apollo::planning::PathData * const final_path_data) (piecewise_jerk_path_optimizer.cc:61)apollo::planning::PathOptimizer::Execute(apollo::planning::PathOptimizer * const this, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * const reference_line_info) (path_optimizer.cc:45)apollo::planning::scenario::lane_follow::LaneFollowStage::PlanOnReferenceLine(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * reference_line_info) (lane_follow_stage.cc:167)apollo::planning::scenario::lane_follow::LaneFollowStage::Process(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame) (lane_follow_stage.cc:116)apollo::planning::scenario::Scenario::Process(apollo::planning::scenario::Scenario * const this, const apollo::common::TrajectoryPoint & planning_init_point, apollo::planning::Frame * frame) (scenario.cc:76)apollo::planning::PublicRoadPlanner::Plan(apollo::planning::PublicRoadPlanner * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ADCTrajectory * ptr_computed_trajectory) (public_road_planner.cc:38)apollo::planning::OnLanePlanning::Plan(apollo::planning::OnLanePlanning * const this, const double current_time_stamp, const std::vector<apollo::common::TrajectoryPoint, std::allocator<apollo::common::TrajectoryPoint> > & stitching_trajectory, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:572)apollo::planning::OnLanePlanning::RunOnce(apollo::planning::OnLanePlanning * const this, const apollo::planning::LocalView & local_view, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:417)apollo::planning::PlanningTestBase::RunPlanning(apollo::planning::PlanningTestBase * const this, const std::__cxx11::string & test_case_name, int case_num, bool no_trajectory_point) (planning_test_base.cc:229)apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test::TestBody(apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test * const this) (sunnyvale_big_loop_test.cc:191)

二次规划定义二次规划问题的定义如下：

piecewise jerk path optimizer Apollo二次规划算法解析

文章插图

Apollo使用osqp进行二次规划问题的求解。第一行为代价函数，第二行为约束条件。二次优化的目标就是在满足约束条件的基础上，找到优化变量使得代价函数的值最小。二次规划只在代价函数为凸函数的时候能够收敛到最优解，因此这需要P矩阵为半正定矩阵，这是非常重要的一个条件。这反映在Apollo中的规划算法则为需要进行求解的空间为凸空间，这样二次规划才能收敛到一条最优路径。
Apollo 中二次规划问题的构造