文章目錄

• 前言
• 1. 路徑規(guī)劃算法總體介紹
• • 1.1 Task： LANE_CHANGE_DECIDER
  • 1.2 Task： PATH_REUSE_DECIDER
  • 1.3 Task： PATH_BORROW_DECIDER
  • 1.4 Task： PATH_BOUNDS_DECIDER
  • 1.5 Task： PIECEWISE_JERK_PATH_OPTIMIZER
  • 1.6 Task： PATH_ASSESSMENT_DECIDER
  • 1.7 Task： PATH_DECIDER
• 2. 基于二次規(guī)劃的路徑規(guī)劃算法
• • 2.1 二次規(guī)劃問(wèn)題標(biāo)準(zhǔn)型
  • 2.2 定義優(yōu)化變量
  • 2.3 設(shè)計(jì)目標(biāo)函數(shù)
  • 2.4 設(shè)計(jì)約束
  • 2.5 求解器求解
  • • 2.5.1 設(shè)定OSQP求解參數(shù)
    • 2.5.2 計(jì)算QP系數(shù)矩陣
    • 2.5.3 構(gòu)造OSQP求解器
    • 2.5.4 獲取優(yōu)化結(jié)果
• 3. 路徑規(guī)劃算法代碼解讀
• • 3.1 總流程和決策\(yùn)優(yōu)化的入口函數(shù)
  • 3.2 產(chǎn)生換道決策
  • 3.3 產(chǎn)生借道決策
  • 3.4 產(chǎn)生路徑邊界
  • 3.5 路徑優(yōu)化
  • 3.6 選擇最優(yōu)路徑
• 4. 路徑規(guī)劃算法實(shí)踐
• • 4.1 觀察借道繞行、自車道繞行障礙物、自車道巡航、換道時(shí)路徑的邊界和規(guī)劃的路徑
  • 4.2 借道繞行場(chǎng)景，調(diào)整$l,l’,l’’$的權(quán)重系數(shù)，觀察路徑的變化
  • 4.3 靠邊停車實(shí)踐，開(kāi)啟靠邊停車功能，觀察路徑的變化

前言

Apollo星火計(jì)劃課程鏈接如下
星火計(jì)劃2.0基礎(chǔ)課：https://apollo.baidu.com/community/online-course/2
星火計(jì)劃2.0專項(xiàng)課：https://apollo.baidu.com/community/online-course/12

1. 路徑規(guī)劃算法總體介紹

????Apollo中對(duì)路徑規(guī)劃解耦，分為路徑規(guī)劃與速度規(guī)劃兩部分。并將規(guī)劃分為決策與優(yōu)化兩個(gè)部分。
? 路徑規(guī)劃 —— 靜態(tài)環(huán)境（道路，靜止/低速障礙物）
? 速度規(guī)劃 —— 動(dòng)態(tài)環(huán)境（中/高速障礙物）

????路徑規(guī)劃的配置文件在lane_follow_config.pb.txt中

// /home/yuan/apollo-edu/modules/planning/conf/scenario/lane_follow_config.pb.txtscenario_type: LANE_FOLLOW stage_type: LANE_FOLLOW_DEFAULT_STAGE stage_config: { //路徑規(guī)劃stage_type: LANE_FOLLOW_DEFAULT_STAGEenabled: truetask_type: LANE_CHANGE_DECIDERtask_type: PATH_REUSE_DECIDERtask_type: PATH_LANE_BORROW_DECIDERtask_type: PATH_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_PATH_OPTIMIZER//速度規(guī)劃 task_type: PATH_ASSESSMENT_DECIDERtask_type: PATH_DECIDERtask_type: RULE_BASED_STOP_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDERtask_type: PIECEWISE_JERK_SPEED_OPTIMIZER# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_type: RSS_DECIDER

_DECIDER結(jié)尾的為決策部分 _OPTIMIZER結(jié)尾的為優(yōu)化部分。

1.1 Task： LANE_CHANGE_DECIDER

產(chǎn)生是否換道的決策，更新?lián)Q道狀態(tài)

????首先判斷是否產(chǎn)生多條參考線，若只有一條參考線，則保持直行。若有多條參考線，則根據(jù)一些條件（主車的前方和后方一定距離內(nèi)是否有障礙物，旁邊車道在一定距離內(nèi)是否有障礙物）進(jìn)行判斷是否換道，當(dāng)所有條件都滿足時(shí)，則進(jìn)行換道決策。

