1 流程图
1.1 开放空间倒车决策流程图
1.2 无碰撞轨迹判断流程图
2 代码分析
代码分析如下：
加载配置文件，
OpenSpaceFallbackDecider::OpenSpaceFallbackDecider(const TaskConfig& config)
: Decider(config) {}
对于二次型公式：ax^2+bx+c=0，返回较小的解。非常正常的解法。
bool OpenSpaceFallbackDecider::QuardraticFormulaLowerSolution(const double a,
const double b,
const double c,
double* sol) {
// quardratic formula: ax^2 + bx + c = 0, return lower solution
// TODO(QiL): use const from common::math
const double kEpsilon = 1e-6;
*sol = 0.0;
if (std::abs(a) < kEpsilon) {
return false;
}
double tmp = b * b - 4 * a * c;
if (tmp < kEpsilon) {
return false;
}
double sol1 = (-b + std::sqrt(tmp)) / (2.0 * a);
double sol2 = (-b - std::sqrt(tmp)) / (2.0 * a);
*sol = std::abs(std::min(sol1, sol2));
ADEBUG << "QuardraticFormulaLowerSolution finished with sol: " << *sol
<< "sol1: " << sol1 << ", sol2: " << sol2 << "a: " << a << "b: " << b
<< "c: " << c;
return true;
}
对于开放空间自动泊车的具体处理算法，具体代码如下：
Status OpenSpaceFallbackDecider::Process(Frame* frame)
创建预测环境，输入为障碍物信息，输出为预测的环境矩形边界。
std::vector<std::vector<common::math::Box2d>> predicted_bounding_rectangles;
size_t first_collision_index = 0;
size_t fallback_start_index = 0;
BuildPredictedEnvironment(frame->obstacles(), predicted_bounding_rectangles);
建立预测环境的代码如下：
首先删除二维矩形中的所有元素，相对时间设置为0。
判断开放空间预测时间段是否大于0，如果是，则对于所有障碍物，如果障碍物不是虚拟的，则需要获取障碍物在此时间的轨迹点，边界，并保存边界信息为二维矩形。然后获取下一个障碍物的轨迹点和边界并分别存放。
void OpenSpaceFallbackDecider::BuildPredictedEnvironment(
const std::vector<const Obstacle*>& obstacles,
std::vector<std::vector<common::math::Box2d>>&
predicted_bounding_rectangles) {
predicted_bounding_rectangles.clear();
double relative_time = 0.0;
while (relative_time < config_.open_space_fallback_decider_config()
.open_space_prediction_time_period()) {
std::vector<Box2d> predicted_env;
for (const Obstacle* obstacle : obstacles) {
if (!obstacle->IsVirtual()) {
TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
Box2d box = obstacle->GetBoundingBox(point);
predicted_env.push_back(std::move(box));
}
}
predicted_bounding_rectangles.emplace_back(std::move(predicted_env));
relative_time += FLAGS_trajectory_time_resolution;
}
}
判断是否是无碰撞轨迹，代码如下：
if (!IsCollisionFreeTrajectory(
frame->open_space_info().chosen_paritioned_trajectory(),
predicted_bounding_rectangles, &fallback_start_index,
&first_collision_index))
是否是无碰撞轨迹代码：
输入：选择的轨道和档位位置（轨迹档位队），障碍物边界，回退/开始索引，首次碰撞索引
首先获取车辆的配置信息，车辆的长度和宽度，轨迹，轨迹点的个数。
之后查找轨迹点下界点,即轨迹开始点。（具体代码在之后）。
bool OpenSpaceFallbackDecider::IsCollisionFreeTrajectory(
const TrajGearPair& trajectory_gear_pair,
const std::vector<std::vector<common::math::Box2d>>&
predicted_bounding_rectangles,
size_t* current_index, size_t* first_collision_index) {
// prediction time resolution: FLAGS_trajectory_time_resolution
const auto& vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
double ego_length = vehicle_config.vehicle_param().length();
double ego_width = vehicle_config.vehicle_param().width();
auto trajectory_pb = trajectory_gear_pair.first;
const size_t point_size = trajectory_pb.NumOfPoints();
*current_index = trajectory_pb.QueryLowerBoundPoint(0.0);
对于所有轨迹点，做如下操作：
求出轨迹点和在该点的航向角；
获取中心点、航向、长度和宽度的构造函数，中心点为轨迹点，长度和宽度分别为车的长度和宽度，航向为矩形朝向。
移动距离=车长/2-车后边缘到中心距离。
将在x轴，y轴上的移动距离保存为二维向量，并按照该向量移动车的矩形框。
for (size_t i = *current_index; i < point_size; ++i) {
const auto& trajectory_point = trajectory_pb.TrajectoryPointAt(i);
double ego_theta = trajectory_point.path_point().theta();
Box2d ego_box(
{trajectory_point.path_point().x(), trajectory_point.path_point().y()},
ego_theta, ego_length, ego_width);
double shift_distance =
ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
Vec2d shift_vec{shift_distance * std::cos(ego_theta),
shift_distance * std::sin(ego_theta)};
ego_box.Shift(shift_vec);
令预测范围为所有障碍物的个数。
对于所有障碍物执行以下操作：
对于障碍物所有矩形，如果车的矩形框和障碍物的矩形框有重叠部分，则输出提示信息，并获取车的坐标，如果轨迹点的相对时间减去点的位置与采样时间的乘积开放空间碰撞缓冲时间，则输出第一次碰撞的指标。
size_t predicted_time_horizon = predicted_bounding_rectangles.size();
for (size_t j = 0; j < predicted_time_horizon; j++) {
for (const auto& obstacle_box : predicted_bounding_rectangles[j]) {
if (ego_box.HasOverlap(obstacle_box)) {
ADEBUG << "HasOverlap(obstacle_box) [" << i << "]";
const auto& vehicle_state = frame_->vehicle_state();
Vec2d vehicle_vec({vehicle_state.x(), vehicle_state.y()});
// remove points in previous trajectory
if (std::abs(trajectory_point.relative_time() -
static_cast<double>(j) *
FLAGS_trajectory_time_resolution) <
config_.open_space_fallback_decider_config()
.open_space_fallback_collision_time_buffer() &&
trajectory_point.relative_time() > 0.0) {
ADEBUG << "first_collision_index: [" << i << "]";
*first_collision_index = i;
return false;
}
}
}
}
}
return true;
}
查询轨迹的开始点。如果相对时间大于时间序列的最后一个时间点（即时间超出），则返回错误。
求出序列中相对时间的下一个时间点的序列迭代器，之后返回该迭代器指向的轨迹点和开始轨迹点之间的点的个数。
size_t DiscretizedTrajectory::QueryLowerBoundPoint(const double relative_time,
const double epsilon) const {
CHECK(!empty());
if (relative_time >= back().relative_time()) {
return size() - 1;
}
auto func = [&epsilon](const TrajectoryPoint& tp,
const double relative_time) {
return tp.relative_time() + epsilon < relative_time;
};
auto it_lower = std::lower_bound(begin(), end(), relative_time, func);
return std::distance(begin(), it_lower);
}
设定回退标志位：真
在安全距离内，根据当前分配轨迹生成回退轨迹，车速降至零
设定未来的碰撞点，回退开始点，回退开始点的车速。
// change flag
frame_->mutable_open_space_info()->set_fallback_flag(true);
// generate fallback trajectory base on current partition trajectory
// vehicle speed is decreased to zero inside safety distance
TrajGearPair fallback_trajectory_pair_candidate =
frame->open_space_info().chosen_paritioned_trajectory();
// auto* ptr_fallback_trajectory_pair =
// frame_->mutable_open_space_info()->mutable_fallback_trajectory();
const auto future_collision_point =
fallback_trajectory_pair_candidate.first[first_collision_index];
/ Fallback starts from current location but with vehicle velocity
auto fallback_start_point =
fallback_trajectory_pair_candidate.first[fallback_start_index];
const auto& vehicle_state =
apollo::common::VehicleStateProvider::Instance()->vehicle_state();
fallback_start_point.set_v(vehicle_state.linear_velocity());
*(frame_->mutable_open_space_info()->mutable_future_collision_point()) =
future_collision_point;
设置最小停车距离=0.5回退开始点车速的平方/4；
（s=0.5at^2
=0.5a*(v/a)^2 由于最大加速度为4，所以
=0.5*v^2/4）
停车距离：如果倒车，则碰撞点-开始点，最大为0，1为缓冲距离，前进档，计算方式类似。
设定车的最大加速度和最大减速度。
// min stop distance: (max_acc)
double min_stop_distance =
0.5 * fallback_start_point.v() * fallback_start_point.v() / 4.0;
// TODO(QiL): move 1.0 to configs
double stop_distance =
fallback_trajectory_pair_candidate.second == canbus::Chassis::GEAR_DRIVE
? std::max(future_collision_point.path_point().s() -
fallback_start_point.path_point().s() - 1.0,
0.0)
: std::min(future_collision_point.path_point().s() -
fallback_start_point.path_point().s() + 1.0,
0.0);
ADEBUG << "stop distance : " << stop_distance;
// const auto& vehicle_config =
// common::VehicleConfigHelper::Instance()->GetConfig();
const double vehicle_max_acc = 4.0; // vehicle_config.max_acceleration();
const double vehicle_max_dec = -4.0; // vehicle_config.max_deceleration();
double stop_deceleration = 0.0;
根据停车总距离求加速度值（R档为负）
计算方法为：
s=0.5at^2 =0.5a*(v/a)^2 =0.5*v/a
if (fallback_trajectory_pair_candidate.second ==
canbus::Chassis::GEAR_REVERSE) {
stop_deceleration =
std::min(fallback_start_point.v() * fallback_start_point.v() /
(2.0 * (stop_distance + 1e-6)),
vehicle_max_acc);
stop_distance = std::min(-1 * min_stop_distance, stop_distance);
} else {
stop_deceleration =
std::max(-fallback_start_point.v() * fallback_start_point.v() /
(2.0 * (stop_distance + 1e-6)),
vehicle_max_dec);
stop_distance = std::max(min_stop_distance, stop_distance);
}
按距离搜索所选轨迹中的停止索引
size_t stop_index = fallback_start_index;
for (size_t i = fallback_start_index;
i < fallback_trajectory_pair_candidate.first.NumOfPoints(); ++i) {
if (std::abs(
fallback_trajectory_pair_candidate.first[i].path_point().s()) >=
std::abs(fallback_start_point.path_point().s() + stop_distance)) {
stop_index = i;
break;
}
}
在回退开始索引之前，对轨迹中的车速和停车加速度进行设置。
for (size_t i = 0; i < fallback_start_index; ++i) {
fallback_trajectory_pair_candidate.first[i].set_v(
fallback_start_point.v());
fallback_trajectory_pair_candidate.first[i].set_a(stop_deceleration);
}
回退开始索引是否就是停车索引。
如果是，则：
将回退初始车速设置为0，加速度设定为最大值。
在停止索引后删除所有轨迹点
附加相同位置但速度为零的轨迹点
// TODO(QiL): refine the logic and remove redundant code, change 0.5 to from
// loading optimizer configs
// If stop_index == fallback_start_index;
if (fallback_start_index == stop_index) {
// 1. Set fallback start speed to 0, accleration to max accleration.
AINFO << "Stop distance within safety buffer, stop now!";
fallback_start_point.set_v(0.0);
fallback_start_point.set_a(0.0);
fallback_trajectory_pair_candidate.first[stop_index].set_v(0.0);
fallback_trajectory_pair_candidate.first[stop_index].set_a(0.0);
// 2. Trim all trajectory points after stop index
fallback_trajectory_pair_candidate.first.erase(
fallback_trajectory_pair_candidate.first.begin() + stop_index + 1,
fallback_trajectory_pair_candidate.first.end());
// 3. AppendTrajectoryPoint with same location but zero velocity
for (int i = 0; i < 20; ++i) {
common::TrajectoryPoint trajectory_point(
fallback_trajectory_pair_candidate.first[stop_index]);
trajectory_point.set_relative_time(
i * 0.5 + 0.5 +
fallback_trajectory_pair_candidate.first[stop_index]
.relative_time());
fallback_trajectory_pair_candidate.first.AppendTrajectoryPoint(
trajectory_point);
}
*(frame_->mutable_open_space_info()->mutable_fallback_trajectory()) =
fallback_trajectory_pair_candidate;
return Status::OK();
}
如果回退初始索引小于停止索引，则对于轨迹中所有的轨迹点：
建立方程：ax^2 + bx + c = 0，其中a=停车减速度，b=2回退开始点车速，
c=-2回退点走过的距离，求得的解为相对时间。
即为：s=o.5at^2 + bt + c
如果能够求得该时间且走过的距离小于停车距离，则
车速=回退估计开始点车速+停车减速度*相对时间
对车速进行限制。
设置加速度为停车减速度。
for (size_t i = fallback_start_index; i <= stop_index; ++i) {
double new_relative_time = 0.0;
double temp_v = 0.0;
double c =
-2.0 * fallback_trajectory_pair_candidate.first[i].path_point().s();
if (QuardraticFormulaLowerSolution(stop_deceleration,
2.0 * fallback_start_point.v(), c,
&new_relative_time) &&
std::abs(
fallback_trajectory_pair_candidate.first[i].path_point().s()) <=
std::abs(stop_distance)) {
ADEBUG << "new_relative_time" << new_relative_time;
temp_v =
fallback_start_point.v() + stop_deceleration * new_relative_time;
// speed limit
if (std::abs(temp_v) < 1.0) {
fallback_trajectory_pair_candidate.first[i].set_v(temp_v);
} else {
fallback_trajectory_pair_candidate.first[i].set_v(
temp_v / std::abs(temp_v) * 1.0);
}
fallback_trajectory_pair_candidate.first[i].set_a(stop_deceleration);
fallback_trajectory_pair_candidate.first[i].set_relative_time(
new_relative_time);
}
否则，提示方程无解，
索引如果不等于0，则
更新轨迹点为上一时刻轨迹点，将车速，加速度设置为0，更新时间戳；
否则，更新车速，加速度为0，不更新时间戳。
else {
AINFO << "QuardraticFormulaLowerSolution solving failed, stop "
"immediately!";
if (i != 0) {
fallback_trajectory_pair_candidate.first[i]
.mutable_path_point()
->CopyFrom(
fallback_trajectory_pair_candidate.first[i - 1].path_point());
fallback_trajectory_pair_candidate.first[i].set_v(0.0);
fallback_trajectory_pair_candidate.first[i].set_a(0.0);
fallback_trajectory_pair_candidate.first[i].set_relative_time(
fallback_trajectory_pair_candidate.first[i - 1].relative_time() +
0.5);
} else {
fallback_trajectory_pair_candidate.first[i].set_v(0.0);
fallback_trajectory_pair_candidate.first[i].set_a(0.0);
}
}
}
将停车索引之后的轨迹点擦除。
// 2. Erase afterwards
fallback_trajectory_pair_candidate.first.erase(
fallback_trajectory_pair_candidate.first.begin() + stop_index + 1,
fallback_trajectory_pair_candidate.first.end());
附加相同位置但速度为零的轨迹点
// 3. AppendTrajectoryPoint with same location but zero velocity
for (int i = 0; i < 20; ++i) {
common::TrajectoryPoint trajectory_point(
fallback_trajectory_pair_candidate.first[stop_index]);
trajectory_point.set_relative_time(
i * 0.5 + 0.5 +
fallback_trajectory_pair_candidate.first[stop_index].relative_time());
fallback_trajectory_pair_candidate.first.AppendTrajectoryPoint(
trajectory_point);
}
如果无碰撞，则设置回退失败。
else {
frame_->mutable_open_space_info()->set_fallback_flag(false);
}