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 ( 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
Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn_第1张图片
定义速度函数
Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn_第2张图片
d d d是位置 x x xESDF中的距离值, e e e是欧拉值, v m v_m vm是最大速度
得速度函数如图
Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn_第3张图片
可见当离障碍近(d小)速度小;远,速度大,构造得速度场如下
Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn_第4张图片
可见有障碍的地方速度为0,然后根据之前说的fast marching方法可以获得路径如下
Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn_第5张图片
这里为加速搜索,采用欧氏距离做启发式函数 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每个方向上进行拓展知道碰到障碍物,最后删除重复多余的路径节点,完成飞行走廊的生成
Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn_第6张图片

Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn_第7张图片

Trajectory Genration

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

你可能感兴趣的:(Motion,Planning)