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

贝塞尔曲线：

$$f(t)=\sum _{i=0}^m{p}_i{b}_m^i(t)=\text{p}\text{b}(t)$$
其$t\in [0,1]$，$p_i$未贝塞尔曲线的参数,即$p_i$决定了贝塞尔曲线的形状,$\text{p}=[p_0,p_1,...,p_m]$, $\text{b}=[b_m^0(t),b_m^1(t),...,b_m^m(t){]}^{\top }$。

贝塞尔曲线的性质

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

问题定义与转化

目标函数：
$$J={\int }_0^1(\frac{d^{3}f(t)}{d_t^3}{)}^2{d}_t$$
表示对曲线的三阶导进行惩罚，其中$f(t)$为五阶贝塞尔曲线，即$m=5$，由$hodograph$性质可得贝塞尔曲线的导数也为贝塞尔曲线，所以
$\frac{df(t)}{d_t}$也为贝塞尔曲线，且其控制点为$p_i^{(1)}=m(p_{i+1}-p_i)$，
$\frac{d^{2}f(t)}{d_t^2}$的控制点为$p_i^{(2)}=m(p_{i+1}^{(1)}-p_i^1))=m\ast (m-1)(p_{i+2}-2p_{i+1}+p_i)$,依次类推，
$\frac{d^{3}f(t)}{d_t^3}$的控制点为$p_i^{(3)}=m(p_{i+2}^{(1)}-p_i^{(2)}))=m\ast (m-1)\ast (m-2)(p_{i+3}-3p_{i+2}+3\ast p_{i+1}-p_i)$
使用五次贝塞尔曲线，则$m\ast (m-1)\ast (m-2)=60$
则目标函数可以写成：
$$J={\int }_0^1{g}^2(t)d_t$$
其中$g(t)$为三阶贝塞尔曲线,$g(t)=[p_0^{(3)},p_1^{(3)},p_2^{(3)}][(1-t{)}^2,2t(1-t),t^2]\top$,
其中控制点与原函数控制点之间的关系为：
[ 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∗⎣ ⎡​−100​3−10​−33−1​1−33​01−3​001​⎦ ⎤​[p0​,p1​,p2​,p3​,p4​,p5​]⊤
目标函数可以写成：
$$\begin{gathered} J={\int }_0^1{g}^2(t)d_t={\int }_0^1{\text{P}}^{(3)}\text{b}(t)\text{b}(t)\top {\text{P}}^{(3)}\top d_t \\ ={\text{P}}^{(3)}\ast {\int }_0^1\text{b}(t)\text{b}(t)\top d_t\ast {\text{P}}^{(3)}\top \end{gathered}$$
其中：
$$\begin{gathered} {\text{P}}^{(3)}=\text{P}\top A\top \\ A=60\ast \left[\begin{array}{cccccc}-1& 3& -3& 1& 0& 0\\ 0& -1& 3& -3& 1& 0\\ 0& 0& -1& 3& -3& 1\end{array}\right] \end{gathered}$$
$$\begin{gathered} \text{b}(t)=[(1-t{)}^2,2(1-t)t,t^2]\top \\ 令\text{C}={\int }_0^1\text{b}(t)\text{b}(t)\top d_t={\int }_0^1\left[\begin{array}{ccc}(1-t{)}^4& 2t(1-t{)}^3& (1-t{)}^2{t}^2\\ 2t(1-t{)}^3& 4t^2(1-t{)}^2& 2t^3(1-t)\\ (1-t{)}^2{t}^2& 2t^3(1-t)& t^4\end{array}\right]d_t \\ =\left[\begin{array}{ccc}\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{array}\right] \end{gathered}$$
所以目标函数可以写成：
$$\begin{gathered} J={\text{P}}^{(3)}\ast {\int }_0^1\text{b}(t)\text{b}(t)\top d_t\ast {\text{P}}^{(3)}\top \\ =\text{P}\top A\top \text{C}A\text{P} \end{gathered}$$
其中
$$\begin{gathered} 令\text{Q}=A\top \text{C}A=3600\ast {\left[\begin{array}{cccccc}-1& 3& -3& 1& 0& 0\\ 0& -1& 3& -3& 1& 0\\ 0& 0& -1& 3& -3& 1\end{array}\right]}^{\top }\ast \left[\begin{array}{ccc}\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{array}\right]\ast \left[\begin{array}{cccccc}-1& 3& -3& 1& 0& 0\\ 0& -1& 3& -3& 1& 0\\ 0& 0& -1& 3& -3& 1\end{array}\right] \\ =3600\ast \left[\begin{array}{cccccc}\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{array}\right] \\ =\left[\begin{array}{cccccc}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{array}\right] \end{gathered}$$
目标函数可写为:
$$J=\text{P}\top A\top \text{C}A\text{P}=\text{P}\top \text{Q}\text{P}$$
上式为标准的二次型行驶,$\text{Q}$为系数矩阵,$\text{P}$为原始贝塞尔曲线(即五阶贝塞尔曲线)的系数矩阵(控制点矩阵)

以上推导了目标函数为：
$$J={\int }_0^1(\frac{d^{3}f(t)}{d_t^3}{)}^2{d}_t$$
的二次型形式，实际问题中，我们通常将$f(t)={\sum }_{i=0}^m{p}_i{b}_m^i(t)$写为
$$f(u)=\alpha \sum _{i=0}^m{p}_i{b}_m^i(\frac{u-t_0}{t_1-t_0})$$,
其中$u\in [t_0,t_1]$
实际中，我们所求的目标函数为：
$$\begin{gathered} J={\int }_{t_0}^{t_1}(\frac{d^{3}f(u)}{d_u^3}{)}^{2}du \\ ={\int }_0^1(\frac{\alpha d^{3}f(t)}{d_{{(\alpha t)}^3}}{)}^{2}d\alpha t \\ =\frac{1}{{\alpha }^3}{\int }_0^1(\frac{d^{3}f(t)}{d_{t^3}}{)}^{2}dt \end{gathered}$$