1.2 Task： PATH_REUSE_DECIDER

路徑是否可重用,提高幀間平順性
????主要判斷是否可以重用上一幀規(guī)劃的路徑。若上一幀的路徑未與障礙物發(fā)生碰撞，則可以重用，提高穩(wěn)定性，節(jié)省計(jì)算量。若上一幀的規(guī)劃出的路徑發(fā)生碰撞，則重新規(guī)劃路徑。

1.3 Task： PATH_BORROW_DECIDER

產(chǎn)生是否借道的決策

????該決策有以下的判斷條件：
? 是否只有一條車道
? 是否存在阻塞道路的障礙物
? 阻塞障礙物是否遠(yuǎn)離路口
? 阻塞障礙物長(zhǎng)期存在
? 旁邊車道是實(shí)線還是虛線
????當(dāng)所有判斷條件都滿足時(shí)，會(huì)產(chǎn)生借道決策。

1.4 Task： PATH_BOUNDS_DECIDER

產(chǎn)生路徑邊界

????利用前幾個(gè)決策器，根據(jù)相應(yīng)條件，產(chǎn)生相應(yīng)的SL邊界。這里說(shuō)明以下Nudge障礙物的概念——主車旁邊的障礙物未完全阻擋主車，主車可以通過(guò)繞行避過(guò)障礙物（注意圖中的邊界）。

1.5 Task： PIECEWISE_JERK_PATH_OPTIMIZER

基于二次規(guī)劃算法，對(duì)每個(gè)邊界規(guī)劃出最優(yōu)路徑.

1.6 Task： PATH_ASSESSMENT_DECIDER

路徑評(píng)價(jià)，選出最優(yōu)路徑
????依據(jù)以下規(guī)則，進(jìn)行評(píng)價(jià)。
路徑是否和障礙物碰撞
路徑長(zhǎng)度
路徑是否會(huì)停在對(duì)向車道
路徑離自車遠(yuǎn)近
哪個(gè)路徑更早回自車道
…
????路徑兩兩進(jìn)行對(duì)比，選出最優(yōu)的路徑。

1.7 Task： PATH_DECIDER

根據(jù)選出的路徑給出對(duì)障礙物的決策
????若是繞行的路徑，則產(chǎn)生繞行的決策；若前方有障礙物阻塞，則產(chǎn)生停止的決策。

2. 基于二次規(guī)劃的路徑規(guī)劃算法

2.1 二次規(guī)劃問(wèn)題標(biāo)準(zhǔn)型

????牛津大學(xué)推出過(guò)二次規(guī)劃的求解器，支持C/C++、python、Matlab等多種語(yǔ)言。
????二次規(guī)劃問(wèn)題的標(biāo)準(zhǔn)形式為：$$\begin{array}{ll}\mathrm{minimize}& \frac{1}{2}{x}^{T}Px+q^{T}x\\ \mathrm{subjectto}& l\le Ax\le u\end{array}$$????where $x\in {\mathbf{R}}^n$ is the optimization variable. The objective function is defined by a positive semidefinite matrix $P\in {\mathbf{S}}_{+}^n$and vector $q\in {\mathbf{R}}^n$ . The linear constraints are defined by matrix $A\in {\mathbf{R}}^{m\times n}$ and vectors $l$ and $u$ so that $l_i\in \mathbf{R}\cup \{?\infty \}$ and $‘u_i\in \mathbf{R}\cup \{+\infty \}$ for all $i\in \{1,\dots ,m\}$ .
????二次規(guī)劃優(yōu)化問(wèn)題為二次型，其約束為線性型。$x$是要優(yōu)化的變量，是一個(gè)$n$維的向量。$p$是二次項(xiàng)系數(shù)，是正定矩陣。$q$是一次項(xiàng)系數(shù)，是$n$維向量。$A$是一個(gè)$m$x$n$的矩陣，$A$為約束函數(shù)的一次項(xiàng)系數(shù)，$m$為約束函數(shù)的個(gè)數(shù)。$l$和$u$分別為約束函數(shù)的下邊界和上邊界。

常用二次規(guī)劃求解器：

• OSQP :使用ADMM方法求解。 對(duì)于規(guī)模大的，含有大量等式或不等式約束的問(wèn)題有較好的求解效率。
• qpOASES: 用可行域法，對(duì)于約束較少的小規(guī)模問(wèn)題，qpOASES求解更快。

