参考线平滑-QpSplineReferenceLineSmoother
Apollo
中提供了QpSplineReferenceLineSmoother
,即分段五次多项式曲线平滑参考线方法。将其构造成二次规划问题形式,使用OSQP
求解。OSQP
求解的问题形式如下:
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 (0) minimize \;\;\; \frac{1}{2}x^T P x + q^T x \\ subject \; to \;\;\; l \leq Ax \leq u \tag{0} minimize21xTPx+qTxsubjecttol≤Ax≤u(0)
1. 分段多项式
将原始参考线以 25 m 25m 25m的长度切片,每段的 x , y x,y x,y分别是 s s s的五次多项式,即:
{ x = f ( s ) = a 0 + a 1 s + a 2 s 2 + a 3 s 3 + a 4 s 4 + a 5 s 5 y = g ( s ) = b 0 + b 1 s + b 2 s 2 + b 3 s 3 + b 4 s 4 + b 5 s 5 (1-1) \begin{cases} x = f(s) = a_0 + a_1 s + a_2 s^2 + a_3 s^3 + a_4 s^4 + a_5 s^5 \\ y = g(s) = b_0 + b_1 s + b_2 s^2 + b_3 s^3 + b_4 s^4 + b_5 s^5 \\ \end{cases} \tag{1-1} {x=f(s)=a0+a1s+a2s2+a3s3+a4s4+a5s5y=g(s)=b0+b1s+b2s2+b3s3+b4s4+b5s5(1-1)
此外,将每段多项式的 s s s范围归一化到 [ 0 , 1 ] [0, 1] [0,1]范围内,将原始参考点在 U T M UTM UTM上的坐标转换为基于第一个参考点的相对坐标。
bool QpSplineReferenceLineSmoother::Sampling() {const double length = anchor_points_.back().path_point.s() -anchor_points_.front().path_point.s();//以25m的长度切片uint32_t num_spline =std::max(1u, static_cast<uint32_t>(length / config_.qp_spline().max_spline_length() + 0.5));// 将多项式自变量归一化到[0,1]for (std::uint32_t i = 0; i <= num_spline; ++i) {t_knots_.push_back(i * 1.0);}// normalize point xy// 将坐标变化为相对坐标ref_x_ = anchor_points_.front().path_point.x();ref_y_ = anchor_points_.front().path_point.y();return true;
}
在这里使用了参数化方程,是因为自动驾驶车辆的行驶路径曲线在多数时候无法使用显示的表达式 y = f ( x ) y = f(x) y=f(x),而是需要隐式表达式 F ( x , y ) = 0 F(x,y)=0 F(x,y)=0,例如车辆在环岛行驶的路径为 x 2 + y 2 = r 2 x^2 + y^2 = r^2 x2+y2=r2。此外,参数化方程增加了一个维度 s s s,可以使低维下无法表示处理的几何在高纬度上方便的表现出来。
阶次越高的曲线可以拟合的曲线就越多,但是也容易出现龙格现象,因此使用了分段五次多项式,分段可以较为精确的拟合复杂道路,例如直角弯、 U U U型弯和 S S S型弯等,五次多项式又可以保证拟合曲线的高阶信息的平滑性(二阶对应速度,三阶对应加速度,四阶对应冲击度,因此五次多项式可以保证拟合的曲线足够平滑)。
将公式 ( 1 − 1 ) (1-1) (1−1)写为:
{ x i = f i ( s ) = a i 0 + a i 1 s + a i 2 s 2 + a i 3 s 3 + a i 4 s 4 + a i 5 s 5 = A i T S y i = g i ( s ) = b i 0 + b i 1 s + b i 2 s 2 + b i 3 s 3 + b i 4 s 4 + b i 5 s 5 = B i T S (1-2) \begin{cases} x_i = f_i(s) = a_{i0} + a_{i1} s + a_{i2} s^2 + a_{i3} s^3 + a_{i4} s^4 + a_{i5} s^5 = A^T_i S \\ y_i = g_i(s) = b_{i0} + b_{i1} s + b_{i2} s^2 + b_{i3} s^3 + b_{i4} s^4 + b_{i5} s^5 = B^T_i S \\ \end{cases} \tag{1-2} {xi=fi(s)=ai0+ai1s+ai2s2+ai3s3+ai4s4+ai5s5=AiTSyi=gi(s)=bi0+bi1s+bi2s2+bi3s3+bi4s4+bi5s5=BiTS(1-2)
其中 i i i表示第 i i i段五次多项式, A i = [ a i 0 , a i 1 , a i 2 , a i 3 , a i 4 , a i 5 ] T , B i = [ b i 0 , b i 1 , b i 2 , b i 3 , b i 4 , b i 5 ] T , S = [ 1 , s , s 2 , s 3 , s 4 , s 5 ] T A_i = [a_{i0}, a_{i1}, a_{i2}, a_{i3}, a_{i4}, a_{i5}]^T, B_i = [b_{i0}, b_{i1}, b_{i2}, b_{i3}, b_{i4}, b_{i5}]^T, S = [1, s, s^2, s^3, s^4, s^5]^T Ai=[ai0,ai1,ai2,ai3,ai4,ai5]T,Bi=[bi0,bi1,bi2,bi3,bi4,bi5]T,S=[1,s,s2,s3,s4,s5]T。需要优化求解的即是 A i T , B i T A^T_i, B^T_i AiT,BiT。假设有 N N N段多项式,则需要求解的优化变量数目为 2 ∗ 6 ∗ N 2 * 6 *N 2∗6∗N。
2. cost function
因为是平滑问题,那么优化问题的cost function自然是多项式曲率以及其变化率最小,由于曲率计算公式是非线性的,可以近似为多项式的二阶导数和三阶导数和最小,因此参考线平滑的cost function为:
J = ∑ i = 1 N ( ∫ s i , s t a r t s i , e n d ( f ′ ′ ( s ) ) 2 d s + ∫ s i , s t a r t s i , e n d ( g ′ ′ ( s ) ) 2 d s + ∫ s i , s t a r t s i , e n d ( f ′ ′ ′ ( s ) ) 2 d s + ∫ s i , s t a r t s i , e n d ( g ′ ′ ′ ( s ) ) 2 d s ) (2-1) J = \sum^N_{i=1} (\int_{s_{i,start}}^{s_{i,end}} {(f^{\prime \prime}(s))^2} ds + \int_{s_{i,start}}^{s_{i,end}} {(g^{\prime \prime}(s))^2} ds + \int_{s_{i,start}}^{s_{i,end}} {(f^{\prime \prime \prime}(s))^2} ds + \int_{s_{i,start}}^{s_{i,end}} {(g^{\prime \prime \prime}(s))^2} ds) \tag{2-1} J=i=1∑N(∫si,startsi,end(f′′(s))2ds+∫si,startsi,end(g′′(s))2ds+∫si,startsi,end(f′′′(s))2ds+∫si,startsi,end(g′′′(s))2ds)(2-1)
由于每段多项式因变量的范围都是 [ 0 , 1 ] [0,1] [0,1],所有公式 ( 2 − 1 ) (2-1) (2−1)可以写为:
J = ∑ i = 1 N ( ∫ 0 1 ( f ′ ′ ( s ) ) 2 d s + ∫ 0 1 ( g ′ ′ ( s ) ) 2 d s + ∫ 0 1 ( f ′ ′ ′ ( s ) ) 2 d s + ∫ 0 1 ( g ′ ′ ′ ( s ) ) 2 d s ) (2-2) J = \sum^N_{i=1} (\int_{0}^{1} {(f^{\prime \prime}(s))^2} ds + \int_{0}^{1} {(g^{\prime \prime}(s))^2} ds + \int_{0}^{1} {(f^{\prime \prime \prime}(s))^2} ds + \int_{0}^{1} {(g^{\prime \prime \prime}(s))^2} ds) \tag{2-2} J=i=1∑N(∫01(f′′(s))2ds+∫01(g′′(s))2ds+∫01(f′′′(s))2ds+∫01(g′′′(s))2ds)(2-2)
cost function有四部分组分,下面分别计算五次多项式的一阶导数、二阶导数和三阶导数的积分。
2.1 一阶导数积分
∫ 0 1 ( f ′ ( s ) ) 2 d s = ∫ 0 1 ( A T S ′ ) 2 d s = ∫ 0 1 ( A T S ′ S ′ T A ) d s = A T ∫ 0 1 ( S ′ S ′ T ) d s A (2-3) \int_{0}^{1} {(f^{\prime}(s))^2} ds = \int_{0}^{1} (A^T S^{\prime}) ^2 ds = \int_{0}^{1} (A^T S^{\prime} S^{{\prime}T} A) ds = A^T \int_{0}^{1} ( S^{\prime} S^{{\prime}T} ) ds A \tag{2-3} ∫01(f′(s))2ds=∫01(ATS′)2ds=∫01(ATS′S′TA)ds=AT∫01(S′S′T)dsA(2-3)
其中, S ′ S^{\prime} S′是 S S S的导数向量, S ′ = [ 1 , s , s 2 , s 3 , s 4 , s 5 ] ′ T = [ 0 , 1 , 2 s , 3 s 2 , 4 s 3 , 5 s 4 ] T S^{\prime} = [1, s, s^2, s^3, s^4, s^5]^{{\prime}T} = [0, 1, 2s, 3s^2, 4s^3, 5s^4]^{T} S′=[1,s,s2,s3,s4,s5]′T=[0,1,2s,3s2,4s3,5s4]T。代入公式 ( 2 − 3 ) (2-3) (2−3)可得:
∫ 0 1 ( f ′ ( s ) ) 2 d s = A T ∫ 0 1 ( S ′ S ′ T ) d s A = A T ( ∫ 0 1 [ 0 1 2 s 3 s 2 4 s 3 5 s 4 ] [ 0 1 2 s 3 s 2 4 s 3 5 s 4 ] d s ) A = A T ( ∫ 0 1 [ 0 0 0 0 0 0 0 1 2 s 3 s 2 4 s 3 5 s 4 0 2 s 4 s 2 6 s 3 8 s 4 10 s 5 0 3 s 2 6 s 3 9 s 4 12 s 5 15 s 6 0 4 s 3 8 s 4 12 s 5 16 s 6 20 s 7 0 5 s 4 10 s 5 15 s 6 20 s 7 25 s 8 ] d s ) A = A T [ 0 0 0 0 0 0 0 s s 2 s 3 s 4 s 5 0 s 2 4 3 s 3 6 4 s 4 8 5 s 5 10 6 s 6 0 3 3 s 3 6 4 4 9 5 s 5 12 6 s 6 15 7 s 7 0 4 4 s 4 8 5 s 5 12 6 s 6 16 7 s 7 20 8 s 8 0 5 5 s 5 10 6 s 6 15 7 s 7 20 8 s 8 25 9 s 9 ] A = A T [ 0 0 0 0 0 0 0 1 1 1 1 1 0 1 4 3 6 4 8 5 10 6 0 1 6 4 9 5 12 6 15 7 0 1 8 5 12 6 16 7 20 8 0 1 10 6 15 7 20 8 25 9 ] A (2-4) \begin{aligned} \int_{0}^{1} {(f^{\prime}(s))^2} ds &= A^T \int_{0}^{1} ( S^{\prime} S^{{\prime}T} ) ds A \\ &= A^T (\int_{0}^{1} \begin{bmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4\end{bmatrix} \begin{bmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{bmatrix} ds) A \\ &= A^T (\int_{0}^{1} \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \\ 0 & 2s & 4s^2 & 6s^3 & 8s^4 & 10s^5 \\ 0 & 3s^2 & 6s^3 & 9s^4 & 12s^5 & 15s^6 \\ 0 & 4s^3 & 8s^4 & 12s^5 & 16s^6 & 20s^7 \\ 0 & 5s^4 & 10s^5 & 15s^6 & 20s^7 & 25s^8 \end{bmatrix} ds) A \\ &= A^T \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & s & s^2 & s^3 & s^4 & s^5 \\ 0 & s^2 & \frac{4}{3}s^3 & \frac{6}{4}s^4 & \frac{8}{5}s^5 & \frac{10}{6}s^6 \\ 0 & \frac{3}{3}s^3 & \frac{6}{4}^4 & \frac{9}{5}s^5 & \frac{12}{6}s^6 & \frac{15}{7}s^7 \\ 0 & \frac{4}{4}s^4 & \frac{8}{5}s^5 & \frac{12}{6}s^6 & \frac{16}{7}s^7 & \frac{20}{8}s^8 \\ 0 & \frac{5}{5}s^5 & \frac{10}{6}s^6 & \frac{15}{7}s^7 & \frac{20}{8}s^8 & \frac{25}{9}s^9 \end{bmatrix} A \\ &= A^T \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 1 & 1 & 1 & 1 \\ 0 & 1 & \frac{4}{3} & \frac{6}{4} & \frac{8}{5} & \frac{10}{6} \\ 0 & 1 & \frac{6}{4} & \frac{9}{5} & \frac{12}{6} & \frac{15}{7} \\ 0 & 1 & \frac{8}{5} & \frac{12}{6} & \frac{16}{7} & \frac{20}{8} \\ 0 & 1 & \frac{10}{6} & \frac{15}{7} & \frac{20}{8} & \frac{25}{9} \end{bmatrix} A \end{aligned} \tag{2-4} ∫01(f′(s))2ds=AT∫01(S′S′T)dsA=AT(∫01⎣⎢⎢⎢⎢⎢⎢⎡012s3s24s35s4⎦⎥⎥⎥⎥⎥⎥⎤[012s3s24s35s4]ds)A=AT(∫01⎣⎢⎢⎢⎢⎢⎢⎡000000012s3s24s35s402s4s26s38s410s503s26s39s412s515s604s38s412s516s620s705s410s515s620s725s8⎦⎥⎥⎥⎥⎥⎥⎤ds)A=AT⎣⎢⎢⎢⎢⎢⎢⎡0000000ss233s344s455s50s234s346458s5610s60s346s459s5612s6715s70s458s5612s6716s7820s80s5610s6715s7820s8925s9⎦⎥⎥⎥⎥⎥⎥⎤A=AT⎣⎢⎢⎢⎢⎢⎢⎡00000001111101344658610014659612715015861271682001610715820925⎦⎥⎥⎥⎥⎥⎥⎤A(2-4)
令
Q 1 = [ 0 0 0 0 0 0 0 1 1 1 1 1 0 1 4 3 6 4 8 5 10 6 0 1 6 4 9 5 12 6 15 7 0 1 8 5 12 6 16 7 20 8 0 1 10 6 15 7 20 8 25 9 ] (2-5) Q^1 = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 1 & 1 & 1 & 1 \\ 0 & 1 & \frac{4}{3} & \frac{6}{4} & \frac{8}{5} & \frac{10}{6} \\ 0 & 1 & \frac{6}{4} & \frac{9}{5} & \frac{12}{6} & \frac{15}{7} \\ 0 & 1 & \frac{8}{5} & \frac{12}{6} & \frac{16}{7} & \frac{20}{8} \\ 0 & 1 & \frac{10}{6} & \frac{15}{7} & \frac{20}{8} & \frac{25}{9} \end{bmatrix} \tag{2-5} Q1=⎣⎢⎢⎢⎢⎢⎢⎡00000001111101344658610014659612715015861271682001610715820925⎦⎥⎥⎥⎥⎥⎥⎤(2-5)
则
Q i j 1 = { 0 , i = 0 o r j = 0 i ∗ j i + j − 1 , i ≠ 0 a n d j ≠ 0 (2-6) Q^1_{ij} = \begin{cases} 0, & i=0 & or & j = 0 \\ \frac{i * j}{i + j - 1}, & i \neq 0 & and & j \neq 0 \end{cases} \tag{2-6} Qij1={0,i+j−1i∗j,i=0i=0orandj=0j=0(2-6)
Eigen::MatrixXd SplineSegKernel::DerivativeKernel(const uint32_t num_params,const double accumulated_x) {if (num_params > reserved_order_ + 1) {// 计算s^n的系数,得到的结果是Q1矩阵CalculateDerivative(num_params);}Eigen::MatrixXd term_matrix;// 计算s的积分,由于s在区间[0,1],因此积分后的元素值为1IntegratedTermMatrix(num_params, accumulated_x, "derivative", &term_matrix);return kernel_derivative_.block(0, 0, num_params, num_params).cwiseProduct(term_matrix);
}
2.2 二阶导数积分
同一阶导数积分,可得:
∫ 0 1 ( f ′ ′ ( s ) ) 2 d s = A T Q 2 A (2-7) \int_{0}^{1} {(f^{\prime \prime}(s))^2} ds = A^T Q^2 A \tag{2-7} ∫01(f′′(s))2ds=ATQ2A(2-7)
其中:
Q i j 2 = { 0 , i ≤ 1 o r j ≤ 1 ( i ∗ i − i ) ( j ∗ j − j ) i + j − 3 , i > 1 a n d j > 1 (2-8) Q^2_{ij} = \begin{cases} 0, & i \leq 1 & or & j \leq 1 \\ \frac{(i * i - i)(j * j - j)}{i + j - 3}, & i > 1 & and & j > 1 \\ \end{cases} \tag{2-8} Qij2={0,i+j−3(i∗i−i)(j∗j−j),i≤1i>1orandj≤1j>1(2-8)
Eigen::MatrixXd SplineSegKernel::SecondOrderDerivativeKernel(const uint32_t num_params, const double accumulated_x) {if (num_params > reserved_order_ + 1) {CalculateSecondOrderDerivative(num_params);}Eigen::MatrixXd term_matrix;IntegratedTermMatrix(num_params, accumulated_x, "second_order", &term_matrix);return kernel_second_order_derivative_.block(0, 0, num_params, num_params).cwiseProduct(term_matrix);
}
2.3 三阶导数积分
同一阶导数积分,可得:
∫ 0 1 ( f ′ ′ ′ ( s ) ) 2 d s = A T Q 3 A (2-9) \int_{0}^{1} {(f^{\prime \prime \prime}(s))^2} ds = A^T Q^3 A \tag{2-9} ∫01(f′′′(s))2ds=ATQ3A(2-9)
其中:
Q i j 3 = { 0 , i ≤ 2 o r j ≤ 2 ( i ∗ i − i ) ( i − 2 ) ( j ∗ j − j ) ( j − 2 ) i + j − 5 , i > 2 a n d j > 2 (2-10) Q^3_{ij} = \begin{cases} 0, & i \leq 2 & or & j \leq 2 \\ \frac{(i * i - i)(i-2)(j * j - j)(j-2)}{i + j - 5}, & i > 2 & and & j > 2 \\ \end{cases} \tag{2-10} Qij3={0,i+j−5(i∗i−i)(i−2)(j∗j−j)(j−2),i≤2i>2orandj≤2j>2(2-10)
Eigen::MatrixXd SplineSegKernel::ThirdOrderDerivativeKernel(const uint32_t num_params, const double accumulated_x) {if (num_params > reserved_order_ + 1) {CalculateThirdOrderDerivative(num_params);}Eigen::MatrixXd term_matrix;IntegratedTermMatrix(num_params, accumulated_x, "third_order", &term_matrix);return (kernel_third_order_derivative_.block(0, 0, num_params, num_params)).cwiseProduct(term_matrix);
}
3. Constraint
约束有边界约束、起点约束和连续性约束。
3.1 边界约束
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-I1xhYtIx-1668078058001)(images/coord-trans.png)]
如上图所示, P 0 P_0 P0是参考线的第一个点, P i P_i Pi是参考线的第 i i i个点, θ \theta θ是点 P i P_i Pi的航向,点 P i P_i Pi处的航向方向为 P i x ⃗ \vec{P_i x} Pix ,其法向量方向为 P i y ⃗ \vec{P_i y} Piy 。边界约束即是约束平滑后的点和原始点在 P i x ⃗ \vec{P_i x} Pix 和 P i y ⃗ \vec{P_i y} Piy 方向上误差在一定阈值内。根据坐标变换公式可得:
{ ∣ P i A ⃗ ∣ = − ( ∣ P i B ⃗ ∣ + ∣ A B ⃗ ∣ ) = − ( Δ x cos θ + Δ y sin θ ) = − x i cos θ − y i sin θ ∣ P i M ⃗ ∣ = − ( ∣ P i M ⃗ ∣ − ∣ N M ⃗ ∣ ) = − ( Δ y cos θ − Δ x sin θ ) = x i sin θ − y i cos θ (3-1) \begin{cases} |\vec{P_i A}| = -(|\vec{P_i B}| + |\vec{AB}|) = -(\Delta x \cos{\theta} + \Delta y \sin{\theta}) = - x_i \cos{\theta} - y_i \sin{\theta} \\ |\vec{P_i M}| = -(|\vec{P_i M}| - |\vec{NM}|) = -(\Delta y \cos{\theta} - \Delta x \sin{\theta}) = x_i \sin{\theta} - y_i \cos{\theta} \\ \end{cases} \tag{3-1} {∣PiA ∣=−(∣PiB ∣+∣AB ∣)=−(Δxcosθ+Δysinθ)=−xicosθ−yisinθ∣PiM ∣=−(∣PiM ∣−∣NM ∣)=−(Δycosθ−Δxsinθ)=xisinθ−yicosθ(3-1)
const double d_lateral = SignDistance(ref_point[i], angle[i]);
const double d_longitudinal = SignDistance(ref_point[i], angle[i] - M_PI / 2.0);double Spline2dConstraint::SignDistance(const Vec2d& xy_point,const double angle) const {return common::math::InnerProd(xy_point.x(), xy_point.y(),-common::math::sin(common::math::Angle16::from_rad(angle)),common::math::cos(common::math::Angle16::from_rad(angle)));
}
从Apollo
的代码实现来看,以P_i
为原点的坐标的 x y xy xy轴方向是和图中相反的。d_longitudinal
是指航向方向投影,d_lateral
是指航向的法向量方向投影。
计算平滑后的点在原参考点坐标系上的投影,同样使用了公式 ( 3 − 1 ) (3-1) (3−1):
std::vector<double> Spline2dConstraint::AffineCoef(const double angle,const double t) const {const uint32_t num_params = spline_order_ + 1;std::vector<double> result(num_params * 2, 0.0);double x_coef = -common::math::sin(common::math::Angle16::from_rad(angle));double y_coef = common::math::cos(common::math::Angle16::from_rad(angle));for (uint32_t i = 0; i < num_params; ++i) {result[i] = x_coef;result[i + num_params] = y_coef;x_coef *= t;y_coef *= t;}return result;
}
最终,边界约束的代码实现为:
bool Spline2dConstraint::Add2dBoundary(const std::vector<double>& t_coord, const std::vector<double>& angle,const std::vector<Vec2d>& ref_point,const std::vector<double>& longitudinal_bound,const std::vector<double>& lateral_bound) {if (t_coord.size() != angle.size() || angle.size() != ref_point.size() ||ref_point.size() != lateral_bound.size() ||lateral_bound.size() != longitudinal_bound.size()) {return false;}Eigen::MatrixXd affine_inequality =Eigen::MatrixXd::Zero(4 * t_coord.size(), total_param_);Eigen::MatrixXd affine_boundary =Eigen::MatrixXd::Zero(4 * t_coord.size(), 1);for (uint32_t i = 0; i < t_coord.size(); ++i) {// 计算原始参考点的横纵向投影const double d_lateral = SignDistance(ref_point[i], angle[i]);const double d_longitudinal =SignDistance(ref_point[i], angle[i] - M_PI / 2.0);// 计算点在第几段五次多项式上const uint32_t index = FindIndex(t_coord[i]);// 将s归一化到[0,1]内const double rel_t = t_coord[i] - t_knots_[index];// y = g(s)的多项式系数对应的优化变量的索引值const uint32_t index_offset = 2 * index * (spline_order_ + 1);// 计算平滑后点的做投影计算后各个系数std::vector<double> longi_coef = AffineCoef(angle[i], rel_t);std::vector<double> longitudinal_coef =AffineCoef(angle[i] - M_PI / 2, rel_t);for (uint32_t j = 0; j < 2 * (spline_order_ + 1); ++j) {// upper longiaffine_inequality(4 * i, index_offset + j) = longi_coef[j];// lower longiaffine_inequality(4 * i + 1, index_offset + j) = -longi_coef[j];// upper longitudinalaffine_inequality(4 * i + 2, index_offset + j) = longitudinal_coef[j];// lower longitudinalaffine_inequality(4 * i + 3, index_offset + j) = -longitudinal_coef[j];}// 上下边界约束affine_boundary(4 * i, 0) = d_lateral - lateral_bound[i]; // 这里应该是 d_lateral + lateral_bound[i]affine_boundary(4 * i + 1, 0) = -d_lateral - lateral_bound[i];affine_boundary(4 * i + 2, 0) = d_longitudinal - longitudinal_bound[i]; //这里应该是 d_longitudinal + longitudinal_bound[i]affine_boundary(4 * i + 3, 0) = -d_longitudinal - longitudinal_bound[i];}return AddInequalityConstraint(affine_inequality, affine_boundary);
}
3.2 连续性约束
相邻两段五次多项式在连接处需要满足零阶、一阶和二阶导数相等。即:
{ f i ( 1 ) = a i , 0 + a i , 1 + a i , 2 + a i , 3 + a i , 4 + a i , 5 = a i + 1 , 0 = f i + 1 ( 0 ) g i ( 1 ) = b i , 0 + b i , 1 + b i , 2 + b i , 3 + b i , 4 + b i , 5 = b i + 1 , 0 = g i + 1 ( 0 ) f i ′ ( 1 ) = a i , 1 + 2 a i , 2 + 3 a i , 3 + 4 a i , 4 + 5 a i , 5 = a i + 1 , 1 = f i + 1 ′ ( 0 ) g i ′ ( 1 ) = b i , 1 + 2 b i , 2 + 3 b i , 3 + 4 b i , 4 + 5 b i , 5 = b i + 1 , 1 = g i + 1 ′ ( 0 ) f i ′ ′ ( 1 ) = 2 a i , 2 + 6 a i , 3 + 12 a i , 4 + 20 a i , 5 = 2 a i + 1 , 2 = f i + 1 ′ ′ ( 0 ) g i ′ ′ ( 1 ) = 2 b i , 2 + 6 b i , 3 + 12 b i , 4 + 20 b i , 5 = 2 b i + 1 , 2 = g i + 1 ′ ′ ( 0 ) (3-2) \begin{cases} f_i(1) = a_{i,0} + a_{i,1} + a_{i,2} + a_{i,3} + a_{i,4} + a_{i,5} = a_{i+1,0} = f_{i+1} (0) \\ g_i(1) = b_{i,0} + b_{i,1} + b_{i,2} + b_{i,3} + b_{i,4} + b_{i,5} = b_{i+1,0} = g_{i+1} (0) \\ f^{\prime}_i(1) = a_{i,1} + 2a_{i,2} + 3a_{i,3} + 4a_{i,4} + 5a_{i,5} = a_{i+1,1} = f^{\prime}_{i+1} (0) \\ g^{\prime}_i(1) = b_{i,1} + 2b_{i,2} + 3b_{i,3} + 4b_{i,4} + 5b_{i,5} = b_{i+1,1} = g^{\prime}_{i+1} (0) \\ f^{\prime \prime}_i(1) = 2a_{i,2} + 6a_{i,3} + 12a_{i,4} + 20a_{i,5} = 2a_{i+1,2} = f^{\prime \prime}_{i+1} (0) \\ g^{\prime \prime}_i(1) = 2b_{i,2} + 6b_{i,3} + 12b_{i,4} + 20b_{i,5} = 2b_{i+1,2} = g^{\prime \prime}_{i+1} (0) \\ \end{cases} \tag{3-2} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧fi(1)=ai,0+ai,1+ai,2+ai,3+ai,4+ai,5=ai+1,0=fi+1(0)gi(1)=bi,0+bi,1+bi,2+bi,3+bi,4+bi,5=bi+1,0=gi+1(0)fi′(1)=ai,1+2ai,2+3ai,3+4ai,4+5ai,5=ai+1,1=fi+1′(0)gi′(1)=bi,1+2bi,2+3bi,3+4bi,4+5bi,5=bi+1,1=gi+1′(0)fi′′(1)=2ai,2+6ai,3+12ai,4+20ai,5=2ai+1,2=fi+1′′(0)gi′′(1)=2bi,2+6bi,3+12bi,4+20bi,5=2bi+1,2=gi+1′′(0)(3-2)
bool Spline2dConstraint::AddSecondDerivativeSmoothConstraint() {if (t_knots_.size() < 3) {return true;}Eigen::MatrixXd affine_equality =Eigen::MatrixXd::Zero(6 * (t_knots_.size() - 2), total_param_);Eigen::MatrixXd affine_boundary =Eigen::MatrixXd::Zero(6 * (t_knots_.size() - 2), 1);for (uint32_t i = 0; i + 2 < t_knots_.size(); ++i) {const double rel_t = t_knots_[i + 1] - t_knots_[i];const uint32_t num_params = spline_order_ + 1;const uint32_t index_offset = 2 * i * num_params;std::vector<double> power_t = PolyCoef(rel_t);std::vector<double> derivative_t = DerivativeCoef(rel_t);std::vector<double> second_derivative_t = SecondDerivativeCoef(rel_t);for (uint32_t j = 0; j < num_params; ++j) {affine_equality(6 * i, j + index_offset) = power_t[j];affine_equality(6 * i + 1, j + index_offset) = derivative_t[j];affine_equality(6 * i + 2, j + index_offset) = second_derivative_t[j];affine_equality(6 * i + 3, j + index_offset + num_params) = power_t[j];affine_equality(6 * i + 4, j + index_offset + num_params) =derivative_t[j];affine_equality(6 * i + 5, j + index_offset + num_params) =second_derivative_t[j];}affine_equality(6 * i, index_offset + 2 * num_params) = -1.0;affine_equality(6 * i + 1, index_offset + 2 * num_params + 1) = -1.0;affine_equality(6 * i + 2, index_offset + 2 * num_params + 2) = -2.0;affine_equality(6 * i + 3, index_offset + 3 * num_params) = -1.0;affine_equality(6 * i + 4, index_offset + 3 * num_params + 1) = -1.0;affine_equality(6 * i + 5, index_offset + 3 * num_params + 2) = -2.0;}return AddEqualityConstraint(affine_equality, affine_boundary);
}
3.3 起点约束
从Apollo
的代码实现可以看出,当采用参考线拼接时,要求平滑轨迹在原始参考线(未平滑)的第一个点位置的航向方向一致,但是大小不要求一致,同时不要求 ( x , y ) (x,y) (x,y)坐标一致。结合第3.1小节中的图,也就是要求,在 P 0 x ⃗ \vec{P_0 x} P0x 方向上投影方向一致,即可以转化为不等式约束,如果参考点投影在 x x x轴正方向,则平滑后点在 x x x轴的投影值应该大于零,如果参考点投影在 x x x轴负方向,则平滑后点在 x x x轴的投影值应该小于零。同时平滑后点在 y y y轴的投影值应该等于零。
bool Spline2dConstraint::AddPointAngleConstraint(const double t,const double angle) {const uint32_t index = FindIndex(t);const uint32_t num_params = spline_order_ + 1;const uint32_t index_offset = index * 2 * num_params;const double rel_t = t - t_knots_[index];// add equality constraint// 在航向的法向量上投影为0Eigen::MatrixXd affine_equality = Eigen::MatrixXd::Zero(1, total_param_);Eigen::MatrixXd affine_boundary = Eigen::MatrixXd::Zero(1, 1);std::vector<double> line_derivative_coef = AffineDerivativeCoef(angle, rel_t);for (uint32_t i = 0; i < line_derivative_coef.size(); ++i) {affine_equality(0, i + index_offset) = line_derivative_coef[i];}// 在航向方向上投影应该保证同向,判断参考点的在航向方向上投影的方向// add inequality constraintEigen::MatrixXd affine_inequality = Eigen::MatrixXd::Zero(2, total_param_);const Eigen::MatrixXd affine_inequality_boundary =Eigen::MatrixXd::Zero(2, 1);std::vector<double> t_coef = DerivativeCoef(rel_t);int x_sign = 1;int y_sign = 1;double normalized_angle = fmod(angle, M_PI * 2);if (normalized_angle < 0) {normalized_angle += M_PI * 2;}if (normalized_angle > (M_PI / 2) && normalized_angle < (M_PI * 1.5)) {x_sign = -1;}if (normalized_angle >= M_PI) {y_sign = -1;}for (uint32_t i = 0; i < t_coef.size(); ++i) {affine_inequality(0, i + index_offset) = t_coef[i] * x_sign;affine_inequality(1, i + index_offset + num_params) = t_coef[i] * y_sign;}if (!AddEqualityConstraint(affine_equality, affine_boundary)) {return false;}return AddInequalityConstraint(affine_inequality, affine_inequality_boundary);
}
4. 正则化
在机器学习中,损失函数后面一般都会添加一个额外项,一般为 l 1 − n o r m l_1-norm l1−norm和 l 2 − n o r m l_2-norm l2−norm。
- L 1 L1 L1正则化是指权值向量 w w w中各个元素的绝对值之和,表示为 ∣ ∣ w ∣ ∣ 1 ||w||_1 ∣∣w∣∣1,可以用来进行特征选取;
- L 2 L2 L2正则化是指权值向量 w w w中各个元素的平方和,表示为 ∣ ∣ w ∣ ∣ 2 ||w||_2 ∣∣w∣∣2,可以防止模型过拟合;
在本问题中,正则化中权值向量相对于多项式的系数,也就是优化问题的决策变量,选择 L 2 L2 L2正则化。
参考线平滑-QpSplineReferenceLineSmoother相关推荐
- Apollo星火计划学习笔记——参考线平滑算法解析及实现(以U型弯道场景仿真调试为例)
文章目录 1. Apollo参考线介绍 1.1 参考线的作用 1.2 导航规划的路线 1.3 为什么需要重新生成参考线 1.4 ReferenceLine数据结构 1.5 ReferencePoint ...
- 无人驾驶算法——Baidu Apollo代码解析之ReferenceLine Smoother参考线平滑
无人驾驶算法--Baidu Apollo代码解析之ReferenceLine Smoother参考线平滑 Apollo 参考线平滑类 reference_line_provider.cc 代价函数 c ...
- Apollo planning之参考线平滑算法
Apollo studio 官网:Apollo开发者社区 (baidu.com) 星火计划2.0基础课:Apollo星火计划2.0_Apollo精品课 (baidu.com) 星火计划2.0专项课:A ...
- 百度Apollo代码阅读:参考线平滑FemPosDeviationSmoother
在Apollo 5.0中,新增加了FemPosDeviationSmoother参考线平滑方法.在reference_line_provider.cc中可以看到,Apollo主要的参考线平滑类有三个: ...
- 2.参考线平滑算法解析及实现
星火计划2.0基础课:https://apollo.baidu.com/community/online-course/2 星火计划2.0专项课:https://apollo.baidu.com/co ...
- Apollo 参考线平滑方法Fem Pos Deviation Smoother
配置文件: 平滑器的配置文件 位置: modules/planning/conf/planning.conf –smoother_config_filename=/apollo/modules/pla ...
- Apollo:参考线
Apollo项目的源码可以从github上获取:ApolloAuto/apollo. 本文中贴出的源码取自2019年2月7日的版本(版本:3.5) 概述 什么是参考线 就是高精地图所提供的道路的中心线 ...
- appollo-参考线平滑设定
参考线平滑设定 from https://github.com/ApolloAuto/apollo/blob/master/docs/specs/reference_line_smoother_cn. ...
- Apollo:参考线ReferenceLine是如何定义的
ReferenceLine 位于modules/planning/reference_line/reference_line.h 它是根据高清地图上的map_path_生成的,如下图: 默认情况下,只 ...
最新文章
- 上传漏洞学习——upload-labs 闯关(二)
- matlab绘制sign函数,MATLAB的Symbolic Math Toolbox详解
- SQL优化:从设计表结构开始(层次型表结构设计方法)
- leetcode406. 根据身高重建队列
- [2021-07-19 内测NOIP] 操作(状压DP),异或(字典树),等级(线段树),矩阵(DP)
- 别再问“自媒体怎么做”
- Qt总结之八:绘制仪表盘
- (暴力求解)百钱买百鸡问题升级版
- java拉起服务,从Java调用Restful服务
- Win11、Linux 双系统安装方法
- HP笔记本430 G3 ubuntu系统无线网卡驱动安装
- POS打印机设置字体大小
- c++ 编写函数返回两个值最小值_2020重新出发,MySql基础,MySql的函数amp;运算符
- 创新电影院5G的未来
- 成都启之航电商:抖音小店使用效果+性价比产品+直播
- 移动机器人轮式里程计
- flex、grid布局实践——神仙网址分享
- dwasp连接mysql_asp连接access数据库代码(ASP连接ACCESS数据库方式方法)包含.mdb和.accdb两种格式...
- c++ 随机字符串_连载|想用Python做自动化测试?了解数值计算和随机数生成神器...
- NFT是什么?一篇文章搞懂NFT的概念
热门文章
- Golang中Int32转换为int16丢失精度的具体过程
- SqlServer存储过程入门
- 爬虫管理平台Crawlab v0.4.3发布(界面上点几下就可安装pip或npm依赖)
- jsbox 导入_JSBox: 一个创造工具的工具
- 计算机课程美术课程的整合,试论信息技术与小学美术学科整合
- 亚马逊WEEE将强制执行不合规商品,如何结合测评来快速提高店铺产品权重?
- 当生活把我们打哭的时候,你怎么办?人生哲理
- 制作Ubuntu系统ISO镜像(制作启动盘)
- sqlserver200864位下载_SQL Server 2008 SP3
- 安卓实现判断手机网络连接状态是否联网,连接的是移动流量数据还是WIFI连接