Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn

在线状态估计和环境感知，基于Fast Marching在速度场的路径搜索，利用Euclidean signed distance field(ESDF)优化轨迹时间分配，使用飞行走廊（flight corridor）来壁障，利用贝赛尔曲线生成有界高阶的轨迹，构造凸规划

FAST MARCHING-BASED PATH SEARCHING

Fast Marching Method

Fast Marching Method由Level Set method发展而来
level set （水平集）算法是什么？ - 杨树下的狐狸的回答 - 知乎

Fast Marching Methods: A boundary value formulation

期望通过fast marching在速度场上获取到达时间最小的路径
与势能场搜索方法不同的是，fast marching方法总能找到全局最优而无局部最优的问题，并且耗时与A*一样，其时间复杂度为$O(Nlog(N))$
并且由于是在速度场上做的前向递推，所以实际是知道了点与点之间的时间，而传统的搜索算法仅是给出点与点间的距离信息

Fast Marching Method in Distance Field

Euclidean signed distance field (ESDF):欧拉距离的有向距离场表达了离障碍物的远近程度
双曲正切函数 $tanh$

定义速度函数

$d$是位置$x$在ESDF中的距离值,$e$是欧拉值，$v_m$是最大速度
得速度函数如图

可见当离障碍近（d小）速度小；远，速度大，构造得速度场如下

可见有障碍的地方速度为0，然后根据之前说的fast marching方法可以获得路径如下

这里为加速搜索，采用欧氏距离做启发式函数$h(x)=d^{\ast }(x)/v_m$，$d^{\ast }(x)$是当前位置$x$但终点的欧氏距离

Flight Corridor Generation

对路径中的每个点通过ESDF寻找最近障碍距离，得到一个内接圆，然后往$x,y,z$每个方向上进行拓展知道碰到障碍物，最后删除重复多余的路径节点，完成飞行走廊的生成

Trajectory Genration

通过飞行走廊，连续约束，动力学约束，安全约束等构造二次规划（QP）问题，求解得优化后的轨迹