Apollo规划代码ros移植-Lattcie的二次规划
大家好，我是研究生小铭，今天给大家分享我成功移植的Apollo规划-Lattcie的二次规划代码。我们的实验环境是ros，就是有发布和订阅特性的环境，这里给大家介绍实现步骤，需要完整代码可以留言或者私聊，如果觉得写得不错可以一键三连哦。
可以先看我之前写的：
链接: Apollo规划代码ros移植-Lattice规划框架.
二次规划的实现步骤：
1.要进行二次规划，我们必须离散化参考线，也就是参考线是有一系列密集的点构成。
2.我们要根据车的位置，计算出实时的规划初始纵向距离s_init。
3.计算出障碍物的横向偏移和纵向位置，在这里目前我只测试了静态障碍物。计算横向偏移和纵向位置的方法就是：
根据障碍物的边界顶点，计算顶点到参考线最近的位置点，找到与其对应的匹配点,根据参考线上的这个匹配点，计算纵向距离，匹配点与顶点的距离即为横向偏移。注意这里的横向偏移，有正负之分，在参考线右边为负，在参考线左边为正。（这是我一开始遇到的bug，后面细说）。
4.Aopllo的二次规划是前瞻60m（可以改，但是没必要），即我们要令
s_max=s_init+60(注意s_init是车的实时纵向距离位置，不一定都是0，因算法而异)。还有，需要delta_s=1.0，即每隔1m对横向偏移进行一次计算。
在计算中，我们要考虑车的宽度，除去车的宽度和障碍物的占领位置后，计算剩下的横向边界。于是可以得到边界值，
b o u n d s u p = [ d 0 , d 1 , . . . . . . . , d 59 ] b o u n d s l o w e r = [ d 0 , d 1 , . . . . . . . , d 59 ] bounds_{up} =[d_0,d_1,.......,d_{59}]\\ bounds_{lower} =[d_0,d_1,.......,d_{59}] boundsup=[d0,d1,.......,d59]boundslower=[d0,d1,.......,d59]
5.获得车的实时横向状态:
d s t a t e = [ d i , d i ˙ , d i ¨ ] d_{state}=[d_{i},\dot{d_{i}},\ddot{d_{i}}] dstate=[di,di˙,di¨]
6.至此，需要输入二次规划求解器（osqp）的参数已经齐了。
7.二次规划的原理：
m i n i m i z e 1 2 x T P x + q T x s u b j e c t t o l ≤ A x ≤ u minimize ~~~~\frac{1}{2} x^{T} Px+q^{T}x \\ subject ~~to ~~~~l\le Ax\le u minimize 21xTPx+qTxsubject to l≤Ax≤u
二次规划的一般形式如上式所示，第一行为代价函数，第二行为约束条件。二次优化的目标就是在满足约束条件的基础上，找到优化变量使得代价函数的值最小。
(1)构造出P权重矩阵即代价函数:
将此转化为矩阵形式的代码：(懒得画图哈哈哈)
void LateralOSQPOptimizer::CalculateKernel(const std::vector>& d_bounds,
std::vector* P_data, std::vector* P_indices,
std::vector* P_indptr)
{
const int kNumParam = 3 * static_cast(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(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(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 l l都是针对自变量 s s s来构建关系的。对于每一个采样点，存在两个不等式和两个等式约束，一共60个采样点，那么约束至少应该为60x4=240个约束条件。根据状态量的形式，约束矩阵A也就定了。
(4)边界约束的建立
其实就是前面提到的：
b o u n d s u p = [ d 0 , d 1 , . . . . . . . , d 59 ] b o u n d s l o w e r = [ d 0 , d 1 , . . . . . . . , d 59 ] bounds_{up} =[d_0,d_1,.......,d_{59}]\\ bounds_{lower} =[d_0,d_1,.......,d_{59}] boundsup=[d0,d1,.......,d59]boundslower=[d0,d1,.......,d59]
将-2,2作为缺省值不充值，凑够矩阵运算数量，最终用于：
b o u n d l o w ≤ A T ⋅ x ≤ b o u n d u p bound_{low}\leq A^{T}\cdot x \leq bound_{up} boundlow≤AT⋅x≤boundup
(5)状态量的确定
要求的状态量可以得到：
x = [ l 59 , l 58 , . . . , l 0 , l 59 ′ , l 58 ′ , . . . , l 0 ′ , l 59 ′ ′ , l 58 ′ ′ , . . . , l 0 ′ ′ ] T x=[l_{59}, l_{58} ,..., l_{0}, l_{59}', l_{58}' ,..., l_{0}', l_{59}'', l_{58}'' ,..., l_{0}'' ]^{T} x=[l59,l58,...,l0,l59′,l58′,...,l0′,l59′′,l58′′,...,l0′′]T
Ros移植的代码搭建：
-
移植核心的Lattice规划代码，这个可以参考我之前写的文章
链接: Apollo规划代码ros移植-Lattice规划框架. -
Qp求解器的核心代码移植，代码比如下面这些。
-
根据SL图（已经在Lattice规划中搭建），我们可以得出那些横向偏移的值。
-
我们需要移植参数，比如这一些：
效果：
车绕开静态障碍物：
车过不去会停下来：
Bug解决方法：
1.计算横向偏移的时候，记得有负号，但是大小的比较，要用绝对值比较，也就是说，-1.6比-1的横向偏移要大。不然会导致有一边没有求解出最优路径，即不能绕开障碍物。
2.可以将官网的前瞻60m改掉，但是要注意RIO范围，要跟自己的算法对得上，不然会导致改避开的障碍物没有避开。
这里有核心我移植的参考代码，注意只是我们测试车整体的规划部分的一小部分，里面的代码仅供参考，谨慎下载，代码不能直接运行，可以参考里面的代码，里面包括了Lattice离散空间采样规划。
代码下载链接: Lattice离散空间采样规划.