在线状态估计和环境感知,基于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)问题,求解得优化后的轨迹