Apollo规划代码ros移植-Lattcie的二次规划

标题Apollo规划代码ros移植-Lattcie的二次规

想要可跑工程，看这里：
链接: Apollo6.0规划代码ros移植-路径规划可跑工程分享.

二次规划的实现步骤：

1.要进行二次规划，我们必须离散化参考线，也就是参考线是有一系列密集的点构成。
2.我们要根据车的位置，计算出实时的规划初始纵向距离s_init。
3.计算出障碍物的横向偏移和纵向位置，在这里目前我只测试了静态障碍物。计算横向偏移和纵向位置的方法就是：
根据障碍物的边界顶点，计算顶点到参考线最近的位置点，找到与其对应的匹配点,根据参考线上的这个匹配点，计算纵向距离，匹配点与顶点的距离即为横向偏移。注意这里的横向偏移，有正负之分，在参考线右边为负，在参考线左边为正。（这是我一开始遇到的bug，后面细说）。
4.Aopllo的二次规划是前瞻60m（可以改，但是没必要），即我们要令
s_max=s_init+60(注意s_init是车的实时纵向距离位置，不一定都是0，因算法而异)。还有，需要delta_s=1.0，即每隔1m对横向偏移进行一次计算。

在计算中，我们要考虑车的宽度，除去车的宽度和障碍物的占领位置后，计算剩下的横向边界。于是可以得到边界值，
$$\begin{gathered} bound{s}_{up}=[d_0,d_1,.......,d_{59}] \\ bound{s}_{lower}=[d_0,d_1,.......,d_{59}] \end{gathered}$$
5.获得车的实时横向状态:
$$d_{state}=[d_i,\dot{d_i},\ddot{d_i}]$$
6.至此，需要输入二次规划求解器（osqp）的参数已经齐了。
7.二次规划的原理：
$$\begin{gathered} minimize\frac{1}{2}{x}^{T}Px+q^{T}x \\ subjecttol\le Ax\le u \end{gathered}$$
二次规划的一般形式如上式所示，第一行为代价函数，第二行为约束条件。二次优化的目标就是在满足约束条件的基础上，找到优化变量使得代价函数的值最小。
(1)构造出P权重矩阵即代价函数:

将此转化为矩阵形式的代码：(懒得画图哈哈哈)

void LateralOSQPOptimizer::CalculateKernel(const std::vector<std::pair<double, double>>& d_bounds,
                                          std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
                                          std::vector<c_int>* P_indptr)
{
 const int kNumParam = 3 * static_cast<int>(d_bounds.size());
 P_data->resize(kNumParam);
 P_indices->resize(kNumParam);
 P_indptr->resize(kNumParam + 1);

 for (int i = 0; i < kNumParam; ++i)
 {
   if (i < static_cast<int>(d_bounds.size()))
   {
     P_data->at(i) = 2.0 * FLAGS_weight_lateral_offset + 2.0 * FLAGS_weight_lateral_obstacle_distance;
   }
   else if (i < 2 * static_cast<int>(d_bounds.size()))
   {
     P_data->at(i) = 2.0 * FLAGS_weight_lateral_derivative;
   }
   else
   {
     P_data->at(i) = 2.0 * FLAGS_weight_lateral_second_order_derivative;
   }
   P_indices->at(i) = i;
   P_indptr->at(i) = i;
 }
 P_indptr->at(kNumParam) = kNumParam;
 CHECK_EQ(P_data->size(), P_indices->size());
}

(2)左右边界作为偏差项q用以控制轨迹和参考线的偏离程度

   double q[kNumParam];
  for (int i = 0; i < kNumParam; ++i)
  {
    if (i < num_var)
    {
      q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance * (d_bounds[i].first + d_bounds[i].second);
    }
    else
    {
      q[i] = 0.0;
    }
  }

(3)车辆横向轨迹约束的建立

分别表示：
1）自车不能与障碍物碰撞或驶出边界。
2） 轨迹应该保持连续。
3）轨迹应该保持光滑。
4）横向加加速度(相对于s的二阶偏导)应在一定范围内
可以看出$l$都是针对自变量$s$来构建关系的。对于每一个采样点，存在两个不等式和两个等式约束，一共60个采样点，那么约束至少应该为60x4=240个约束条件。根据状态量的形式，约束矩阵A也就定了。

(4)边界约束的建立
其实就是前面提到的：
$$\begin{gathered} bound{s}_{up}=[d_0,d_1,.......,d_{59}] \\ bound{s}_{lower}=[d_0,d_1,.......,d_{59}] \end{gathered}$$
将-2,2作为缺省值不充值，凑够矩阵运算数量，最终用于：
$$boun{d}_{low}\le A^T\cdot x\le boun{d}_{up}$$
(5)状态量的确定
要求的状态量可以得到：
$$x=[l_{59},l_{58},...,l_0,l_{59}^{\prime },l_{58}^{\prime },...,l_0^{\prime },l_{59}^{\prime \prime },l_{58}^{\prime \prime },...,l_0^{\prime \prime }{]}^T$$

Ros移植的代码搭建：

1. 移植核心的Lattice规划代码，这个可以参考我之前写的文章
   链接: Apollo规划代码ros移植-Lattice规划框架.

2. Qp求解器的核心代码移植，代码比如下面这些。
   

3. 根据SL图（已经在Lattice规划中搭建），我们可以得出那些横向偏移的值。

4. 我们需要移植参数，比如这一些：
   效果可看视频链接
   从零开始搭建自动驾驶决策规划平台