标题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对横向偏移进行一次计算。
在计算中,我们要考虑车的宽度,除去车的宽度和障碍物的占领位置后,计算剩下的横向边界。于是可以得到边界值,
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<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
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规划中搭建),我们可以得出那些横向偏移的值。
-
我们需要移植参数,比如这一些:
效果可看视频链接
从零开始搭建自动驾驶决策规划平台