piecewise

本文链接地址:阿波罗二次规划算法解析——慢回味

Apollo中最重要的模块是感知和规划模块，最重要的模块是路径规划和速度规划任务，对应的是ROS机器人中的局部规划。Apollo的规划模块首先根据当前情况调度多个场景，每个场景包含一个或多个阶段，每个阶段由几个功能独立的小模块任务完成。路径规划分段加加速度优化器的任务前面已经介绍过了，本文将解释速度规划分段加加速度优化器的任务。SL规划保证车辆的侧向偏差足够平滑，ST规划保证车辆前进方向的变速足够平滑。

内容:

Apollo 中二次规划问题的构造Apollo代码的实现Apollo 中二次规划问题的构造

首先，列出成本函数:

Apollo代码的实现

同上，只是P和q的构造不同:
P的构造:
见式8。

void piecewisejerkspeedplay::calculate kernel(STD::vector & lt；c _ float & gt* P_data，STD::vector & lt；c _ int & gt* P _ indices，STD::vector & lt；c _ int & gt* P _ indptr){ const int n = static _ cast & lt；int & gt(结的数目)；const int kNumParam = 3 * n；const int knum value = 4 * n-1；STD::vector & lt；STD::vector & lt；STD::pair & lt；c_int，c _ float & gt& gt& gt列；columns . resize(knum param)；int value _ index = 0；//x (I) 2 * w _ x _ ref见公式5 for(int I = 0；我& ltn-1；++i) {列[i]。launte _ back(I，weight _ x _ ref _/(scale _ factor _[0]* scale _ factor _[0])；++ value _ index；} // x(n-1)^2 * (w_x_ref + w_end_x)列[n – 1]。launte _ back(n-1，(weight _ x _ ref _+weight _ end _ state _[0])/(scale _ factor _[0]* scale _ factor _[0])；++ value _ index；//x(I)& # 39；2 * (w _ dx _ ref+penalty _ dx)见公式6 for(int I = 0；我& ltn-1；++i) {列[n + i]。launte _ back(n+I，(weight _ dx _ ref _+penalty _ dx _[I])/(scale _ factor _[1]* scale _ factor _[1])；++ value _ index；}//x(n-1)& # 39；^2 *(w _ dx _ ref+penalty _ dx+w _ end _ dx)列[2 * n – 1]。launte _ back(2 * n-1，(weight _ dx _ ref _+penalty _ dx _[n-1]+weight _ end _ state _[1])/(scale _ factor _[1]* scale _ factor _[1])；++ value _ index；auto delta _ s _ square = delta _ s _ * delta _ s _；//x(I)& # 39；'^2 * (w_ddx + 2 * w_dddx / delta_s^2)列[2 * n]。launte _ back(2 * n，(weight _ ddx _+weight _ dddx _/delta _ s _ square)/(scale _ factor _[2]* scale _ factor _[2])；++ value _ index；for(int I = 1；我& ltn-1；++i) {列[2 * n + i]。launte _ back(2 * n+I，(weight _ ddx _+2.0 * weight _ dddx _/delta _ s _ square)/(scale _ factor _[2]* scale _ factor _[2])；++ value _ index；}列[3 * n – 1]。launte _ back(3 * n-1，(weight _ ddx _+weight _ dddx _/delta _ s _ square+weight _ end _ state _[2])/(scale _ factor _[2]* scale _ factor _[2])；++ value _ index；//-2 * w _ dddx/delta_s^2 * x(I)& # 39；'* x(I+1)& # 39；'对于(int i = 0，见等式7；我& ltn-1；++i) {列[2 * n + i]。launte _ back(2 * n+I+1，-2.0 * weight _ dddx _/delta _ s _ square/(scale _ factor _[2]* scale _ factor _[2])；++ value _ index；} CHECK_EQ(value_index，knum value)；int ind _ p = 0；for(int I = 0；我& ltkNumParam++ I){ P _ indptr-& gt；push _ back(ind _ p)；for(const auto & row _ data _ pair:columns[I]){ P _ data-& gt；push _ back(row _ data _ pair . second * 2.0)；p _ indexes-& gt；push _ back(row _ data _ pair . first)；++ ind _ p；} } P _ indptr-& gt；push _ back(ind _ p)；}

问:
见等式9。

void piecewisejerkspeedproject::calculate offset(STD::vector & lt；c _ float & gt* q){ CHECK _ not null(q)；const int n = static _ cast & ltint & gt(结的数目)；const int kNumParam = 3 * n；q->；调整大小(knum param)；for(int I = 0；我& ltn；++ I){ if(has _ x _ ref _){ q-& gt；at(I)+=-2.0 * weight _ x _ ref _ * x _ ref _[I]/scale _ factor _[0]；} if(has _ dx _ ref _){ q-& gt；at(n+I)+=-2.0 * weight _ dx _ ref _ * dx _ ref _/scale _ factor _[1]；} } if(has _ end _ state _ ref _){ q-& gt；at(n-1)+=-2.0 * weight _ end _ state _[0]* end _ state _ ref _[0]/scale _ factor _ 0；q->；at(2 * n-1)+=-2.0 * weight _ end _ state _[1]* end _ state _ ref _[1]/scale _ factor _ 1；q->；at(3 * n-1)+=-2.0 * weight _ end _ state _[2]* end _ state _ ref _[2]/scale _ factor _ 2；}}