最小SNAP轨迹生成
说明：
snap对应的是推力的倒数，snap最小化使得推力变化速度最小化，来节省能量。
jerk对应的是推力、角速度，jerk最小化使得角速度最小，可以使得视觉方向的处理方便（图形畸变更小）。
其实最小化snap或者jerk都是可以的，按照需求来选择最小化的量即可，这里选用的是最小化snap。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ttLsjKGP-1638242819728)(E:\深蓝学院作业\第五章\image-20211129194901096.png)]
一、Introduction
1、为什么要平滑轨迹：
适合自主移动。
速度/高阶动力学不能立即改变。
机器人不应该在转弯时停下来。
节约能源。
2、平滑轨迹生成：
边界条件：起点、目标位置(方向)
中间条件：路点位置(方位)·可以通过路径规划(A*、RRT*等)找到路点·
平滑度标准：通常翻译为最小化“输入”的变化率（路径点左右的极限相等）
二、最小snap优化（Minimum Snap Optimization）
1.微分平坦(Differential Flatness)
证明四旋翼的状态和输入可以用四个状态量[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DlEEuoR9-1638242819734)(C:\Users\chh\AppData\Roaming\Typora\typora-user-images\image-20211129161751656.png)]及其导数来表示。
也就是说只要控制这四个状态量以及其多阶倒数，既能控制无人机的位置和姿态
（1）闭环控制（Close the planning-control loop）
u1:力
u2 , u3 , u4 :力矩
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-JlZobA3T-1638242819738)(C:\Users\chh\AppData\Roaming\Typora\typora-user-images\image-20211129162353972.png)]
（2）多项式轨迹（Polynomial Trajectories）
指定平坦输出空间中的轨迹可以用多项式函数来表示
多项式函数轨迹求解的优点：
多项式阶数光滑性判据的简易确定·
导数的简易封闭式计算·
三维解耦轨迹生成
2、最小化snap（Minimum-snap）
（1）平滑一维轨迹（Smooth 1D Trajectory）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-fOmeqJjB-1638242819742)(C:\Users\chh\AppData\Roaming\Typora\typora-user-images\image-20211129191849972.png)]
（2）平滑多段轨迹（Smooth Multi-Segment Trajectory）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Gc7vwjs2-1638242819746)(C:\Users\chh\AppData\Roaming\Typora\typora-user-images\image-20211129193210291.png)]
（3）平滑多段一维轨迹（Smooth Multi-Segment 1D Trajectory）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SbIziNlG-1638242819749)(C:\Users\chh\AppData\Roaming\Typora\typora-user-images\image-20211129192321193.png)]
（4）平滑三维轨迹（Smooth 3D Trajectory）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-s02IrbX8-1638242819752)(E:\深蓝学院作业\第五章\image-20211129194831261.png)]
（5）基于优化的轨迹生成（Optimization-based Trajectory Generation）
snap对应的是推力的倒数，snap最小化使得推力变化速度最小化，来节省能量。
jerk对应的是推力、角速度，jerk最小化使得角速度最小，可以使得视觉方向的处理方便（图形畸变更小）。
其实最小化snap或者jerk都是可以的，按照需求来选择最小化的量即可，这里选用的是最小化snap。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-uVIAmn24-1638242819754)(E:\深蓝学院作业\第五章\image-20211129194901096.png)]
（6）最小snap轨迹生成（Minimum Snap Trajectory Generation）
多项式方程
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZLMS2Za2-1638242819758)(E:\深蓝学院作业\第五章\image-20211129200524843.png)]
每一段都是一个多项式。
没必要固定多项式阶数，但是保持相同的阶数可以使问题简单。·
每段的时间间隔必须是已知的
需要满足的约束
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-RcHQrwGn-1638242819760)(E:\深蓝学院作业\第五章\image-20211129202453242.png)]
需要求解的多项式方程满足：边界点（起点终点）的倒数约束（位置、速度。加速度）；中间点的连续性约束
确定轨迹的阶数
确保一段轨迹平滑的最小次数多项式：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bPikIX7N-1638242819763)(E:\深蓝学院作业\第五章\image-20211129204403053.png)]
确保k段轨迹光滑性的最小次数多项式：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6242VbqC-1638242819765)(E:\深蓝学院作业\第五章\image-20211129204333362.png)]
多项式的成本函数求解：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-cxxh8LaU-1638242819768)(E:\深蓝学院作业\第五章\image-20211129210739447.png)]
转化为二次规划问题（QP问题）：也是一个凸优化问题，说明这是一定可以求解的问题
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-3wCUAQi3-1638242819770)(E:\深蓝学院作业\第五章\image-20211129210947751.png)]
3、凸优化（Convex Optimization）
（1）凸优化（Convex optimization）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BkELc4vc-1638242819773)(E:\深蓝学院作业\第五章\image-20211129212005665.png)]
（2）成熟的凸优化方法（Disciplined convex optimization programs）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-u9SQRyQM-1638242819775)(E:\深蓝学院作业\第五章\image-20211129212034481.png)]
三、闭式求解最小 Snap（Closed-form Solution to Minimum Snap）
1、优化变量转换（Decision variable mapping）
之前我们优化的是分段多项式的每一段的系数，这里就存在一些问题：求解的p都是没有物理含义的值，可能会带来轨迹优化数值上的不稳定性。所以我们通过M映射矩阵，把P转换为d（轨迹上路径点的v、a)。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-AGY107ny-1638242819779)(E:\深蓝学院作业\第五章\image-20211129221945850.png)]
这样我们就可以把这个问题从优化多项式的系数这一没有实际物理含义的比较不稳定的优化问题的决策变量，转化为优化轨迹上这些点的v、a作为决策变量。这样数值稳定性就会好很多。
2、固定变量和自由变量分离（Fixed and free variable separation）
使用选择矩阵C把决策变量dj分为df和dp
固定变量(已经确定了的变量)：起点终点的p、v、a；中间点的p
自由变量（需要进行优化的变量）：中间点的v、a
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qqWeNZal-1638242819782)(E:\深蓝学院作业\第五章\image-20211130095549614.png)]
3、构建选择矩阵(Build the selection matrix)
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-YwB2nlmW-1638242819785)(E:\深蓝学院作业\第五章\image-20211130102811739.png)]
4、与凸优化结果相同（Same results as convex optimization）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HdFXhNRk-1638242819787)(E:\深蓝学院作业\第五章\image-20211130102834011.png)]
5、分层方法（Hierarchical approach）
先路径规划，后轨迹生成（优化）
6、安全问题（Hierarchical approach）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ctPucPPT-1638242819789)(E:\深蓝学院作业\第五章\image-20211130103048889.png)]
两点之间轨迹碰到障碍物，则把两点之间的中点也作为路径点，放在轨迹优化考虑之内。
7、迭代法（Iterative approach ）
先是尽可能少的点，生成足够光滑的轨迹，如果存在安全问题，则使用以上方法，将中点加入优化范围之内，不断进行迭代，最后生成首先光滑然后安全的轨迹。
8、RRT* + 最小snap（RRT* + minimum snap）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jVJXvP60-1638242819792)(E:\深蓝学院作业\第五章\image-20211130103616795.png)]
四、工程实现细节（Implementation Details）
1、凸解算器（Convex Solvers）
求解凸轨迹生成程序（Solve a convex trajectory generation program）
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-1ABW78uu-1638242819796)(E:\深蓝学院作业\第五章\image-20211130104603247.png)]
2、时间线问题
使用相对时间线
把每一段轨迹时间进行归一化处理成1s，然后进行缩放
3、是否应该进行三轴独立求解
解一个基本的mini snap问题完全可以三轴独立求解，求解软约束问题需要三个轴一起求解
4、闭式求解一定总是好的吗？
闭式求解需要求解矩阵求逆，对于比较大的矩阵算的时间长、解的鲁棒性不高。所以很多商用设备会把这个问题变成QP问题，求二次型解，这种方法鲁棒性很高。所以闭式求解不一定就是好的
每一段轨迹时间进行归一化处理成1s，然后进行缩放
3、是否应该进行三轴独立求解
解一个基本的mini snap问题完全可以三轴独立求解，求解软约束问题需要三个轴一起求解
4、闭式求解一定总是好的吗？
闭式求解需要求解矩阵求逆，对于比较大的矩阵算的时间长、解的鲁棒性不高。所以很多商用设备会把这个问题变成QP问题，求二次型解，这种方法鲁棒性很高。所以闭式求解不一定就是好的
