shenlan-homework

project_01 PID

主要完成部分pid_controller.cpp

project_02 Stanley_Control

主要完成部分stanley_control.cpp

核心思想:获取轨迹点和车的当前位置,计算距离车最近的点,用于计算横向和航向误差,找到课程中所学习的几何关系,得到横向的误差 e 和航向误差 e_theta ,在根据横向误差得到对应的角度,两个角度之和就是方向盘控制的角度了。

project_03 LQR

主要完成部分lqr_controller.cpp

步骤一：状态矩阵A

这里分matrix_a和 matrix_a_coeff是因为在后续离散的步骤方便。

步骤二：动力矩阵B

步骤三：计算横向误差

ComputeLateralErrors函数如上，因为描述系统状态是通过误差的方式，所以选用横向误差和航向误差以及他们的变化率为状态变量。

步骤四：更新状态A并将状态矩阵A离散化

这里采用双线性变换发进行离散处理：

步骤五：LQR

这里主要是根据lqr公式计算Algebraic Riccati equation公式，求得最佳的P值后，带入得到K。

步骤六：计算反馈 u = -K *（系统输入）

步骤七：计算前馈控制（主要是参考百度Apollo控制部分代码）

project_04 Frenet Optimal Trajectory Planner

主要完成部分frenet_optimal_trajectory.cpp

步骤一：轨迹采样

分别在横向和纵向构建多项式拟合曲线

横向采样：

构建五次多项式因为已知起点和终点的p、v、a，因为巡航过程，希望横向的速度和加速度均为0.

通过构建的多项式，计算在每个离散时间下的p v a jerk

纵向采样：

构建四次多项式，因为巡航在s坐标下我们不设定终点

计算每个路径的代价：

得到采样的轨迹后，对每个轨迹进行代价的计算，最后选择代价最小的作为规划轨迹

根据参考论文，

横向轨迹评价函数如下：

$C_d=w_JJ_t(d(t))+w_dd^2+w_tT$

其中： $J_t(d(t))=\int_{t_0}^{t_1}\dddot{d(\tau)d}\tau$ ，即jerk在采样时间内的变化幅度，用来评价舒适性； $w_dd^2$ 表示目标状态偏离中心线的指标； $w_tT$ 为车辆行驶效率评价指标。

纵向轨迹评价函数如下：

$C_s=w_JJ_t(d(t))+w_s(\dot{s_1}-\dot{s_{set}})^2+w_tT$

与横向评价函数唯一不同的是 $w_s(\dot{s_1}-\dot{s_{set}})^2$ 代表目标配置速度与设定期望速度的差距损失。

最后将二者进行合并，加入权重系数，即可得到最优路径评价函数：

$C=K_{lat}C_d+K_{lon}C_s$

通常轨迹评价函数越小，代表该轨迹的代价越小，优先度越高。

步骤二：计算其他的参数（航向角、曲率等）为步骤三做准备

步骤三：检查路径，添加最大速度、最大加速度、最大曲率约束、碰撞约束

遍历采样的路径，经过第二步计算出其最大速度、最大加速度、最大曲率，这些路径可能代价很小，但不安全，所以应该剔除

补充：替换成Apollo中lattice planner的代价

下图来自网络

在lattice planner中，对路径提出了6个代价，

纵向上：

目标cost：即保证纵向上速度越接近给定的值，cost越小，行驶距离越远，cost越小

舒适cost：舒适度与jerk息息相关，2为jerk的边界值，当jerk>2，则jerk/2 >1,(jerk/2)^2更大，以此来表示加速度的cost

碰撞cost：这里我们直接在第三步，当轨迹有障碍物时，直接踢出了，并没有作为一个cost

向心加速度cost：根据曲率而来

横向上：

偏移cost：当与起点的lat正向时，权重为1,否则权重为10,通过这样，横向上便宜较小的cost低

舒适型cost：公式为l‘’ * (s_dot)^2 + l' * s_dotdot 可以理解为当横向加速度较大的时候纵向速度应该小一点，当纵向加速度较大的时候，横向速度应该小一点。

//目标cost
double LonObjectiveCost(const FrenetPath &fp)
{
  double dist_s =
      fp.s.back() - fp.s.front();

  double speed_cost_sqr_sum = 0.0;
  double speed_cost_weight_sum = 0.0;
  for (size_t i = 0; i < fp.s.size(); ++i) {
    double cost = fp.s_d[i] - TARGET_SPEED;
    speed_cost_sqr_sum += fp.t[i] * fp.t[i] * std::fabs(cost);
    speed_cost_weight_sum += fp.t[i] * fp.t[i];
  }
  //关于速度的代价 速度越接近目标值，cost越小，t^2放大这一误差
  double speed_cost =
      speed_cost_sqr_sum / (speed_cost_weight_sum + 1e-6);
  //关于距离的代价
  double dist_travelled_cost = 1.0 / (1.0 + dist_s);
  return (speed_cost * FLAGS_weight_target_speed +
          dist_travelled_cost * FLAGS_weight_dist_travelled) /
         (FLAGS_weight_target_speed + FLAGS_weight_dist_travelled);
}

//纵向舒适型cost
double LonComfortCost(const FrenetPath& fp)
{
  double cost_sqr_sum = 0.0;
  double cost_abs_sum = 0.0;
  for (size_t i = 0; i < fp.s.size();++i)
  {
    double jerk = fp.s_ddd[i];
    //舒适度关于到jerk
    double cost = jerk / FLAGS_longitudinal_jerk_upper_bound;
    cost_sqr_sum += cost * cost;
    cost_abs_sum += std::fabs(cost);
  }
  return cost_sqr_sum / (cost_abs_sum + 1e-6);
}

//向心加速度
double CentripetalAccelerationCost(const FrenetPath& fp)
{
  // Assumes the vehicle is not obviously deviate from the reference line.
  double centripetal_acc_sum = 0.0;
  double centripetal_acc_sqr_sum = 0.0;
  for(size_t i = 0; i < fp.s.size();++i)
  {
    double s = fp.s[i];
    double v = fp.s_d[i];
    double centripetal_acc = v * v * fp.c[i];
    centripetal_acc_sum += std::fabs(centripetal_acc);
    centripetal_acc_sqr_sum += centripetal_acc * centripetal_acc;
  }

  return centripetal_acc_sqr_sum /
         (centripetal_acc_sum + FLAGS_numerical_epsilon);
}

//横向偏移代价
double LatOffsetCost(const FrenetPath& fp)
{
  double lat_offset_start = fp.d[0];
  double cost_sqr_sum = 0.0;
  double cost_abs_sum = 0.0;
  for (size_t i = 0; i < fp.s.size(); ++i)
  {
    double lat_offset = fp.d[i];
    double cost = lat_offset / FLAGS_lat_offset_bound;
    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;
    }
  }
  return cost_sqr_sum / (cost_abs_sum + 1e-6);
}

//横向舒适cost
double LatComfortCost(const FrenetPath& fp)
{
  double max_cost = 0.0;
  for (size_t i = 0; i < fp.s.size(); ++i)
  {
    double s = fp.s[i];
    double s_dot = fp.s_d[i];
    double s_dotdot = fp.s_dd[i];

    double d = fp.d[i];
    double d_dot = fp.d_d[i];
    double d_dotdot = fp.d_dd[i];
    double cost = d_dotdot * s_dot * s_dot + d_dot * s_dotdot;
    max_cost = std::max(max_cost, std::fabs(cost));
  }
  return max_cost;
}

tanruocan / homework

shenlan-homework

project_01 PID

project_02 Stanley_Control