????二次規(guī)劃問(wèn)題的求解往往有以下幾個(gè)步驟：定義目?jī)?yōu)化變量、設(shè)計(jì)目標(biāo)函數(shù)、設(shè)計(jì)約束、求解器求解等幾個(gè)步驟。

2.2 定義優(yōu)化變量

????路徑規(guī)劃一般是在Frenet坐標(biāo)系中進(jìn)行的。$s$為沿著參考線的方向，$l$為垂直于坐標(biāo)系的方向。????如圖所示，將障礙物分別投影到SL坐標(biāo)系上。在$s$方向上，以間隔$\Delta s$作為一個(gè)間隔點(diǎn)，從$s_0$,$s_1$,$s_2$一直到$s_{n?1}$，構(gòu)成了規(guī)劃的路徑。選取每個(gè)間隔點(diǎn)的$l$作為優(yōu)化的變量，同時(shí)也將$\dot{l}$和$\ddot{l}$也作為優(yōu)化變量。
????如此，就構(gòu)成了優(yōu)化變量$x$，$x$有三個(gè)部分組成：從$l_0$,$l_1$,$l_2$到$l_{n?1}$，從${\dot{l}}_0$,${\dot{l}}_1$,${\dot{l}}_2$到${\dot{l}}_{n?1}$,從${\ddot{l}}_0$,${\ddot{l}}_1$,${\ddot{l}}_2$到${\ddot{l}}_{n?1}$.

2.3 設(shè)計(jì)目標(biāo)函數(shù)

????對(duì)于目標(biāo)函數(shù)的設(shè)計(jì)，我們需要明確以下目標(biāo)：

• 確保安全、禮貌的駕駛，盡可能貼近車道中心線行駛：$\left|l_i\right|\downarrow$
• 確保舒適的體感，盡可能降低橫向速度/加速度/加加速度：∣l˙i∣↓\left| {{{\dot l}_i}} \right| \downarrow?l˙i??↓，∣l¨i∣↓\left| {{{\ddot l}_i}} \right| \downarrow?l¨i??↓，$\left|{l^{\prime \prime \prime }}_{i\to i+1}\right|\downarrow$
• 確保終點(diǎn)接近參考終點(diǎn)（這個(gè)往往用在靠邊停車場(chǎng)景之中國(guó)）：$l_{end}=l_{ref}$

????最后會(huì)得到以下目標(biāo)函數(shù)：
????對(duì)每個(gè)點(diǎn)設(shè)計(jì)二次的目標(biāo)函數(shù)并對(duì)代價(jià)值進(jìn)行求和，式中的$w_l,w_{dl},w_{ddl},w_{dddl}$都是對(duì)于優(yōu)化變量的懲罰項(xiàng)，以及偏移終點(diǎn)的懲罰項(xiàng)$w_{end?l},w_{end?dl},w_{end?ddl},w_{end?dddl}$。

ps：三階導(dǎo)的求解方式為：$\frac{{l^{\prime \prime }}_{i+1}?{l^{\prime \prime }}_i}{\Delta s}$
????按照二次型的標(biāo)準(zhǔn)型，將目標(biāo)函數(shù)的二次項(xiàng)系數(shù)和一次項(xiàng)系數(shù)用矩陣表示：$$\begin{array}{ll}\mathrm{minimize}& \frac{1}{2}{x}^{T}Px+q^{T}x\\ \mathrm{subjectto}& l\le Ax\le u\end{array}$$

2.4 設(shè)計(jì)約束

