QP
c o s t = 1 2 x ⊤ H x + f ⊤ x A x ≤ b \begin{align} cost={}&\frac{1}{2}x^\top Hx+f^\top x\\ Ax\leq{}&b \end{align} cost=Ax≤21x⊤Hx+f⊤xb
QP spline
第k段曲线的表达式
l k = l k ( s ) = ∑ i = 0 5 a k i s i = a k 0 + a k 1 s + a k 2 s 2 + a k 3 s 3 + a k 4 s 4 + a k 5 s 5 = [ 1 s s 2 s 3 s 4 s 5 ] [ a k 0 a k 1 a k 2 a k 3 a k 4 a k 5 ] \begin{align} l_k={}&l_k(s)=\sum_{i=0}^5a_{ki}s^i=a_{k0}+a_{k1}s+a_{k2}s^2+a_{k3}s^3+a_{k4}s^4+a_{k5}s^5\notag\\ ={}&\left[\begin{matrix} 1&s&s^2&s^3&s^4&s^5 \end{matrix}\right] \left[\begin{matrix} a_{k0}\\ a_{k1}\\ a_{k2}\\ a_{k3}\\ a_{k4}\\ a_{k5} \end{matrix}\right] \end{align} lk==lk(s)=i=0∑5akisi=ak0+ak1s+ak2s2+ak3s3+ak4s4+ak5s5[1ss2s3s4s5] ak0ak1ak2ak3ak4ak5
令 a = [ a k 0 a k 1 a k 2 a k 3 a k 4 a k 5 ] ⊤ a=\left[\begin{matrix}a_{k0}&a_{k1}&a_{k2}&a_{k3}&a_{k4}&a_{k5}\end{matrix}\right]^\top a=[ak0ak1ak2ak3ak4ak5]⊤，其各阶导数
l k ′ = [ 0 1 2 s 3 s 2 4 s 3 5 s 4 ] a \begin{align} l_k'={}&\left[\begin{matrix} 0&1&2s&3s^2&4s^3&5s^4 \end{matrix}\right]a \end{align} lk′=[012s3s24s35s4]a
l k ′ ′ = [ 0 0 2 6 s 12 s 2 20 s 3 ] a \begin{align} l_k''={}&\left[\begin{matrix} 0&0&2&6s&12s^2&20s^3 \end{matrix}\right]a \end{align} lk′′=[0026s12s220s3]a
l k ′ ′ ′ = [ 0 0 0 6 24 s 60 s 2 ] a \begin{align} l_k'''={}&\left[\begin{matrix} 0&0&0&6&24s&60s^2 \end{matrix}\right]a \end{align} lk′′′=[000624s60s2]a
等式约束
曲线连接处等式约束（ 3 3 3阶连续），共 4 4 4个
{ l k − 1 ( s k ) = l k ( s k ) l k − 1 ′ ( s k ) = l k ′ ( s k ) l k − 1 ′ ′ ( s k ) = l k ′ ′ ( s k ) l k − 1 ′ ′ ′ ( s k ) = l k ′ ′ ′ ( s k ) , k = 1 , 2 , ⋯ , n \begin{align} \left\{\begin{aligned} l_{k-1}(s_k)&=l_{k}(s_k)\\ l_{k-1}'(s_k)&=l_{k}'(s_k)\\ l_{k-1}''(s_k)&=l_{k}''(s_k)\\ l_{k-1}'''(s_k)&=l_{k}'''(s_k)\\ \end{aligned}\right.,k=1,2,\cdots,n \end{align} ⎩ ⎨ ⎧lk−1(sk)lk−1′(sk)lk−1′′(sk)lk−1′′′(sk)=lk(sk)=lk′(sk)=lk′′(sk)=lk′′′(sk),k=1,2,⋯,n
首末端约束，共 4 × 2 4\times 2 4×2个
{
l
0
(
s
0
)
=
l
0
,
l
n
(
s
n
)
=
l
n
l
0
′
(
s
0
)
=
l
0
′
,
l
n
′
(
s
n
)
=
l
n
′
l
0
′
′
(
s
0
)
=
l
0
′
′
，
l
n
′
′
(
s
n
)
=
l
n
′
′
l
0
′
′
′
(
s
0
)
=
l
0
′
′
′
，
l
n
′
′
′
(
s
n
)
=
l
n
′
′
′
\begin{align} \left\{\begin{aligned} l_{0}(s_0)&=l_{0}, &l_n(s_n)&=l_n\\ l_{0}'(s_0)&=l_{0}', &l_n'(s_n)&=l_n'\\ l_{0}''(s_0)&=l_{0}''， &l_n''(s_n)&=l_n''\\ l_{0}'''(s_0)&=l_{0}'''， &l_n'''(s_n)&=l_n'''\\ \end{aligned}\right. \end{align}
⎩
⎨
⎧l0(s0)l0′(s0)l0′′(s0)l0′′′(s0)=l0,=l0′,=l0′′，=l0′′′，ln(sn)ln′(sn)ln′′(sn)ln′′′(sn)=ln=ln′=ln′′=ln′′′
等式约束共
4
(
n
+
2
)
4(n+2)
4(n+2)个，剩余自由度
2
n
−
8
2n-8
2n−8个。
QP可行的必要条件为：样条段数 n ≥ 4 n\geq 4 n≥4
通过设置约束采样弧长来调整约束格式，即样条段数
不等式约束
{ l ( s ) ∈ [ l l , l u ] l ′ ( s ) ∈ [ − l ˉ ′ , l ˉ ′ ] l ′ ′ ( s ) ∈ [ − l ˉ ′ ′ , l ˉ ′ ′ ] l ′ ′ ′ ( s ) ∈ [ − l ˉ ′ ′ ′ , l ˉ ′ ′ ′ ] \begin{align} \left\{\begin{aligned} l(s)&\in [l_l, l_u]\\ l'(s)&\in [-\bar{l}', \bar{l}']\\ l''(s)&\in [-\bar{l}'', \bar{l}'']\\ l'''(s)&\in [-\bar{l}''', \bar{l}''']\\ \end{aligned}\right. \end{align} ⎩ ⎨ ⎧l(s)l′(s)l′′(s)l′′′(s)∈[ll,lu]∈[−lˉ′,lˉ′]∈[−lˉ′′,lˉ′′]∈[−lˉ′′′,lˉ′′′]
其中， 0 0 0阶不等式约束通过道路边界和障碍物求得， 1 , 2 , 3 1,2,3 1,2,3阶约束通过参数给定。
不等式约束的作用：
-
可以减小求解空间，加快解算速度
-
约束曲线形状（曲率）
直角坐标系中曲率
κ = ∣ l ′ ′ ∣ ( 1 + l ′ 2 ) 3 / 2 ≤ ∣ l ′ ′ ∣ \begin{align} \kappa=\frac{|l''|}{(1+l'^2)^{3/2}}\leq|l''| \end{align} κ=(1+l′2)3/2∣l′′∣≤∣l′′∣
若reference_line平直时，约束二阶导可以对曲率起到约束作用（仅在约束点附近满足）
但不等式约束不能太过严苛，否则容易使求解空间为空或难以求得可行解
代价函数
c o s t = w r e f ∫ ( l ( s ) − l D P ( s ) ) 2 d s + w 1 ∫ l ′ 2 ( s ) d s + w 2 ∫ l ′ ′ 2 ( s ) d s + w 3 ∫ l ′ ′ ′ 2 ( s ) d s + w r e g ∥ a ∥ 2 \begin{align} cost=w_{\mathrm{ref}}\int(l(s)-l_{\mathrm{DP}}(s))^2 \mathrm{d}s+w_1\int l'^2(s)\mathrm{d}s+w_2\int l''^2(s)\mathrm{d}s+w_3\int l'''^2(s)\mathrm{d}s+w_{reg}\Vert a\Vert^2 \end{align} cost=wref∫(l(s)−lDP(s))2ds+w1∫l′2(s)ds+w2∫l′′2(s)ds+w3∫l′′′2(s)ds+wreg∥a∥2
w r e f w_{\mathrm{ref}} wref：与DP结果的相似性
w 1 w_{1} w1：曲线斜率
w 2 w_{2} w2：曲线曲率
w 3 w_{3} w3：曲线曲率变化率
w r e g w_{\mathrm{reg}} wreg：提高可解性，即hessian的正定性，加速求解，曲线平直
QP spline的规模为 i n p u t _ n u m × ( s p l i n e _ o r d e r + 1 ) \mathrm{input\_num}\times (\mathrm{spline\_order}+1) input_num×(spline_order+1)
主要约束数量为：等式约束4input_num，不等式约束4constrains_num
效果
暂时无法在飞书文档外展示此内容
Piecewise
将两个采样点之间用三阶曲线连接，或者说用离散的三阶积分串联模型来拟合路径，即
l ′ ′ ′ = l ′ ′ ′ \begin{align}l'''=l'''\end{align} l′′′=l′′′
离散后定积分有（前向）
{ l k + 1 ′ ′ = l k ′ ′ + l k ′ ′ ′ d s l k + 1 ′ = l k ′ + l k ′ ′ d s + 1 2 l k ′ ′ ′ d s 2 l k + 1 = l k + l k ′ d s + 1 2 l k ′ ′ d s 2 + 1 6 l k ′ ′ ′ d s 3 \begin{align} \left\{\begin{aligned} l''_{k+1}={}&l_k''+l_k'''\mathrm{d}s\\ l_{k+1}'={}&l_k'+l_k''\mathrm{d}s+\frac{1}{2}l_k'''\mathrm{d}s^2\\ l_{k+1}={}&l_k+l_k'\mathrm{d}s+\frac{1}{2}l_k''\mathrm{d}s^2+\frac{1}{6}l_k'''\mathrm{d}s^3 \end{aligned}\right. \end{align} ⎩ ⎨ ⎧lk+1′′=lk+1′=lk+1=lk′′+lk′′′dslk′+lk′′ds+21lk′′′ds2lk+lk′ds+21lk′′ds2+61lk′′′ds3
基于离散模型直接优化 0 , 1 , 2 0,1,2 0,1,2阶数值 l ( s k ) , l ′ ( s k ) , l ′ ′ ( s k ) l(s_k),l'(s_k),l''(s_k) l(sk),l′(sk),l′′(sk)， 自由度 3 3 3
每个piece， l ′ ′ l'' l′′定常，考虑积分
{ l ′ ′ ′ ∈ [ l l ′ ′ ′ , l u ′ ′ ′ ] l ′ ′ − l k ′ ′ = ∫ l ′ ′ ′ d s = l ′ ′ ′ d s ∈ [ l l ′ ′ ′ d s , l u ′ ′ ′ d s ] l ′ − l k ′ = ∫ l ′ ′ d s = ∫ ∫ l ′ ′ ′ d s 2 + l k ′ ′ d s = ∫ l ′ ′ ′ d s d s + l k ′ ′ d s = 1 2 ( l ′ ′ + l k ′ ′ ) d s l − l k = ∫ l ′ d s = ∫ ∫ l ′ ′ d s 2 + l k ′ d s = ∫ ∫ ∫ l ′ ′ ′ d s 3 + 1 2 l k ′ ′ d s 2 + l k ′ d s = ∫ ∫ l ′ ′ ′ d s d s 2 + 1 2 l k ′ ′ d s 2 + l k ′ d s = 1 6 l ′ ′ ′ d s 3 + 1 2 l k ′ ′ d s 2 + l k ′ d s = 1 6 l ′ ′ d s 2 + 1 3 l k ′ ′ d s 2 + l k ′ d s \begin{align} \left\{\begin{aligned} l'''\in{}&[l'''_l,l'''_u]\\ l''-l_k''={}&\int l'''\mathrm{d}s=l'''\mathrm{d}s\in[l'''_l\mathrm{d}s,l'''_u\mathrm{d}s]\\ l'-l_k'={}&\int l''\mathrm{d}s=\int\int l'''\mathrm{d}s^2+l_k''\mathrm{d}s=\int l'''\mathrm{d}s\mathrm{d}s+l_k''\mathrm{d}s=\frac{1}{2}(l''+l_k'')\mathrm{d}s\\ l-l_k={}&\int l'\mathrm{d}s=\int\int l''\mathrm{d}s^2+l_k'\mathrm{d}s=\int\int\int l'''\mathrm{d}s^3+\frac{1}{2}l''_k\mathrm{d}s^2+l_k'\mathrm{d}s\\ ={}&\int\int l'''\mathrm{d}s\mathrm{d}s^2+\frac{1}{2}l''_k\mathrm{d}s^2+l_k'\mathrm{d}s= \frac{1}{6}l'''\mathrm{d}s^3+\frac{1}{2}l''_k\mathrm{d}s^2+l_k'\mathrm{d}s\\ ={}&\frac{1}{6}l''\mathrm{d}s^2+\frac{1}{3}l''_k\mathrm{d}s^2+l_k'\mathrm{d}s \end{aligned}\right. \end{align} ⎩ ⎨ ⎧l′′′∈l′′−lk′′=l′−lk′=l−lk===[ll′′′,lu′′′]∫l′′′ds=l′′′ds∈[ll′′′ds,lu′′′ds]∫l′′ds=∫∫l′′′ds2+lk′′ds=∫l′′′dsds+lk′′ds=21(l′′+lk′′)ds∫l′ds=∫∫l′′ds2+lk′ds=∫∫∫l′′′ds3+21lk′′ds2+lk′ds∫∫l′′′dsds2+21lk′′ds2+lk′ds=61l′′′ds3+21lk′′ds2+lk′ds61l′′ds2+31lk′′ds2+lk′ds
等式约束
首端约束，共 3 3 3个
{ l 0 ( s 0 ) = l 0 l 0 ′ ( s 0 ) = l 0 ′ l 0 ′ ′ ( s 0 ) = l 0 ′ ′ \begin{align} \left\{\begin{aligned} l_{0}(s_0)&=l_{0}\\ l_{0}'(s_0)&=l_{0}'\\ l_{0}''(s_0)&=l_{0}''\\ \end{aligned}\right. \end{align} ⎩ ⎨ ⎧l0(s0)l0′(s0)l0′′(s0)=l0=l0′=l0′′
为了保证优化结果满足导数关系，需要导数约束（ 3 ( n − 1 ) 3(n-1) 3(n−1)个， 2 ( n − 1 ) 2(n-1) 2(n−1)个等式约束， n − 1 n-1 n−1个不等式约束）
{ l ′ ′ ′ ∈ [ − l ˉ ′ ′ ′ , l ˉ ′ ′ ′ ] l k + 1 ′ ′ − l k ′ ′ ∈ [ l l ′ ′ ′ d s , l u ′ ′ ′ d s ] l k + 1 ′ − l k ′ = 1 2 ( l k ′ ′ + l k + 1 ′ ′ ) d s l k + 1 − l k = l k ′ d s + ( 1 3 l k ′ ′ + 1 6 l k + 1 ′ ′ ) d s 2 k = 0 , 1 , ⋯ , n − 2 \begin{align} \left\{\begin{aligned} l'''\in{}&[-\bar{l}''',\bar{l}''']\\ l''_{k+1}-l_k''\in{}&[l'''_l\mathrm{d}s,l'''_u\mathrm{d}s]\\ l_{k+1}'-l_k'={}&\frac{1}{2}(l''_k+l''_{k+1})\mathrm{d}s\\ l_{k+1}-l_k={}&l'_k\mathrm{d}s+(\frac{1}{3}l''_k+\frac{1}{6}l''_{k+1})\mathrm{d}s^2 \end{aligned}\right.k=0,1,\cdots,n-2 \end{align} ⎩ ⎨ ⎧l′′′∈lk+1′′−lk′′∈lk+1′−lk′=lk+1−lk=[−lˉ′′′,lˉ′′′][ll′′′ds,lu′′′ds]21(lk′′+lk+1′′)dslk′ds+(31lk′′+61lk+1′′)ds2k=0,1,⋯,n−2
其中， l ˉ ′ ′ ′ = δ ˙ m a x L v \bar{l}'''=\frac{\dot{\delta}_\mathrm{max}}{Lv} lˉ′′′=Lvδ˙max可以被视作最大曲率变化率，具体推导在后面给出
共 3 3 3个自由度，等式约束 2 n + 2n+ 2n+个，QP必然可行
不等式约束
{ l ( s ) ∈ [ l l , l u ] l ′ ( s ) ∈ [ − l ˉ ′ , l ˉ ′ ] l ′ ′ ( s ) ∈ [ − l ˉ ′ ′ , l ˉ ′ ′ ] \begin{align} \left\{\begin{aligned} l(s)&\in [l_l, l_u]\\ l'(s)&\in [-\bar{l}', \bar{l}']\\ l''(s)&\in [-\bar{l}'', \bar{l}'']\\ \end{aligned}\right. \end{align} ⎩ ⎨ ⎧l(s)l′(s)l′′(s)∈[ll,lu]∈[−lˉ′,lˉ′]∈[−lˉ′′,lˉ′′]
其中， 0 0 0阶不等式约束通过道路边界和障碍物求得（同qp sl spline）， 1 1 1阶约束通过参数给定， 2 2 2阶约束用于约束曲率。轨迹曲率为
κ = ( ( l ′ ′ + ( κ ˙ r l + κ r l ′ ) tan d θ ) cos 2 d θ + ( 1 − κ r l ) κ r ) cos d θ ( 1 − κ r l ) 2 \begin{align}\kappa=\frac{\left((l''+(\dot{\kappa}_rl+\kappa_rl')\tan\mathrm{d}\theta)\cos^2\mathrm{d}\theta+(1-\kappa_rl)\kappa_r\right)\cos\mathrm{d}\theta}{(1-\kappa_rl)^2}\end{align} κ=(1−κrl)2((l′′+(κ˙rl+κrl′)tandθ)cos2dθ+(1−κrl)κr)cosdθ
当参考线曲率和横向偏移都很小时，忽略其乘积 κ r l \kappa_rl κrl并忽略角度微分，有
l ′ ′ ≈ κ − κ r = tan δ L − κ r ≈ δ L − κ r \begin{align}l''\approx\kappa-\kappa_r=\frac{\tan\delta}{L}-\kappa_r\approx\frac{\delta}{L}-\kappa_r\end{align} l′′≈κ−κr=Ltanδ−κr≈Lδ−κr
因此
l ˉ ′ ′ = δ m a x L − κ r \begin{align}\bar{l}''=\frac{\delta_\mathrm{max}}{L}-\kappa_r\end{align} lˉ′′=Lδmax−κr
对于三阶导，有
l ′ ′ ′ = d 3 l d s 3 = d d t d 2 l d s 2 ⋅ d t d s = d l ′ ′ d t 1 v = l ˙ ′ ′ 1 v \begin{align} l'''=\frac{\mathrm{d}^3l}{\mathrm{d}s^3} = \frac{\mathrm{d}}{\mathrm{d}t}\frac{\mathrm{d}^2l}{\mathrm{d}s^2}\cdot\frac{\mathrm{d}t}{\mathrm{d}s}= \frac{\mathrm{d}l''}{\mathrm{d}t}\frac{1}{v} =\dot{l}''\frac{1}{v} \end{align} l′′′=ds3d3l=dtdds2d2l⋅dsdt=dtdl′′v1=l˙′′v1
由 l ′ ′ ≈ δ L − κ r l''\approx\frac{\delta}{L}-\kappa_r l′′≈Lδ−κr可知
l ′ ′ ′ = l ˙ ′ ′ 1 v ≈ δ ˙ L v l ˉ ′ ′ ′ = δ ˙ m a x L v \begin{align} l'''={}&\dot{l}''\frac{1}{v}\approx\frac{\dot\delta}{Lv}\\ \bar{l}'''={}&\frac{\dot{\delta}_\mathrm{max}}{Lv} \end{align} l′′′=lˉ′′′=l˙′′v1≈Lvδ˙Lvδ˙max
代价函数
2 c o s t = w 0 ∑ i = 0 n − 1 l i 2 + w 1 ∑ i = 0 n − 1 l i ′ 2 + w 2 ∑ i = 0 n − 1 l i ′ ′ 2 + w 3 ∑ i = 0 n − 2 l i ′ ′ ′ 2 + w r e f ∑ i = 0 n − 1 ( l i − l D P i ) 2 + w e n d 0 ( l n − 1 − 0 ) 2 + w e n d 1 ( l n − 1 ′ − 0 ) 2 + w e n d 2 ( l n − 1 ′ ′ − 0 ) 2 + w e n d r e f ( l n − 1 − l D P n − 1 ) 2 \begin{align} 2cost={}&w_0\sum_{i=0}^{n-1}l_i^2+w_1\sum_{i=0}^{n-1}l_i'^2+w_2\sum_{i=0}^{n-1}l_i''^2+w_3\sum_{i=0}^{n-2}l_i'''^2+w_\mathrm{ref}\sum_{i=0}^{n-1}(l_i-l_{\mathrm{DP}i})^2\notag\\ {}& +w_{\mathrm{end}0}(l_{n-1}-0)^2+w_{\mathrm{end}1}(l_{n-1}'-0)^2+w_{\mathrm{end}2}(l_{n-1}''-0)^2+w_{\mathrm{endref}}(l_{n-1}-l_{\mathrm{DP}n-1})^2 \end{align} 2cost=w0i=0∑n−1li2+w1i=0∑n−1li′2+w2i=0∑n−1li′′2+w3i=0∑n−2li′′′2+wrefi=0∑n−1(li−lDPi)2+wend0(ln−1−0)2+wend1(ln−1′−0)2+wend2(ln−1′′−0)2+wendref(ln−1−lDPn−1)2
由 l i ′ ′ ′ = l i + 1 ′ ′ − l i ′ ′ d s l'''_i=\frac{l''_{i+1}-l''_i}{\mathrm{d}s} li′′′=dsli+1′′−li′′，可得
l i ′ ′ ′ 2 = l i + 1 ′ ′ 2 − 2 l i ′ ′ l i + 1 ′ ′ + l i ′ ′ 2 d s 2 = 1 d s 2 l i + 1 ′ ′ 2 + 1 d s 2 l i ′ ′ 2 − 2 d s 2 l i ′ ′ l i + 1 ′ ′ \begin{align} l_i'''^2=\frac{l_{i+1}''^2-2l''_il''_{i+1}+l_i''^2}{\mathrm{d}s^2}=\frac{1}{\mathrm{d}s^2}l_{i+1}''^2+\frac{1}{\mathrm{d}s^2}l_i''^2-\frac{2}{\mathrm{d}s^2}l''_il''_{i+1} \end{align} li′′′2=ds2li+1′′2−2li′′li+1′′+li′′2=ds21li+1′′2+ds21li′′2−ds22li′′li+1′′
代入成本函数
2 c o s t = w 0 ∑ i = 0 n − 1 l i 2 + w 1 ∑ i = 0 n − 1 l i ′ 2 + w 2 ∑ i = 0 n − 1 l i ′ ′ 2 + w r e f ∑ i = 0 n − 1 ( l i − l D P i ) 2 + w 3 d s 2 ( l 0 ′ ′ 2 + 2 ∑ i = 1 n − 2 l i ′ ′ + l n − 1 ′ ′ 2 − 2 ∑ i = 0 n − 2 l i ′ ′ l i + 1 ′ ′ ) + w e n d 0 ( l n − 1 − 0 ) 2 + w e n d 1 ( l n − 1 ′ − 0 ) 2 + w e n d 2 ( l n − 1 ′ ′ − 0 ) 2 + w e n d r e f ( l n − 1 − l D P n − 1 ) 2 \begin{align} 2cost={}&w_0\sum_{i=0}^{n-1}l_i^2+w_1\sum_{i=0}^{n-1}l_i'^2+w_2\sum_{i=0}^{n-1}l_i''^2+w_\mathrm{ref}\sum_{i=0}^{n-1}(l_i-l_{\mathrm{DP}i})^2\notag\\ {}&+\frac{w_3}{\mathrm{d}s^2}\left(l_0''^2+2\sum_{i=1}^{n-2}l_i''+l_{n-1}''^2-2\sum_{i=0}^{n-2}l_i''l_{i+1}''\right)\notag\\ {}& +w_{\mathrm{end}0}(l_{n-1}-0)^2+w_{\mathrm{end}1}(l_{n-1}'-0)^2+w_{\mathrm{end}2}(l_{n-1}''-0)^2+w_{\mathrm{endref}}(l_{n-1}-l_{\mathrm{DP}n-1})^2 \end{align} 2cost=w0i=0∑n−1li2+w1i=0∑n−1li′2+w2i=0∑n−1li′′2+wrefi=0∑n−1(li−lDPi)2+ds2w3(l0′′2+2i=1∑n−2li′′+ln−1′′2−2i=0∑n−2li′′li+1′′)+wend0(ln−1−0)2+wend1(ln−1′−0)2+wend2(ln−1′′−0)2+wendref(ln−1−lDPn−1)2
零阶hessian
| w 0 + w r e f w_0+w_\mathrm{ref} w0+wref | |||
|---|---|---|---|
| ⋱ \ddots ⋱ | |||
| w 0 + w r e f w_0+w_\mathrm{ref} w0+wref | |||
| w 0 + w r e f + w e n d 0 w_0+w_\mathrm{ref}+w_{\mathrm{end}0} w0+wref+wend0 |
一阶hessian
| w 1 w_1 w1 | |||
|---|---|---|---|
| ⋱ \ddots ⋱ | |||
| w 1 w_1 w1 | |||
| w 1 + w e n d 1 w_1+w_{\mathrm{end}1} w1+wend1 |
二阶hessian
| w 2 + w 3 d s 2 w_2+\frac{w_3}{\mathrm{d}s^2} w2+ds2w3 | − w 3 d s 2 -\frac{w_3}{\mathrm{d}s^2} −ds2w3 | |||
|---|---|---|---|---|
| − w 3 d s 2 -\frac{w_3}{\mathrm{d}s^2} −ds2w3 | w 2 + 2 w 3 d s 2 w_2+2\frac{w_3}{\mathrm{d}s^2} w2+2ds2w3 | ⋱ \ddots ⋱ | ||
| ⋱ \ddots ⋱ | ⋱ \ddots ⋱ | ⋱ \ddots ⋱ | ||
| ⋱ \ddots ⋱ | w 2 + 2 w 3 d s 2 w_2+2\frac{w_3}{\mathrm{d}s^2} w2+2ds2w3 | − w 3 d s 2 -\frac{w_3}{\mathrm{d}s^2} −ds2w3 | ||
| − w 3 d s 2 -\frac{w_3}{\mathrm{d}s^2} −ds2w3 | w 2 + w e n d 3 + w 3 d s 2 w_2+w_{\mathrm{end}3}+\frac{w_3}{\mathrm{d}s^2} w2+wend3+ds2w3 |
Gradient
| − w r e f l D P 0 -w_{\mathrm{ref}}l_{\mathrm{DP}0} −wreflDP0 | − w r e f l D P 1 -w_{\mathrm{ref}}l_{\mathrm{DP}1} −wreflDP1 | ⋯ \cdots ⋯ | − w r e f l D P n − 2 -w_{\mathrm{ref}}l_{\mathrm{DP}n-2} −wreflDPn−2 | − w r e f l D P n − 1 − w r e f e n d l D P n − 1 -w_{\mathrm{ref}}l_{\mathrm{DP}n-1}-w_{\mathrm{refend}}l_{\mathrm{DP}n-1} −wreflDPn−1−wrefendlDPn−1 |
|---|---|---|---|---|
| 规模 3 n 3n 3n，等式约束数量 2 n + 1 2n+1 2n+1，不等式约束数量 4 n − 1 4n-1 4n−1 |
Piecewise防止小数被淹没
需要将原变量放大，假设原变量为 l o l_o lo，尺度因子为 c i c_i ci，定义缩放后的变量为
l = x c \begin{align} l=\frac{x}{c} \end{align} l=cx
等式约束
首端约束，共 3 3 3个
{ x 0 ( s 0 ) = c 0 l 0 x 0 ′ ( s 0 ) = c 1 l 0 ′ x 0 ′ ′ ( s 0 ) = c 2 l 0 ′ ′ \begin{align} \left\{\begin{aligned} x_{0}(s_0)&=c_0l_{0}\\ x_{0}'(s_0)&=c_1l_{0}'\\ x_{0}''(s_0)&=c_2l_{0}''\\ \end{aligned}\right. \end{align} ⎩ ⎨ ⎧x0(s0)x0′(s0)x0′′(s0)=c0l0=c1l0′=c2l0′′
为了保证优化结果满足导数关系，需要导数约束（ 3 ( n − 1 ) 3(n-1) 3(n−1)个， 2 ( n − 1 ) 2(n-1) 2(n−1)个等式约束， n − 1 n-1 n−1个不等式约束）
{ l ′ ′ ′ ∈ [ − l ˉ ′ ′ ′ , l ˉ ′ ′ ′ ] x k + 1 ′ ′ − x k ′ ′ ∈ c 2 [ l l ′ ′ ′ d s , l u ′ ′ ′ d s ] c 2 ( x k + 1 ′ − x k ′ ) = c 1 2 ( x k ′ ′ + x k + 1 ′ ′ ) d s c 1 c 2 ( x k + 1 − x k ) = c 0 c 2 x k ′ d s + c 0 c 1 ( 1 3 x k ′ ′ + 1 6 x k + 1 ′ ′ ) d s 2 k = 0 , 1 , ⋯ , n − 2 \begin{align} \left\{\begin{aligned} l'''\in{}&[-\bar{l}''',\bar{l}''']\\ x''_{k+1}-x_k''\in{}&c_2[l'''_l\mathrm{d}s,l'''_u\mathrm{d}s]\\ c_2(x_{k+1}'-x_k')={}&\frac{c_1}{2}(x''_k+x''_{k+1})\mathrm{d}s\\ c_1c_2(x_{k+1}-x_k)={}&c_0c_2x'_k\mathrm{d}s+c_0c_1(\frac{1}{3}x''_k+\frac{1}{6}x''_{k+1})\mathrm{d}s^2 \end{aligned}\right.k=0,1,\cdots,n-2 \end{align} ⎩ ⎨ ⎧l′′′∈xk+1′′−xk′′∈c2(xk+1′−xk′)=c1c2(xk+1−xk)=[−lˉ′′′,lˉ′′′]c2[ll′′′ds,lu′′′ds]2c1(xk′′+xk+1′′)dsc0c2xk′ds+c0c1(31xk′′+61xk+1′′)ds2k=0,1,⋯,n−2
其中， l ˉ ′ ′ ′ = δ ˙ m a x L v \bar{l}'''=\frac{\dot{\delta}_\mathrm{max}}{Lv} lˉ′′′=Lvδ˙max可以被视作最大曲率变化率，具体推导在后面给出
共 3 3 3个自由度，等式约束 2 n + 2n+ 2n+个，QP必然可行
不等式约束
{ x ( s ) ∈ c 0 [ l l , l u ] x ′ ( s ) ∈ c 1 [ − l ˉ ′ , l ˉ ′ ] x ′ ′ ( s ) ∈ c 2 [ − l ˉ ′ ′ , l ˉ ′ ′ ] \begin{align} \left\{\begin{aligned} x(s)&\in c_0[l_l, l_u]\\ x'(s)&\in c_1[-\bar{l}', \bar{l}']\\ x''(s)&\in c_2[-\bar{l}'', \bar{l}'']\\ \end{aligned}\right. \end{align} ⎩ ⎨ ⎧x(s)x′(s)x′′(s)∈c0[ll,lu]∈c1[−lˉ′,lˉ′]∈c2[−lˉ′′,lˉ′′]
l ˉ ′ ′ = δ m a x L − κ r \begin{align}\bar{l}''=\frac{\delta_\mathrm{max}}{L}-\kappa_r\end{align} lˉ′′=Lδmax−κr
l ˉ ′ ′ ′ = δ ˙ m a x L v \begin{align} \bar{l}'''={}&\frac{\dot{\delta}_\mathrm{max}}{Lv} \end{align} lˉ′′′=Lvδ˙max
代价函数
2 c o s t = w 0 c 0 2 ∑ i = 0 n − 1 x i 2 + w 1 c 1 2 ∑ i = 0 n − 1 x i ′ 2 + w 2 c 2 2 ∑ i = 0 n − 1 x i ′ ′ 2 + w 3 ∑ i = 0 n − 2 l i ′ ′ ′ 2 + w r e f c 0 2 ∑ i = 0 n − 1 ( x i − x D P i ) 2 + w e n d 0 c 0 2 ( x n − 1 − 0 ) 2 + w e n d 1 c 1 2 ( x n − 1 ′ − 0 ) 2 + w e n d 2 c 2 2 ( x n − 1 ′ ′ − 0 ) 2 + w r e f e n d c 0 2 ( x n − 1 − x D P n − 1 ) 2 \begin{align} 2cost={}&\frac{w_0}{c_0^2}\sum_{i=0}^{n-1}x_i^2+\frac{w_1}{c_1^2}\sum_{i=0}^{n-1}x_i'^2+\frac{w_2}{c_2^2}\sum_{i=0}^{n-1}x_i''^2+w_3\sum_{i=0}^{n-2}l_i'''^2+\frac{w_\mathrm{ref}}{c_0^2}\sum_{i=0}^{n-1}(x_i-x_{\mathrm{DP}i})^2\notag\\ {}& +\frac{w_{\mathrm{end}0}}{c_0^2}(x_{n-1}-0)^2+\frac{w_{\mathrm{end}1}}{c_1^2}(x_{n-1}'-0)^2+\frac{w_{\mathrm{end}2}}{c_2^2}(x_{n-1}''-0)^2+\frac{w_\mathrm{refend}}{c_0^2}(x_{n-1}-x_{\mathrm{DP}n-1})^2 \end{align} 2cost=c02w0i=0∑n−1xi2+c12w1i=0∑n−1xi′2+c22w2i=0∑n−1xi′′2+w3i=0∑n−2li′′′2+c02wrefi=0∑n−1(xi−xDPi)2+c02wend0(xn−1−0)2+c12wend1(xn−1′−0)2+c22wend2(xn−1′′−0)2+c02wrefend(xn−1−xDPn−1)2
由 l i ′ ′ ′ = l i + 1 ′ ′ − l i ′ ′ d s l'''_i=\frac{l''_{i+1}-l''_i}{\mathrm{d}s} li′′′=dsli+1′′−li′′，可得
l i ′ ′ ′ 2 = l i + 1 ′ ′ 2 − 2 l i ′ ′ l i + 1 ′ ′ + l i ′ ′ 2 d s 2 = 1 d s 2 c 2 2 x i + 1 ′ ′ 2 + 1 d s 2 c 2 2 x i ′ ′ 2 − 2 d s 2 c 2 2 x i ′ ′ x i + 1 ′ ′ \begin{align} l_i'''^2=\frac{l_{i+1}''^2-2l''_il''_{i+1}+l_i''^2}{\mathrm{d}s^2}=\frac{1}{\mathrm{d}s^2c_2^2}x_{i+1}''^2+\frac{1}{\mathrm{d}s^2c_2^2}x_i''^2-\frac{2}{\mathrm{d}s^2c_2^2}x''_ix''_{i+1} \end{align} li′′′2=ds2li+1′′2−2li′′li+1′′+li′′2=ds2c221xi+1′′2+ds2c221xi′′2−ds2c222xi′′xi+1′′
代入成本函数
2 c o s t = w 0 c 0 2 ∑ i = 0 n − 1 x i 2 + w 1 c 1 2 ∑ i = 0 n − 1 x i ′ 2 + w 2 c 2 2 ∑ i = 0 n − 1 x i ′ ′ 2 + w r e f c 0 2 ∑ i = 0 n − 1 ( x i − x D P i ) 2 + w 3 c 2 2 d s 2 ( x 0 ′ ′ 2 + 2 ∑ i = 1 n − 2 x i ′ ′ + x n − 1 ′ ′ 2 − 2 ∑ i = 0 n − 2 x i ′ ′ x i + 1 ′ ′ ) + w e n d 0 c 0 2 ( x n − 1 − 0 ) 2 + w e n d 1 c 1 2 ( x n − 1 ′ − 0 ) 2 + w e n d 2 c 2 2 ( x n − 1 ′ ′ − 0 ) 2 + w r e f e n d c 0 2 ( x n − 1 − x D P n − 1 ) 2 \begin{align} 2cost={}&\frac{w_0}{c_0^2}\sum_{i=0}^{n-1}x_i^2+\frac{w_1}{c_1^2}\sum_{i=0}^{n-1}x_i'^2+\frac{w_2}{c_2^2}\sum_{i=0}^{n-1}x_i''^2+\frac{w_\mathrm{ref}}{c_0^2}\sum_{i=0}^{n-1}(x_i-x_{\mathrm{DP}i})^2\notag\\ {}&+\frac{w_3}{c_2^2\mathrm{d}s^2}\left(x_0''^2+2\sum_{i=1}^{n-2}x_i''+x_{n-1}''^2-2\sum_{i=0}^{n-2}x_i''x_{i+1}''\right)\notag\\ {}& +\frac{w_{\mathrm{end}0}}{c_0^2}(x_{n-1}-0)^2+\frac{w_{\mathrm{end}1}}{c_1^2}(x_{n-1}'-0)^2+\frac{w_{\mathrm{end}2}}{c_2^2}(x_{n-1}''-0)^2+\frac{w_\mathrm{refend}}{c_0^2}(x_{n-1}-x_{\mathrm{DP}n-1})^2 \end{align} 2cost=c02w0i=0∑n−1xi2+c12w1i=0∑n−1xi′2+c22w2i=0∑n−1xi′′2+c02wrefi=0∑n−1(xi−xDPi)2+c22ds2w3(x0′′2+2i=1∑n−2xi′′+xn−1′′2−2i=0∑n−2xi′′xi+1′′)+c02wend0(xn−1−0)2+c12wend1(xn−1′−0)2+c22wend2(xn−1′′−0)2+c02wrefend(xn−1−xDPn−1)2
零阶hessian
| w 0 c 0 2 + w r e f c 0 2 \frac{w_0}{c_0^2}+\frac{w_\mathrm{ref}}{c_0^2} c02w0+c02wref | |||
|---|---|---|---|
| ⋱ \ddots ⋱ | |||
| w 0 c 0 2 + w r e f c 0 2 \frac{w_0}{c_0^2}+\frac{w_\mathrm{ref}}{c_0^2} c02w0+c02wref | |||
| w 0 c 0 2 + w r e f c 0 2 + w e n d 0 c 0 2 \frac{w_0}{c_0^2}+\frac{w_\mathrm{ref}}{c_0^2}+\frac{w_{\mathrm{end}0}}{c_0^2} c02w0+c02wref+c02wend0 |
一阶hessian
| w 1 c 1 2 \frac{w_1}{c_1^2} c12w1 | |||
|---|---|---|---|
| ⋱ \ddots ⋱ | |||
| w 1 c 1 2 \frac{w_1}{c_1^2} c12w1 | |||
| w 1 c 1 2 + w e n d 1 c 1 2 \frac{w_1}{c_1^2}+\frac{w_{\mathrm{end}1}}{c_1^2} c12w1+c12wend1 |
二阶hessian
| w 2 c 2 2 + w 3 c 2 2 d s 2 \frac{w_2}{c_2^2}+\frac{w_3}{c_2^2\mathrm{d}s^2} c22w2+c22ds2w3 | − w 3 c 2 2 d s 2 -\frac{w_3}{c_2^2\mathrm{d}s^2} −c22ds2w3 | |||
|---|---|---|---|---|
| − w 3 c 2 2 d s 2 -\frac{w_3}{c_2^2\mathrm{d}s^2} −c22ds2w3 | w 2 c 2 2 + 2 w 3 c 2 2 d s 2 \frac{w_2}{c_2^2}+2\frac{w_3}{c_2^2\mathrm{d}s^2} c22w2+2c22ds2w3 | ⋱ \ddots ⋱ | ||
| ⋱ \ddots ⋱ | ⋱ \ddots ⋱ | ⋱ \ddots ⋱ | ||
| ⋱ \ddots ⋱ | w 2 c 2 2 + 2 w 3 c 2 2 d s 2 \frac{w_2}{c_2^2}+2\frac{w_3}{c_2^2\mathrm{d}s^2} c22w2+2c22ds2w3 | − w 3 c 2 2 d s 2 -\frac{w_3}{c_2^2\mathrm{d}s^2} −c22ds2w3 | ||
| − w 3 c 2 2 d s 2 -\frac{w_3}{c_2^2\mathrm{d}s^2} −c22ds2w3 | w 2 + w e n d 3 + w 3 d s 2 w_2+w_{\mathrm{end}3}+\frac{w_3}{\mathrm{d}s^2} w2+wend3+ds2w3 |
Gradient
| − w r e f c 0 l D P 0 -\frac{w_\mathrm{ref}}{c_0}l_{\mathrm{DP}0} −c0wreflDP0 | − w r e f c 0 l D P 1 -\frac{w_\mathrm{ref}}{c_0}l_{\mathrm{DP}1} −c0wreflDP1 | ⋯ \cdots ⋯ | − w r e f c 0 l D P n − 2 -\frac{w_\mathrm{ref}}{c_0}l_{\mathrm{DP}n-2} −c0wreflDPn−2 | − w r e f c 0 l D P n − 1 − w r e f c 0 l D P n − 1 -\frac{w_\mathrm{ref}}{c_0}l_{\mathrm{DP}n-1}-\frac{w_\mathrm{ref}}{c_0}l_{\mathrm{DP}n-1} −c0wreflDPn−1−c0wreflDPn−1 |
|---|
4698
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
