轨迹优化 | Minimum-jerk

使用路径规划可以得到一系列的路径点，如下图所示有5个路径点，这条路径是很曲折的，这不利于机器人的移动，试想一个机器人直接沿着这条路径进行移动，当到达一个路径点后速度减到零，原地旋转，指向下一个路径点，再继续移动，这不仅智障，而且浪费能量。于是我们就想，能不能规划出一条轨迹，使机器人能够在每一个路径点处能够平滑过渡，从而表现得机灵一点。这就是轨迹规划问题。

路径规划得到的路径点	轨迹优化得到的轨迹(橙色)

二次规划(Quadratic Programming, QP)

由于轨迹规划问题可以归结为一个二次规划问题，所以这里先对二次规划进行一个简单的介绍

二次型

含有$n$个变量$x=x_1,x_2,\cdots ,x_{n-1},x_n$的二次齐次函数：
$$\begin{array}{rl}f(x_1,x_2,\cdots ,x_n)& =a_{11}{x}_1^2+a_{22}{x}_2^2+\cdots +a_{nn}{x}_n^2\\ & +2a_{12}{x}_1{x}_2+2a_{13}{x}_1{x}_3+\cdots +2a_{n-1,n}{x}_{n-1}{x}_n\end{array}$$
称为二次型。取$a_{ij}=a_{ji}$，则有$2a_{ij}{x}_i{x}_j=a_{ij}{x}_i{x}_j+a_{ji}{x}_j{x}_i$，上式可表示为：
$$f=\sum _{i,j=1}^n{a}_{ij}{x}_i{x}_j$$
矩阵表示为：
$$f=x^{T}Ax$$

二次规划问题

当目标函数$f$为二次型，且约束为线性约束时，该优化问题就是二次规划问题，一般形式表述如下：
$$\begin{array}{rl}\underset{x}{\min }f(x)& =\frac{1}{2}{x}^{T}Qx+q^{T}x\\ s.t.Ax& =b\\ Gx& \le h\end{array}$$
二次规划是一类凸优化问题，目前有很多商业或者开源的求解器来求解这类问题，不再赘述

轨迹表示形式

使用路径规划可以得到一系列的路径点，这些路径点是不带时间信息的，而轨迹则是时间$t$的函数，一般用$n$阶多项式表示，即：
$$\begin{array}{rl}f(t)& =p_0+p_{1}t+p_2{t}^2+\cdots +p_n{t}^n\\ & =\sum _{i=0}^n{p}_i{t}^i\end{array}$$

其中$p_0,p_1,\cdots ,p_n$是轨迹参数，也是我们的优化参数。将$f(t)$写成向量相乘形式，得到：
$$f(t)=\left[\begin{array}{cccc}1& t& \cdots & t^n\end{array}\right]\left[\begin{array}{c}{p}_0\\ p_1\\ ⋮\\ p_n\end{array}\right]$$
对轨迹函数求导，还能写出速度、加速度、jerk等参数随时间变化的函数：

$$\begin{array}{rl}vel(t)& =f^{(1)}(t)=\left[\begin{array}{cccccc}0& 1& 2t& 3t^2& \cdots & \frac{n!}{(n-1)!}{t}^{n-1}\end{array}\right]\cdot p\\ acc(t)& =f^{(2)}(t)=\left[\begin{array}{cccccc}0& 0& 2& 6t& \cdots & \frac{n!}{(n-2)!}{t}^{n-2}\end{array}\right]\cdot p\\ jerk(t)& =f^{(3)}(t)=[\begin{array}{cccccc}0& 0& 0& 6& \cdots & (n</\end{array}\end{array}$$