Lattice與Piecewise方法明顯的不同，Lattice是沿參考線分解，橫向運動（l,l',l''），縱向運動（s,s',s''），然后進行軌跡合成。
?

引言：

?Apollo中在2.5版本就引入了Lattice palnner，該算法基于Frenet坐標系，通過采樣的方式生成軌跡。在橫向采樣中，Apollo引入了優化的思想，這與之后的Piecewise jerk Path optimization同根同源。

雖然該算法本身具有一定的缺陷，但作為一個體系完整的規劃器，依舊很值得我們學習。

基礎工作：ReferenceLine、Pncmap、障礙物映射（數據流動）等

Apollo 中 Planning 模塊的框架_牛仔很忙吧的博客-CSDN博客

0. 啟動流程

主流程在planning\planner\lattice\lattice_planner.cc，需要調用以下模塊：

common/math/cartesian_frenet_conversion.cc //坐標轉換 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過程中運行palnning_base類的RunOnce()函數周期性地運行規劃主程序plan(),即根據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()

運行接口,從frame中獲取多條reference_line_info并逐條進行規劃.多條參考線形成跟車/換道/繞行等決策效果.

PlanOnReferenceLine()

運行主體.官方注釋中分為大步驟.
此部分的代碼寫的層次比較清晰,結合注釋能把每一步的作用看懂,并把操作都封裝到了一個個單獨的類中.共7步,下面對每步進行解讀.

1. 獲取參考線數據結構轉換

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. 匹配在參考線上最近點

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找到歐式距離最近的點以及下一個點,然后線性插值得到垂直最近點.

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); }

代碼學習點:用三目運算符代替簡單的if-else

3. 根據參考線起始點計算Frenet坐標系,并構造預測后障礙物查詢散列表

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);

其中構造了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主要適用于高速場景,因此駕駛任務主要分為:巡航,跟車,換道,剎停.完成這幾樣任務,需要得知目標速度(道路限速,駕駛員設定,前車速度,曲率限速等,從reference_line數據結構中獲取),目標(無目標巡航,有目標時,跟車,換道,換道超車,剎停等).

這些任務的決策是通過SL,TS圖的分析得到的.

auto ptr_path_time_graph = std::make_shared<PathTimeGraph>(ptr_prediction_querier->GetObstacles(), *ptr_reference_line,reference_line_info, init_s[0],init_s[0] + FLAGS_speed_lon_decision_horizon, 0.0,FLAGS_trajectory_time_length, init_d);double speed_limit =reference_line_info->reference_line().GetSpeedLimitFromS(init_s[0]); reference_line_info->SetLatticeCruiseSpeed(speed_limit);PlanningTarget planning_target = reference_line_info->planning_target(); if (planning_target.has_stop_point()) { ADEBUG << "Planning target stop s: " << planning_target.stop_point().s()<< "Current ego s: " << init_s[0]; }

建圖PathTimeGraph類

在構造智能指針的過程中構造PathTimeGraph()類.
輸入列表為:

const std::vector<const Obstacle*>& obstacles, //障礙物 const std::vector<common::PathPoint>& discretized_ref_points, //參考線離散點 const ReferenceLineInfo* ptr_reference_line_info, //參考線信息 const double s_start, //s,t的始終 const double s_end, const double t_start, const double t_end, const std::array<double, 3>& init_d //s的kappa信息

構造函數內容為:初始化成員變量,設置障礙物SetupObstacles(obstacles, discretized_ref_points).
根據有沒有預測軌跡,將障礙物分為虛擬/靜態/動態障礙物.

1. 靜態障礙物

SetStaticObstacle(const Obstacle* obstacle,const std::vector<PathPoint>&discretized_ref_points)

SL圖

先用ComputeObstacleBoundary()計算SLBoundary(msg).具體為障礙物的頂點轉化為Frenet下,求得s與l的上下限即為SLBoundary.
接著獲取參考線的長度以及道路寬度,過濾超出長度以及道路寬度(分左右側)的障礙物.最后存到static_obs_sl_boundaries_中.

TS圖

分別設置ST圖的矩形四個角點,矩形的長度t由標定參數FLAGS_trajectory_time_length決定.最后存到path_time_obstacle_map_中.

2. 動態障礙物

