在线状态估计和环境感知，基于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
(
N
l
o
g
(
N
)
)
O(Nlog(N))
O(Nlog(N))
并且由于是在速度场上做的前向递推，所以实际是知道了点与点之间的时间，而传统的搜索算法仅是给出点与点间的距离信息
Fast Marching Method in Distance Field
Euclidean signed distance field (ESDF):欧拉距离的有向距离场表达了离障碍物的远近程度
双曲正切函数
t
a
n
h
tanh
tanh
定义速度函数
d
d
d是位置
x
x
x在ESDF中的距离值,
e
e
e是欧拉值，
v
m
v_m
vm是最大速度
得速度函数如图
可见当离障碍近（d小）速度小；远，速度大，构造得速度场如下
可见有障碍的地方速度为0，然后根据之前说的fast marching方法可以获得路径如下
这里为加速搜索，采用欧氏距离做启发式函数
h
(
x
)
=
d
∗
(
x
)
/
v
m
h(x) = d^*(x)/v_m
h(x)=d∗(x)/vm，
d
∗
(
x
)
d^*(x)
d∗(x)是当前位置
x
x
x但终点的欧氏距离
Flight Corridor Generation
对路径中的每个点通过ESDF寻找最近障碍距离，得到一个内接圆，然后往
x
,
y
,
z
x,y,z
x,y,z每个方向上进行拓展知道碰到障碍物，最后删除重复多余的路径节点，完成飞行走廊的生成
Trajectory Genration
通过飞行走廊，连续约束，动力学约束，安全约束等构造二次规划（QP）问题，求解得优化后的轨迹