文中ppt来源：深蓝学院，强烈推荐
重新学习一下软约束轨迹优化的理论知识。
Distance-based Trajectory Optimization
Motivation：
硬约束的可能的不足：
安全走廊内的区域是等价的，所以轨迹有可能离障碍物很近，控制不好就撞了。
对于视觉无人机，受限于视觉传感器的范围和质量，如果有噪点，那飞行走廊质量就下降了。
软约束的不足：
不能保证每一种约束都满足，而是倾向于满足。如果目标函数设计的不好，是有可能碰撞的。
问题描述：
目标函数包括光滑项、碰撞项、动力学项。
光滑项：根minimum snap中的是一样的。直接用上一节课推导的闭式解。区别在于本解不在限制所有的waypoints固定，而是只有首位固定，也就是dF中很多项都放在了dP中。（这里理解即可，先不推导）
碰撞惩罚项：沿着轨迹积分，积的轨迹点所在的距离场产生的推力。
这里就需要ESDF地图，欧式距离场，障碍物内是-1，随着远离障碍物，依次为1，2，3....
那其实我自己也可以很简单的构建，一圈一圈往外扩就行啦。
注意：这里是沿着曲线的积分，而不是对时间的积分。如果是对时间的积分，时间变短（速度变快），也可以使得积分结果变小，而我们想要的是因为距离障碍物远而得到的。
如何积分：
将ds近似成速度*时间，曲线分成很多段直线段。
动力学惩罚项：
对于超过速度和加速度限制的进行惩罚。与碰撞惩罚项类似。
目标函数求导/Jacobian：
整个问题，肯定不是凸的，因为首先ESDF距离场就不是凸的，那碰撞惩罚项就不是凸的。
这里也没有办法转化成凸优化的问题，所以只能通过非线性优化求解：写出目标函数->求导数。
第一项Js的求导，这个比较简单，对dp求导即可（注意这里的dp跟minimum snap中推导的有些差异，中间的waypoints不在固定，所以都加到了dp中）
第二项Jc的求导，对c(pt) * ||v(t)||求导。这里μ代表了x,y,z方向。
先对c(pt)求导，c()这个函数就是自己定义了，根据离障碍物的位置远近，定义一个。
到现在为止，起码导数都是可以推导出来的，如果推导不出来，就只能数值上加扰动。
一阶导数是Jacbian,代表梯度。优化的话，一阶导数就可以了。
二阶导数是海森，代表梯度的梯度。牛顿法的话是二阶导数。
ESDF地图：
可以用指数函数作为cost function c(p(t))。当然，简单的话，我觉得可以用一个分段函数。
动力学项，也可以用指数的函数，超过限制时，爆炸式增长。
数值优化：
无约束非线性优化问题，求最小值，也就是求导数为零。
不是全局最优的，初值敏感。
利用梯度，往导数的负方向走。
二阶方法就不是简单的往梯度的下降的方向了，比如牛顿法，是用-一阶导数/二阶导数
LM方法，结合了一阶和二阶方法，lambda很大时，就是一阶法，lambda很小时，就是二阶法。
ceres--最小二乘、非线性优化
NLopt--收录的求解器特别多，作planning经常用。
写出目标函数，写出梯度，就可以用求解器求解，上边的两个求解器，甚至都不用你写梯度。
初值选择：
基于梯度的优化，初始值如何选择。
方法1.用前端得到的路径，好处是是不碰撞的，但光滑性很差。
方法2.前端路径，然后用闭式minimum snap求解以后，作为初值。好处是光滑，但是不安全。
一般来说，我们更愿意初始解是安全的。基于这个初始解，再加上指数爆炸上升的碰撞惩罚函数，可以确保最后轨迹安全。
优化策略：
第一次优化规划，只保留Jc碰撞项。使得轨迹离障碍物远一点。
第二次再把Js和JD加进来。
缺点：
碰撞项和动力学项，都需要积分，而且需要离散成很多小段，如果离散的太多细密，对算力复杂度是负担。
解决方法：B-样条，B-样条和贝塞尔一样，具有凸包性质，就不用积分了，直接限制控制点即可。
这里的光滑项也是TEB planner使用的。