????接下來(lái)談?wù)劶s束的設(shè)計(jì)。
????約束要滿足：
? 主車必須在道路邊界內(nèi)，同時(shí)不能和障礙物有碰撞$$l_i\in (l_{\min }^i,l_{\max }^i)$$????邊界的約束已經(jīng)在1.4 Task： PATH_BOUNDS_DECIDER里講述過(guò)了。
? 根據(jù)當(dāng)前狀態(tài)，主車的橫向速度/加速度/加加速度有特定運(yùn)動(dòng)學(xué)限制：
????首先是曲率的約束，車輛在行駛時(shí)有最大曲率半徑的限制，根據(jù)Frenet坐標(biāo)的轉(zhuǎn)換公式（該公式來(lái)源于這篇論文——Werling M, Ziegler J, Kammel S, et al. Optimal trajectory generation for dynamic street scenarios in a frenet frame[C]//2010 IEEE International Conference on Robotics and Automation. IEEE, 2010: 987-993.）：????$l$的二階導(dǎo)于曲率有如上關(guān)系，但我們無(wú)法直接將其應(yīng)用于約束的設(shè)計(jì)（約束函數(shù)為一次）之中，對(duì)此需要對(duì)其進(jìn)行簡(jiǎn)化。
假設(shè)1：參考線規(guī)劃：$\theta ?{\theta }_{ref}=\Delta \theta \approx 0$，即航向角幾乎為0.
假設(shè)2：規(guī)劃的曲率數(shù)值上很小，所以兩個(gè)曲率相乘近乎為0.$$0<{\kappa }_{ref}<\kappa ?1\to {\kappa }_{ref}\kappa \approx 0$$????依據(jù)上述假設(shè)，我們將上述關(guān)系簡(jiǎn)化為：$$\frac{d^{2}l}{d{s}^2}=\kappa ?{\kappa }_{ref}$$????根據(jù)車輛運(yùn)動(dòng)學(xué)關(guān)系計(jì)算最大曲率：$${\kappa }_{\max }=\frac{\tan ({\alpha }_{\max })}{L}$$????得到$\ddot{l}$的約束范圍：$$?{\kappa }_{\max }?{\kappa }_{ref}<{\ddot{l}}_i<{\kappa }_{\max }?{\kappa }_{ref}$$????另外還得滿足曲率變化率的要求（即規(guī)劃處的路徑能使方向盤(pán)在最大角速度下能夠及時(shí)的轉(zhuǎn)過(guò)來(lái)）：$$\frac{d^{3}l}{d{s}^3}=\frac{d\frac{d^{2}t}{d{l}^2}}{dt}?\frac{dt}{ds}$$????主路行駛中，實(shí)際車輪轉(zhuǎn)角很小$\alpha \to 0$，近似有$tan\alpha \approx \alpha$,從而有：$$\frac{d^{2}l}{d{s}^2}\approx \kappa ?{\kappa }_{ref}=\frac{\tan ({\alpha }_{\max })}{L}?{\kappa }_{ref}\approx \frac{\alpha }{L}?{\kappa }_{ref}$$????同時(shí)假設(shè)，在一個(gè)周期內(nèi)規(guī)劃的路徑上車輛的速度是恒定的$$v=\frac{dt}{ds}$$????代入三階導(dǎo)公式得到三階導(dǎo)的邊界$$\frac{d^{3}l}{d{s}^3}=\frac{{\alpha }^{\prime }}{Lv}<\frac{{{\alpha }^{\prime }}_{\max }}{Lv}$$

總結(jié)

• 必須在道路邊界內(nèi)，同時(shí)不能和障礙物有碰撞$$l_i\in (l_{\min }^i,l_{\max }^i)$$
• 根據(jù)當(dāng)前狀態(tài)，主車的橫向速度/加速度/加加速度有特定運(yùn)動(dòng)學(xué)限制：

• 必須滿足基本的物理原理：

????三階導(dǎo)$l^{\prime \prime \prime }$可以積成二階導(dǎo)$l^{\prime \prime }$，二階導(dǎo)$l^{\prime \prime }$可以積成一階導(dǎo)$l^{\prime }$,一階導(dǎo)$l^{\prime }$可以積成$l$。三階導(dǎo)$l^{\prime \prime \prime }$為常量，二階導(dǎo)$l^{\prime \prime }$，一階導(dǎo)$l^{\prime }$，$l$為連續(xù)可導(dǎo)的。每個(gè)點(diǎn)之間都是三界多項(xiàng)的關(guān)系。
????將上述內(nèi)容轉(zhuǎn)化為約束矩陣。????$l_0=l_{init}$,${\dot{l}}_0=l_{init}$,${\ddot{l}}_0=l_{init}$滿足的是起點(diǎn)的約束，即為實(shí)際車輛規(guī)劃起點(diǎn)的狀態(tài)。

2.5 求解器求解

????最后是運(yùn)用求解器進(jìn)行求解。求解器求解同樣需要四個(gè)步驟：設(shè)定OSQP求解參數(shù)、計(jì)算QP系數(shù)矩陣、構(gòu)造OSQP求解器、獲取優(yōu)化結(jié)果。

2.5.1 設(shè)定OSQP求解參數(shù)

