使用贝塞尔曲线进行优化的方法

贝塞尔曲线:

f ( t ) = ∑ i = 0 m p i b m i ( t ) = pb ( t ) f(t) = \sum_{i=0}^{m}p_ib_m^{i}(t) =\textbf{p} \textbf{b}(t) f(t)=i=0mpibmi(t)=pb(t)
t ∈ [ 0 , 1 ] t \in [0,1] t[0,1] p i p_i pi未贝塞尔曲线的参数,即 p i p_i pi决定了贝塞尔曲线的形状, p = [ p 0 , p 1 , . . . , p m ] \textbf{p}=[p_0, p_1, ... , p_m] p=[p0,p1,...,pm], b = [ b m 0 ( t ) , b m 1 ( t ) , . . . , b m m ( t ) ] ⊤ \textbf{b}=[b_m^0(t), b_m^1(t), ..., b_m^m(t)]^\top b=[bm0(t),bm1(t),...,bmm(t)]

贝塞尔曲线的性质

  1. convex hull, 比较方便地约束位置
  2. hodograph 性质,贝塞尔曲线的导数同样是贝塞尔曲线,控制点与原贝塞尔曲线的关系为: p i ( 1 ) = m ( p i + 1 − p i ) p_i^{(1)} = m(p_{i+1}-p_i) pi(1)=m(pi+1pi), 该性质对施加速度,加速度,以及jerk约束非常友好,对推导优化的目标函数也非常有利

问题定义与转化

