QP样条路径优化器

QP样条路径优化器
提示:要阅读文档中的方程式,建议您使用带有插件的Chrome或将乳胶方程式复制到在线编辑器中

二次编程+样条插值

1.目标函数
1.1获取路径长度
路径在站点横向协调系统中定义。在小号范围从车辆的当前位置到默认的规划路径长度。

1.2获取样条线段
将路径分成n个段。每个线段轨迹由多项式定义。

1.3为每个样条线段定义功能
每个段我都有累积距离d一世沿参考线。默认情况下,线段的轨迹定义为次数为5的多项式。

l = f i ( s ) = a i 0 + a i 1   c d o t s + a i 2   c d o t s 2 + a i 3   c d o t s 3 + a i 4   c d o t s 4 + a i 5   c d o t s 5 ( 0   l e q s   l e q d i ) l = f_i(s) = a_ {i0} + a_ {i1} \ cdot s + a_ {i2} \ cdot s ^ 2 + a_ {i3} \ cdot s ^ 3 + a_ {i4} \ cdot s ^ 4 + a_ {i5} \ cdot s ^ 5(0 \ leq s \ leq d_ {i}) l=fis=ai0+ai1 cdots+ai2 cdots2+ai3 cdots3+ai4 cdots4+ai5 cdots50 leqs leqdi
1.4为每个细分定义优化的目标函数
c o s t =   s u m i = 1 n   B i g ( w 1   c d o t   i n t   l i m i t s 0 d i ( f i ′ ) 2 ( s ) d s + w 2   c d o t   i n t   l i m i t s 0 d i ( f i ′ ′ ) 2 ( s ) d s + w 3   c d o t   i n t   l i m i t s 0 d i ( f i   p r i m e   p r i m e   p r i m e ) 2 ( s ) d s   B i g ) cost = \ sum_ {i = 1} ^ {n} \ Big(w_1 \ cdot \ int \ limits_ {0} ^ {d_i}(f_i')^ 2(s)ds + w_2 \ cdot \ int \ limits_ {0} ^ {d_i}(f_i'')^ 2(s)ds + w_3 \ cdot \ int \ limits_ {0} ^ {d_i}(f_i ^ {\ prime \ prime \ prime})^ 2(s) ds \ Big) cost= sumi=1n Bigw1 cdot int limits0difi2sds+w2 cdot int limits0difi2sds+w3 cdot int limits0difi prime prime prime2sds Big
1.5将成本函数转换为QP公式
QP公式:

  b e g i n a l i g n e d 最 小 化 &   f r a c 12   c d o t x T   c d o t H   c d o t x + f T   c d o t x s t   q q u a d 和 L B   l e q x   l e q U B & A e q x = b e q & A x   g e q b   e n d a l i g n e d \ begin {aligned} 最小化&\ frac {1} {2} \ cdot x ^ T \ cdot H \ cdot x + f ^ T \ cdot x \\ st \ qquad和LB \ leq x \ leq UB \\ &A_ {eq} x = b_ {eq} \\ &Ax \ geq b \ end {aligned}  beginaligned frac12 cdotxT cdotH cdotx+fT cdotxst qquadLB leqx leqUBAeqx=beqAx geqb endaligned
以下是将成本函数转换为QP公式的示例。

f i ( s ) =   b e g i n v m a t r i x 1 & s & s 2 & s 3 & s 4 & s 5   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x f_i(s)= \ begin {vmatrix} 1&s&s ^ 2&s ^ 3&s ^ 4&s ^ 5 \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \\ a_ {i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} fis beginvmatrix1ss2s3s4s5 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix

f i ′ ( s ) =   b e g i n v m a t r i x 0 & 1 & 2 s & 3 s 2 & 4 s 3 & 5 s 4   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x f_i'(s)= \ begin {vmatrix} 0&1&2s&3s ^ 2&4s ^ 3&5s ^ 4 \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \\ a_ { i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} fis= beginvmatrix012s3s24s35s4 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix

f i ′ ( s ) 2 =   b e g i n v m a t r i x a i 0 & a i 1 & a i 2 & a i 3 & a i 4 & a i 5   e n d v m a t r i x   c d o t   b e g i n v m a t r i x 0 1 2 s 3 s 2 4 s 3 5 s 4   e n d v m a t r i x   c d o t   b e g i n v m a t r i x 0 & 1 & 2 s & 3 s 2 & 4 s 3 & 5 s 4   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x f_i'(s)^ 2 = \ begin {vmatrix} a_ {i0}&a_ {i1}&a_ {i2}&a_ {i3}&a_ {i4}&a_ {i5} \ end {vmatrix} \ cdot \ begin {vmatrix} 0 \\ 1 \\ 2s \\ 3s ^ 2 \\ 4s ^ 3 \\ 5s ^ 4 \ end {vmatrix} \ cdot \ begin {vmatrix} 0&1&2s&3s ^ 2& 4s ^ 3&5s ^ 4 \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \\ a_ {i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} fis2= beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix cdot beginvmatrix012s3s24s35s4 endvmatrix cdot beginvmatrix012s3s24s35s4 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix
那么我们有

  i n t   l i m i t s 0 d i f i ′ ( s ) 2 d s =   i n t   l i m i t s 0 d i   b e g i n v m a t r i x a i 0 & a i 1 & a i 2 和 a i 3 和 a i 4 和 a i 5   e n d v m a t r i x   c d o t   b e g i n v m a t r i x 0 1 2 s 3 s 2 4 s 3 5 s 4   e n d v m a t r i x   c d o t   b e g i n v m a t r i x 0 & 1 & 2 s & 3 s 2 & 4 s 3 & 5 s 4   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x d s \ int \ limits_ {0} ^ {d_i} f_i'(s)^ 2 ds = \ int \ limits_ {0} ^ {d_i} \ begin {vmatrix} a_ {i0}&a_ {i1}&a_ { i2}和a_ {i3}和a_ {i4}和a_ {i5} \ end {vmatrix} \ cdot \ begin {vmatrix} 0 \\ 1 \\ 2s \\ 3s ^ 2 \\ 4s ^ 3 \\ 5s ^ 4 \ end {vmatrix} \ cdot \ begin {vmatrix} 0&1&2s&3s ^ 2&4s ^ 3&5s ^ 4 \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \\ a_ { i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} ds  int limits0difis2ds int limits0di beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix cdot beginvmatrix012s3s24s35s4 endvmatrix cdot beginvmatrix012s3s24s35s4 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrixds
在集成之外提取const,我们有

  i n t   l i m i t s 0 d i f ′ ( s ) 2 d s =   b e g i n v m a t r i x a i 0 & a i 1 & a i 2 & a i 3 & a i 4 & a i 5   e n d v m a t r i x   c d o t   i n t   l i m i t s 0 d i   b e g i n v m a t r i x 0 1 2 s 3 s 2 4 s 3 5 s 4   e n d v m a t r i x   c d o t   b e g i n v m a t r i x 0 & 1 & 2 s & 3 s 2 & 4 s 3 & 5 s 4   e n d v m a t r i x d s   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x \ int \ limits_ {0} ^ {d_i} f'(s)^ 2 ds = \ begin {vmatrix} a_ {i0}&a_ {i1}&a_ {i2}&a_ {i3}&a_ {i4 }&a_ {i5} \ end {vmatrix} \ cdot \ int \ limits_ {0} ^ {d_i} \ begin {vmatrix} 0 \\ 1 \\ 2s \\ 3s ^ 2 \\ 4s ^ 3 \\ 5s ^ 4 \ end {vmatrix} \ cdot \ begin {vmatrix} 0&1&2s&3s ^ 2&4s ^ 3&5s ^ 4 \ end {vmatrix} ds \ cdot \ begin {vmatrix} a_ {i0} \\ a_ {i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix}  int limits0difs2ds beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix cdot int limits0di beginvmatrix012s3s24s35s4 endvmatrix cdot beginvmatrix012s3s24s35s4 endvmatrixds cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix
KaTeX parse error: Undefined control sequence: \开 at position 5: = \̲开̲始{vmatrix} a_ {…
最后,我们有

KaTeX parse error: Undefined control sequence: \开 at position 48: …'_i(s)^ 2 ds = \̲开̲始{vmatrix} a_ {…
请注意,我们得到了一个6 x 6的矩阵来表示5阶样条曲线的衍生成本。

类似的推论也可以用于计算二阶和三阶导数的成本。

2个约束
2.1初始点约束
假设第一点是(s0, 升0), (s0, 升′0)和(s0, 升′′0),在哪里 升0 , 升′0 和 升′′0 是横向偏移量及其在计划路径的初始点上的一阶和二阶导数,并根据 F一世(小号), F′一世(小号)和 F一世(s)′′.

使用以下命令将这些约束转换为QP相等约束:

A e q x = b e q A_ {eq} x = b_ {eq} Aeqx=beq
以下是转换步骤。

f i ( s 0 ) =   b e g i n v m a t r i x 1 & s 0 & s 0 2 & s 0 3 & s 0 4 & s 0 5   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x = l 0 f_i(s_0)= \ begin {vmatrix} 1&s_0&s_0 ^ 2&s_0 ^ 3&s_0 ^ 4&s_0 ^ 5 \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \\ a_ {i1 } \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} = l_0 fis0= beginvmatrix1s0s02s03s04s05 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix=l0

f i ′ ( s 0 ) =   b e g i n v m a t r i x 0 & 1 & 2 s 0 & 3 s 0 2 & 4 s 0 3 & 5 s 0 4   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x = l 0 ′ f'_i(s_0)= \ begin {vmatrix} 0&1&2s_0&3s_0 ^ 2&4s_0 ^ 3&5 s_0 ^ 4 \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \\ a_ { i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} = l'_0 fis0= beginvmatrix012s03s024s035s04 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix=l0

f i ′ ′ ( s 0 ) =   b e g i n v m a t r i x 0 & 0 & 2 & 3   t i m e s 2 s 0 & 4   t i m e s 3 s 0 2 & 5   t i m e s 4 s 0 3   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0    a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x = l 0 ′ ′ f''_ i(s_0)= \ begin {vmatrix} 0&0&2&3 \ times2s_0&4 \ times3s_0 ^ 2&5 \ times4s_0 ^ 3 \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \ \ a_ {i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} = l''_ 0 fis0= beginvmatrix0023 times2s04 times3s025 times4s03 endvmatrix cdot beginvmatrixai0  ai1ai2ai3ai4ai5 endvmatrix=l0
其中i是包含 s0.

2.2终点约束
与初始点相似,终点 (s是,升是) 是已知的,并且应产生与初始点计算中所述相同的约束。

合并起始点和结束点,并将相等约束显示为:

$$
\ begin {vmatrix}
1&s_0&s_0 ^ 2&s_0 ^ 3&s_0 ^ 4&s_0 ^ 5 \
0&1&2s_0&3s_0 ^ 2&4s_0 ^ 3&5s_0 ^ 4 \
0&0&2&3 \ times2s_0& 4 \ times3s_0 ^ 2&5 \ times4s_0 ^ 3 \
1&s_e&s_e ^ 2&s_e ^ 3&s_e ^ 4&s_e ^ 5 \
0&1&2s_e&3s_e ^^ 2&4s_e ^ 3&5s_e ^ 4 \
0& 0&2&3 \ times2s_e&4 \ times3s_e ^ 2&5 \ times4s_e ^ 3
\ end {vmatrix}
\ cdot
\ begin {vmatrix} a_ {i0} \ a_ {i1} \ a_ {i2} \ a_ {i3 } \ a_ {i4} \ a_ {i5} \ end {vmatrix}

\ begin {vmatrix}
l_0 \
l’0 \
l’’
0 \
l_e \
l’_e \
l’’_e \
\结束{vmatrix}
$$
2.3联合平滑度约束
设计该约束以平滑花键接头。假设两个细分小号ËG至 和 小号ËGk + 1被连接,并且段的累计s小号ËG至 是 s至。计算约束方程为:

f k ( s k ) = f k + 1 ( s 0 ) f_k(s_k)= f_ {k + 1}(s_0) fksk=fk+1s0
以下是计算步骤。

$$
\ begin {vmatrix}
1&s_k&s_k ^ 2&s_k ^ 3&s_k ^ 4&s_k ^ 5 \
\ end {vmatrix}
\ cdot
\ begin {vmatrix}
a_ {k0} \ a_ {k1} \ a_ {k2} \ a_ {k3} \ a_ {k4} \ a_ {k5}
\ end {vmatrix}

\ begin {vmatrix}
1&s_ {0}&s_ {0} ^ 2&s_ {0 } ^ 3&s_ {0} ^ 4&s_ {0} ^ 5 \
\ end {vmatrix}
\ cdot
\ begin {vmatrix}
a_ {k + 1,0} \ a_ {k + 1,1} \ a_ {k + 1,2} \ a_ {k + 1,3} \ a_ {k + 1,4} \ a_ {k + 1,5}
\ end {vmatrix}
$$
然后

  b e g i n v m a t r i x 1 & s k & s k 2 & s k 3 & s k 4 & s k 5 & − 1 & − s 0 & − s 0 2 & − s 0 3 & − s 0 4 & − s 0 5   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a k 0 a k 1 a k 2 a k 3 a k 4 a k 5 a k + 1 , 0 a k + 1 , 1 a k + 1 , 2 a k + 1 , 3 a k + 1 , 4 a k + 1 , 5   e n d v m a t r i x = 0 \ begin {vmatrix} 1&s_k&s_k ^ 2&s_k ^ 3&s_k ^ 4&s_k ^ 5&-1&-s_ {0}&-s_ {0} ^ 2&-s_ {0} ^ 3& -s_ {0} ^ 4&-s_ {0} ^ 5 \\ \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {k0} \\ a_ {k1} \\ a_ {k2} \\ a_ {k3} \\ a_ {k4} \\ a_ {k5} \\ a_ {k + 1,0} \\ a_ {k + 1,1} \\ a_ {k + 1,2} \\ a_ {k + 1, 3} \\ a_ {k + 1,4} \\ a_ {k + 1,5} \ end {vmatrix} = 0  beginvmatrix1sksk2sk3sk4sk51s0s02s03s04s05 endvmatrix cdot beginvmatrixak0ak1ak2ak3ak4ak5ak+1,0ak+1,1ak+1,2ak+13ak+1,4ak+1,5 endvmatrix=0
采用 s0 = 0在等式中。

类似地计算以下项的相等约束:

f k ′ ( s k ) = f k + 1 ′ ( s 0 ) f k ′ ′ ( s k ) = f k + 1 ′ ′ ( s 0 ) f k ′ ′ ′ ( s k ) = f k + 1 ′ ′ ′ ( s 0 ) f'_k(s_k)= f'_ {k + 1}(s_0) \\ f''_ k(s_k)= f''_ {k + 1}(s_0) \\ f'''_ k( s_k)= f'''_ {k + 1}(s_0) fksk=fk+1s0fksk=fk+1s0fksk=fk+1s0
2.4边界约束的采样点
沿路径均匀采样m个点,并检查这些点处的障碍物边界。使用以下方法将约束转换为QP不等式约束:

A x   g e q b Ax \ geq b Ax geqb
首先找到下界 升升b ,Ĵ 在这些时候 (sĴ,升Ĵ) 和 Ĵ ∈ [ 0 ,米]根据道路宽度和周围的障碍物 计算不平等约束为:

  b e g i n v m a t r i x 1 & s 0 & s 0 2 & s 0 3 & s 0 4 & s 0 5 1 & s 1 & s 1 2 & s 1 3 & s 1 4 & s 1 5 . . . & 。 . . & . . . & . . . & . . . & . . . 1 & s m & s m 2 & s m 3 & s m 4 & s m 5   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x   g e q   b e g i n v m a t r i x l l b , 0 l l b , 1 . . . l l b , m   e n d v m a t r i x \ begin {vmatrix} 1&s_0&s_0 ^ 2&s_0 ^ 3&s_0 ^ 4&s_0 ^ 5 \\ 1&s_1&s_1 ^ 2&s_1 ^ 3&s_1 ^ 4&s_1 ^ 5 \\ ...&。 ..&...&...&...&... \\ 1&s_m&s_m ^ 2&s_m ^ 3&s_m ^ 4&s_m ^ 5 \\ \ end {vmatrix} \ cdot \ begin {vmatrix } a_ {i0} \\ a_ {i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4} \\ a_ {i5} \ end {vmatrix} \ geq \ begin {vmatrix} l_ {lb ,0} \\ l_ {lb,1} \\ ... \\ l_ {lb,m} \\ \ end {vmatrix}  beginvmatrix1s0s02s03s04s051s1s12s13s14s15.................1smsm2sm3sm4sm5 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix geq beginvmatrixllb0llb1...llbm endvmatrix
同样,对于上限 升ü b ,Ĵ,将不等式约束计算为:

  b e g i n v m a t r i x − 1 & − s 0 & − s 0 2 & − s 0 3 & − s 0 4 & − s 0 5 − 1 & − s 1 & − s 1 2 & − s 1 3 & − s 1 4 & − s 1 5 . . . & . . . − & . . . & . . . & . . . & . . .   . . . − 1 & − s m & − s m 2 & − s m 3 & − s m 4 & − s m 5   e n d v m a t r i x   c d o t   b e g i n v m a t r i x a i 0 a i 1 a i 2 a i 3 a i 4 a i 5   e n d v m a t r i x   g e q − 1   c d o t   b e g i n v m a t r i x l u b , 0 l u b , 1 . . . l u b , m   e n d v m a t r i x \ begin {vmatrix} -1&-s_0&-s_0 ^ 2&-s_0 ^ 3&-s_0 ^ 4&-s_0 ^ 5 \\ -1&-s_1&-s_1 ^ 2&-s_1 ^ 3&- s_1 ^ 4&-s_1 ^ 5 \\ ...&...-&...&...&...&... \ ... \\ -1&-s_m&-s_m ^ 2&-s_m ^ 3 &-s_m ^ 4&-s_m ^ 5 \\ \ end {vmatrix} \ cdot \ begin {vmatrix} a_ {i0} \\ a_ {i1} \\ a_ {i2} \\ a_ {i3} \\ a_ {i4 } \\ a_ {i5} \ end {vmatrix} \ geq -1 \ cdot \ begin {vmatrix} l_ {ub,0} \\ l_ {ub,1} \\ ... \\ l_ {ub,m} \\ \ end {vmatrix}  beginvmatrix1s0s02s03s04s051s1s12s13s14s15.................. ...1smsm2sm3sm4sm5 endvmatrix cdot beginvmatrixai0ai1ai2ai3ai4ai5 endvmatrix geq1 cdot beginvmatrixlub0lub1...lubm endvmatrix

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值