// /home/yuan/apollo-edu/modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc OSQPSettings* PiecewiseJerkSpeedProblem::SolverDefaultSettings() {// Define Solver default settingsOSQPSettings* settings =reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));osqp_set_default_settings(settings);settings->eps_abs = 1e-4;settings->eps_rel = 1e-4;settings->eps_prim_inf = 1e-5;settings->eps_dual_inf = 1e-5;settings->polish = true;settings->verbose = FLAGS_enable_osqp_debug;settings->scaled_termination = true;return settings; }

2.5.2 計(jì)算QP系數(shù)矩陣

// /home/yuan/apollo-edu/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc OSQPData* PiecewiseJerkProblem::FormulateProblem() {// calculate kernelstd::vector<c_float> P_data;std::vector<c_int> P_indices;std::vector<c_int> P_indptr;CalculateKernel(&P_data, &P_indices, &P_indptr); // 二次項(xiàng)系數(shù)P的矩陣// calculate affine constraintsstd::vector<c_float> A_data;std::vector<c_int> A_indices;std::vector<c_int> A_indptr;std::vector<c_float> lower_bounds;std::vector<c_float> upper_bounds;CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,&upper_bounds); // 約束項(xiàng)系數(shù)A的矩陣// calculate offsetstd::vector<c_float> q;CalculateOffset(&q); // 一次項(xiàng)系數(shù)q的向量OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));CHECK_EQ(lower_bounds.size(), upper_bounds.size());size_t kernel_dim = 3 * num_of_knots_;size_t num_affine_constraint = lower_bounds.size();data->n = kernel_dim;data->m = num_affine_constraint;data->P = csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data),CopyData(P_indices), CopyData(P_indptr));data->q = CopyData(q);data->A =csc_matrix(num_affine_constraint, kernel_dim, A_data.size(),CopyData(A_data), CopyData(A_indices), CopyData(A_indptr));data->l = CopyData(lower_bounds);data->u = CopyData(upper_bounds);return data; }

2.5.3 構(gòu)造OSQP求解器

OSQPWorkspace* osqp_work = nullptr;osqp_work = osqp_setup(data, settings);

2.5.4 獲取優(yōu)化結(jié)果

osqp_solve(osqp_work);auto status = osqp_work->info->status_val;// 獲取優(yōu)化值if (status < 0 || (status != 1 && status != 2)) {AERROR << "failed optimization status:\t" << osqp_work->info->status;osqp_cleanup(osqp_work);FreeData(data);c_free(settings);return false;} else if (osqp_work->solution == nullptr) {AERROR << "The solution from OSQP is nullptr";osqp_cleanup(osqp_work);FreeData(data);c_free(settings);return false;}// extract primal resultsx_.resize(num_of_knots_);dx_.resize(num_of_knots_);ddx_.resize(num_of_knots_);for (size_t i = 0; i < num_of_knots_; ++i) {x_.at(i) = osqp_work->solution->x[i] / scale_factor_[0];dx_.at(i) = osqp_work->solution->x[i + num_of_knots_] / scale_factor_[1];ddx_.at(i) =osqp_work->solution->x[i + 2 * num_of_knots_] / scale_factor_[2];} //優(yōu)化變量的取值// Cleanuposqp_cleanup(osqp_work);FreeData(data);c_free(settings);return true;

3. 路徑規(guī)劃算法代碼解讀

3.1 總流程和決策\(yùn)優(yōu)化的入口函數(shù)

????路徑規(guī)劃從參考線平滑開(kāi)始，參考線模塊結(jié)束后，會(huì)將中間計(jì)算結(jié)果保存在ReferenceLineinfo之中。之后按照路徑規(guī)劃的任務(wù)，依次執(zhí)行上圖中的任務(wù)。任務(wù)包括了決策器以及優(yōu)化器的任務(wù)。

決策器的入口函數(shù)。輸入每一幀的數(shù)據(jù)結(jié)構(gòu)總類frame、參考線的中間計(jì)算結(jié)果current_reference_line_info到求解器中進(jìn)行求解，最后結(jié)果保存在current_reference_line_info中。
優(yōu)化器的入口函數(shù)。輸入speed_data、reference_line、init_point、path_reusable、final_path_data等信息到求解器中求解，最后將結(jié)果保存在reference_line中。

3.2 產(chǎn)生換道決策