目标函数:
J = ∫ 0 1 ( d 3 f ( t ) d t 3 ) 2 d t J= \int_0^1 (\frac{d^3f(t)}{d_t^3})^2d_t J=01(dt3d3f(t))2dt
表示对曲线的三阶导进行惩罚,其中 f ( t ) f(t) f(t)为五阶贝塞尔曲线,即 m = 5 m=5 m=5,由 h o d o g r a p h hodograph hodograph性质可得贝塞尔曲线的导数也为贝塞尔曲线,所以
d f ( t ) d t \frac{df(t)}{d_t} dtdf(t)也为贝塞尔曲线,且其控制点为 p i ( 1 ) = m ( p i + 1 − p i ) p_i^{(1)} = m(p_{i+1}-p_i) pi(1)=m(pi+1pi)
d 2 f ( t ) d t 2 \frac{d^2f(t)}{d_t^2} dt2d2f(t)的控制点为 p i ( 2 ) = m ( p i + 1 ( 1 ) − p i 1 ) ) = m ∗ ( m − 1 ) ( p i + 2 − 2 p i + 1 + p i ) p_i^{(2)} = m(p_{i+1}^{(1)}-p_i^{1}))=m*(m-1)(p_{i+2}-2p_{i+1}+p_i) pi(2)=m(pi+1(1)pi1))=m(m1)(pi+22pi+1+pi),依次类推,
d 3 f ( t ) d t 3 \frac{d^3f(t)}{d_t^3} dt3d3f(t)的控制点为 p i ( 3 ) = m ( p i + 2 ( 1 ) − p i ( 2 ) ) ) = m ∗ ( m − 1 ) ∗ ( m − 2 ) ( p i + 3 − 3 p i + 2 + 3 ∗ p i + 1 − p i ) p_i^{(3)} = m(p_{i+2}^{(1)}-p_i^{(2)}))=m*(m-1)*(m-2)(p_{i+3}-3p_{i+2}+3*p_{i+1}-p_i) pi(3)=m(pi+2(1)pi(2)))=m(m1)(m2)(pi+33pi+2+3pi+1pi)
使用五次贝塞尔曲线,则 m ∗ ( m − 1 ) ∗ ( m − 2 ) = 60 m*(m-1)*(m-2)=60 m(m1)(m2)=60
则目标函数可以写成:
J = ∫ 0 1 g 2 ( t ) d t J= \int_0^1 g^2(t)d_t J=01g2(t)dt
其中 g ( t ) g(t) g(t)为三阶贝塞尔曲线, g ( t ) = [ p 0 ( 3 ) , p 1 ( 3 ) , p 2 ( 3 ) ] [ ( 1 − t ) 2 , 2 t ( 1 − t ) , t 2 ] ⊤ g(t) = [p_0^{(3)},p_1^{(3)},p_2^{(3)}][(1-t)^2, 2t(1-t),t^2]\top g(t)=[p0(3),p1(3),p2(3)][(1t)2,2t(1t),t2],
其中控制点与原函数控制点之间的关系为:
[ p 0 ( 3 ) , p 1 ( 3 ) , p 2 ( 3 ) ] ⊤ = 60 ∗ [ − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 ] [ p 0 , p 1 , p 2 , p 3 , p 4 , p 5 ] ⊤ [p_0^{(3)},p_1^{(3)},p_2^{(3)}]\top= 60* \begin{bmatrix} -1 & 3& -3 & 1 & 0 & 0\\ 0 & -1 & 3& -3 & 1 & 0\\ 0&0&-1 & 3& -3 & 1 \end{bmatrix} [p_0,p_1,p_2,p_3,p_4,p_5]\top [p0(3),p1(3),p2(3)]=60 100310331133013001 [p0,p1,p2,p3,p4,p5]
目标函数可以写成:
J = ∫ 0 1 g 2 ( t ) d t = ∫ 0 1 P ( 3 ) b ( t ) b ( t ) ⊤ P ( 3 ) ⊤ d t = P ( 3 ) ∗ ∫ 0 1 b ( t ) b ( t ) ⊤ d t ∗ P ( 3 ) ⊤ J=\int_0^1 g^2(t)d_t =\int_0^1 \textbf{P}^{(3)}\textbf{b}(t)\textbf{b}(t)\top\textbf{P}^{(3)}\top d_t \\= \textbf{P}^{(3)}*\int_0^1 \textbf{b}(t)\textbf{b}(t)\top d_t *\textbf{P}^{(3)}\top J=01g2(t)dt=01P(3)b(t)b(t)P(3)dt=P(3)01b(t)b(t)dtP(3)
其中:
P ( 3 ) = P ⊤ A ⊤ A = 60 ∗ [ − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 ] \textbf{P}^{(3)}=\textbf{P}\top A\top \\A =60* \begin{bmatrix} -1 & 3& -3 & 1 & 0 & 0\\ 0 & -1 & 3& -3 & 1 & 0\\ 0&0&-1 & 3& -3 & 1 \end{bmatrix} P(3)=PAA=60 100310331133013001
b ( t ) = [ ( 1 − t ) 2 , 2 ( 1 − t ) t , t 2 ] ⊤ 令 C = ∫ 0 1 b ( t ) b ( t ) ⊤ d t = ∫ 0 1 [ ( 1 − t ) 4 2 t ( 1 − t ) 3 ( 1 − t ) 2 t 2 2 t ( 1 − t ) 3 4 t 2 ( 1 − t ) 2 2 t 3 ( 1 − t ) ( 1 − t ) 2 t 2 2 t 3 ( 1 − t ) t 4 ] d t = [ 1 5 1 10 1 30 1 10 2 15 1 10 1 30 1 10 1 5 ] \textbf{b}(t) =[(1-t)^2, 2(1-t)t, t^2]\top\\ 令\textbf{C}=\int_0^1 \textbf{b}(t)\textbf{b}(t)\top d_t = \int_0^1 \begin{bmatrix} (1-t)^4& 2t(1-t)^3& (1-t)^2t^2 \\ 2t(1-t)^3 & 4t^2(1-t)^2 & 2t^3(1-t)\\ (1-t)^2t^2&2t^3(1-t)&t^4 \end{bmatrix}d_t \\= \begin{bmatrix} \frac{1}{5} &\frac{1}{10} & \frac{1}{30}\\ \frac{1}{10} &\frac{2}{15} & \frac{1}{10}\\ \frac{1}{30} &\frac{1}{10} & \frac{1}{5} \end{bmatrix} b(t)=[(1t)2,2(1t)t,t2]C=01b(t)b(t)dt=01 (1t)42t(1t)3(1t)2t22t(1t)34t2(1t)22t3(1t)(1t)2t22t3(1t)t4 dt= 5110130110115210130110151
所以目标函数可以写成:
J = P ( 3 ) ∗ ∫ 0 1 b ( t ) b ( t ) ⊤ d t ∗ P ( 3 ) ⊤ = P ⊤ A ⊤ C A P J= \textbf{P}^{(3)}*\int_0^1 \textbf{b}(t)\textbf{b}(t)\top d_t *\textbf{P}^{(3)}\top \\= \textbf{P}\top A\top \textbf{C}A\textbf{P} J=P(3)01b(t)b(t)dtP(3)=PACAP
其中
令 Q = A ⊤ C A = 3600 ∗ [ − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 ] ⊤ ∗ [ 1 5 1 10 1 30 1 10 2 15 1 10 1 30 1 10 1 5 ] ∗ [ − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 0 0 0 − 1 3 − 3 1 ] = 3600 ∗ [ 1 5 − 1 2 1 3 0 0 − 1 30 − 1 2 − 4 3 − 1 0 1 6 0 1 3 − 1 1 − 1 3 0 0 0 0 − 1 3 1 − 1 1 3 0 1 6 0 − 1 4 3 − 1 2 − 1 30 0 0 1 3 − 1 2 1 5 ] = [ 720 − 1800 1200 0 0 − 120 − 1800 4800 − 3600 0 600 − 120 1200 − 3800 3600 − 1200 0 0 0 0 − 1200 3600 − 3600 1200 0 600 0 − 3600 4800 − 1800 − 120 0 0 1200 − 1800 720 ] 令\textbf{Q} =A\top \textbf{C}A =3600 * \begin{bmatrix} -1 & 3& -3 & 1 & 0 & 0\\ 0 & -1 & 3& -3 & 1 & 0\\ 0&0&-1 & 3& -3 & 1 \end{bmatrix}^\top*\begin{bmatrix} \frac{1}{5} &\frac{1}{10} & \frac{1}{30}\\ \frac{1}{10} &\frac{2}{15} & \frac{1}{10}\\ \frac{1}{30} &\frac{1}{10} & \frac{1}{5} \end{bmatrix}*\begin{bmatrix} -1 & 3& -3 & 1 & 0 & 0\\ 0 & -1 & 3& -3 & 1 & 0\\ 0&0&-1 & 3& -3 & 1 \end{bmatrix}\\ =3600*\begin{bmatrix} \frac{1}{5} &-\frac{1}{2} & \frac{1}{3} & 0 & 0 & -\frac{1}{30}\\ -\frac{1}{2} &-\frac{4}{3} & -1 & 0 & \frac{1}{6} & 0\\ \frac{1}{3} &-1 & 1 & -\frac{1}{3} & 0 & 0\\ 0 &0 & -\frac{1}{3} & 1 & -1 & \frac{1}{3}\\ 0 &\frac{1}{6} & 0 & -1 & \frac{4}{3} & -\frac{1}{2}\\ -\frac{1}{30} &0 & 0 & \frac{1}{3} & -\frac{1}{2} & \frac{1}{5}\\ \end{bmatrix}\\= \begin{bmatrix} 720 &-1800 & 1200 & 0 & 0 & -120\\ -1800 &4800 & -3600 & 0 & 600 & -120\\ 1200 &-3800 & 3600 & -1200 & 0 & 0\\ 0 &0 & -1200 & 3600 & -3600 & 1200\\ 0 &600 & 0 & -3600 & 4800 & -1800\\ -120 &0 & 0 & 1200 & -1800 & 720\\ \end{bmatrix} Q=ACA=3600 100310331133013001 5110130110115210130110151 100310331133013001 =3600 51213100301213410610311131000031113106101342130100312151 = 72018001200001201800480038000600012003600360012000000120036003600120006000360048001800120120012001800720
目标函数可写为:
J = P ⊤ A ⊤ C A P = P ⊤ QP J= \textbf{P}\top A\top \textbf{C}A\textbf{P}= \textbf{P}\top \textbf{Q}\textbf{P} J=PACAP=PQP
上式为标准的二次型行驶, Q \textbf{Q} Q为系数矩阵, P \textbf{P} P为原始贝塞尔曲线(即五阶贝塞尔曲线)的系数矩阵(控制点矩阵)

