Mission
- 根据给出的多目标点进行路径规划,形成一条平滑轨迹,使用的方法为minimum Snap/Jerk/Acceleration。
一、安装ipopt
1)目的:
minimum Snap/Jerk/Acceleration将路径规划问题转化成一个二次规划问题(QP),因此需要安装求解器。在Linux系统下,可以使用的库有osqp,ipopt等。
2)安装教程:
参考链接教程。
二、算法原理
1)输入信息
-
一系列参考点的位置信息,最终生成的轨迹需要穿过这些参考点,或者参考点到轨迹的距离在一定范围内;
-
起点、终点的速度和加速度;
-
总运动时间。
2)算法逻辑
- 划分时间:根据参考点之间的距离将总运动时间T划分,得到每一个参考点上的时间信息,初始点t=0,终止点t=T;
- 分段v1.0:点与点之间分一段,每段根据要最小化的指标(Snap/Jerk/Acceleration)用相对应的多项式进行优化,对应关系见下表;
指标 | 阶数 | 多项式 |
---|---|---|
Snap | 5 | p 0 + p 1 t + p 2 t 2 + p 3 t 3 + p 4 t 4 + p 5 t 5 p_0+p_1t+p_2t^2+p_3t^3+p_4t^4+p_5t^5 p0+p1t+p2t2+p3t3+p4t4+p5t5 |
Jerk | 4 | p 0 + p 1 t + p 2 t 2 + p 3 t 3 + p 4 t 4 p_0+p_1t+p_2t^2+p_3t^3+p_4t^4 p0+p1t+p2t2+p3t3+p4t4 |
Acceleration | 3 | p 0 + p 1 t + p 2 t 2 + p 3 t 3 p_0+p_1t+p_2t^2+p_3t^3 p0+p1t+p2t2+p3t3 |
- 分段v2.0:由于给定的参考点并不一定均匀,有疏有密,采用分段v1.0的策略不好。因此在给定点的基础上按照一定时间进行采样,通过线性插值的方法获取到多个中间点,同时设置每段中间点个数(反映分段长度),在此基础上再进行轨迹优化;
- 设置目标函数:根据最小化的指标,设置最小化各段多项式函数Snap/Jerk/Acceleration的平方和为目标函数1;由于目标函数1会舍弃掉低次项系数,会引起轨迹震荡,因此设置最小化多项式系数平方和为目标函数2;同时,为了让规划出的轨迹接近参考点,设置最小化优化点和参考点的距离平方和为目标函数3。整理如下:
目标 | 含义 |
---|---|
1 | 各段多项式函数Snap/Jerk/Acceleration的平方和 |
2 | 多项式系数平方和 |
3 | 优化点和参考点的距离平方和 |
- 设置约束函数:各个点的位置,分段节点的位置连续(等式约束),分段节点的速度连续(等式约束),分段节点的加速度连续(等式约束),分段节点的加加速度连续(等式约束)。这样可以保证整个轨迹的速度、加速度曲线连续可导。
- 注:总约束个数 <= 求解变量个数;个人的形象表示:总约束个数 = 等式约束个数 + k * 不等式约束个数,其中k属于(0, 1),不等式约束限制越大,k越接近1。
3)公式推导
-
下面公式以Minimum Snap为例,可推算出Jerk和Acceleration。
m i n f ( p ) minf(p) minf(p)
s . t . A e q p = b e q s.t. A_{eq}p=b_{eq} s.t.Aeqp=beq
A i e q p ≤ b i e q A_{ieq}p \leq b_{ieq} Aieqp≤bieq -
多项式轨迹:
p ( t ) = [ 1 , t , t 2 , . . . , t n ] ⋅ p p(t)=[1,t,t^2, ...,t^n]·p p(t)=[1,t,t2,...,tn]⋅p
s n a p ( t ) = p ( 4 ) ( t ) = [ 0 , 0 , 0 , 0 , 24 , . . . , n ! ( n − 4 ) ! t n − 4 ] ⋅ p snap(t)=p^{(4)}(t)=[0,0,0,0,24,...,\dfrac{n!}{(n-4)!}t^{n-4}]·p snap(t)=p(4)(t)=[0,0,0,0,24,...,(n−4)!n!tn−4]⋅p
m i n f ( p ) = m i n ( p ( 4 ) ( t ) ) 2 minf(p)=min(p^{(4)}(t))^2 minf(p)=min(p(4)(t))2
-
目标函数,求和,求最小值:
m i n ∫ 0 T ( p ( 4 ) ( t ) ) 2 d t min\int_{0}^{T}(p^{(4)}(t))^2dt min∫0T(p(4)(t))2dt
= m i n ∑ i = 1 k ∫ t i − 1 t i ( p ( 4 ) ( t ) ) 2 d t =min\sum_{i=1}^{k}\int_{t_{i-1}}^{t_i}(p^{(4)}(t))^2dt =mini=1∑k∫ti−1ti(p(4)(t))2dt
= m i n ∑ i = 1 k ∫ t i − 1 t i ( [ 0 , 0 , 0 , 0 , 24 , . . . , n ! ( n − 4 ) ! t n − 4 ] ⋅ p ) T [ 0 , 0 , 0 , 0 , 24 , . . . , n ! ( n − 4 ) ! t n − 4 ] ⋅ p d t =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 =mini=1∑k∫ti−1ti([0,0,0,0,24,...,(n−4)!n!tn−4]⋅p)T[0,0,0,0,24,...,(n−4)!n!tn−4]⋅pdt
= m i n ∑ i = 1 k p T Q i p d t =min\sum_{i=1}^{k}p^TQ_ipdt =mini=1∑kpTQipdt
其中,
Q i = ( 0 4 × 4 0 4 × ( n − 3 ) ) 0 ( n − 3 ) × 4 r ! ( r − 4 ) ! c ! ( c − 4 ) ! 1 ( r − 4 ) + ( c − 4 ) + 1 ( t i ( r + c − 7 ) − t i − 1 ( r + c − 7 ) ) ) ( n + 1 ) ∗ ( n + 1 ) 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)} Qi=(0(n−3)×4 (r−4)!r!(c−4)!c!(r−4)+(c−4)+11(ti(r+c−7)−ti−1(r+c−7))04×4 04×(n−3)))(n+1)∗(n+1) -
位置约束:
[ 0 ⋯ 0 ⏟ ( i − 1 ) ( n + 1 ) , 1 , t i , t i 2 , . . . , t i n , 0 ⋯ 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, \underbrace{0 \cdots 0}_{(k-i)(n+1)}]·p=p_i [(i−1)(n+1) 0⋯0,1,ti,ti2,...,tin,(k−i)(n+1) 0⋯0]⋅p=pi -
连续性约束(位置,速度,加速度,加加速度):
[ 0 ⋯ 0 ⏟ ( i − 1 ) ( n + 1 ) , 1 , t i , t i 2 , . . . , t i n , − 1 , − t i , − t i 2 , . . . , − t i n , 0 ⋯ 0 ⏟ ( k − i − 1 ) ( n + 1 ) ] ⋅ p = 0 [\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 [(i−1)(n+1) 0⋯0,1,ti,ti2,...,tin,−1,−ti,−ti2,...,−tin,(k−i−1)(n+1) 0⋯0]⋅p=0
[ 0 ⋯ 0 ⏟ ( i − 1 ) ( n + 1 ) , 0 , 1 , 2 t i 1 , . . . , n t i ( n − 1 ) , 0 , − 1 , − 2 t i 1 , . . . , − n t i ( n − 1 ) , 0 ⋯ 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 [(i−1)(n+1) 0⋯0,0,1,2ti1,...,nti(n−1),0,−1,−2ti1,...,−nti(n−1),(k−i−1)(n+1) 0⋯0]⋅p=0
[ 0 ⋯ 0 ⏟ ( i − 1 ) ( n + 1 ) , 0 , 0 , 2 , 6 t i 1 , . . . , n ( n − 1 ) t i ( n − 2 ) , 0 , 0 , − 2 , − 6 t i 1 , . . . , − n ( n − 1 ) t i ( n − 2 ) , 0 ⋯ 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 [(i−1)(n+1) 0⋯0,0,0,2,6ti1,...,n(n−1)ti(n−2),0,0,−2,−6ti1,...,−n(n−1)ti(n−2),(k−i−1)(n+1) 0⋯0]⋅p=0
[ 0 ⋯ 0 ⏟ ( i − 1 ) ( n + 1 ) , 0 , 0 , 0 , 6 , 24 t i 1 , . . . , n ( n − 1 ) ( n − 2 ) t i ( n − 3 ) , 0 , 0 , 0 , − 6 , − 24 t i 1 , . . . , − n ( n − 1 ) ( n − 2 ) t i ( n − 3 ) , 0 ⋯ 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 [(i−1)(n+1) 0⋯0,0,0,0,6,24ti1,...,n(n−1)(n−2)ti(n−3),0,0,0,−6,−24ti1,...,−n(n−1)(n−2)ti(n−3),(k−i−1)(n+1) 0⋯0]⋅p=0