概述
TrajectoryEvaluator类是apollo planning模块下modules\planning\lattice\trajectory_generation\trajectory_evaluator.cc/.h实现
从类名来看，应该是TrajectoryEvaluator是轨迹评估器类？
从代码来看TrajectoryCombiner类主要是实现：
1.ST图信息，参考线信息，初始纵向状态，横纵向候选轨迹对集合等信息储存到自身的类成员里；
2.根据规划目标(停止点和巡航速度)计算整个纵向轨迹的指引速度；
3.横纵向候选轨迹对集合里的横纵向轨迹两两自由组合。根据横纵向舒适性，对参考线偏移，向心加速度，碰撞风险，对纵向指引速度违背各个维度对所有的轨迹对计算其cost并塞入成员队列中，队列为优先队列，里面按cost从大到小排列，调用该类时通过成员函数不断取出cost最大的轨迹直到取出cost最小又符合条件的那个轨迹对；
4.横纵向舒适性cost，对参考线横向偏移cost，向心加速度cost，碰撞风险cost，对纵向指引速度违背量cost，这些cost的计算函数
简而言之，该类就是能将候选的横纵向轨迹集合里选出按照评价函数最优的那个横纵向轨迹对。
trajectory_evaluator.h
#pragma once
#include <functional>
#include <memory>
#include <queue>
#include <utility>
#include <vector>
#include "modules/planning/lattice/behavior/path_time_graph.h"
#include "modules/planning/math/curve1d/curve1d.h"
#include "modules/planning/proto/lattice_structure.pb.h"
#include "modules/planning/proto/planning_config.pb.h"
namespace apollo {
namespace planning {
class TrajectoryEvaluator {
// normal use
typedef std::pair<
std::pair<std::shared_ptr<Curve1d>, std::shared_ptr<Curve1d>>, double>
PairCost;
// auto tuning
typedef std::pair<
std::pair<std::shared_ptr<Curve1d>, std::shared_ptr<Curve1d>>,
std::pair<std::vector<double>, double>>
PairCostWithComponents;
public:
//轨迹插值器的构造函数，用来对类对象进行初始化
//输入参数 初始的纵向状态s,s',s'',
//输入参数：规划目标类对象 PlanningTarget里面包含停止点的和巡航速度信息。
//输入参数 纵向轨迹集合(多个纵向轨迹的候选集？)
//输入参数 横向轨迹集合(多个横向轨迹的候选集？)
//输入参数 应该就是指ST图
//输入参数：参考线类对象
//该函数的作用：就是将输入的ST图信息，参考线信息，初始纵向状态信息储存到自身的类成员里，然后根据规划目标(停止点和巡航速度)计算整个纵向轨迹的指引速度，然后将输入的横纵向轨迹两两自由组合，并调用计算cost的类函数Evaluate()，将对应的横纵向轨迹及其cost存入类成员cost_queue_队列中
//cost_queue_是一个优先队列，定义的比较方式按cost值从大到小排列
TrajectoryEvaluator(
const std::array<double, 3>& init_s,
const PlanningTarget& planning_target,
const std::vector<std::shared_ptr<Curve1d>>& lon_trajectories,
const std::vector<std::shared_ptr<Curve1d>>& lat_trajectories,
std::shared_ptr<PathTimeGraph> path_time_graph,
std::shared_ptr<std::vector<apollo::common::PathPoint>> reference_line);
//默认析构函数
virtual ~TrajectoryEvaluator() = default;
//检查cost_queue_队列中是否还有更多的横纵向轨迹对
bool has_more_trajectory_pairs() const;
//返回cost_queue_队列中横纵向轨迹对的数量 const修饰禁止修改
size_t num_of_trajectory_pairs() const;
//返回cost最大的轨迹对，同时会将其从队列中取出
std::pair<std::shared_ptr<Curve1d>, std::shared_ptr<Curve1d>>
next_top_trajectory_pair();
//获取最大cost
double top_trajectory_pair_cost() const;
//这个函数甚至都没有定义？
std::vector<double> top_trajectory_pair_component_cost() const;
private:
//输入规划目标(停止点和巡航速度信息)，纵向轨迹，横向轨迹计算cost
//cost函数考虑 对指引速度的违背/横纵向舒适性/纵向加加速度
//lon_objective_cost对纵向指引速度(根据规划目标里的停止点和巡航速度算出)的违背量*权重，违背越大cost越大
//lon_jerk_cost 纵向加加速度的cost，jerk越大，cost越大
//lon_collision_cost纵向碰撞的cost 到障碍物的距离越大cost越大
//centripetal_acc_cost 向心加速度越大,cost越大
//lat_offset_cost 就是偏离参考线d绝对值越大，cost就越大
//lat_comfort_cost d''*s'*s'+d'*s''不清楚原理，但是也是d'',d'越大,cost就越大
double Evaluate(const PlanningTarget& planning_target,
const std::shared_ptr<Curve1d>& lon_trajectory,
const std::shared_ptr<Curve1d>& lat_trajectory,
std::vector<double>* cost_components = nullptr) const;
// Lateral costs横向的cost,对原本道路参考线(车道中心线)的偏移
//输入横向轨迹以及离散采样的s序列(该序列对应着横向轨迹)
double LatOffsetCost(const std::shared_ptr<Curve1d>& lat_trajectory,
const std::vector<double>& s_values) const;
//计算横向舒适性的cost，输入参数为横纵向轨迹
//其实就是等效于评估轨迹最大横向加速度，类似于用整体轨迹的最大横向加速度来计算cost?
double LatComfortCost(const std::shared_ptr<Curve1d>& lon_trajectory,
const std::shared_ptr<Curve1d>& lat_trajectory) const;
//纵向舒适性cost计算，输入参数就是纵向轨迹，就是根据纵向的jerk计算
double LonComfortCost(const std::shared_ptr<Curve1d>& lon_trajectory) const;
//纵向碰撞cost计算，同样也是只要输入纵向轨迹即可 e^(-2*dist * dist),距离越大，cost越小，越安全
double LonCollisionCost(const std::shared_ptr<Curve1d>& lon_trajectory) const;
//纵向违背参考速度的cost? 参考指引速度的计算在该.cc文件中被ComputeLongitudinalGuideVelocity定义
//输入参数纵向轨迹，规划目标(规划停止点和巡航速度)，参考指引速度序列，在每个离散轨迹点处的速度，其实就是参考的s'
double LonObjectiveCost(const std::shared_ptr<Curve1d>& lon_trajectory,
const PlanningTarget& planning_target,
const std::vector<double>& ref_s_dot) const;
//向心加速度的cost计算,输入参数是纵向轨迹
double CentripetalAccelerationCost(
const std::shared_ptr<Curve1d>& lon_trajectory) const;
//计算纵向指引速度
std::vector<double> ComputeLongitudinalGuideVelocity(
const PlanningTarget& planning_target) const;
//这个函数没有用到
bool InterpolateDenseStPoints(
const std::vector<apollo::common::SpeedPoint>& st_points, double t,
double* traj_s) const;
//cost队列的比较函数，表明是从大到小排列
struct CostComparator
: public std::binary_function<const PairCost&, const PairCost&, bool> {
bool operator()(const PairCost& left, const PairCost& right) const {
return left.second > right.second;
}
};
std::priority_queue<PairCost, std::vector<PairCost>, CostComparator>
cost_queue_;
std::shared_ptr<PathTimeGraph> path_time_graph_;
std::shared_ptr<std::vector<apollo::common::PathPoint>> reference_line_;
std::vector<std::vector<std::pair<double, double>>> path_time_intervals_;
std::array<double, 3> init_s_;
std::vector<double> reference_s_dot_;
};
} // namespace planning
} // namespace apollo
trajectory_evaluator.cc
#include "modules/planning/lattice/trajectory_generation/trajectory_evaluator.h"
#include <algorithm>
#include <limits>
#include "cyber/common/log.h"
#include "modules/common/math/path_matcher.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory1d/piecewise_acceleration_trajectory1d.h"
#include "modules/planning/constraint_checker/constraint_checker1d.h"
#include "modules/planning/lattice/trajectory_generation/piecewise_braking_trajectory_generator.h"
namespace apollo {
namespace planning {
using apollo::common::PathPoint;
using apollo::common::SpeedPoint;
using apollo::common::math::PathMatcher;
using Trajectory1d = Curve1d;
using PtrTrajectory1d = std::shared_ptr<Trajectory1d>;
using Trajectory1dPair =
std::pair<std::shared_ptr<Curve1d>, std::shared_ptr<Curve1d>>;
//轨迹插值器的构造函数，用来对类对象进行初始化
//输入参数 初始的纵向状态s,s',s'',
//输入参数：规划目标类对象 PlanningTarget 类由modules\planning\proto\lattice_structure.proto定义，谷歌protobuf根据.proto文件生成相应的c++类,里面包含停止点的和巡航速度信息。
//输入参数 纵向轨迹集合(多个纵向轨迹的候选集？)
//输入参数 横向轨迹集合(多个横向轨迹的候选集？)
//输入参数 路径时间图？应该就是指ST图
//输入参数：参考线类对象
TrajectoryEvaluator::TrajectoryEvaluator(
const std::array<double, 3>& init_s, const PlanningTarget& planning_target,
const std::vector<PtrTrajectory1d>& lon_trajectories,
const std::vector<PtrTrajectory1d>& lat_trajectories,
std::shared_ptr<PathTimeGraph> path_time_graph,
std::shared_ptr<std::vector<PathPoint>> reference_line)
: path_time_graph_(path_time_graph), //输入的路径时间图参数，参考线参数，初始纵向状态都用来初始化类成员了
reference_line_(reference_line),
init_s_(init_s) {
//定义起始时间0.0s
const double start_time = 0.0;
//定义终端时间是从planning_gflags.cc里取出的trajectory_time_length，也就是8.0S代表规划轨迹的时间长度
const double end_time = FLAGS_trajectory_time_length;
//路径的时间间隔 = 路径-时间图类对象调用成员函数 获取路径阻塞的间隔？
//GetPathBlockingIntervals获取整个ST图时间轴发范围的路径阻塞纵向间隔，返回包含多个[s_lower,s_upper]的数组
//其实就是获取在每个离散的时间点处，阻塞参考线的障碍物在ST图上投影的s_lower,s_upper
path_time_intervals_ = path_time_graph_->GetPathBlockingIntervals(
start_time, end_time, FLAGS_trajectory_time_resolution);
//计算纵向的指引(参考)速度
reference_s_dot_ = ComputeLongitudinalGuideVelocity(planning_target);
//如果我们有一个停止点在参考线上
//过滤掉通过停止点之后的纵向轨迹
//首先初始化停止点的s为一个非常大的double数
double stop_point = std::numeric_limits<double>::max();
//如果规划的目标有停止点的化，把停止点置为这个规划目标里的停止点的S
if (planning_target.has_stop_point()) {
stop_point = planning_target.stop_point().s();
}
//遍历候选的纵向轨迹集合
for (const auto& lon_trajectory : lon_trajectories) {
//根据终端时间去遍历的那条纵向轨迹上插值出终端时间对应的s坐标
double lon_end_s = lon_trajectory->Evaluate(0, end_time);
//如果初始纵向状态小于停止点 并且 遍历的该条纵向轨迹的终端s+0.02>停止点s
//要过滤掉那些会通过停止点之后的纵向路径
//如果这条纵向轨迹的终端S大于停止点的S，且超过0.02以上，那么就忽略掉，直接跳到下一条纵向轨迹
if (init_s[0] < stop_point &&
lon_end_s + FLAGS_lattice_stop_buffer > stop_point) {
continue;
}
//判断遍历的这条纵向轨迹是否是有效的
//查阅该函数发现其实就是检查纵向轨迹上的v,a,jerk有没有超出上下边界，若有超出则为无效轨迹
if (!ConstraintChecker1d::IsValidLongitudinalTrajectory(*lon_trajectory)) {
continue;
}
//这是针对每一条有效的纵向轨迹再去遍历候选的横向轨迹集合
for (const auto& lat_trajectory : lat_trajectories) {
//横向轨迹的有效性判断被注释掉了？
/**
* The validity of the code needs to be verified.
if (!ConstraintChecker1d::IsValidLateralTrajectory(*lat_trajectory,
*lon_trajectory)) {
continue;
}
*/
//根据遍历的这条横向轨迹下遍历的横向轨迹，计算这两轨迹综合的cost
double cost = Evaluate(planning_target, lon_trajectory, lat_trajectory);
//将计算的cost和对应的横，纵向轨迹一起塞入类成员cost队列里cost_queue_
cost_queue_.emplace(Trajectory1dPair(lon_trajectory, lat_trajectory),
cost);
}
}
//最后答应输出有效的轨迹数
ADEBUG << "Number of valid 1d trajectory pairs: " << cost_queue_.size();
}
//判断轨迹评估器类里的cost队列的数量是否为空，一对有效的横纵向轨迹就对应一个cost
bool TrajectoryEvaluator::has_more_trajectory_pairs() const {
return !cost_queue_.empty();
}
//返回轨迹评估器类里的cost队列的数量
size_t TrajectoryEvaluator::num_of_trajectory_pairs() const {
return cost_queue_.size();
}
//优先队列按照横纵向轨迹对对应的cost从大到小排列，取出队列头部的那个元素，也就是cost最大的那个轨迹对返回，并从队列cost_queue_中剔除
//返回cost最大的轨迹对并在cost_queue_队列中剔除
std::pair<PtrTrajectory1d, PtrTrajectory1d>
TrajectoryEvaluator::next_top_trajectory_pair() {
ACHECK(has_more_trajectory_pairs());
auto top = cost_queue_.top();
cost_queue_.pop();
return top.first;
}
//返回最大的cost
double TrajectoryEvaluator::top_trajectory_pair_cost() const {
return cost_queue_.top().second;
}
//输入规划目标(停止点和巡航速度信息)，纵向轨迹，横向轨迹计算cost，另外cost中每一项惩罚项塞入cost_components这个vector数组
double TrajectoryEvaluator::Evaluate(
const PlanningTarget& planning_target,
const PtrTrajectory1d& lon_trajectory,
const PtrTrajectory1d& lat_trajectory,
std::vector<double>* cost_components) const {
// Costs: cost由这几项组成
// 1. Cost of missing the objective, e.g., cruise, stop, etc. 偏离目标位置速度？
// 2. Cost of longitudinal jerk 纵向加加速度
// 3. Cost of longitudinal collision 纵向碰撞
// 4. Cost of lateral offsets 横向偏移，偏离地图参考线的程度
// 5. Cost of lateral comfort 横向舒适性
// Longitudinal costs 纵向cost
//纵向目标cost计算，输入纵向轨迹，规划目标(停止点和巡航速度信息)，参考点的速度
//计算出纵向目标偏离cost
double lon_objective_cost =
LonObjectiveCost(lon_trajectory, planning_target, reference_s_dot_);
//纵向加加速度cost，输入纵向轨迹计算
double lon_jerk_cost = LonComfortCost(lon_trajectory);
//纵向碰撞cost计算，同样也是只要输入纵向轨迹即可 e^(-2*dist * dist),距离越大，cost越小，越安全
double lon_collision_cost = LonCollisionCost(lon_trajectory);
//向心加速度cost计算，也是只需输入纵向轨迹
double centripetal_acc_cost = CentripetalAccelerationCost(lon_trajectory);
// decides the longitudinal evaluation horizon for lateral trajectories.
//横向的轨迹用纵向的插值区域evaluation_horizon ，也就是0~轨迹长度
double evaluation_horizon =
std::min(FLAGS_speed_lon_decision_horizon,
lon_trajectory->Evaluate(0, lon_trajectory->ParamLength()));
//定义一个s值的vector数组
std::vector<double> s_values;
//遍历整个轨迹纵向长度，每次纵向递增1.0m，trajectory_space_resolution在planning_gflags.cc里定义
//所以最终s_values里存放{0，1，2，3，4，...,path_length}
for (double s = 0.0; s < evaluation_horizon;
s += FLAGS_trajectory_space_resolution) {
s_values.emplace_back(s);
}
// Lateral costs横向的cost,对原本道路参考线(车道中心线)的偏移
//输入横向轨迹以及离散采样的s序列(该序列对应着横向轨迹)
double lat_offset_cost = LatOffsetCost(lat_trajectory, s_values);
//横向舒适性cost计算，输入纵向轨迹，横向轨迹来计算
double lat_comfort_cost = LatComfortCost(lon_trajectory, lat_trajectory);
//如果cost_components不为空指针
//往里塞入这个横纵向轨迹对对应的各项cost
if (cost_components != nullptr) {
cost_components->emplace_back(lon_objective_cost);
cost_components->emplace_back(lon_jerk_cost);
cost_components->emplace_back(lon_collision_cost);
cost_components->emplace_back(lat_offset_cost);
}
//返回这对横纵向轨迹对应的各项cost之和
return lon_objective_cost * FLAGS_weight_lon_objective +
lon_jerk_cost * FLAGS_weight_lon_jerk +
lon_collision_cost * FLAGS_weight_lon_collision +
centripetal_acc_cost * FLAGS_weight_centripetal_acceleration +
lat_offset_cost * FLAGS_weight_lat_offset +
lat_comfort_cost * FLAGS_weight_lat_comfort;
}
//横向对道路参考线偏移的cost计算，输入参数横向轨迹和对应的纵坐标序列(其实就是每间隔1.0m采样一次)
double TrajectoryEvaluator::LatOffsetCost(
const PtrTrajectory1d& lat_trajectory,
const std::vector<double>& s_values) const {
//横向轨迹存放的就是S对应的d,d',d''信息，lat_offset_start 横向偏移的起始用横向轨迹输入s=0来插值0阶(代表横向位置)，就是求s=0时对应的d
double lat_offset_start = lat_trajectory->Evaluate(0, 0.0);
//定义cost开根号初始为0
double cost_sqr_sum = 0.0;
//定义cost绝对值初始为0
double cost_abs_sum = 0.0;
//遍历横向轨迹中每一个采样点(纵向每间隔1.0m采样一次)
for (const auto& s : s_values) {
//首先先插值出横向轨迹遍历的第i个点s坐标处对应的横向偏移d
double lat_offset = lat_trajectory->Evaluate(0, s);
//cost等于当前这个点的横向偏移除以/3.0，3.0是设置的一个横向偏移的很大的值
//通常偏移超过0.5~0.6可能都已经不可接受了
double cost = lat_offset / FLAGS_lat_offset_bound;
//如果当前遍历的这个横向轨迹点的横向偏移和轨迹初始横向偏移符号相反，意思就是横向偏移一左一右的话，那么就对cost的平方和cost的绝对值进行惩罚时采用weight_opposite_side_offset相反偏移权重系数为10；若当前遍历的这个横向轨迹点的横向偏移和轨迹初始横向偏移符号相同，那么就对cost的平方和cost的绝对值进行惩罚时采用FLAGS_weight_same_side_offset相同方向偏移权重系数为1.0
//简单理解为若当前遍历点和初始点横向偏移在道路中心线同一侧，对横向偏移平方项和绝对值项惩罚就很小；若当前遍历点和初始点横向偏移在道路中心线两，就对横向偏移平方项和绝对值项惩罚很大。两者权重系数相差10倍
if (lat_offset * lat_offset_start < 0.0) {
cost_sqr_sum += cost * cost * FLAGS_weight_opposite_side_offset;
cost_abs_sum += std::fabs(cost) * FLAGS_weight_opposite_side_offset;
} else {
cost_sqr_sum += cost * cost * FLAGS_weight_same_side_offset;
cost_abs_sum += std::fabs(cost) * FLAGS_weight_same_side_offset;
}
}
//最后返回的总的cost就是 横向偏移cost平方/横向偏移cost绝对值，后面加一个numerical_epsilon 1e-6是为了防止出现分母为0的异常情况，加上一个极小正数
return cost_sqr_sum / (cost_abs_sum + FLAGS_numerical_epsilon);
}
//计算横向舒适性的cost，输入参数为横纵向轨迹
//其实就是等效于评估轨迹最大横向加速度，类似于用整体轨迹的最大横向加速度来计算cost?
double TrajectoryEvaluator::LatComfortCost(
const PtrTrajectory1d& lon_trajectory,
const PtrTrajectory1d& lat_trajectory) const {
//初始定义最大的cost为0
double max_cost = 0.0;
//首先按照时间0,0.1,0.2,...8.0s遍历，trajectory_time_length 8.0s
//trajectory_time_resolution 0.1s
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
//纵向轨迹根据当前遍历的时间t去插值出s,s',s''
double s = lon_trajectory->Evaluate(0, t);
double s_dot = lon_trajectory->Evaluate(1, t);
double s_dotdot = lon_trajectory->Evaluate(2, t);
//计算出当前遍历时间的相对纵向初始点纵坐标s
double relative_s = s - init_s_[0];
//根据这个相对s去横向轨迹上插值出d',d'',注意d',d''都是横向偏移对相对纵坐标进行求导而不是时间
double l_prime = lat_trajectory->Evaluate(1, relative_s);
double l_primeprime = lat_trajectory->Evaluate(2, relative_s);
//横向舒适性cost = d''*s'*s' + d'*s'' 这个原理是什么，横向加速度？
double cost = l_primeprime * s_dot * s_dot + l_prime * s_dotdot;
//这个max_cost是迭代更新的，相比上一个遍历时间时的max_cost，是否这个时间的cost绝对值更大？是的话就更新max_cost
max_cost = std::max(max_cost, std::fabs(cost));
}
//最后返回这个max_cost，实际上就是遍历每一个时间点，找到里面舒适性cost最大的那个点，这个最大的cost就是舒适性的cost，其实就是等效于评估轨迹最大横向加速度，类似于用整体轨迹的最大横向加速度来计算cost?
return max_cost;
}
//纵向舒适性cost计算，输入参数就是纵向轨迹
double TrajectoryEvaluator::LonComfortCost(
const PtrTrajectory1d& lon_trajectory) const {
//首先初始定义cost的平方项和绝对值项为0
double cost_sqr_sum = 0.0;
double cost_abs_sum = 0.0;
//遍历整个时间长度8.0s，每次时间递增0.1s
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
//计算纵向的jerk，就是纵向轨迹在时间t处的3阶导插值
double jerk = lon_trajectory->Evaluate(3, t);
//cost就等于这个jerk/最大jerk,longitudinal_jerk_upper_bound在planning_gflags.cc里定义为2.0m/s^3，就是用纵向jerk占最大值的百分数来作为cost
double cost = jerk / FLAGS_longitudinal_jerk_upper_bound;
//cost的平方项累加
cost_sqr_sum += cost * cost;
//cost的绝对值项累加
cost_abs_sum += std::fabs(cost);
}
//最后返回 cost的平方项/cost的绝对值 numerical_epsilon为一极小正数1e-6只是为了防止分母为0的异常情况
return cost_sqr_sum / (cost_abs_sum + FLAGS_numerical_epsilon);
}
//纵向违背参考速度的cost? 参考指引速度的计算在该.cc文件中被ComputeLongitudinalGuideVelocity定义
//输入参数纵向轨迹，规划目标(规划停止点和巡航速度)，参考指引速度序列，在每个离散轨迹点处的速度，其实就是参考的s'
double TrajectoryEvaluator::LonObjectiveCost(
const PtrTrajectory1d& lon_trajectory,
const PlanningTarget& planning_target,
const std::vector<double>& ref_s_dots) const {
//获取最大时间，最大时间就是纵向轨迹上的最大时间
double t_max = lon_trajectory->ParamLength();
//获取纵向轨迹的长度，最后一个时间点s - 初始点s
double dist_s =
lon_trajectory->Evaluate(0, t_max) - lon_trajectory->Evaluate(0, 0.0);
//初始定义速度cost平方项为0，
double speed_cost_sqr_sum = 0.0;
//初始定义速度cost权重为0？
double speed_cost_weight_sum = 0.0;
//遍历每一个输入的参考指引速度s'
for (size_t i = 0; i < ref_s_dots.size(); ++i) {
//第i个时间点，就是i*0.1s处
double t = static_cast<double>(i) * FLAGS_trajectory_time_resolution;
//cost就等于参考指引速度-纵向轨迹在该时间点一阶导插值(s')
double cost = ref_s_dots[i] - lon_trajectory->Evaluate(1, t);
//速度违背cost平方项计算叠加每个时间点 t^2*abs(cost)
speed_cost_sqr_sum += t * t * std::fabs(cost);
//速度违背权重项cost=t^2
speed_cost_weight_sum += t * t;
}
//计算速度违背的cost = 速度违背cost平方项/速度违背权重项cost
double speed_cost =
speed_cost_sqr_sum / (speed_cost_weight_sum + FLAGS_numerical_epsilon);
//增加一项驶过距离的cost= 1/(1+length)轨迹距离越长cost越小？
double dist_travelled_cost = 1.0 / (1.0 + dist_s);
//speed_cost权重1.0,驶过距离权重10.0,目标速度权重1.0,这个权重其实也没太看懂，其实就是对目标速度，对目标速度的违背，驶过的距离的倒数进行惩罚？为什么？
return (speed_cost * FLAGS_weight_target_speed +
dist_travelled_cost * FLAGS_weight_dist_travelled) /
(FLAGS_weight_target_speed + FLAGS_weight_dist_travelled);
}
// TODO(all): consider putting pointer of reference_line_info and frame
// while constructing trajectory evaluator
//纵向碰撞cost计算,输入参数是纵向轨迹
double TrajectoryEvaluator::LonCollisionCost(
const PtrTrajectory1d& lon_trajectory) const {
//cost平方项，绝对值项初始为0.0
double cost_sqr_sum = 0.0;
double cost_abs_sum = 0.0;
//遍历path_time_intervals_ 遍历 ST图上每一个时间点的被阻塞s范围
//ST图上每一个时间点所有会阻碍参考线的s范围[s_lower0,s_upper0]，[s_lower1,s_upper1]..塞入数组path_time_intervals_
//path_time_intervals_整个ST图时间轴发范围的路径阻塞纵向间隔，是包含多个[s_lower,s_upper]的数组,其实就是每个时间点被阻塞的s范围，可以理解为某一时间点各个阻碍参考线障碍物的车头车尾的纵向坐标集合
for (size_t i = 0; i < path_time_intervals_.size(); ++i) {
//取第i个时间点中(i*0.1)s对应的被阻塞的s范围 pt_interval
const auto& pt_interval = path_time_intervals_[i];
//如果pt_interval 是空的直接跳到下一个时间点被阻塞的s范围集合
if (pt_interval.empty()) {
continue;
}
//对应的时间就是i*0.1s
//trajectory_time_resolution在planning_gflags.cc里被定义
double t = static_cast<double>(i) * FLAGS_trajectory_time_resolution;
//定义当前遍历的第i个时间点对应的纵向位置，取纵向轨迹上0阶导插值t对应的s
double traj_s = lon_trajectory->Evaluate(0, t);
//这个sigma 是什么？其为0.5
double sigma = FLAGS_lon_collision_cost_std;
//遍历第i个时间点每一个阻塞参考线障碍物对应的纵坐标的范围[s_lower,s_upper]
for (const auto& m : pt_interval) {
double dist = 0.0;
//若第i个时间点对应的纵坐标traj_s < 阻塞障碍物车尾s坐标 - 1.0m的缓冲
//if是计算落后障碍物时距障碍物的纵向距离dist
if (traj_s < m.first - FLAGS_lon_collision_yield_buffer) {
dist = m.first - FLAGS_lon_collision_yield_buffer - traj_s;
//else if是计算领先障碍物时距障碍物的纵向距离
} else if (traj_s > m.second + FLAGS_lon_collision_overtake_buffer) {
dist = traj_s - m.second - FLAGS_lon_collision_overtake_buffer;
}
//计算纵向碰撞的cost
//std::exp就是计算自然底数e的n次方
//这一项就是考察纵向上到障碍物的距离的cost，距离越大cost越大？
double cost = std::exp(-dist * dist / (2.0 * sigma * sigma));
//将第i个时间点的cost平方项，绝对值项叠加上去
cost_sqr_sum += cost * cost;
cost_abs_sum += cost;
}
}
//仍然是老套路，用叠加的cost平方项之和/叠加的cost绝对值项之和
return cost_sqr_sum / (cost_abs_sum + FLAGS_numerical_epsilon);
}
//向心加速度的cost计算,输入参数是纵向轨迹
double TrajectoryEvaluator::CentripetalAccelerationCost(
const PtrTrajectory1d& lon_trajectory) const {
// Assumes the vehicle is not obviously deviate from the reference line.
//假设车辆没有明显的违背参考线
//初步定义向心加速度cost之和为0， 各个时间点向心加速度的平方项cost和为0
double centripetal_acc_sum = 0.0;
double centripetal_acc_sqr_sum = 0.0;
//遍历0,0.1,0.2,...8.0的各个时间点，每次递增0.1s
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
//计算i个时间点的s,v，其实就是用时间t去纵向轨迹上一阶导，0阶导插值
double s = lon_trajectory->Evaluate(0, t);
double v = lon_trajectory->Evaluate(1, t);
//计算参考点当前时间对应纵向轨迹上的s去参考线上找匹配的路径点ref_point
PathPoint ref_point = PathMatcher::MatchToPath(*reference_line_, s);
ACHECK(ref_point.has_kappa());
//计算向心加速度ay=v^2/R=v^2*kappa
double centripetal_acc = v * v * ref_point.kappa();
//向心加速度cost之和叠加上该时间点的向心加速度的绝对值，同理平方项也递加上该时间点对应的横向加速度的平方
centripetal_acc_sum += std::fabs(centripetal_acc);
centripetal_acc_sqr_sum += centripetal_acc * centripetal_acc;
}
//最后计的cost又是=所有时间点的向心加速度cost平方之和/向心加速度cost之和
return centripetal_acc_sqr_sum /
(centripetal_acc_sum + FLAGS_numerical_epsilon);
}
//计算纵向指引速度， 输入参数规划目标(停止点及巡航速度信息)，返回的是在各个时间点对应的纵向速度的vector数组
//其实这个指引速度就两种情况，一是规划目标没有停止点从当前状态出发走巡航速度，二是规划目标有停止点
std::vector<double> TrajectoryEvaluator::ComputeLongitudinalGuideVelocity(
const PlanningTarget& planning_target) const {
//先定义一个空的vector数组用来存放指引速度
std::vector<double> reference_s_dot;
//定义巡航速度 = 规划目标里的巡航速度
double cruise_v = planning_target.cruise_speed();
//如果输入的规划目标里没有停止点
if (!planning_target.has_stop_point()) {
//定义1个分段匀加速1维轨迹对象lon_traj 输入参数是初始的s和巡航速度v
PiecewiseAccelerationTrajectory1d lon_traj(init_s_[0], cruise_v);
//上面这条纵向轨迹加一段加速度为0的匀速运动，时间为8.0s
//就是直接在初始的纵向位置上加上一段8.0s的匀速轨迹
lon_traj.AppendSegment(
0.0, FLAGS_trajectory_time_length + FLAGS_numerical_epsilon);
//遍历所有的时间点0,0.1,0.2,...7.9,8.0s，其实每个点都是匀速，就是把这个匀速巡航速度塞入待返回的指引速度数组里，里面每个元素都是巡航速度
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
reference_s_dot.emplace_back(lon_traj.Evaluate(1, t));
}
} else { //如果规划目标里有停止点
//计算纵向要走的距离dist_s，初始的纵坐标到规划目标停止点的距离
double dist_s = planning_target.stop_point().s() - init_s_[0];
//如果要走的距离小于一个很小的正数，基本就为0或者为负数的话
//那么车辆其实就保持静止
if (dist_s < FLAGS_numerical_epsilon) {
//车辆要走的距离小于一个很小的正数时，初始化定义了一个1维纵向轨迹，其巡航速度设置为0，同时输入初始的纵向位置
PiecewiseAccelerationTrajectory1d lon_traj(init_s_[0], 0.0);
//车辆要走的距离小于一个很小的正数时，这条纵向轨迹后面增加一段，加速度为0，保持巡航速度0一直走到8.0s的轨迹
lon_traj.AppendSegment(
0.0, FLAGS_trajectory_time_length + FLAGS_numerical_epsilon);
//遍历每一个时间点
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
//把纵向的速度塞入待返回的vector数组
reference_s_dot.emplace_back(lon_traj.Evaluate(1, t));
}
//返回这个指引速度数组
return reference_s_dot;
}
//若程序执行到这里还没有返回，说明规划目标有停止点，而且剩余要走的距离不是一个很小的数
//计算舒适的加减速度
//舒适的加速度=加速度边界4 * 放缩系数0.5 = 2
//舒适的减速度就是-2
double a_comfort = FLAGS_longitudinal_acceleration_upper_bound *
FLAGS_comfort_acceleration_factor;
double d_comfort = -FLAGS_longitudinal_acceleration_lower_bound *
FLAGS_comfort_acceleration_factor;
//规划目标有停止点，而且剩余要走的距离不是一个很小的数时，纵向参考轨迹就用分段刹车轨迹生成器类生成刹停的纵向轨迹。分段刹车轨迹生成器的Generate()函数输入参数 规划目标的停止点的s，初始的纵向位置，规划目标的巡航速度，初始的纵向速度，舒适的加速度2m/s^2，舒适的减速度-2m/s^2,轨迹的时间长度8.0s,输入这些参数后Generate函数就会返回一条分段刹车的纵向轨迹lon_ref_trajectory
std::shared_ptr<Trajectory1d> lon_ref_trajectory =
PiecewiseBrakingTrajectoryGenerator::Generate(
planning_target.stop_point().s(), init_s_[0],
planning_target.cruise_speed(), init_s_[1], a_comfort, d_comfort,
FLAGS_trajectory_time_length + FLAGS_numerical_epsilon);
//遍历每一个时间点0,0.1,0.2,0.3,...7.9,8.0s
//把求到的分段刹车的纵向轨迹的速度塞入待返回的指引速度数组里reference_s_dot
for (double t = 0.0; t < FLAGS_trajectory_time_length;
t += FLAGS_trajectory_time_resolution) {
reference_s_dot.emplace_back(lon_ref_trajectory->Evaluate(1, t));
}
}
return reference_s_dot;
}
//该函数貌似没有被用到？插值更密的ST点，直接略过
bool TrajectoryEvaluator::InterpolateDenseStPoints(
const std::vector<SpeedPoint>& st_points, double t, double* traj_s) const {
CHECK_GT(st_points.size(), 1U);
if (t < st_points[0].t() || t > st_points[st_points.size() - 1].t()) {
AERROR << "AutoTuning InterpolateDenseStPoints Error";
return false;
}
for (uint i = 1; i < st_points.size(); ++i) {
if (t <= st_points[i].t()) {
*traj_s = st_points[i].t();
return true;
}
}
return false;
}
} // namespace planning
} // namespace apollo
577
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