以上推导了目标函数为:
J = ∫ 0 1 ( d 3 f ( t ) d t 3 ) 2 d t J= \int_0^1 (\frac{d^3f(t)}{d_t^3})^2d_t J=01(dt3d3f(t))2dt
的二次型形式,实际问题中,我们通常将 f ( t ) = ∑ i = 0 m p i b m i ( t ) f(t)= \sum_{i=0}^{m}p_ib_m^{i}(t) f(t)=i=0mpibmi(t)写为
f ( u ) = α ∑ i = 0 m p i b m i ( u − t 0 t 1 − t 0 ) f(u)=\alpha \sum_{i=0}^{m}p_ib_m^{i}(\frac{u-t_0}{t_1-t_0}) f(u)=αi=0mpibmi(t1t0ut0),
其中 u ∈ [ t 0 , t 1 ] u\in[t_0, t_1] u[t0,t1]
实际中,我们所求的目标函数为:
J = ∫ t 0 t 1 ( d 3 f ( u ) d u 3 ) 2 d u = ∫ 0 1 ( α d 3 f ( t ) d ( α t ) 3 ) 2 d α t = 1 α 3 ∫ 0 1 ( d 3 f ( t ) d t 3 ) 2 d t J= \int_{t_0}^{t_1} (\frac{d^3f(u)}{d_u^3})^2du \\= \int_{0}^{1} (\frac{\alpha d^3f(t)}{d_{{(\alpha t)}^3}})^2d\alpha t \\= \frac{1}{\alpha^3}\int_{0}^{1} (\frac{ d^3f(t)}{d_{{ t}^3}})^2d t J=t0t1(du3d3f(u))2du=01(d(αt)3αd3f(t))2dαt=α3101(dt3d3f(t))2dt

  • 5
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值