// /home/yuan/apollo-edu/modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc void LaneChangeDecider::UpdateStatus(double timestamp,ChangeLaneStatus::Status status_code,const std::string& path_id) {auto* lane_change_status = injector_->planning_context()->mutable_planning_status()->mutable_change_lane();lane_change_status->set_timestamp(timestamp);lane_change_status->set_path_id(path_id);lane_change_status->set_status(status_code); }

????產(chǎn)生換道的狀態(tài)，之后將結(jié)果保存在injector中。

3.3 產(chǎn)生借道決策

// /home/yuan/apollo-edu/modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.cc// By default, don't borrow any lane.reference_line_info->set_is_path_lane_borrow(false);// Check if lane-borrowing is needed, if so, borrow lane.if (Decider::config_.path_lane_borrow_decider_config().allow_lane_borrowing() &&IsNecessaryToBorrowLane(*frame, *reference_line_info)) {reference_line_info->set_is_path_lane_borrow(true);}

????當(dāng)產(chǎn)生階導(dǎo)決策時(shí)，會(huì)將相應(yīng)標(biāo)志位置true。

if (!left_borrowable && !right_borrowable) {mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);return false;} else {mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);if (left_borrowable) {mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::LEFT_BORROW);}if (right_borrowable) {mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::RIGHT_BORROW);}}

????同時(shí)，在進(jìn)行借道決策時(shí)，會(huì)對(duì)左右借道進(jìn)行判斷。借道的狀態(tài)保存在injetor里。

3.4 產(chǎn)生路徑邊界

????根據(jù)現(xiàn)有決策在參考線上進(jìn)行采樣，獲得每個(gè)點(diǎn)在$l$的邊界。有四種邊界決策：GenerateRegularPathBound（自車道行駛）、GenerateFallbackPathBound（失敗回退）、GenerateLaneChangePathBound、GeneratePullOverPathBound。最后將邊界保存在SetCandidatePathBoundaries中，供下一步使用。

3.5 路徑優(yōu)化

piecewise_jerk_problem.set_x_ref(std::move(weight_x_ref_vec),path_reference_l_ref);}// for debug:here should use std::movepiecewise_jerk_problem.set_weight_x(w[0]);piecewise_jerk_problem.set_weight_dx(w[1]);piecewise_jerk_problem.set_weight_ddx(w[2]);piecewise_jerk_problem.set_weight_dddx(w[3]);piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});auto start_time = std::chrono::system_clock::now();piecewise_jerk_problem.set_x_bounds(lat_boundaries);piecewise_jerk_problem.set_dx_bounds(-FLAGS_lateral_derivative_bound_default,FLAGS_lateral_derivative_bound_default);piecewise_jerk_problem.set_ddx_bounds(ddl_bounds);// Estimate lat_acc and jerk boundary from vehicle_paramsconst auto& veh_param =common::VehicleConfigHelper::GetConfig().vehicle_param();const double axis_distance = veh_param.wheel_base();const double max_yaw_rate =veh_param.max_steer_angle_rate() / veh_param.steer_ratio() / 2.0;const double jerk_bound = EstimateJerkBoundary(std::fmax(init_state[1], 1.0),axis_distance, max_yaw_rate);piecewise_jerk_problem.set_dddx_bound(jerk_bound);bool success = piecewise_jerk_problem.Optimize(max_iter);

????調(diào)用piecewise_jerk_problem類進(jìn)行求解，會(huì)設(shè)置一些權(quán)重以及一些約束，利用Optimize函數(shù)進(jìn)行求解。

