Apollo_Lattice palnner
Lattice與Piecewise方法明顯的不同，Lattice是沿參考線分解，橫向運(yùn)動（l,l',l''），縱向運(yùn)動（s,s',s''），然后進(jìn)行軌跡合成。
?
引言：
?Apollo中在2.5版本就引入了Lattice palnner，該算法基于Frenet坐標(biāo)系，通過采樣的方式生成軌跡。在橫向采樣中，Apollo引入了優(yōu)化的思想，這與之后的Piecewise jerk Path optimization同根同源。
雖然該算法本身具有一定的缺陷，但作為一個體系完整的規(guī)劃器，依舊很值得我們學(xué)習(xí)。
基礎(chǔ)工作：ReferenceLine、Pncmap、障礙物映射（數(shù)據(jù)流動）等
Apollo 中 Planning 模塊的框架_牛仔很忙吧的博客-CSDN博客
0. 啟動流程
主流程在planning\planner\lattice\lattice_planner.cc，需要調(diào)用以下模塊：
common/math/cartesian_frenet_conversion.cc //坐標(biāo)轉(zhuǎn)換 common/math/path_matcher.cc planning/common/planning_gflags.cc planning/constraint_checker/collision_checker.cc //碰撞檢測 planning/constraint_checker/constraint_checker.cc //約束檢測 planning/lattice/behavior/path_time_graph.cc planning/lattice/behavior/prediction_querier.cc planning/lattice/trajectory_generation/backup_trajectory_generator.cc planning/lattice/trajectory_generation/lattice_trajectory1d.cc planning/lattice/trajectory_generation/trajectory1d_generator.cc planning/lattice/trajectory_generation/trajectory_combiner.cc //軌跡拼接 planning/lattice/trajectory_generation/trajectory_evaluator.cc //軌跡評估CyberRT的planning_component類里Init過程中運(yùn)行palnning_base類的RunOnce()函數(shù)周期性地運(yùn)行規(guī)劃主程序plan(),即根據(jù)config與scenario等決定哪一種planner。
planner中的PlannerWithReferenceLine子類中有四個子類,其中之一是LatticePlanner。
LatticePlanner類頭文件如下(刪除暫時無用的部分):
//FILE:modules/planning/planner/lattice/lattice_planner.h class LatticePlanner : public PlannerWithReferenceLine {public:explicit LatticePlanner(const std::shared_ptr<DependencyInjector>& injector): PlannerWithReferenceLine(injector) {}common::Status Plan(const common::TrajectoryPoint& planning_init_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) override;common::Status PlanOnReferenceLine(const common::TrajectoryPoint& planning_init_point, Frame* frame,ReferenceLineInfo* reference_line_info) override; };LatticePlanner::Plan()
運(yùn)行接口,從frame中獲取多條reference_line_info并逐條進(jìn)行規(guī)劃.多條參考線形成跟車/換道/繞行等決策效果.
PlanOnReferenceLine()
運(yùn)行主體.官方注釋中分為大步驟.
此部分的代碼寫的層次比較清晰,結(jié)合注釋能把每一步的作用看懂,并把操作都封裝到了一個個單獨(dú)的類中.共7步,下面對每步進(jìn)行解讀.
1. 獲取參考線數(shù)據(jù)結(jié)構(gòu)轉(zhuǎn)換
obtain a reference line and transform it to the PathPoint format
ReferencePoint -> PathPoint，同時計算路徑累計長度s.
auto ptr_reference_line =std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(reference_line_info->reference_line().reference_points()));2. 匹配在參考線上最近點(diǎn)
compute the matched point of the init planning point on the reference line
PathPoint matched_point = PathMatcher::MatchToPath(*ptr_reference_line, planning_init_point.path_point().x(),planning_init_point.path_point().y());(下圖為盜圖,侵刪,下面的盜圖同)先在reference_line找到歐式距離最近的點(diǎn)以及下一個點(diǎn),然后線性插值得到垂直最近點(diǎn).
PathPoint PathMatcher::MatchToPath(const std::vector<PathPoint>& reference_line,const double x, const double y) {CHECK_GT(reference_line.size(), 0);auto func_distance_square = [](const PathPoint& point, const double x,const double y) {double dx = point.x() - x;double dy = point.y() - y;return dx * dx + dy * dy;};double distance_min = func_distance_square(reference_line.front(), x, y);std::size_t index_min = 0;for (std::size_t i = 1; i < reference_line.size(); ++i) {double distance_temp = func_distance_square(reference_line[i], x, y);if (distance_temp < distance_min) {distance_min = distance_temp;index_min = i;}}std::size_t index_start = (index_min == 0) ? index_min : index_min - 1;std::size_t index_end =(index_min + 1 == reference_line.size()) ? index_min : index_min + 1;if (index_start == index_end) {return reference_line[index_start];}//線性插值時注意把kappa,dkappa也做處理.return FindProjectionPoint(reference_line[index_start],reference_line[index_end], x, y); }代碼學(xué)習(xí)點(diǎn):用三目運(yùn)算符代替簡單的if-else
3. 根據(jù)參考線起始點(diǎn)計算Frenet坐標(biāo)系,并構(gòu)造預(yù)測后障礙物查詢散列表
according to the matched point, compute the init state in Frenet frame
std::array<double, 3> init_s;std::array<double, 3> init_d;ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);auto ptr_prediction_querier = std::make_shared<PredictionQuerier>(frame->obstacles(), ptr_reference_line);其中構(gòu)造了PredictionQuerier障礙物查詢器,利用google protobuf庫的InsertIfNotPresent()把reference_line里的障礙物提取出來成為散列表.
PredictionQuerier::PredictionQuerier(const std::vector<const Obstacle*>& obstacles,const std::shared_ptr<std::vector<common::PathPoint>>& ptr_reference_line): ptr_reference_line_(ptr_reference_line) {for (const auto ptr_obstacle : obstacles) {if (common::util::InsertIfNotPresent(&id_obstacle_map_, ptr_obstacle->Id(),ptr_obstacle)) {obstacles_.push_back(ptr_obstacle);} else {AWARN << "Duplicated obstacle found [" << ptr_obstacle->Id() << "]";}} }4. 建圖(PathTimeGraph)獲取speed_limit與PlanningTarget
parse the decision and get the planning target
由于Lattice主要適用于高速場景,因此駕駛?cè)蝿?wù)主要分為:巡航,跟車,換道,剎停.完成這幾樣任務(wù),需要得知目標(biāo)速度(道路限速,駕駛員設(shè)定,前車速度,曲率限速等,從reference_line數(shù)據(jù)結(jié)構(gòu)中獲取),目標(biāo)(無目標(biāo)巡航,有目標(biāo)時,跟車,換道,換道超車,剎停等).
這些任務(wù)的決策是通過SL,TS圖的分析得到的.
建圖PathTimeGraph類
在構(gòu)造智能指針的過程中構(gòu)造PathTimeGraph()類.
輸入列表為:
構(gòu)造函數(shù)內(nèi)容為:初始化成員變量,設(shè)置障礙物SetupObstacles(obstacles, discretized_ref_points).
根據(jù)有沒有預(yù)測軌跡,將障礙物分為虛擬/靜態(tài)/動態(tài)障礙物.
1. 靜態(tài)障礙物
SetStaticObstacle(const Obstacle* obstacle,const std::vector<PathPoint>&discretized_ref_points)
SL圖
先用ComputeObstacleBoundary()計算SLBoundary(msg).具體為障礙物的頂點(diǎn)轉(zhuǎn)化為Frenet下,求得s與l的上下限即為SLBoundary.
接著獲取參考線的長度以及道路寬度,過濾超出長度以及道路寬度(分左右側(cè))的障礙物.最后存到static_obs_sl_boundaries_中.
TS圖
分別設(shè)置ST圖的矩形四個角點(diǎn),矩形的長度t由標(biāo)定參數(shù)FLAGS_trajectory_time_length決定.最后存到path_time_obstacle_map_中.
2. 動態(tài)障礙物
SetDynamicObstacle()類似與靜態(tài)障礙物,只不過t維度上無法采用靜止的一條直線描述,只能用0.1s-8.0s(FLAGS_trajectory_time_resolution間隔)采樣描述出s維度上的2個點(diǎn),然后連起來.每一個時間點(diǎn)先把障礙物用GetBoundingBox()轉(zhuǎn)換為common::math::Box2d,然后得到四個頂點(diǎn)GetAllCorners(),也過濾掉超界障礙物,得到ST圖,path_time_obstacle_map_.
3.排序
靜態(tài)障礙物根據(jù)start_s大小排序static_obs_sl_boundaries_SL圖容器.
動態(tài)障礙物直接存到path_time_obstacles_ST圖容器中.
其中,障礙物是通過common::math::Polygon2d類來表達(dá)的,此類包括了多邊形的常用運(yùn)算,例如求面積/距離/包含/重合,還有后續(xù)碰撞檢測用的AABoundingBox().
獲取限速
速度限制優(yōu)先級:ReferenceLine的每段s處的限速>Lane限速(lane_waypoint.lane->lane().speed_limit())>地圖道路限速(hdmap::Road::HIGHWAY)).
通過ReferenceLine::GetSpeedLimitFromS(const double s)函數(shù)獲取.ReferenceLine類有私有成員變量struct SpeedLimit與std::vector<SpeedLimit>記錄每段處的限速.
設(shè)置規(guī)劃目標(biāo)
設(shè)置為proto msg
message PlanningTarget {optional StopPoint stop_point = 1; //內(nèi)含停止s值,HARD/SOFT類型optional double cruise_speed = 2; }?5. 采樣生成橫+縱向軌跡群
generate 1d trajectory bundle for longitudinal and lateral respectively
/ 5. generate 1d trajectory bundle for longitudinal and lateral respectively.Trajectory1dGenerator trajectory1d_generator(init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);std::vector<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;std::vector<std::shared_ptr<Curve1d>> lat_trajectory1d_bundle;trajectory1d_generator.GenerateTrajectoryBundles(planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);ADEBUG << "Trajectory_Generation_Time = "<< (Clock::NowInSeconds() - current_time) * 1000;current_time = Clock::NowInSeconds();進(jìn)入GenerateTrajectoryBundles函數(shù)
void Trajectory1dGenerator::GenerateTrajectoryBundles(const PlanningTarget& planning_target,Trajectory1DBundle* ptr_lon_trajectory_bundle,Trajectory1DBundle* ptr_lat_trajectory_bundle) {//縱向速度規(guī)劃GenerateLongitudinalTrajectoryBundle(planning_target,ptr_lon_trajectory_bundle);//橫向軌跡規(guī)劃GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle); }5.1 Trajectory1dGenerator類-規(guī)劃軌跡生成器
構(gòu)造函數(shù)中首先計算TS以及VS圖中的FeasibleRegion.具體為根據(jù)車輛縱向加速度的上下限計算SUpper,SLower,VUpper,VLower,TLower.
5.2??縱向規(guī)劃軌跡生成
五次多項(xiàng)式（跟車/超車、停車）、四次多項(xiàng)式（巡航）
void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(const PlanningTarget& planning_target,Trajectory1DBundle* ptr_lon_trajectory_bundle) const {// cruising trajectories are planned regardlessly.GenerateSpeedProfilesForCruising(planning_target.cruise_speed(),ptr_lon_trajectory_bundle);GenerateSpeedProfilesForPathTimeObstacles(ptr_lon_trajectory_bundle);if (planning_target.has_stop_point()) {GenerateSpeedProfilesForStopping(planning_target.stop_point().s(),ptr_lon_trajectory_bundle);} }GenerateLongitudinalTrajectoryBundle()函數(shù).縱向主要是在TS圖中可以直觀感受到,S的一階導(dǎo)數(shù)為V,即可得到VS圖(這里沒有使用官方的ST圖的表述是因?yàn)榕cSL圖表示一致,自變量在前因變量在后).
輸出的標(biāo)準(zhǔn)數(shù)據(jù)結(jié)構(gòu)是struct SamplePoint即帶速度的STPoint,最小的分辨率為運(yùn)行周期0.1s.
巡航
????????GenerateSpeedProfilesForCruising(): 根據(jù)planning_target的巡航速度得到終止條件(終止條件采樣器),用GenerateTrajectory1DBundle<4>計算得速度的四次多項(xiàng)式QuarticPolynomialCurve1d.
????????由于不需要確定末狀態(tài)的S值,所以只有五個變量(起始點(diǎn)s、v、a和末點(diǎn)的v、a,不需要求解五次多項(xiàng)式,求解四次即可(apollo對性能還是很敏感的啊).
????????這里介紹一下巡航時采樣器EndConditionSampler::SampleLonEndConditionsForCruising().
需要輸入巡航速度然后輸出采樣點(diǎn)時間點(diǎn)處的速度即可.具體是采樣9個點(diǎn),范圍是[FLAGS_polynomial_minimal_param,FLAGS_trajectory_time_length*i].實(shí)際值好象是[0.01, 1, 2, 3, 4…7, 8]s.在每個時刻,計算FeasibleRegion里對應(yīng)個采樣時間點(diǎn)處的速度上下限,用線性插值的方式增加采樣點(diǎn).如下圖:
總采樣點(diǎn)為2+6×8=50個.
然后再說一下曲線的類,繼承關(guān)系如下:
Curve1d->PolynomialCurve1d->QuarticPolynomialCurve1d/QuinticPolynomialCurve1d
Curve1d->LatticeTrajectory1d
對于每段軌跡的初始化,使用智能指針管理.
auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(std::shared_ptr<Curve1d>(new QuarticPolynomialCurve1d())
構(gòu)造函數(shù)里有QuarticPolynomialCurve1d::ComputeCoefficients()函數(shù)計算出多項(xiàng)式系數(shù),得到縱向規(guī)劃曲線bundle.
跟車/超車
GenerateSpeedProfilesForPathTimeObstacles():此時的采樣器為EndConditionSampler::SampleLonEndConditionsForPathTimePoints().
不管是跟車還是超車是通過QueryPathTimeObstacleSamplePoints()函數(shù)根據(jù)車輛尺寸(front_edge_to_center),
分別查詢跟車QueryFollowPathTimePoints()與超車QueryOvertakePathTimePoints()均采集采樣點(diǎn),而在TS圖里跟車與超車分別意味著在障礙物的下方/上方采點(diǎn).這里是不對跟車/超車進(jìn)行決策,決策是通過后續(xù)的cost計算來決策的.
具體過程是先查詢障礙物的四個角點(diǎn):PathTimeGraph::GetObstacleSurroundingPoints().跟車的話,對下方2角點(diǎn)進(jìn)行線性插值得到FLAGS_time_min_density分辨率下的采樣點(diǎn).
超車的話,對上方2角點(diǎn)進(jìn)行線性插值.
提一下這里的線性插值,下圖TS圖中的斜率代表了障礙物的速度,勻速時為直線,加速時為開口向上的多項(xiàng)式曲線,減速時開口向下.但是這里采用線性插值是有點(diǎn)問題的.減速時插值的直線在實(shí)際曲線的下方,也就是比實(shí)際更嚴(yán)格.但是加速時插值的直線在實(shí)際曲線的上方,對于前期緩慢加速,后期突然快速加速的前車誤差最大,可能導(dǎo)致碰撞(還是說交給后面的碰撞檢測來填坑?).
采樣點(diǎn)SamplePoint里的速度則是通過函數(shù)PredictionQuerier::ProjectVelocityAlongReferenceLine()把障礙物的速度投射到Frenet坐標(biāo)系中得到速度.只不過考慮到超車后的安全,s方向上加了一個offset:FLAGS_default_lon_buffer.
然后得到的采樣點(diǎn)用FeasibleRegion過濾掉不滿足的點(diǎn)得到最終的縱向終止點(diǎn)集end_s_conditions.
對所有終止點(diǎn)求解五次多項(xiàng)式得到速度軌跡,求解函數(shù)Trajectory1dGenerator::GenerateTrajectory1DBundle<5>為common function的求解函數(shù).
軌跡曲線:auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(std::shared_ptr<Curve1d>(new QuinticPolynomialCurve1d()).
停車
GenerateSpeedProfilesForStopping():由開關(guān)planning_target.has_stop_point()來控制.
此時的采樣器為EndConditionSampler::SampleLonEndConditionsForCruising().
由于終止點(diǎn)的速度/加速度都為0,采樣點(diǎn)需要決定的數(shù)據(jù)為2個,一個是剎停距離std::max(init_s_[0], ref_stop_point),ref_stop_point為輸入的參考剎停距離,
第二個是時間間隔同采樣器SampleLonEndConditionsForCruising().
然后用common function的求解函數(shù)求得速度軌跡.
5.3?橫向規(guī)劃軌跡生成
GenerateLateralTrajectoryBundle()函數(shù).橫向的采樣器為EndConditionSampler::SampleLatEndConditions()
采樣點(diǎn)為d方向:0.0, -0.5, 0.5三個點(diǎn),s方向:10.0, 20.0, 40.0, 80.0四個點(diǎn),一共12個點(diǎn).
換道的橫向軌跡規(guī)劃是通過調(diào)整referenceLine來實(shí)現(xiàn)的.
通過FLAGS_lateral_optimization開關(guān)控制是否經(jīng)過優(yōu)化的方式還是直接計算五次多項(xiàng)式的形式計算橫向軌跡.
直接計算五次多項(xiàng)式同縱向的common function.
優(yōu)化計算是通過繼承了LateralQPOptimizer類的LateralOSQPOptimizer來完成的.輸出的軌跡類型為PiecewiseJerkTrajectory1d.
void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(Trajectory1DBundle* ptr_lat_trajectory_bundle) const {//是否使用優(yōu)化軌跡，true，采用五次多項(xiàng)式規(guī)劃if (!FLAGS_lateral_optimization) {auto end_conditions = end_condition_sampler_.SampleLatEndConditions();// Use the common function to generate trajectory bundles.GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,ptr_lat_trajectory_bundle);} else {double s_min = init_lon_state_[0];double s_max = s_min + FLAGS_max_s_lateral_optimization; //FLAGS_max_s_lateral_optimization = 60double delta_s = FLAGS_default_delta_s_lateral_optimization; //規(guī)劃間隔為1m//橫向邊界auto lateral_bounds =ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);// LateralTrajectoryOptimizer lateral_optimizer;std::unique_ptr<LateralQPOptimizer> lateral_optimizer(new LateralOSQPOptimizer);// 采用的是OSQP求解器lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();ptr_lat_trajectory_bundle->push_back(std::make_shared<PiecewiseJerkTrajectory1d>(lateral_trajectory));} }5.4?軌跡類型
這里稍微介紹一下planning/module/trajectory1d下面的幾個軌跡類型.
StandingStillTrajectory1d:在s處不的的軌跡描述.
ConstantJerkTrajectory1d:jerk不變的軌跡描述,
ConstantDecelerationTrajectory1d:減速度不變的軌跡描述,
PiecewiseJerkTrajectory1d:ConstantJerkTrajectory1d的vector,即數(shù)段連接在一起的軌跡.對外的接口是:增加線段,獲取總長度,獲取order階數(shù)時t處值.
PiecewiseAccelerationTrajectory1d:多段段內(nèi)加速度不變的曲線連接在一起的軌跡,使用s,v,a,t的vector來表達(dá).
PiecewiseTrajectory1d:多段Curve1d連接在一起的軌跡.
PiecewiseBrakingTrajectoryGenerator:在planning/lattice/trajectory_generation下面,分段剎車的軌跡,得到為PiecewiseAccelerationTrajectory1d軌跡.
6. 計算軌跡cost并排序,過濾可能碰撞的軌跡
first, evaluate the feasibility of the 1d trajectories according to dynamic constraints. second, evaluate the feasible longitudinal and lateral trajectory pairs and sort them according to the cost.
TrajectoryEvaluator trajectory_evaluator(init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle,ptr_path_time_graph, ptr_reference_line); // Get instance of collision checker and constraint checker CollisionChecker collision_checker(frame->obstacles(), init_s[0], init_d[0],*ptr_reference_line, reference_line_info,ptr_path_time_graph);通過TrajectoryEvaluator類的構(gòu)造函數(shù)實(shí)現(xiàn).
濾除不合格的軌跡
cost計算
也通過TrajectoryEvaluator類的構(gòu)造函數(shù)實(shí)現(xiàn).
主要計算如下5個cost.
時間點(diǎn)集TT一般為[0,FLAGS_trajectory_time_length]時間范圍內(nèi)以FLAGS_trajectory_time_resolution為分辨率的點(diǎn).
(后面把這個時間段的選擇表述為[0:FLAGS_trajectory_time_resolution:FLAGS_trajectory_time_length]).
注意,下面的順序并沒有完全按照源碼里的順序來解讀.
1.目標(biāo)cost
Cost of missing the objective, e.g., cruise, stop, etc.
首先需要構(gòu)造一個理想的目標(biāo)速度軌跡,通過ComputeLongitudinalGuideVelocity()實(shí)現(xiàn).
用TS圖來表達(dá)的話為TT各個點(diǎn)上的s值vector.
詳細(xì)分為3種情況
不剎停
生成PiecewiseAccelerationTrajectory1d勻速直線.
剎停,現(xiàn)在開始剎
生成PiecewiseAccelerationTrajectory1d減速到0.
剎停,未來某點(diǎn)開始剎直到剎停
生成PiecewiseBrakingTrajectoryGenerator::Generate().又分為幾種情況.
緊急剎車無法達(dá)到舒適性剎車,立即最大剎停.
可以舒適性剎車,典型的情況如下:加速到目標(biāo)速度->勻速->舒適性減速->剎停.
根據(jù)實(shí)際情況可以變成:減速到目標(biāo)速度->勻速->舒適性減速->剎停/ 減速到目標(biāo)速度->舒適性減速->剎停/ 加速到目標(biāo)速度->舒適性減速->剎停.如下圖:
如果時間點(diǎn)集還有剩余則用水平線補(bǔ)齊.
這里使用Piecewise曲線,通過一段段曲線添加的形式,個人覺得非常值得借鑒.
注:PiecewiseBrakingTrajectoryGenerator類中所有的成員函數(shù)均為static類型,應(yīng)該是安全性考慮,值得借鑒.
cost分為2部分,speed偏差與軌跡總距離(總距離與cost成反比這一點(diǎn)有點(diǎn)想不明白…).事先計算出理想?yún)⒖键c(diǎn)與評價軌跡的速度之差,評價軌跡的總距離.
2. 縱向舒適性cost(jerk)
3. 縱向碰撞cost
Cost of longitudinal collision
通過PathTimeGraph::GetPathBlockingIntervals()函數(shù)獲取TT上截取所有障礙物的s跨度(參考縱向采樣時的線性插值).
TrajectoryEvaluator::LonCollisionCost()計算碰撞cost.
首先計算時間titi處縱向軌跡的s值,si
4. 縱向向心加速度cost
找到titi處匹配的參考線上的曲率ki.
5. 橫向偏移cost
Cost of lateral offsets
先獲取比較的s的span:標(biāo)定量FLAGS_speed_lon_decision_horizon與評價軌跡長度的小值,[0:FLAGS_trajectory_space_resolution:S].
6. 橫向舒適度cost
cost of later comfort
最后把6項(xiàng)cost加權(quán)相加得到最終的總cost.
同時把縱向橫向的軌跡組合以及其cost放入優(yōu)先隊(duì)列?std::priority_queue<PairCost, std::vector<PairCost>, CostComparator> cost_queue_;中.
碰撞檢測環(huán)境創(chuàng)建
CollisionChecker的構(gòu)造函數(shù)里BuildPredictedEnvironment()函數(shù)創(chuàng)建檢測環(huán)境,具體是濾除不相關(guān)的障礙物以及在障礙物形狀上加offset膨脹.
- 濾除的障礙物有:
- 膨脹有效障礙物,在Box2d的橫縱向各擴(kuò)張標(biāo)定系數(shù)倍.
7. 過濾超界軌跡,拼接橫縱向軌跡,如果沒有合格軌跡生成backup軌跡
always get the best pair of trajectories to combine; return the first collision-free trajectory.
流程如下:
7.1?隊(duì)列操作
選取cost最大的軌跡,并pop出去隊(duì)列top指向第二大的軌跡.
while (trajectory_evaluator.has_more_trajectory_pairs()) { double trajectory_pair_cost =trajectory_evaluator.top_trajectory_pair_cost(); auto trajectory_pair = trajectory_evaluator.next_top_trajectory_pair();// combine two 1d trajectories to one 2d trajectory auto combined_trajectory = TrajectoryCombiner::Combine(*ptr_reference_line, *trajectory_pair.first, *trajectory_pair.second,planning_init_point.relative_time());// check longitudinal and lateral acceleration // considering trajectory curvatures auto result = ConstraintChecker::ValidTrajectory(combined_trajectory); // check collision with other obstacles if (collision_checker.InCollision(combined_trajectory)) {++collision_failure_count;continue; } // put combine trajectory into debug data const auto& combined_trajectory_points = combined_trajectory; num_lattice_traj += 1; reference_line_info->SetTrajectory(combined_trajectory); reference_line_info->SetCost(reference_line_info->PriorityCost() +trajectory_pair_cost); reference_line_info->SetDrivable(true);// cast auto lattice_traj_ptr =std::dynamic_pointer_cast<LatticeTrajectory1d>(trajectory_pair.first); if (!lattice_traj_ptr) {ADEBUG << "Dynamically casting trajectory1d ptr. failed."; }if (lattice_traj_ptr->has_target_position()) {ADEBUG << "Ending Lon. State s = " << lattice_traj_ptr->target_position()<< " ds = " << lattice_traj_ptr->target_velocity()<< " t = " << lattice_traj_ptr->target_time(); } break; }
7.2?實(shí)現(xiàn)橫向/縱向軌跡的拼接.
TrajectoryCombiner::Combine()
- 在時間[0:FLAGS_trajectory_time_resolution:FLAGS_trajectory_time_length]上每個時刻分別根據(jù)縱橫向軌跡計算得到s,ds,dds與l,dl,ddl.
- 找到參考線上匹配的最近的點(diǎn),然后將Frenet坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到笛卡爾坐標(biāo)系下,得到x,y,s,theta,kappa,dkappa,relative_time的TrajectoryPoint.
- 通過DiscretizedTrajectory::AppendTrajectoryPoint()方法把軌跡點(diǎn)放入到離散軌跡中,完成拼接.
7.3?約束驗(yàn)證.
ConstraintChecker::ValidTrajectory()
分為?VALID, LON_VELOCITY_OUT_OF_BOUND, LON_ACCELERATION_OUT_OF_BOUND,LON_JERK_OUT_OF_BOUND, LAT_VELOCITY_OUT_OF_BOUND, LAT_ACCELERATION_OUT_OF_BOUND, LAT_JERK_OUT_OF_BOUND,CURVATURE_OUT_OF_BOUND結(jié)果,過程是檢查報錯結(jié)果的那些指標(biāo)有沒有出界,都沒有出界的情況下合格.
7.4?碰撞檢測.
CollisionChecker::InCollision()
在每個TrajectoryPoint處構(gòu)造一個shift后的自車Box2d,檢查之前濾除完成后障礙物vectorpredicted_bounding_rectangles_中的每個障礙物與自車HasOverlap()的情況. Shift的vector為Vec2d shift_vec{shift_distance * std::cos(ego_theta),shift_distance * std::sin(ego_theta)};,其中double shift_distance = ego_length/2.0 - vehicle_config.vehicle_param().back_edge_to_center();.
碰撞檢測是通過矩形Box2d重疊HasOverlap()實(shí)現(xiàn)的.
將自車/障礙物抽象成Box2d有如下2種方式:
AABB—Axis Aligned Bounding Box
OBB—Oriented Bounding Box
AABB更簡單,構(gòu)造上只要找到x,y坐標(biāo)軸上的極值即可,操作上只需通過x,y坐標(biāo)的比較很快實(shí)現(xiàn),缺點(diǎn)是沒有那么精確,存在大塊空白區(qū),容易引起誤檢.區(qū)別如下圖:
Apollo使用AABB來構(gòu)造Box2d.但自車的幾何中心與后輪中心不重疊,需要Shift一個vector.
雖然AABB沒有這么精確,Apollo通過2段檢測法來平衡性能與效果.先用AABB做粗略檢測.如果障礙物與自車在AABB重疊了,進(jìn)行下一步分離軸定理檢測.
這一步讓我想起了做車輛計劃工程師review汽車零件之間clearance的工作,在CAD軟件上通過截不同的軸面觀測零件到截面的距離來獲取clearance.當(dāng)然這個工作大批量的情況下會由仿真部門自動檢測,但初期布置的情況下需要手動觀察.
如下圖,只要觀測到
即可證明沒有碰撞.需要以自車的四個邊為投影軸做檢測直到發(fā)項(xiàng)有一個滿足即可退出. 具體的計算過程,我會在Apollo::Common::Math解讀篇進(jìn)行分析.
7.5?保存合格軌跡
能夠跨過這么多道坎還能留存下來的是可行駛的軌跡無疑了.將軌跡相關(guān)信息壓入ReferenceLineInfo中,包括SetTrajectory(),SetCost(),SetDrivable().返回執(zhí)行狀態(tài)Status::OK().最后的輸出為帶cost的一束拼接軌跡,供后續(xù)使用.
是的,到現(xiàn)在還是不清楚前文提出問題,超車的話是怎么改變ReferenceLine的選擇,需要進(jìn)一步研究一下代碼.
7.6?backup軌跡
那要是沒有軌跡能跨過這些坎呢?還有backup軌跡.BackupTrajectoryGenerator類生成backup軌跡.原理是對a進(jìn)行固定采樣{-0.1, -1.0, -2.0, -3.0, -4.0}生成固定減速度的軌跡ConstantDecelerationTrajectory1d.根據(jù)新的初始條件重新規(guī)劃橫向GenerateLateralTrajectoryBundle().之后的過程與上面的障礙物檢測,橫縱向軌跡拼接一致.
if (num_lattice_traj > 0) { ADEBUG << "Planning succeeded"; num_planning_succeeded_cycles += 1; reference_line_info->SetDrivable(true); return Status::OK(); } else { AERROR << "Planning failed"; if (FLAGS_enable_backup_trajectory) {AERROR << "Use backup trajectory";BackupTrajectoryGenerator backup_trajectory_generator(init_s, init_d, planning_init_point.relative_time(),std::make_shared<CollisionChecker>(collision_checker),&trajectory1d_generator);DiscretizedTrajectory trajectory =backup_trajectory_generator.GenerateTrajectory(*ptr_reference_line);reference_line_info->AddCost(FLAGS_backup_trajectory_cost);reference_line_info->SetTrajectory(trajectory);reference_line_info->SetDrivable(true);return Status::OK();} else {reference_line_info->SetCost(std::numeric_limits<double>::infinity()); }}7.7?失敗報錯
還是無法找到合格的軌跡只能報錯,Status(ErrorCode::PLANNING_ERROR, "No feasible trajectories").
經(jīng)過這7步,完成了lattice_planner.
補(bǔ)充：
軌跡拼接（規(guī)劃起點(diǎn)、規(guī)劃周期、Repaln問題）
總結(jié)：
Planning Lattice規(guī)劃器 - 知乎
Apollo Lattice Planner學(xué)習(xí)記錄 - Challenging-eXtraordinary
下面這篇博客，作者自己按照自己的思路實(shí)現(xiàn)了一遍，分析的比較詳細(xì)?
基于Frenet坐標(biāo)系的無人車軌跡規(guī)劃詳解與實(shí)現(xiàn)_robinvista的博客-CSDN博客_apollo frenet坐標(biāo)
Frenet坐標(biāo)系與Cartesian坐標(biāo)系互轉(zhuǎn)_昔風(fēng)不起，唯有努力生存！-CSDN博客_frenet坐標(biāo)系
百度 Apollo 軌跡規(guī)劃技術(shù)分享筆記 - 知乎? ?提問匯總
Baidu Apollo代碼解析之軌跡規(guī)劃中的軌跡評估代價函數(shù) - 知乎??Lattice Planner 和 EM Planner
自動駕駛路徑規(guī)劃-Lattice Planner算法?半杯茶的小酒杯，匯總了一些提問
下面這個實(shí)踐總結(jié)的很詳細(xì)：
Apollo中Lattice規(guī)劃器結(jié)構(gòu)梳理_王不二的路-CSDN博客_apollo lattice
?Lattice Planner從學(xué)習(xí)到放棄（一）.額不....到實(shí)踐_王不二的路-CSDN博客_lattice plan
Lattice Planner從學(xué)習(xí)到放棄（二）：二次規(guī)劃的應(yīng)用與調(diào)試_王不二的路-CSDN博客_二次規(guī)劃求解器
關(guān)于動態(tài)障礙物的處理，需要思考學(xué)習(xí)：?
?Lattice Planner從學(xué)習(xí)到放棄（三）：動態(tài)障礙物的處理與應(yīng)用 - 知乎
?Apollo中Lattice軌跡碰撞檢測_王不二的路-CSDN博客_apollo 碰撞檢測
?社群分享內(nèi)容 | Lattice Planner規(guī)劃算法
Apollo規(guī)劃代碼ros移植-Lattice規(guī)劃框架_夏融化了這季節(jié)的博客-CSDN博客
Apollo規(guī)劃代碼ros移植-Lattcie的二次規(guī)劃_夏融化了這季節(jié)的博客-CSDN博客
Lattice Planner規(guī)劃算法原理 - 知乎
狀態(tài)空間lattice算法梳理（按執(zhí)行邏輯） - 知乎
?Lattice規(guī)劃與Matlab實(shí)現(xiàn)（1） - 知乎
關(guān)于多項(xiàng)式的理解：
Lattice Planner-Matlab - 知乎
關(guān)于五次多項(xiàng)式：還有很多疑問，對多項(xiàng)式的理解？
軌跡優(yōu)化 | Minimum-jerk - 知乎（以及高飛的課程）
自動駕駛決策規(guī)劃算法第一章第一節(jié) 細(xì)說五次多項(xiàng)式_嗶哩嗶哩_bilibili
五次多項(xiàng)式軌跡（matlab） - 知乎
MATLAB中的支持
Lattice Planner-Matlab - 知乎
Highway Trajectory Planning Using Frenet Reference Path- MATLAB & Simulink- MathWorks 中國
總結(jié)
以上是生活随笔為你收集整理的Apollo_Lattice palnner的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 「Odoo 基础教程系列」第七篇——从
- 下一篇： 传感器是新技术革命和当前信息社会的重要技