apollo 代码代码文档,转载从github
apollo文档
侵删
参考线平滑设定
二次规划(QP)+样条插值
1. 目标函数
1.1 分段寻路路径
将寻路路径划分为 n 段,每段用2个多项式表示:
x = f i ( t ) = a i 0 + a i 1 ∗ t + a i 2 ∗ t 2 + a i 3 ∗ t 3 + a i 4 ∗ t 4 + a i 5 ∗ t 5 x = f_i(t) = a_{i0} + a_{i1} * t + a_{i2} * t^2 + a_{i3} * t^3 + a_{i4} * t^4 + a_{i5} * t^5 x=fi(t)=ai0+ai1∗t+ai2∗t2+ai3∗t3+ai4∗t4+ai5∗t5
y = g i ( t ) = b i 0 + b i 1 ∗ t + b i 2 ∗ t 2 + b i 3 ∗ t 3 + b i 4 ∗ t 4 + b i 5 ∗ t 5 y = g_i(t) = b_{i0} + b_{i1} * t + b_{i2} * t^2 + b_{i3} * t^3 + b_{i4} * t^4 + b_{i5} * t^5 y=gi(t)=bi0+bi1∗t+bi2∗t2+bi3∗t3+bi4∗t4+bi5∗t5
1.2 定义样条段优化目标函数
c o s t = ∑ i = 1 n ( ∫ 0 t i ( f i ′ ′ ′ ) 2 ( t ) d t + ∫ 0 t i ( g i ′ ′ ′ ) 2 ( t ) d t ) cost = \sum_{i=1}^{n} \Big( \int\limits_{0}^{t_i} (f_i''')^2(t) dt + \int\limits_{0}^{t_i} (g_i''')^2(t) dt \Big) cost=i=1∑n(0∫ti(fi′′′)2(t)dt+0∫ti(gi′′′)2(t)dt)
1.3 将开销(cost)函数转换为QP公式
QP公式:
1 2 ⋅ x T ⋅ H ⋅ x + f T ⋅ x s . t . L B ≤ x ≤ U B A e q x = b e q A x ≤ b \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. LB \leq x \leq UB \\ A_{eq}x = b_{eq} \\ Ax \leq b 21⋅xT⋅H⋅x+fT⋅xs.t.LB≤x≤UBAeqx=beqAx≤b
2 约束条件
2.1 平滑节点约束
该约束的目的是使样条的节点更加平滑。假设两个段 s e g k seg_k segk 和 s e g k + 1 seg_{k+1} segk+1互相连接,且 s e g k seg_k segk的累计值 s 为 s k s_k sk。计算约束的等式为:
f k ( s k ) = f k + 1 ( s 0 ) f_k(s_k) = f_{k+1} (s_0) fk(sk)=fk+1(s0)
同样地,该公式也适用于下述等式:
f k ′ ( s k ) = f k + 1 ′ ( s 0 ) f k ′ ′ ( s k ) = f k + 1 ′ ′ ( s 0 ) f k ′ ′ ′ ( s k ) = f k + 1 ′ ′ ′ ( s 0 ) g k ( s k ) = g k + 1 ( s 0 ) g k ′ ( s k ) = g k + 1 ′ ( s 0 ) g k ′ ′ ( s k ) = g k + 1 ′ ′ ( s 0 ) g k ′ ′ ′ ( s k ) = g k + 1 ′ ′ ′ ( s 0 ) f'_k(s_k) = f'_{k+1} (s_0) \\ f''_k(s_k) = f''_{k+1} (s_0) \\ f'''_k(s_k) = f'''_{k+1} (s_0) \\ g_k(s_k) = g_{k+1} (s_0) \\ g'_k(s_k) = g'_{k+1} (s_0) \\ g''_k(s_k) = g''_{k+1} (s_0) \\ g'''_k(s_k) = g'''_{k+1} (s_0) fk′(sk)=fk+1′(s0)fk′′(sk)=fk+1′′(s0)fk′′′(sk)=fk+1′′′(s0)gk(sk)=gk+1(s0)gk′(sk)=gk+1′(s0)gk′′(sk)=gk+1′′(s0)gk′′′(sk)=gk+1′′′(s0)
2.2 点采样边界约束
在路径上均匀的取样 m 个点并检查这些点的预定义边界。
f i ( t l ) − x l < b o u n d a r y g i ( t l ) − y l < b o u n d a r y f_i(t_l) - x_l< boundary \\ g_i(t_l) - y_l< boundary fi(tl)−xl<boundarygi(tl)−yl<boundary