标签：Deviation begin Smoother end cdot 平滑 Fem vmatrix cost

目录：modules/planning/math/discretized_points_smoothing/fem_pos_deviation_osqp_interface.cc

平滑类比较独立，所以单独摘出来看

设计了三个代价函数，分别代表了平滑性、曲线总长度和参考点距离

另外注意到，大部分场景都是只考虑平滑性（平滑性权重默认设置为1e7，其他的权重设置为1）

[只有在OpenSpacePlanner中，多考虑了一点到原参考点距离，权重设置的也比较低，只有1e3](https://github.com/ApolloAuto/apollo/commit/3decb9251da7e10b8ddf730c92cb1834a551b099)

因此下方，以6个点P0,P1,P2...P6为例子，只考虑平滑性代价时：
$cost = \sum_{i=0}^{n-2}((x_i + x_{i+2}-2\times x_{i+1})^2 + (y_i + y_{i+2}-2\times y_{i+1})^2)$
将上方cost乘开，转为QP公式
$$
\frac{1}{2}\cdot x^T\cdot Q\cdot x+p^T\cdot x\\
s.t. LB \leq A\cdot x \leq UB
$$
没有一次项，p矩阵为0
Q矩阵如下：
$$
\begin{vmatrix}
1 & -2 & 1 & 0 & 0 & 0\\ 
-2 & 5 & -4 & 1 & 0 & 0\\ 
1 & -4 & 6 & -4 & 1 & 0\\ 
0 & 1 & -4 & 6 & -4 & 1\\ 
0 & 0 & 1 & -4 & 5 & -2\\ 
0 & 0 & 0 & 1 & 2 & 1
\end{vmatrix}
$$

LB和UB是根据左右道路宽度来取的
设置完成后调用osqp求解器进行求解

除了平滑性，还配置了曲线总长度和参考线距离两个代价函数。
曲线总长度代价，使用积分进行运算：
$cost = \sum_{i=0}^{n-1}(x_i - x_{i+1})^2 + (y_i - y_{i+1})^2$
转为QP公式，一次项为0
二次项矩阵：
$$
\begin{vmatrix}
1 & -2 & 0 & 0 & 0 & 0\\ 
0 & 2 & -2 & 0 & 0 & 0\\ 
0 & 0 & 2 & -2 & 0 & 0\\ 
0 & 0 & 0 & 2 & -2 & 0\\ 
0 & 0 & 0 & 0 & 2 & -2\\ 
0 & 0 & 0 & 0 & 0 & 1
\end{vmatrix}
$$

到原参考点的距离：
$cost = \sum_{i=0}^{n-1}(x_i - x_{ref})^2 + (y_i - y_{ref})^2$
Q矩阵：
$$
\begin{vmatrix}
1 & 0 & 0 & 0 & 0 & 0\\ 
0 & 1 & 0 & 0 & 0 & 0\\ 
0 & 0 & 1 & 0 & 0 & 0\\ 
0 & 0 & 0 & 1 & 0 & 0\\ 
0 & 0 & 0 & 0 & 1 & 0\\ 
0 & 0 & 0 & 0 & 0 & 1
\end{vmatrix}
$$
q矩阵：
$$
\begin{vmatrix}
-2\cdot x_{ref1}\\
-2\cdot x_{ref2}\\
-2\cdot x_{ref3}\\
-2\cdot x_{ref4}\\
-2\cdot x_{ref5}\\
-2\cdot x_{ref6}
\end{vmatrix}
$$

标签：Deviation,begin,Smoother,end,cdot,平滑,Fem,vmatrix,cost
来源： https://www.cnblogs.com/icathianrain/p/14407757.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。