Minimum Snap轨迹优化学习总结

最新推荐文章于 2024-07-16 10:04:02 发布

编程流觞

最新推荐文章于 2024-07-16 10:04:02 发布

阅读量553

点赞数 1

分类专栏： ADAS相关文章标签：算法

本文链接：https://blog.csdn.net/ls104174/article/details/119173282

版权

4 篇文章 1 订阅

订阅专栏

Mission

minimum Snap/Jerk/Acceleration将路径规划问题转化成一个二次规划问题（QP），因此需要安装求解器。在Linux系统下，可以使用的库有osqp，ipopt等。

参考链接教程。

分段v2.0：由于给定的参考点并不一定均匀，有疏有密，采用分段v1.0的策略不好。因此在给定点的基础上按照一定时间进行采样，通过线性插值的方法获取到多个中间点，同时设置每段中间点个数（反映分段长度），在此基础上再进行轨迹优化；
设置目标函数：根据最小化的指标，设置最小化各段多项式函数Snap/Jerk/Acceleration的平方和为目标函数1；由于目标函数1会舍弃掉低次项系数，会引起轨迹震荡，因此设置最小化多项式系数平方和为目标函数2；同时，为了让规划出的轨迹接近参考点，设置最小化优化点和参考点的距离平方和为目标函数3。整理如下：

目标	含义
1	各段多项式函数Snap/Jerk/Acceleration的平方和
2	多项式系数平方和
3	优化点和参考点的距离平方和

设置约束函数：各个点的位置，分段节点的位置连续（等式约束），分段节点的速度连续（等式约束），分段节点的加速度连续（等式约束），分段节点的加加速度连续（等式约束）。这样可以保证整个轨迹的速度、加速度曲线连续可导。
注：总约束个数 <= 求解变量个数；个人的形象表示：总约束个数 = 等式约束个数 + k * 不等式约束个数，其中k属于(0, 1)，不等式约束限制越大，k越接近1。

下面公式以Minimum Snap为例，可推算出Jerk和Acceleration。
$m i n f (p)$
$s.t. A_{eq}p=b_{eq}$
$A_{ieq}p \leq b_{ieq}$
多项式轨迹：
$p(t)=[1,t,t^2, ...,t^n]·p$
$snap(t)=p^{(4)}(t)=[0,0,0,0,24,...,\dfrac{n!}{(n-4)!}t^{n-4}]·p$

$minf(p)=min(p^{(4)}(t))^2$

目标函数，求和，求最小值：
$min\int_{0}^{T}(p^{(4)}(t))^2dt$
$=min\sum_{i=1}^{k}\int_{t_{i-1}}^{t_i}(p^{(4)}(t))^2dt$
$=min\sum_{i=1}^{k}\int_{t_{i-1}}^{t_i}([0,0,0,0,24,...,\dfrac{n!}{(n-4)!}t^{n-4}]·p)^T[0,0,0,0,24,...,\dfrac{n!}{(n-4)!}t^{n-4}]·pdt$
$=min\sum_{i=1}^{k}p^TQ_ipdt$
其中，
$Q_i = {0_{4 \times 4}\space \space \space \space \space \space\space \space\space \space \space \space \space 0_{4 \times (n-3))}\choose 0_{(n-3) \times 4} \space \space \space \space\space \space\space \dfrac{r!}{(r-4)!} \dfrac{c!}{(c-4)!} \dfrac{1}{(r-4)+(c-4)+1}(t^{(r+c-7)}_{i}-t^{(r+c-7)}_{i-1})}_{(n+1)*(n+1)}$
位置约束：
$[\underbrace{0 \cdots 0}_{(i-1)(n+1)}, 1,t_i,t^2_i,...,t^n_i, \underbrace{0 \cdots 0}_{(k-i)(n+1)}]·p=p_i$
连续性约束（位置，速度，加速度，加加速度）：
$[\underbrace{0 \cdots 0}_{(i-1)(n+1)}, 1,t_i,t^2_i,...,t^n_i, -1,-t_i,-t^2_i,...,-t^n_i, \underbrace{0 \cdots 0}_{(k-i-1)(n+1)}]·p=0$
$[\underbrace{0 \cdots 0}_{(i-1)(n+1)}, 0,1,2t^1_i,...,nt^{(n-1)}_i, 0,-1,-2t^1_i,...,-nt^{(n-1)}_i, \underbrace{0 \cdots 0}_{(k-i-1)(n+1)}]·p=0$
$[\underbrace{0 \cdots 0}_{(i-1)(n+1)}, 0,0,2,6t^1_i,...,n(n-1)t^{(n-2)}_i, 0,0,-2,-6t^1_i,...,-n(n-1)t^{(n-2)}_i, \underbrace{0 \cdots 0}_{(k-i-1)(n+1)}]·p=0$
$[\underbrace{0 \cdots 0}_{(i-1)(n+1)}, 0,0,0,6,24t^1_i,...,n(n-1)(n-2)t^{(n-3)}_i, 0,0,0,-6,-24t^1_i,...,-n(n-1)(n-2)t^{(n-3)}_i, \underbrace{0 \cdots 0}_{(k-i-1)(n+1)}]·p=0$

关注