CostThetaSmoother是Apollo中离散点参考线平滑方法的一种。
1. 优化目标
1.1 平滑性
参考线平滑的首要目标当然是平滑性，使用向量P1P2⃗\vec{P_1 P_2}P1P2和P2P3⃗\vec{P_2 P_3}P2P3之间夹角的余弦值来表示，显然cosθ\cos{\theta}cosθ越小，三个点P1,P2,P3P_1,P_2,P_3P1,P2,P3越接近一条直线，越平滑。因为θ\thetaθ越接近000，cosθ\cos{\theta}cosθ越大，所以应该使−cosθ-\cos{\theta}−cosθ越小。
Jsmooth=−∑i=2N−1cosθi=−∑i=2N−1Pi−1Pi⃗⋅PiPi+1⃗∣Pi−1Pi⃗∣∣PiPi+1⃗∣=−∑i=2N−1(xi−xi−1)(xi+1−xi)+(yi−yi−1)(yi+1−yi)(xi−1−xi)2+(yi−1−yi)2(xi+1−xi)2+(yi+1−yi)2(1-1) J_{smooth} = - \sum^{N-1}_{i=2} \cos{\theta_i} = - \sum^{N-1}_{i=2} \frac{\vec{P_{i-1} P_i} \cdot \vec{P_i P_{i+1}}} {|\vec{P_{i-1} P_i}| |\vec{P_i P_{i+1}}|} = - \sum^{N-1}_{i=2} \frac{(x_i - x_{i-1}) (x_{i+1} - x_i) + (y_i - y_{i-1}) (y_{i+1} - y_i)} {\sqrt{(x_{i-1} - x_i)^2 + (y_{i-1} - y_i)^2} \sqrt{(x_{i+1} - x_i)^2 + (y_{i+1} - y_i)^2}} \tag{1-1} Jsmooth=−i=2∑N−1cosθi=−i=2∑N−1∣Pi−1Pi∣∣PiPi+1
最低0.47元/天 解锁文章
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
