运动规划论文——EMplanner

有约束样条多项式和二次规划

Fan H, Zhu F, Liu C, et al. Baidu apollo em motion planner[J]. arXiv preprint arXiv:1807.08048, 2018.
附录2：Constrained Smoothing Spline and Quadratic Programming部分翻译

QP路径和速度规划使用同样的有约束平滑的样条多项式表示，下面我们将平滑样条多项式问题转化为具有线性约束的二次规划问题，因为一个具有二次型形式目标函数和线性约束的优化问题可以稳定且快速的求解。

在二次规划设置下，我们在给定二次目标函数和线性约束的情况下优化三阶平滑的m次函数$f(x)$，这要求m>=5（参见Minimum-jerk）
$$f(x)={\Sigma }_{i=0}^m{p}_i{x}^i={\mathbf{p}}^T\mathbf{x}\text{\hspace{1em}(1)}$$
将定义域$(a,b),a=x_0,b=x_n$上的平滑样条函数$f(x)$，表示为基本的n个分段多项式的线性组合而成。将$x_0=a,x_1,x_2,...,x_n=b$ 表示为节点（knots），则$f(x)$可表示为分段函数：
$$f(x)=\{\begin{array}{ll}{f}_0\left(x-x_0\right)& x\in \left[x_0,x_1\right)\\ f_1\left(x-x_1\right)& x\in \left[x_1,x_2\right)\\ & \cdots \\ f_{n-1}\left(x-x_{n-1}\right)& x\in \left[x_{n-1},x_n\right]\end{array}\text{\hspace{1em}(2)}$$
其中每一项$f_k(x)=p_{k0}+p_{k1}x+\dots p_{km}{x}^m$都是多项式，而$f_k$的系数为${\mathbf{p}}_k={\left(p_{k0},p_{k1},\dots ,p_{km}\right)}^T$。平滑样条参数向量定义为$\mathbf{p}={\left({\mathbf{p}}_0^T,{\mathbf{p}}_1^T,\dots ,{\mathbf{p}}_{n-1}^T\right)}^T$。

在数学上，样条求解器试图找到一个函数$\hat{f}$来优化再现核希尔伯特空间 (RKHS) 定义域上函数的线性组合，该域空间被描述为
$$\begin{array}{l}\Omega =\{f:[a,b]\to \mathbf{R}\mid f,f^{(1)},f^{(2)},f^{(3)}\\ \\ \text{ is abs. conti. and }{\int }_a^b{\left(f^{(m)}\right)}^{2}dx<\infty ,m=0,1,2,3\}.\end{array}\text{\hspace{1em}(3)}$$
样条 QP 问题由目标函数、线性化约束和二次规划求解器构成。
$$\begin{array}{c}\arg {\min }_{f\in \Omega }\mathbf{P}(f)={\sum }_{i=0}^3{w}_i{\mathbf{P}}_i(f)\\ \\ s.t.\mathbf{L}(f)=\mathbf{L}({\mathbf{p}}^{\mathbf{T}}\mathbf{x})<=0\end{array}\text{\hspace{1em}(4)}$$
目标函数$\mathbf{P}(f)$是$\mathbf{\Omega }$ 上四个函数${\mathbf{P}}_i,i=0,1,2,3$的线性组合。具体的
$$\begin{array}{l}{\mathbf{P}}_0(f)={\int }_a^b(f(x)-g(x){)}^2\mathrm{d}x\\ {\mathbf{P}}_1(f)={\int }_a^b{\left(f^{(1)}(x)\right)}^2\mathrm{d}x\\ {\mathbf{P}}_2(f)={\int }_a^b{\left(f^{(2)}(x)\right)}^2\mathrm{d}x\\ {\mathbf{P}}_3(f)={\int }_a^b{\left(f^{(3)}(x)\right)}^2\mathrm{d}x\end{array}\text{\hspace{1em}(5)}$$
其中 $g$ 是在$\mathbf{\Omega }$域内预先指定的准则函数。第一方程表示到引导线$g$的距离，而其余三个描述$f$的平滑度。约束泛函$\mathbf{L}(f(x))$根据问题设置包括以下类型：

1. 边界约束。即在给定位置 $x$，满足$L\le f^{(m)}(x)\le U,m=0,1,2,3$。
2. 0至$m$阶导数的平滑性约束。由于$f$是分段多项式，因此多项式在样条节点 $x_k,k=1,2,...,n-1$处，0至$m$阶导数对应相等。
3. 单调性约束。该约束是专门为速度平滑器设计的，它保证路径在一系列指定点处是单调的。

定理1：式（4）中定义的目标函数$\mathbf{P}$具有二次型形式，并且约束函数$\mathbf{L}$对于参数$\mathbf{p}$呈线性。

证明：为简单起见，将变量向量$x_{(i)},i=0,1,2,3$表示为$f$关于参数$\mathbf{p}$在$x\in [x_k,x_{k+1}),k=0,1,...,n-1$上的$i$阶导数。也就是说，
$$\begin{array}{l}{\mathbf{x}}_{(0)}={\left(1,\tilde{x},{\tilde{x}}^2,\dots ,{\tilde{x}}^m\right)}^T\\ {\mathbf{x}}_{(1)}={\left(0,1,2\tilde{x},\dots ,\tilde{m}{x}^{m-1}\right)}^T\\ {\mathbf{x}}_{(2)}={\left(0,0,2,\dots ,m(m-1){\tilde{x}}^{m-2}\right)}^T\\ {\mathbf{x}}_{(3)}={\left(0,0,0,\dots ,m(m-1)(m-2){\tilde{x}}^{m-3}\right)}^T\end{array}\text{\hspace{1em}(6)}$$
其中$\tilde{x}=x-x_k,x\in [x_k,x_{k+1}),k=0,1,...,n-1$，根据式（2），每一个${\mathbf{P}}_i,i=1,2,3$可以表示为积分的和
$$\begin{array}{l}{\mathbf{P}}_i(f)={\sum }_{k=0}^{n-1}{\int }_{x_k}^{x_{k+1}}{\left(f^{(i)}\right)}^2\mathrm{d}x\\ \\ ={\sum }_{k=0}^{n-1}{\int }_{x_k}^{x_{k+1}}{\mathbf{p}}_k^T{\mathbf{x}}_{(i)}{\mathbf{x}}_{(i)}^T{\mathbf{p}}_k\mathrm{d}x\\ \\ ={\mathbf{p}}^T{\mathbf{X}}_{(i)}\mathbf{p}\end{array}\text{\hspace{1em}(7)}$$
其中${\mathbf{X}}_{(i)}$是分块对角阵$\mathrm{Diag}\left({\int }_{x_0}^{x_1}{\mathbf{x}}_{(i)}{\mathbf{x}}_{(i)}^{T}dx,\dots ,{\int }_{x_k}^{x_{k+1}}{\mathbf{x}}_{(i)}{\mathbf{x}}_{(i)}^{T}dx\right)$，类似的对$i=0$有，
$$\begin{array}{l}{\mathbf{P}}_0(f)={\sum }_{k=0}^{n-1}{\int }_{x_k}^{x_{k+1}}(f(x)-g(x){)}^2\mathrm{d}x\\ \\ ={\sum }_{k=0}^{n-1}{\int }_{x_k}^{x_{k+1}}\left({\mathbf{p}}_k^T{\mathbf{x}}_{(0)}{\mathbf{x}}_{(0)}^T{\mathbf{p}}_k-2{\mathbf{p}}_k^T{\mathbf{x}}_{(0)}g(x)+(g(x){)}^2\right)\mathrm{d}x\\ \\ ={\mathbf{p}}^T{\mathbf{X}}_{(0)}\mathbf{p}-2{\mathbf{p}}^T\mathbf{c}+\text{ const. }\end{array}$$
其中${\mathbf{X}}_{(0)}$是分块对角阵$\mathrm{Diag}\left({\int }_{x_0}^{x_1}{\mathbf{x}}_{(0)}{\mathbf{x}}_{(0)}^{T}dx,\dots ,{\int }_{x_k}^{x_{k+1}}{\mathbf{x}}_{(0)}{\mathbf{x}}_{(0)}^{T}dx\right)$，且$\mathbf{c}={\int }_{x_k}^{x_{k+1}}{\mathbf{x}}_{(0)}g(x)dx$。

因此，目标函数$\mathbf{P}(f)$对于样条系数矢量$\mathbf{p}$具有标准二次型形式，具有凸性，证毕！