hw_5_1:最优轨迹生成--最优性条件构造线性方程组求解（无约束条件，MINCO (Minimum Control Effort)）

目标：写一个minimun jerk的轨迹生成程序。使用BIVP求解，注意这里是无约束的BIVP。即没有考虑动力学约束，障碍物碰撞等约束的轨迹生成问题求解。

已知：（1）初始点和末尾点的p,v,a。（M-1）个中间点的位置P。（假设有M段轨迹）

           （2）每一段轨迹的相对时间（使用梯形速度进行时间分配）。

输入：通过RVIZ里面点击三维位置点，作为起始点、中间点和末尾点。

输出：三维的最小化jerk轨迹的三维的独立的每段轨迹的多项式系数。

整个求解可以分为：问题构建和问题求解两部分。

1.问题构建（Problem Formulation）

        虽然是最小化jerk轨迹，求解每一段轨迹多项式的系数。由于已有论文给出可以不直接通过构建目标函数（最小化jerk），直接根据最优性条件（轨迹的最优平坦输出的充分必要条件为最优性条件），构建系数线性方程组，直接求解c矩阵。

2.问题求解

        根据最优性条件，minimun jerk，3阶导数为输入，s=3,为2s-1=5次多项式轨迹；,则。

• BIVP的最优平坦输出为5次多项式。
• 是2di-1=4次连续可微（p,v,a,jerk,snap存在且连续）。
• 初始、末尾、中间值约束。

2.1 边界值条件---初始条件

        BIVP的最优平坦输出为5次多项式可以得到的5次多项式表达式。根据p0,v0,a0带入多项式表达式可以得到F0(3X6)，b0;

2.2 边界值条件---末尾条件

        BIVP的最优平坦输出为5次多项式可以得到的5次多项式表达式。根据pM,vM,aM带入多项式表达式可以得到EM(3X6)，bM;

2.3 中间条件

       对每一个中间点，都可以得到一个Ei,Fi，bi；Ei和Fi都是6X6的矩阵。矩阵是6行代表有6各方程：

第一个：过点P

第二个：在P点连续

第三个：在V连续

第四个：在A连续

第五个：在jerk连续

第六个：在snap连续。

注：对于同一段轨迹，三个轴的多项式表达式系数是不同的，是各自独立的。但它们共享相同的时间参数和时间分配，所以共享一个M矩阵。

3.利用求解器求解矩阵c

直接求逆，数值稳定性差

使用LU分解

只考虑边界条件和中间值条件的最优轨迹生成（无约束轨迹求解BIVP）

        在后续不断增加新的点后，由于前面2个点之间的距离dist是不变的，所以前面每段轨迹的时间分配t不变。

可视化终端的M矩阵和时间分配

（1）只有起点和末尾点

（2）三个点：起点-中间点-末尾点

（3）4个点：起点-中间点1-中间点2-末尾点

（4）5个点：起点-中间点1-中间点2-中间点3-末尾点

使用QR分解

使用直接矩阵求逆和使用LU分解求解c对比，这里只有8个点

对比看来，在8个点以内，这2个求解方法没有什么区别。

待解决问题：从rviz的图中可以看到，存在轨迹交叉部分。

参考文章（构建M矩阵的思路和过程写的很好，但是其中一个矩阵有点问题）：https://blog.youkuaiyun.com/qq_37746927/article/details/136153560