文中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使用的。