const auto& path_boundaries =reference_line_info_->GetCandidatePathBoundaries();ADEBUG << "There are " << path_boundaries.size() << " path boundaries.";const auto& reference_path_data = reference_line_info_->path_data();std::vector<PathData> candidate_path_data;for (const auto& path_boundary : path_boundaries) {size_t path_boundary_size = path_boundary.boundary().size();

????reference_line_info_->GetCandidatePathBoundaries();保存候選路徑。

if (candidate_path_data.empty()) {return Status(ErrorCode::PLANNING_ERROR,"Path Optimizer failed to generate path");}reference_line_info_->SetCandidatePathData(std::move(candidate_path_data));

3.6 選擇最優(yōu)路徑

bool ComparePathData(const PathData& lhs, const PathData& rhs,const Obstacle* blocking_obstacle) {ADEBUG << "Comparing " << lhs.path_label() << " and " << rhs.path_label();// Empty path_data is never the larger one.if (lhs.Empty()) {ADEBUG << "LHS is empty.";return false;}if (rhs.Empty()) {ADEBUG << "RHS is empty.";return true;}

???? 調(diào)用ComparePathData函數(shù)，對(duì)路徑進(jìn)行兩兩比較。

*(reference_line_info->mutable_path_data()) = valid_path_data.front();reference_line_info->SetBlockingObstacle(valid_path_data.front().blocking_obstacle_id());

???? 將最優(yōu)的路徑保存在reference_line_info中。將阻塞障礙物最近的放在reference_line_info中，供速度規(guī)劃進(jìn)一步處理。

4. 路徑規(guī)劃算法實(shí)踐

云實(shí)驗(yàn)地址——Apollo規(guī)劃之路徑規(guī)劃仿真調(diào)試

4.1 觀察借道繞行、自車道繞行障礙物、自車道巡航、換道時(shí)路徑的邊界和規(guī)劃的路徑

(1）在終端中輸入以下指令，啟動(dòng)dreamview

bash scripts/bootstrap_neo.sh

(2）模式選擇Mkz Standard Debug，地圖選擇Apollo Virutal Map，打開(kāi)Sim_Control模式，打開(kāi)PNC Monitor,等待屏幕中間區(qū)域出現(xiàn)Mkz車模型和地圖后即表示成功進(jìn)入仿真模式。
(3）點(diǎn)擊左側(cè)Tab欄Module Controller，啟動(dòng)Planning,Prediction，Routing模塊，如果需要錄制數(shù)據(jù)則打開(kāi)Recorder模塊。(4）模塊啟動(dòng)完成后，點(diǎn)擊左側(cè)Tab欄Profile，選擇Scenario Profiles里的course場(chǎng)景集，右上角選擇場(chǎng)景場(chǎng)景開(kāi)始仿真，分別點(diǎn)擊自車道內(nèi)行駛，繞行障礙物，借道繞行，換道場(chǎng)景。觀察路徑曲線和路徑邊界.Layer Menu中控制Planning的各個(gè)path和boundary開(kāi)關(guān)可以顯示和關(guān)閉每一條候選路徑.

借道繞行
借道繞行
自車道行駛

自車道行駛
自車道繞行（Nudge障礙物）

自車道繞行（Nudge障礙物）
變道

變道

4.2 借道繞行場(chǎng)景，調(diào)整$l,l’,l’’$的權(quán)重系數(shù)，觀察路徑的變化

在modules/planning/conf/planning_config.pb.txt配置文件中，包含了路徑規(guī)劃任務(wù)的相關(guān)參數(shù)，我們可以過(guò)對(duì)這些參數(shù)的調(diào)整，達(dá)到我們期望路徑規(guī)劃效果。

(1)使用在線編輯工具修改/apollo/modules/planning/conf目錄下的planning_config.pb.txt文件，增大default_path_config的l_weight，減小dl_weight ddl_weight dddl_weight。

default_task_config: {task_type: PIECEWISE_JERK_PATH_OPTIMIZERpiecewise_jerk_path_optimizer_config {default_path_config {l_weight: 1.0dl_weight: 20.0ddl_weight: 1000.0dddl_weight: 50000.0}lane_change_path_config {l_weight: 1.0dl_weight: 5.0ddl_weight: 800.0dddl_weight: 30000.0}} }

(2)修改好代碼參數(shù)后，保存這個(gè)文件，在ModuleController中重啟planning模塊（必須步驟)。
(3）重新選擇借道繞行場(chǎng)景，觀察軌跡和調(diào)整前有何變化

更晚借道，提前回到原先車道，軌跡曲率更大。

4.3 靠邊停車實(shí)踐，開(kāi)啟靠邊停車功能，觀察路徑的變化

恢復(fù)參數(shù)為默認(rèn)值，在planning.conf中添加命令行參數(shù)–enable_scenario_pull_over=true，啟動(dòng)靠邊停車，修改好代碼參數(shù)后，保存這個(gè)文件，在Module Controller中重啟planning模塊（必須步驟）。
靠邊停車目標(biāo)點(diǎn)產(chǎn)生懲罰項(xiàng)使得規(guī)劃出的軌跡偏離原參考線。

總結(jié)

以上是生活随笔為你收集整理的Apollo星火计划学习笔记——Apollo路径规划算法原理与实践的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。