Apollo IterativeAnchoringSmoother Open Space Planner中的离散点光滑算法

[TOC]
本文介绍 Open Space Planner 中的一个离散点光滑的算法– IterativeAnchoringSmoother.
相对独立嵌入在整个 Open Space Planner 算法里, 虽然有些冗余, 但是些思想还是值得借鉴的. 例如使用正态分布产生随机采点代替优化过程. 故记录之.

算法文件位置: /modules/planning/open_space/trajectory_smoother/iterative_anchoring_smoother.h.

这个类在整个 Open Space Planner 的位置见后续博文.

构造函数与输入

构造函数: 仅需要车长/宽/几何中心与后轴中心 offset.

输入: 接口 Smooth 成员函数

Smooth( const Eigen::MatrixXd& xWS, //Hybrid A*结果
const double init_a, const double init_v, //初始a/v
const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec,//广义障碍物vector
DiscretizedTrajectory* discretized_trajectory) //输出结果

数据准备

• CheckGear() 检查gear:

  获取本段的前进/后退档位, 不是从底盘获取而是用初始 heading angle 与行进 angle 的夹角判断.

  return std::abs(NormalizeAngle(init_tracking_angle - init_heading_angle)) < M_PI_2;

• 障碍物数据类型转换:

  obstacles_vertices_vec 换 LineSegment2d 类型的 obstacles_linesegments_vec.

• 粗 DiscretizedPath 获取:

  把 xWs 转换成 DiscretizedPath, 然后根据标定数据 interpolated_delta_s 间隔进行线性插值得到 interpolated_warm_start_point2ds (仅x,y). 过程中如果插值后点数小于4, 报错无法进行 heading continuity, 小于 6, 报错无法 initial zero kappa.

• AdjustStartEndHeading() 调整首末 heading:

  Adjust heading to ensure heading continuity. 保证首末点不会突变.

  见下图:

  

• SetPathProfile():

  Reset path profile by discrete point heading and curvature estimation. 上面的粗 DiscretizedPath 不含角度信息. 这里利用 DiscretePointsMath::ComputePathProfile() 计算 heading, kappa, dkappa, accumulated_s 输出成含角度信息的 DiscretizedPath.

• GenerateInitialBounds() 为避障环节预处理一下边界信息:

  Generate feasible bounds for each path point. 根据 estimate_bound 标志位设置成统一的 default_bound 或者是计算 path 上每个点到每个障碍物线段的距离, 最后的结果要减去 vehicle_shortest_dimension, 即自车的形状.

碰撞检测与重新规划点

• CheckCollisionAvoidance() 碰撞检测:

  判断在 Path 的每个点上自车 Box 与障碍物 LineSegment2d 是否 hasOverlap(), 然后把碰撞的点放入 input_colliding_point_index_ 点列中.

  这里的 overlap 算法以及后面的线性插值会在后续的 Apollo Math 库篇介绍.

