apollo 参考线平滑

内容来自：
1.解析百度Apollo之参考线与轨迹
2.参考线提供器: ReferenceLineProvider
3.Apollo决策规划的基础:平滑的参考线
4.Matlab路径平滑实现

参考线提供器主要完成的工作是计算车辆在规划路径上的短期可行路径。

图片.png

图片.png

图片.png

参考线平滑器使用了二次规划（Quadratic programming ）和样条插值（Spline interpolation）算法。

图片.png

图片.png

执行QPSplinereferencelinesmooth，主要步骤：
1.路径点采样;
2.knots分段/二次规划平滑；
路径点采样
简单，anchor point是对原始Path进行采样，采样间隔为smoother_config_.max_constraint_interval()，默认5m一个点。knots的采样其实也是相似的，采样间隔为config_.qp_spline().max_spline_length()，默认25m：
最后得到的knots节点有num_spline+1个。得到了所有的knots，也就意味着可到了所有的段，很明显这里就需要拟合num_spline个段，每个段有x和y两个多项式函数。
处理
还需要对anchor_point的自变量s做处理，本来s是从0到length_递增。就是将自变量s从[0,length_]区间按比例映射到[0,num_spline]区间，这样每个段内anchor point的s都属于[a,a+1]内，如果在减去knots[a]那么所有自变量的取值范围就是[0,1]。同时还需要对应变量(x,y)做处理，处理方法：可以看到x和y都需要减去Path第一个点的世界坐标系坐标，说白了2n个(2*num_spline)函数的坐标原点是Path的第一个点。
约束条件

图片.png

边界约束

图片.png

真实点投影/预测点投影约束。
方向约束
第一个anchor point的heading应该和第一段的多项式函数f1和g1的偏导数方向一致，大小可以不一致。也就是： heading = argtan(g1'(s), f1'(s))
各段衔接处

图片.png

cost设置

图片.png

图片.png

图片.png

开源软件qpOASES求解。
平滑参考线采样

图片.png

参考线校验

图片.png

参考点航向计算

图片.png

图片.png