# homework

**Repository Path**: tanruocan/homework

## Basic Information

- **Project Name**: homework
- **Description**: planning and control
- **Primary Language**: C++
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 2
- **Forks**: 0
- **Created**: 2022-10-31
- **Last Updated**: 2024-05-15

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 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

![image-20221031194123054](README.assets/image-20221031194123054.png)

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

### 步骤二：动力矩阵B

![image-20221031194134869](README.assets/image-20221031194134869.png)

### 步骤三：计算横向误差

![image-20220627151711928](README.assets/image-20220627151711928-16672163005431.png)

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



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

![image-20220627151928651](README.assets/image-20220627151928651-16672163233332.png)

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

![img](https://img-blog.csdnimg.cn/20190410191218617.png)

### 步骤五：LQR

![image-20220627152144397](README.assets/image-20220627152144397-16672163497083.png)

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



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

![image-20220704143033425](README.assets/image-20220704143033425-16672164104615.png)

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

![image-20220704143046403](README.assets/image-20220704143046403-16672164186606.png)



## project_04 Frenet Optimal Trajectory Planner

主要完成部分frenet_optimal_trajectory.cpp

### 步骤一：轨迹采样

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

**横向采样**：

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

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

![image-20221101155340068](README.assets/image-20221101155340068.png)



**纵向采样：**

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

![image-20221101155754144](README.assets/image-20221101155754144.png)

计算每个路径的代价：

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

根据参考论文，

**横向轨迹评价函数如下：**

![C_d=w_JJ_t(d(t))+w_dd^2+w_tT](https://latex.codecogs.com/gif.latex?C_d%3Dw_JJ_t%28d%28t%29%29+w_dd%5E2+w_tT)

其中：![J_t(d(t))=\int_{t_0}^{t_1}\dddot{d(\tau)d}\tau](https://latex.codecogs.com/gif.latex?J_t%28d%28t%29%29%3D%5Cint_%7Bt_0%7D%5E%7Bt_1%7D%5Cdddot%7Bd%28%5Ctau%29d%7D%5Ctau)，即jerk在采样时间内的变化幅度，用来评价舒适性；![w_dd^2](https://latex.codecogs.com/gif.latex?w_dd%5E2)表示目标状态偏离中心线的指标；![w_tT](https://latex.codecogs.com/gif.latex?w_tT)为车辆行驶效率评价指标。

**纵向轨迹评价函数如下：**

![C_s=w_JJ_t(d(t))+w_s(\dot{s_1}-\dot{s_{set}})^2+w_tT](https://latex.codecogs.com/gif.latex?C_s%3Dw_JJ_t%28d%28t%29%29+w_s%28%5Cdot%7Bs_1%7D-%5Cdot%7Bs_%7Bset%7D%7D%29%5E2+w_tT)

与横向评价函数唯一不同的是![w_s(\dot{s_1}-\dot{s_{set}})^2](https://latex.codecogs.com/gif.latex?w_s%28%5Cdot%7Bs_1%7D-%5Cdot%7Bs_%7Bset%7D%7D%29%5E2)代表目标配置速度与设定期望速度的差距损失。

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

![C=K_{lat}C_d+K_{lon}C_s](https://latex.codecogs.com/gif.latex?C%3DK_%7Blat%7DC_d+K_%7Blon%7DC_s)

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

![image-20221101155829054](README.assets/image-20221101155829054.png)



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

![image-20221101161524929](README.assets/image-20221101161524929.png)

![image-20221101161536378](README.assets/image-20221101161536378.png)



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

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

![image-20221101161815840](README.assets/image-20221101161815840.png)



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

下图来自网络

![img](https://img-blog.csdnimg.cn/20200606204625663.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zNDk0NTgwMw==,size_16,color_FFFFFF,t_70#pic_center)

![img](https://img-blog.csdnimg.cn/20200606204806233.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zNDk0NTgwMw==,size_16,color_FFFFFF,t_70#pic_center)

在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 可以理解为当横向加速度较大的时候纵向速度应该小一点，当纵向加速度较大的时候，横向速度应该小一点。

```c++
//目标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;
}

```