SetDynamicObstacle()類似與靜態障礙物,只不過t維度上無法采用靜止的一條直線描述,只能用0.1s-8.0s(FLAGS_trajectory_time_resolution間隔)采樣描述出s維度上的2個點,然后連起來.每一個時間點先把障礙物用GetBoundingBox()轉換為common::math::Box2d,然后得到四個頂點GetAllCorners(),也過濾掉超界障礙物,得到ST圖,path_time_obstacle_map_.

3.排序

靜態障礙物根據start_s大小排序static_obs_sl_boundaries_SL圖容器.
動態障礙物直接存到path_time_obstacles_ST圖容器中.

其中,障礙物是通過common::math::Polygon2d類來表達的,此類包括了多邊形的常用運算,例如求面積/距離/包含/重合,還有后續碰撞檢測用的AABoundingBox().

獲取限速

速度限制優先級:ReferenceLine的每段s處的限速>Lane限速(lane_waypoint.lane->lane().speed_limit())>地圖道路限速(hdmap::Road::HIGHWAY)).
通過ReferenceLine::GetSpeedLimitFromS(const double s)函數獲取.ReferenceLine類有私有成員變量struct SpeedLimit與std::vector<SpeedLimit>記錄每段處的限速.

設置規劃目標

設置為proto msg