• ReAnchoring() 重新规划点:

  对碰撞点进行随机位置调整直到不碰撞为止, 随机是使用正态分布, 尝试次数 reanchoring_trails_num 是标定值. 目前这个函数与步骤是被注释掉的(v5.5))

  首先 Path 点列里第一, 二个, 倒数第一, 二个碰撞的话直接返回错误, 调整也没啥意义.

  然后对碰撞点列进行循环, 如果是第一个碰撞点, theta 调整为上一个点的 theta, 距离随机为

  double rand_dev = common::math::Clamp(length_dis(gen), 0.8, -0.8); //上下截距
  double adjusted_delta_s = delta_s * (1.0 + rand_dev);
  path_points->at(index).set_x(path_points->at(index - 1).x() + adjusted_delta_s * std::cos(adjust_theta));

  正态分布随机数的产生见下面:

  std::random_device rd;
  std::default_random_engine gen = std::default_random_engine(rd());
  std::normal_distribution<> pos_dis{0, reanchoring_pos_stddev}; //位置调整随机数
  std::normal_distribution<> length_dis{0, reanchoring_length_stddev}; //距离调整随机数

  如果是倒数第二个碰撞点, theta 调整为最后一个碰撞点的 theta, 距离随机与上面类似.

  其他碰撞点

  double rand_dev_x =common::math::Clamp(pos_dis(gen), 2.0 * reanchoring_pos_stddev,-2.0 * reanchoring_pos_stddev);
  double rand_dev_y =common::math::Clamp(pos_dis(gen), 2.0 * reanchoring_pos_stddev,-2.0 * reanchoring_pos_stddev);
  path_points->at(index).set_x(path_points->at(index).x() + rand_dev_x);
  path_points->at(index).set_y(path_points->at(index).y() + rand_dev_y);

  最后根据 x,y 点列计算 theta, 利用函数 CalcHeadings(): 自点的上下游两个点进行计算(除了首末点)

  dx = 0.5 * (path_points[index + 1].x() - path_points[index - 1].x());

光滑 SmoothPah()

• AdjustPathBounds() 再调整一下 bounds:

  对 colliding_point_index 中每个点进行放松, bounds 元素乘以 collision_decrease_ratio.

  bounds 首末点约束设置为 0, kappa 统一设置为 0,bounds->at(2) = 0.0(但是 colliding_point_index 代码里还是空的,因此此函数暂时无效).

• FemPosDeviationSmoother() 离散点光滑库:

  使用FemPosDeviationSmoother.Solve()进行离散点的平滑计算.
  关于此处的离散点平滑算法, 可以参考此文章.

• SetPathProfile()

  把光滑后的点列变换成DiscretizedPath.

• CheckCollisionAvoidance() 最后进行碰撞检测

纵向规划

• SmoothSpeed()

  构造 PiecewiseJerkSpeedProblem 问题进行五次多项式纵向规划. 时刻数: $T=20\frac{s_{total}}{a_{max_r}}$,$n=\frac{T}{\Delta t}$

  速度 ds 的上限为std::fmax(max_v, std::abs(init_v))

  最后把求得结果放入 smoothed_speeds->AppendSpeedPoint 中,

  jerk 的计算: (dds[i] - dds[i - 1]) / delta_t).

• GenerateStopProfileFromPolynomial()(目前被注释掉了, 被 PiecewiseJerkSpeedProblem 代替)

  简单版的速度规划, 对纵向规划进行五次多项式拟合(终点速度为0), 时间采样 s=[2.0:0.2:8.0], 利用 QuinticPolynomialCurve1d 类构造.

  QuinticPolynomialCurve1d curve(0.0, init_speed, init_acc, stop_distance,0.0, 0.0, t);
  QuinticPolynomialCurve1d(const std::array<double, 3>& start,const std::array<double, 3>& end,const double param);

  IsValidPolynomialProfile() 函数, 初步删除不合格的拟合线, 判断方法是时间 s 上采样, 如果 v<0 或者 a>1 滤除. 只要拟合成功一个就结束把拟合结果放入 smoothed_speeds 里, 结束纵向规划.

整合输出

• CombinePathAndSpeed() 合并横纵向:

  先对纵向规划点 SpeedData 进行时间上线性插值 EvaluateByTime().时间单位 HkDenseTimeResolution. 当 SpeedData 的 s 大于 PathPoint 时终止插值.

  然后依照插值后的 SpeedData 对 path_points 按照 s 进行线性插值 Evaluate(). 最后得到一个 DiscretizedTrajectory.

• AdjustPathAndSpeedByGear() 根据 gear 调整正负:

  前进挡的情况下保持不变.

  后退挡时, Theta:NormalizeAngle(*trajectory_point*.path_point().theta() + M_PI)

  $s=-s, kappa=-kappa, dkappa=dkappa, v=-v, a=-a$.