SHENLAN_路径规划_Chapter5_HW1_通过数值优化方法解Minimum Snap的轨迹生成问题

前言：
第一次写优快云博客，用我家蛋蛋记录一下，谢谢！ 写博客的主要目的是想和大家相互交流学习

1.原理

求解Q的方法:

解目标函数为Minimum Snap，多项式表达式为七次多项式
$$f(t)=\sum _i{p}_i{t}^i=p_7{t}^7+p_6{t}^6+p_5{t}^5+p_4{t}^4+p_3{t}^3+p_2{t}^2+p_{1}t+p_0$$
Cost funciton为Minimum Snap，
每段轨迹的Cost funciton为f(t)的4次导数的平方 $$J_k(T)={\int }_{T_{k-1}}^{T_k}(f^4(t){)}^{2}dt$$
经过推导得出
$$J_k(T)={\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]}^T\left[\begin{array}{ccc}& ⋮& \\ \cdots & \frac{i(i-1)(i-2)(i-3)l(l-1)(l-2)(l-3)}{i+l-7}{T}^{i+l-7}& \cdots \\ & ⋮& \end{array}\right]\left[\begin{array}{c}⋮\\ p_i\\ ⋮\end{array}\right]=p_k^T{Q}_k{p}_k$$
其中i和j=[0:n_order],n_order为每段多项式的阶数。Qk为(n_order+1)*n_order+1)的矩阵。
k代表第k段轨迹。
算出每一段轨迹的Qk矩阵,通过对角对阵构造的方法，构造出经过轨迹的总的Q矩阵：
$$Q=\left[\begin{array}{cccc}{Q}_1& 0& \cdots & 0\\ 0& Q_k& \cdots & 0\\ ⋮& ⋮& \ddots & 0\\ 0& 0& \dots & Q_{nseg}\end{array}\right]$$
则
$$J=p^{T}Qp={\left[\begin{array}{c}{p}_1\\ ⋮\\ p_k\\ ⋮\\ p_{nseg}\end{array}\right]}^T\left[\begin{array}{cccc}{Q}_1& 0& \cdots & 0\\ 0& Q_k& \cdots & 0\\ ⋮& ⋮& \ddots & 0\\ 0& 0& \dots & Q_{nseg}\end{array}\right][\begin{array}{c}\end{array}$$