message PlanningTarget {optional StopPoint stop_point = 1; //內含停止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();

進入GenerateTrajectoryBundles函數

void Trajectory1dGenerator::GenerateTrajectoryBundles(const PlanningTarget& planning_target,Trajectory1DBundle* ptr_lon_trajectory_bundle,Trajectory1DBundle* ptr_lat_trajectory_bundle) {//縱向速度規劃GenerateLongitudinalTrajectoryBundle(planning_target,ptr_lon_trajectory_bundle);//橫向軌跡規劃GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle); }

5.1 Trajectory1dGenerator類-規劃軌跡生成器

構造函數中首先計算TS以及VS圖中的FeasibleRegion.具體為根據車輛縱向加速度的上下限計算SUpper,SLower,VUpper,VLower,TLower.

5.2??縱向規劃軌跡生成

五次多項式（跟車/超車、停車）、四次多項式（巡航）

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()函數.縱向主要是在TS圖中可以直觀感受到,S的一階導數為V,即可得到VS圖(這里沒有使用官方的ST圖的表述是因為與SL圖表示一致,自變量在前因變量在后).
輸出的標準數據結構是struct SamplePoint即帶速度的STPoint,最小的分辨率為運行周期0.1s.

巡航

????????GenerateSpeedProfilesForCruising(): 根據planning_target的巡航速度得到終止條件(終止條件采樣器),用GenerateTrajectory1DBundle<4>計算得速度的四次多項式QuarticPolynomialCurve1d.
????????由于不需要確定末狀態的S值,所以只有五個變量(起始點s、v、a和末點的v、a,不需要求解五次多項式,求解四次即可(apollo對性能還是很敏感的啊).
????????這里介紹一下巡航時采樣器EndConditionSampler::SampleLonEndConditionsForCruising().
需要輸入巡航速度然后輸出采樣點時間點處的速度即可.具體是采樣9個點,范圍是[FLAGS_polynomial_minimal_param,FLAGS_trajectory_time_length*i].實際值好象是[0.01, 1, 2, 3, 4…7, 8]s.在每個時刻,計算FeasibleRegion里對應個采樣時間點處的速度上下限,用線性插值的方式增加采樣點.如下圖:

總采樣點為2+6×8=50個.

然后再說一下曲線的類,繼承關系如下:
Curve1d->PolynomialCurve1d->QuarticPolynomialCurve1d/QuinticPolynomialCurve1d
Curve1d->LatticeTrajectory1d

對于每段軌跡的初始化,使用智能指針管理.
auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(std::shared_ptr<Curve1d>(new QuarticPolynomialCurve1d())
構造函數里有QuarticPolynomialCurve1d::ComputeCoefficients()函數計算出多項式系數,得到縱向規劃曲線bundle.

跟車/超車

GenerateSpeedProfilesForPathTimeObstacles():此時的采樣器為EndConditionSampler::SampleLonEndConditionsForPathTimePoints().
不管是跟車還是超車是通過QueryPathTimeObstacleSamplePoints()函數根據車輛尺寸(front_edge_to_center),

分別查詢跟車QueryFollowPathTimePoints()與超車QueryOvertakePathTimePoints()均采集采樣點,而在TS圖里跟車與超車分別意味著在障礙物的下方/上方采點.這里是不對跟車/超車進行決策,決策是通過后續的cost計算來決策的.

具體過程是先查詢障礙物的四個角點:PathTimeGraph::GetObstacleSurroundingPoints().跟車的話,對下方2角點進行線性插值得到FLAGS_time_min_density分辨率下的采樣點.
超車的話,對上方2角點進行線性插值.

提一下這里的線性插值,下圖TS圖中的斜率代表了障礙物的速度,勻速時為直線,加速時為開口向上的多項式曲線,減速時開口向下.但是這里采用線性插值是有點問題的.減速時插值的直線在實際曲線的下方,也就是比實際更嚴格.但是加速時插值的直線在實際曲線的上方,對于前期緩慢加速,后期突然快速加速的前車誤差最大,可能導致碰撞(還是說交給后面的碰撞檢測來填坑?).

采樣點SamplePoint里的速度則是通過函數PredictionQuerier::ProjectVelocityAlongReferenceLine()把障礙物的速度投射到Frenet坐標系中得到速度.只不過考慮到超車后的安全,s方向上加了一個offset:FLAGS_default_lon_buffer.
然后得到的采樣點用FeasibleRegion過濾掉不滿足的點得到最終的縱向終止點集end_s_conditions.

對所有終止點求解五次多項式得到速度軌跡,求解函數Trajectory1dGenerator::GenerateTrajectory1DBundle<5>為common function的求解函數.
軌跡曲線:auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(std::shared_ptr<Curve1d>(new QuinticPolynomialCurve1d()).

停車

GenerateSpeedProfilesForStopping():由開關planning_target.has_stop_point()來控制.
此時的采樣器為EndConditionSampler::SampleLonEndConditionsForCruising().
由于終止點的速度/加速度都為0,采樣點需要決定的數據為2個,一個是剎停距離std::max(init_s_[0], ref_stop_point),ref_stop_point為輸入的參考剎停距離,
第二個是時間間隔同采樣器SampleLonEndConditionsForCruising().
然后用common function的求解函數求得速度軌跡.

5.3?橫向規劃軌跡生成

GenerateLateralTrajectoryBundle()函數.橫向的采樣器為EndConditionSampler::SampleLatEndConditions()
采樣點為d方向:0.0, -0.5, 0.5三個點,s方向:10.0, 20.0, 40.0, 80.0四個點,一共12個點.
換道的橫向軌跡規劃是通過調整referenceLine來實現的.

通過FLAGS_lateral_optimization開關控制是否經過優化的方式還是直接計算五次多項式的形式計算橫向軌跡.
直接計算五次多項式同縱向的common function.

優化計算是通過繼承了LateralQPOptimizer類的LateralOSQPOptimizer來完成的.輸出的軌跡類型為PiecewiseJerkTrajectory1d.

void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(Trajectory1DBundle* ptr_lat_trajectory_bundle) const {//是否使用優化軌跡，true，采用五次多項式規劃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; //規劃間隔為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,即數段連接在一起的軌跡.對外的接口是:增加線段,獲取總長度,獲取order階數時t處值.
PiecewiseAccelerationTrajectory1d:多段段內加速度不變的曲線連接在一起的軌跡,使用s,v,a,t的vector來表達.
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類的構造函數實現.

濾除不合格的軌跡

過停止位置的縱向軌跡.

ConstraintChecker1d::IsValidLongitudinalTrajectory過濾v/a/jerk超限的縱向軌跡

ConstraintChecker1d::IsValidLateralTrajectory橫向軌跡的濾除好像還在調試

cost計算

也通過TrajectoryEvaluator類的構造函數實現.

主要計算如下5個cost.
時間點集TT一般為[0,FLAGS_trajectory_time_length]時間范圍內以FLAGS_trajectory_time_resolution為分辨率的點.

(后面把這個時間段的選擇表述為[0:FLAGS_trajectory_time_resolution:FLAGS_trajectory_time_length]).

注意,下面的順序并沒有完全按照源碼里的順序來解讀.

1.目標cost

Cost of missing the objective, e.g., cruise, stop, etc.
首先需要構造一個理想的目標速度軌跡,通過ComputeLongitudinalGuideVelocity()實現.
用TS圖來表達的話為TT各個點上的s值vector.
詳細分為3種情況

不剎停
生成PiecewiseAccelerationTrajectory1d勻速直線.

剎停,現在開始剎
生成PiecewiseAccelerationTrajectory1d減速到0.

剎停,未來某點開始剎直到剎停

生成PiecewiseBrakingTrajectoryGenerator::Generate().又分為幾種情況.
緊急剎車無法達到舒適性剎車,立即最大剎停.
可以舒適性剎車,典型的情況如下:加速到目標速度->勻速->舒適性減速->剎停.
根據實際情況可以變成:減速到目標速度->勻速->舒適性減速->剎停/ 減速到目標速度->舒適性減速->剎停/ 加速到目標速度->舒適性減速->剎停.如下圖:

如果時間點集還有剩余則用水平線補齊.
這里使用Piecewise曲線,通過一段段曲線添加的形式,個人覺得非常值得借鑒.
注:PiecewiseBrakingTrajectoryGenerator類中所有的成員函數均為static類型,應該是安全性考慮,值得借鑒.

cost分為2部分,speed偏差與軌跡總距離(總距離與cost成反比這一點有點想不明白…).事先計算出理想參考點與評價軌跡的速度之差,評價軌跡的總距離.

2. 縱向舒適性cost(jerk)

3. 縱向碰撞cost

Cost of longitudinal collision
通過PathTimeGraph::GetPathBlockingIntervals()函數獲取TT上截取所有障礙物的s跨度(參考縱向采樣時的線性插值).
TrajectoryEvaluator::LonCollisionCost()計算碰撞cost.
首先計算時間titi處縱向軌跡的s值,si

4. 縱向向心加速度cost

找到titi處匹配的參考線上的曲率ki.

5. 橫向偏移cost

Cost of lateral offsets

先獲取比較的s的span:標定量FLAGS_speed_lon_decision_horizon與評價軌跡長度的小值,[0:FLAGS_trajectory_space_resolution:S].

6. 橫向舒適度cost

cost of later comfort

最后把6項cost加權相加得到最終的總cost.

同時把縱向橫向的軌跡組合以及其cost放入優先隊列?std::priority_queue<PairCost, std::vector<PairCost>, CostComparator> cost_queue_;中.

碰撞檢測環境創建

CollisionChecker的構造函數里BuildPredictedEnvironment()函數創建檢測環境,具體是濾除不相關的障礙物以及在障礙物形狀上加offset膨脹.

• 濾除的障礙物有:

虛擬障礙物.

障礙物與自車在同一車道內并且在自車后.

障礙物與自車在同一車道內但不在PathTimeGrap內.

• 膨脹有效障礙物,在Box2d的橫縱向各擴張標定系數倍.

7. 過濾超界軌跡,拼接橫縱向軌跡,如果沒有合格軌跡生成backup軌跡

always get the best pair of trajectories to combine; return the first collision-free trajectory.

流程如下:

7.1?隊列操作

選取cost最大的軌跡,并pop出去隊列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?實現橫向/縱向軌跡的拼接.

TrajectoryCombiner::Combine()

• 在時間[0:FLAGS_trajectory_time_resolution:FLAGS_trajectory_time_length]上每個時刻分別根據縱橫向軌跡計算得到s,ds,dds與l,dl,ddl.
• 找到參考線上匹配的最近的點,然后將Frenet坐標系下的坐標轉換到笛卡爾坐標系下,得到x,y,s,theta,kappa,dkappa,relative_time的TrajectoryPoint.
• 通過DiscretizedTrajectory::AppendTrajectoryPoint()方法把軌跡點放入到離散軌跡中,完成拼接.

7.3?約束驗證.

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結果,過程是檢查報錯結果的那些指標有沒有出界,都沒有出界的情況下合格.

7.4?碰撞檢測.

CollisionChecker::InCollision()
在每個TrajectoryPoint處構造一個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()實現的.

將自車/障礙物抽象成Box2d有如下2種方式:

AABB—Axis Aligned Bounding Box

OBB—Oriented Bounding Box

AABB更簡單,構造上只要找到x,y坐標軸上的極值即可,操作上只需通過x,y坐標的比較很快實現,缺點是沒有那么精確,存在大塊空白區,容易引起誤檢.區別如下圖:

Apollo使用AABB來構造Box2d.但自車的幾何中心與后輪中心不重疊,需要Shift一個vector.

雖然AABB沒有這么精確,Apollo通過2段檢測法來平衡性能與效果.先用AABB做粗略檢測.如果障礙物與自車在AABB重疊了,進行下一步分離軸定理檢測.

這一步讓我想起了做車輛計劃工程師review汽車零件之間clearance的工作,在CAD軟件上通過截不同的軸面觀測零件到截面的距離來獲取clearance.當然這個工作大批量的情況下會由仿真部門自動檢測,但初期布置的情況下需要手動觀察.

如下圖,只要觀測到

即可證明沒有碰撞.需要以自車的四個邊為投影軸做檢測直到發項有一個滿足即可退出. 具體的計算過程,我會在Apollo::Common::Math解讀篇進行分析.

7.5?保存合格軌跡

能夠跨過這么多道坎還能留存下來的是可行駛的軌跡無疑了.將軌跡相關信息壓入ReferenceLineInfo中,包括SetTrajectory(),SetCost(),SetDrivable().返回執行狀態Status::OK().最后的輸出為帶cost的一束拼接軌跡,供后續使用.

是的,到現在還是不清楚前文提出問題,超車的話是怎么改變ReferenceLine的選擇,需要進一步研究一下代碼.

7.6?backup軌跡

那要是沒有軌跡能跨過這些坎呢?還有backup軌跡.BackupTrajectoryGenerator類生成backup軌跡.原理是對a進行固定采樣{-0.1, -1.0, -2.0, -3.0, -4.0}生成固定減速度的軌跡ConstantDecelerationTrajectory1d.根據新的初始條件重新規劃橫向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").

經過這7步,完成了lattice_planner.

補充：

軌跡拼接（規劃起點、規劃周期、Repaln問題）

總結：

Planning Lattice規劃器 - 知乎

Apollo Lattice Planner學習記錄 - Challenging-eXtraordinary

下面這篇博客，作者自己按照自己的思路實現了一遍，分析的比較詳細?

基于Frenet坐標系的無人車軌跡規劃詳解與實現_robinvista的博客-CSDN博客_apollo frenet坐標

Frenet坐標系與Cartesian坐標系互轉_昔風不起，唯有努力生存！-CSDN博客_frenet坐標系

百度 Apollo 軌跡規劃技術分享筆記 - 知乎? ?提問匯總

Baidu Apollo代碼解析之軌跡規劃中的軌跡評估代價函數 - 知乎??Lattice Planner 和 EM Planner

自動駕駛路徑規劃-Lattice Planner算法?半杯茶的小酒杯，匯總了一些提問

下面這個實踐總結的很詳細：

Apollo中Lattice規劃器結構梳理_王不二的路-CSDN博客_apollo lattice

?Lattice Planner從學習到放棄（一）.額不....到實踐_王不二的路-CSDN博客_lattice plan

Lattice Planner從學習到放棄（二）：二次規劃的應用與調試_王不二的路-CSDN博客_二次規劃求解器

關于動態障礙物的處理，需要思考學習：?

?Lattice Planner從學習到放棄（三）：動態障礙物的處理與應用 - 知乎

?Apollo中Lattice軌跡碰撞檢測_王不二的路-CSDN博客_apollo 碰撞檢測

?社群分享內容 | Lattice Planner規劃算法

Apollo規劃代碼ros移植-Lattice規劃框架_夏融化了這季節的博客-CSDN博客

Apollo規劃代碼ros移植-Lattcie的二次規劃_夏融化了這季節的博客-CSDN博客

Lattice Planner規劃算法原理 - 知乎

狀態空間lattice算法梳理（按執行邏輯） - 知乎

?Lattice規劃與Matlab實現（1） - 知乎

關于多項式的理解：

Lattice Planner-Matlab - 知乎

關于五次多項式：還有很多疑問，對多項式的理解？

軌跡優化 | Minimum-jerk - 知乎（以及高飛的課程）

自動駕駛決策規劃算法第一章第一節 細說五次多項式_嗶哩嗶哩_bilibili

五次多項式軌跡（matlab） - 知乎

MATLAB中的支持

Lattice Planner-Matlab - 知乎

Highway Trajectory Planning Using Frenet Reference Path- MATLAB & Simulink- MathWorks 中國

總結

以上是生活随笔為你收集整理的Apollo_Lattice palnner的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。