Reference Line Smoother

https://github.com/ApolloAuto/apollo/blob/master/docs/specs/reference_line_smoother.md

Quadratic programming（二次规划）） + Spline（样条曲线） interpolation

1. Objective function

1.1 Segment routing path

Segment routing path into n segments. each segment trajectory is defined by two polynomials（多项式）:

x--纵向位置，y--横向位置

求一次导数得到速度，二阶-加速度，三阶-jerk ...

 

1.2 Define objective function of optimization for each segment

为什么定义这个形式？三阶导数 ，然后平方，得到的函数再求积分

 

1.3 Convert the cost function to QP formulation

QP formulation:

2 Constraints

2.1 Joint smoothness constraints 连接处的平顺性

This constraint smoothes the spline joint. Let's assume two segments, and , are connected and the accumulated s of segment is  Calculate the constraint equation as:

第一段的从时间t :0---t , arc length 变换 s0-sk  （假设速度恒定，匀速圆周运动， t= s）

第二段一样，时间从0-t+1, arc length 变化：s0-sk+1

连接处2段曲线方程相交，对应的加速度，jerk etc.

 

Similarly the formula works for the equality constraints, such as:

2.2 Sampled points for boundary constraint

Evenly sample m points along the path and check the predefined boundaries at those points.