CN110456825B - 一种基于改进快速随机搜索树的无人机在线运动规划方法 - Google Patents

Abstract

本发明提出一种基于改进快速随机搜索树的无人机在线运动规划方法，属于无人机运动规划领域。该方法在第一个规划周期到来时，以无人机实时位姿状态作为根节点初始化生成树和可行轨迹集合；在每个规划周期到来时，在当前生成树中寻找距离无人机初始状态最近节点并判定，更新当前生成树的根节点；根据新根节点对当前生成树剪枝，并在当前规划周期剩余时长进行生成树增长，更新当前可行轨迹集合；在每个规划周期结束时，若无人机实时状态对应节点和可行轨迹连接成功且连接后的轨迹在更新后的地图中可行，则输出该可行轨迹。本发明在保证输出轨迹运动学约束和最优性的基础上提升算法效率，优化轨迹输出稳定性，使实际场景中无人机具备自主飞行的能力。

Description

一种基于改进快速随机搜索树的无人机在线运动规划方法

技术领域

本发明涉及一种基于改进快速随机搜索树的无人机在线运动规划方法，属于无人机运动规划领域。

背景技术

无人机在线运动规划的主要目标是基于从无人机实时状态飞往目标状态的任务，根据无人机自身运动约束和在线更新的地图信息，在线输出连续稳定的可达轨迹。在线运动规划是无人机在实际场景执行任务必须具备的重要能力，也是实现无人机自主飞行的关键技术之一。

在无人机执行任务的实际场景比如封闭化的办公空间或者工厂环境中，可移动障碍物使得环境信息发生动态变化，空间连通的拓扑结构也可能发生改变。此外环境本身的非结构化复杂特性也使得固定起始和目标状态下可达路径的探索难度很高。因此在此类复杂动态环境下，无人机系统必须能够进行在线规划以有效应对地图的动态更新，并同时保证在线轨迹输出的稳定性，尽可能利用之前规划的有效信息以提升规划算法效率，在整个任务执行过程中实现轨迹最优性和稳定性的平衡。

在三维和更高维状态空间中，基于图搜素的方法计算量指数上升，因而基于随机采样的方法应用更为广泛，并能够基于模型在轨迹生成过程中考虑无人机自身运动学特性对生成轨迹的约束。但是在实际场景中不可避免的轨迹跟踪偏差和环境的变化使得规划问题时常发生改变进而需要不断重规划，而基于采样的规划方法由于其采样的随机性会导致生成轨迹的形状和结构发生变化，最终可能引发整个系统在线规划和控制的不稳定性。

发明内容

本发明的目的在于解决上述技术的不足之处，提出一种基于改进快速随机搜索树的无人机在线运动规划方法。本发明在保证输出轨迹运动学约束和最优性的基础上提升了算法效率，并优化了轨迹在线输出的稳定性，能够使实际场景中的无人机具备自主飞行的能力。

本发明提出一种基于改进快速随机搜索树的无人机在线运动规划方法，其特征在于，包括以下步骤：

1)设置规划周期的长度Δt，将每个规划周期的初始时刻记为t时刻；

2)将第一个规划周期作为当前规划周期；

3)在当前规划周期的初始时刻，初始化生成树作为当前生成树；获取无人机实时位姿状态作为无人机当前规划周期的初始状态

n_x表示状态空间维度，将无人机的初始状态x_current(t)作为当前生成树的根节点；

当前生成树中的每个节点中包含从根节点到达该节点的预测时间戳和最短路径长度，以及从该节点到达目标节点x_go_al的最短路径长度信息，x_go_al为无人机运动的目标状态；

4)根据当前生成树中的节点信息初始化可行轨迹集合作为当前可行轨迹集合；若当前可行轨迹集合中存在可达轨迹，则可达轨迹集合中到达目标节点轨迹长度最小的轨迹为当前最优轨迹；若当前可行轨迹集合中不存在可达轨迹，则当前可行轨迹集合中轨迹终点距离目标节点最近的轨迹为当前最优轨迹；

5)更新当前规划周期初始时刻的地图信息χ_free(t)；

6)在当前生成树中寻找距离无人机初始状态x_current(t)最近的节点x_n并进行判定：

如果最近邻节点x_n和x_current(t)距离大于预设距离阈值或者x_n的预测时间戳超出时间阈值，则完全舍弃当前生成树，以无人机当前规划周期初始状态为新的根节点并生成新的当前生成树，清除当前可行轨迹集合和当前最优轨迹信息，进入步骤8)；否则从最近邻节点x_n沿当前最优轨迹方向搜索，根据规划周期时长和当前最优轨迹中节点的时间戳信息，找到当前规划周期结束时刻的预测节点x_p作为新的根节点，进入步骤7)；

7)将步骤6)的最近邻节点x_n和新的根节点x_p连通轨迹的所有中间节点作为保护节点全部暂存，进入步骤8)；

8)根据新的根节点对当前生成树进行剪枝，保留根节点子树，去除其它节点，并根据去除节点删除该节点在当前可行轨迹集合中对应的可行轨迹得到更新后的当前可行轨迹集合，然后更新当前最优轨迹信息，进入步骤9)；

9)在当前规划周期剩余规划时长进行生成树增长，更新当前可行轨迹集合；具体步骤如下：

9-1)在控制器参考目标空间

中进行单点采样

n_r表示参考目标空间维度,并通过补零扩增将该采样点扩增到状态空间采样节点

9-2)在当前生成树中根据节点距离度量和近邻半径得到采样节点的近邻节点集合

其中

为采样连接半径，并选取x_r的最近邻节点x_q和采样节点x_r尝试进行闭环预测连接；如果成功，则进入9-3)；如果失败则返回步骤9-1)重新采样；

9-3)将采样节点的近邻节点集合中每个节点x_w按照节点的到达路径长度cost_to-come(x_w)和节点x_w与采样节点x_r的欧式距离cost(x_w,x_r)之和从小到大排序，并依序访问δ(x_r)中的每个节点x_w，基于闭环预测尝试与采样节点x_r进行连接：如果连接成功，则更新采样节点x_r的父节点以及将连通轨迹中间点

作为新增节点加入当前生成树中；

9-4)对采样节点的近邻节点集合中除开步骤9-3)中实际选取的和采样节点x_r进行连接的近邻节点

之外所有节点依次进行访问，以访问节点为目标节点，使用闭环预测尝试和连通轨迹x(t)中的每个节点x_i进行连接，并生成对应轨迹；如果连接成功，则比较以节点x_i为父节点情况下访问节点的到达路径长度是否比当前维护的值小，如果是则更新访问节点的父节点以及该节点对应所有子树信息，并将连接的连通轨迹中间点作为新增节点加入当前生成树中；否则不进行操作；

9-5)检查步骤9-3)和9-4)的每个新增节点是否在目标区域δ(x_goal,r_goal)中，r_goal为预设目标区域半径；若是，则更新当前可行轨迹集合，记录当前最优轨迹或者终点距离目标节点最近的可行轨迹的叶子节点，保存相应的轨迹长度；

判定当前规划周期是否结束：若当前规划周期未结束，则重新返回步骤9-1)；否则结束树增长过程，得到更新完毕的当前可行轨迹集合，进入步骤10)；

10)当前规划周期结束时获取当前无人机实时状态并更新地图信息，将步骤7)中暂存的保护节点根据连接关系重新添加到当前生成树中；

根据可达性和最优性对所有可行轨迹进行优先级排序，并按照排序依次尝试连接节点和对应可行轨迹：如果节点和对应可行轨迹连接成功并且连接后的对应轨迹在更新后的地图中可行，则立即输出该可行轨迹作为新的当前最优轨迹，在下一个规划周期到来时，将新的规划周期作为当前规划周期，获取无人机实时位姿状态作为无人机当前规划周期的初始状态，重新返回步骤5)；如果节点和对应可行轨迹连接不成功或者连接后的对应轨迹在更新后的地图中不可行，则删除该可行轨迹在当前可行轨迹集合中的对应信息；如果节点和所有可行轨迹全部连接失败或者连接后的所有对应轨迹在更新后的地图中不可行，则输出安全停止轨迹，完全舍弃当前生成树，在下一个规划周期到来时，将新的规划周期作为当前规划周期，获取无人机实时位姿状态作为无人机当前规划周期的初始状态，以无人机初始状态为新的根节点并生成新的当前生成树，清除当前可行轨迹集合和最优轨迹信息，重新返回步骤4)。

本发明的特点及其有益效果在于：

1.本发明方法中设计了一种快速随机搜索树RRT*(Rapidly-exploring RandomTree Star)增长策略和闭环(close-loop)预测节点连接的高效结合方法CL-RRT*，使得规划轨迹能够同时满足无人机本体运动学约束和与RRT*相近的渐进最优性(在采样半径足够大时，CL-RRT*的渐进最优性和RRT*等价)，并保证在规划算法的空间复杂度基本不变的情况下节点增长效率的提升。

2.本发明基于轨迹点时间和空间信息判断是否需要重规划并合理选择在线规划起点，通过剪枝操作实现对之前规划有效信息的保留，保证前后相邻规划周期信息的有效复用和尽可能避免完全重规划，同时设计了应对特殊情况的安全机制。

3.本发明能够具体应用于室内外复杂环境下微小型无人机的空间探索和运动规划，通过和无人机机载设备、导航与控制系统配合，使无人机具备自主飞行的能力。

附图说明

图1为本发明实施例的总体流程框图；

图2为本发明实施例的在线选择规划起点和删除失效信息的示意图；

图3为本发明实施例的单个周期规划中CL-RRT*执行流程框图；

图4为本发明方法中采样节点连接和生成树结构修改示意图；

具体实施方式

本发明提出一种基于改进快速随机搜索树的无人机在线运动规划方法，下面结合附图和具体实施例对本发明进一步详细说明。

本发明提出一种基于改进快速随机搜索树的无人机在线运动规划方法，整体流程如图1所示，具体包括以下步骤：

1)设置规划周期的长度Δt(通常根据地图信息更新频率设置，本实施例的长度为1s)，将每个规划周期的初始时刻记为t时刻；

2)将第一个规划周期作为当前规划周期；

3)在当前规划周期的初始时刻，初始化生成树作为当前生成树；获取无人机实时位姿状态(包含惯性参考系下的三轴位置、速度、姿态角和角速率，对应空间为

n_x表示状态空间维度)作为无人机当前规划周期的初始状态

将无人机的初始状态x_current(t)作为当前生成树的根节点；

当前生成树中的每个节点中包含着从根节点到达该节点的预测时间戳和最短路径长度，以及从该节点到达目标节点x_goal(目标节点即无人机运动的目标状态)的最短路径长度信息(最短路径长度的初值根据更新原则设置为0或者最大浮点值)。

4)根据当前生成树中的节点信息初始化可行轨迹集合作为当前可行轨迹集合，可行轨迹可以由每条轨迹的终点完整描述。若当前可行轨迹集合中存在可达轨迹(可达轨迹为能够到达目标节点的轨迹)，则可达轨迹集合中到达目标节点轨迹长度最小的轨迹为当前最优轨迹；若当前可行轨迹集合中不存在可达轨迹，则当前可行轨迹集合中轨迹终点距离目标节点最近的轨迹为当前最优轨迹。

可行轨迹为满足运动学约束且无碰撞的轨迹，可达轨迹在可行轨迹基础上要求轨迹终点在目标节点邻近区域内。

5)更新当前规划周期初始时刻的地图信息χ_free(t)，即当前规划周期初始时刻整个无人机运动规划的任务空间在设定分辨率(0.25m)下的空间占据情况(通常由机载建图模块或者外部监控设施负责提供)，根据规划任务获取无人机的目标状态即目标节点x_goal，将所述无人机当前规划周期的初始状态、目标状态和初始时刻的地图信息作为运动规划算法的初始输入参数。算法通过后续步骤最终输出可达目标节点的可行轨迹。

所述无人机当前规划周期的初始状态包括无人机当前规划周期初始时刻在惯性系下的三轴位置、速度、姿态角和角速度。所述无人机的目标状态包括无人机在惯性系下期望的三轴位置、速度、姿态角和角速度，在运动规划算法中目标状态可以直接生成独立的目标节点。

6)规划起点选择；图2为本发明实施例的在线选择规划起点和删除失效信息的示意图；如图2所示，黑色较大实心圆表示当前生成树的根节点，灰色较大实心圆表示当前生成树的其它根节点，褐色较小实心圆表示无人机初始状态，保护区表示在这个规划周期之内无人机正在执行跟踪的上周期轨迹片段，由虚线环标注，重连接则指代步骤10)中在当前规划周期结束时对无人机的实时状态重新尝试连接到当前周期得到的最优轨迹。

在当前生成树中寻找距离无人机初始状态x_current(t)最近的节点x_n并进行判定：

如果最近邻节点x_n和x_current(t)距离大于预设距离阈值(通常为0.25m左右)或者x_n的预测时间戳超出时间阈值范围(0.5s左右)，则完全舍弃当前生成树，以无人机当前规划周期初始状态为新的根节点并生成新的当前生成树，清除当前可行轨迹集合和当前最优轨迹信息，进入步骤8)；否则从最近邻节点x_n沿当前最优轨迹方向搜索(如果当前不存在当前最优轨迹，则根据无人机当前规划周期的初始状态以匀速模型进行搜索)，根据规划周期时长(本实施例为1s)和当前最优轨迹中节点的时间戳信息，找到当前规划周期结束时刻的预测节点x_p作为新的根节点，进入步骤7)。

7)将步骤6)的最近邻节点x_n和新的根节点x_p连通轨迹的所有中间节点作为保护节点全部暂存，进入步骤8)。

暂存节点不参与后续树增长过程。

8)生成树剪枝步骤，根据新的根节点对当前生成树进行剪枝，仅保留根节点子树，去除其它节点，并根据去除节点删除该节点在当前可行轨迹集合中对应的可行轨迹得到更新后的当前可行轨迹集合，然后更新当前最优轨迹信息，进入步骤9)。

9)生成树增长步骤，在当前规划周期剩余规划时长进行CL-RRT*树增长，更新当前可行轨迹集合。

10)最优轨迹输出步骤；当前规划周期结束后获取当前无人机实时状态x_current(t+Δt)(Δt为规划周期时长)并更新地图信息，将步骤7)中暂存的保护节点根据连接关系重新添加到当前生成树中。根据可达性(轨迹是否能够到达目标节点)和最优性(轨迹长度)对所有可行轨迹进行优先级排序(若当前可行轨迹集合中存在可达轨迹，则可达轨迹集合中到达目标节点轨迹长度最小的轨迹为当前最优轨迹；若当前可行轨迹集合中不存在可达轨迹，则当前可行轨迹集合中轨迹终点距离目标节点最近的轨迹为当前最优轨迹。)，并按照排序依次尝试连接节点x_current(t+Δt)和对应可行轨迹：如果节点x_current(t+Δt)和对应可行轨迹连接成功并且连接后的对应轨迹在更新后的地图中仍旧可行，则立即输出该可行轨迹作为新的当前最优轨迹到底层控制器，无人机按照当前最优轨迹进行运动，当前规划周期结束，在下一个规划周期到来时，将新的规划周期作为当前规划周期，获取无人机实时位姿状态作为无人机当前规划周期的初始状态，重新返回步骤5)；如果节点x_current(t+Δt)和对应可行轨迹连接不成功或者连接后的对应轨迹在更新后的地图中不可行，则删除该可行轨迹在当前可行轨迹集合中的对应信息；如果节点x_current(t+Δt)和所有可行轨迹全部连接失败或者连接后的所有对应轨迹在更新后的地图中不可行，则输出安全停止轨迹(仅包含无人机当前位置对应轨迹点)，完全舍弃当前生成树，在下一个规划周期到来时，将新的规划周期作为当前规划周期，获取无人机实时位姿状态作为无人机当前规划周期的初始状态，以无人机初始状态为新的根节点并生成新的当前生成树，清除当前可行轨迹集合和最优轨迹信息，重新返回步骤4)。

步骤9)中生成树增长步骤的流程如图3所示。根据本发明实施例的方法，CL-RRT*策略下的树增长步骤具体包括：

9-1)在控制器(一般和无人机底层控制器设计一致)参考目标空间

(n_r表示参考目标空间维度)(位置加偏航角的四维空间)中进行单点采样

并通过补零扩增将该采样点扩增到状态空间采样节点

控制器参考目标空间的采样分为两种采样方式，一种是整个控制器参考目标空间的均匀采样，一种是目标区域采样。其中均匀采样点会和生成树最近邻节点依据最大采样半径进行插值得到最终采样点。实际采样过程中基于给定概率分布选取采样方式。

9-2)在当前生成树中根据节点距离度量和近邻半径得到采样节点的近邻节点集合

(

由具体环境维度和尺度决定，代表采样连接半径)，并选取x_r的最近邻节点x_q和采样节点x_r尝试进行闭环预测连接；如果成功，则进入9-3)；如果失败则返回步骤9-1)重新采样。

9-3)将采样节点的近邻节点集合中每个节点x_w按照节点的到达路径长度cost_to-come(x_w)(从当前生成树根节点到节点x_w的最短通路长度)和节点x_w与采样节点x_r的欧式距离cost(x_w,x_r)之和从小到大排序，并依序访问δ(x_r)中的每个节点x_w，基于闭环预测尝试与采样节点x_r进行连接：如果连接成功，则更新采样节点x_r的父节点以及将连通轨迹中间点

(公式中t的下标指代对应节点)作为新增节点加入当前生成树中。

9-4)对采样节点的近邻节点集合中除开步骤9-3)中实际选取的和采样节点x_r进行连接的近邻节点

之外所有节点依次进行访问，以访问节点为目标节点，使用闭环预测尝试和连通轨迹x(t)中的每个节点x_i(依次从

到x_r，但不包含

)进行连接，并生成对应轨迹。如果连接成功，则比较以节点x_i为父节点情况下访问节点的到达路径长度是否比当前维护的值(在对应节点创立和访问过程中根据父节点和连接情况设置和更新)小，如果是则更新访问节点的父节点以及该节点对应所有子树信息，并将连接的连通轨迹中间点作为新增节点加入当前生成树中；否则不进行操作。

如图4所示，在本发明实例步骤9-2)到9-4)中，针对由闭环预测产生的连通轨迹中间点x_i不进行领域范围内基于到达路径长度最小化的父节点更新操作，但对采样点x_r近邻集合进行父节点更新时，将x_i也纳入备选父节点中。结合闭环预测可以大幅提高RRT*的节点生成和树增长的效率，而选择性忽视中间节点x_i和其邻近节点的调整则有效规避掉耗时的近邻点查找操作，整体上兼顾了算法的效率和性能。

CL-RRT*的渐进最优性可以通过分析轨迹中间节点调整的可忽略性来说明。

类似于平面三角形两边之和大于另一边的定理，对于两点的固定中间点分段连接轨迹长度和是直连轨迹长度的一个上界估计，以及两点直连轨迹长度和以直连轨迹中间点分段连接的轨迹长度和相近，依据这两个假设可得到推论：中间点x_i无需通过邻域查找和尝试连接以变更父节点。也能推出所有轨迹中间点也无需变更父节点操作。因此如果从仅需满足采样邻近尺度r_RRT*的最优性而言，CL-RRT*的渐进最优性和RRT*是等价的，并且能够生成带时间维度的节点树，保证输出给底层控制器是平稳的离散轨迹序列而非相对稀疏的一系列停止点。

9-5)检查步骤9-3)和9-4)的每个新增节点是否在目标区域δ(x_goal,r_goal)中(r_goal为预设目标区域半径)，若存在，则更新当前可行轨迹集合，记录当前最优轨迹或者终点距离目标节点最近的可行轨迹的叶子节点，保存相应的轨迹代价(也即轨迹长度)。

判定当前规划周期是否结束：若当前规划周期未结束，则重新返回步骤9-1)；否则结束树增长过程，得到更新完毕的当前可行轨迹集合，进入步骤10)。

Claims (1)

1.一种基于改进快速随机搜索树的无人机在线运动规划方法，其特征在于，包括以下步骤：

1)设置规划周期的长度Δt，将每个规划周期的初始时刻记为t时刻；

2)将第一个规划周期作为当前规划周期；

3)在当前规划周期的初始时刻，初始化生成树作为当前生成树；获取无人机实时位姿状态作为无人机当前规划周期的初始状态

n_x表示状态空间维度，将无人机的初始状态x_current(t)作为当前生成树的根节点；

当前生成树中的每个节点中包含从根节点到达该节点的预测时间戳和最短路径长度，以及从该节点到达目标节点x_goal的最短路径长度信息，x_goal为无人机运动的目标状态；

4)根据当前生成树中的节点信息初始化可行轨迹集合作为当前可行轨迹集合；若当前可行轨迹集合中存在可达轨迹，则可达轨迹集合中到达目标节点轨迹长度最小的轨迹为当前最优轨迹；若当前可行轨迹集合中不存在可达轨迹，则当前可行轨迹集合中轨迹终点距离目标节点最近的轨迹为当前最优轨迹；

5)更新当前规划周期初始时刻的地图信息

6)在当前生成树中寻找距离无人机初始状态x_current(t)最近的节点x_n并进行判定：

如果最近邻节点x_n和x_current(t)距离大于预设距离阈值或者x_n的预测时间戳超出时间阈值，则完全舍弃当前生成树，以无人机当前规划周期初始状态为新的根节点并生成新的当前生成树，清除当前可行轨迹集合和当前最优轨迹信息，进入步骤8)；否则从最近邻节点x_n沿当前最优轨迹方向搜索，根据规划周期时长和当前最优轨迹中节点的时间戳信息，找到当前规划周期结束时刻的预测节点x_p作为新的根节点，进入步骤7)；

7)将步骤6)的最近邻节点x_n和新的根节点x_p连通轨迹的所有中间节点作为保护节点全部暂存，进入步骤8)；

8)根据新的根节点对当前生成树进行剪枝，保留根节点子树，去除其它节点，并根据去除节点删除该节点在当前可行轨迹集合中对应的可行轨迹得到更新后的当前可行轨迹集合，然后更新当前最优轨迹信息，进入步骤9)；

9)在当前规划周期剩余规划时长进行生成树增长，更新当前可行轨迹集合；具体步骤如下：

9-1)在控制器参考目标空间

中进行单点采样

n_r表示参考目标空间维度,并通过补零扩增将该采样点扩增到状态空间采样节点

9-2)在当前生成树中根据节点距离度量和近邻半径得到采样节点的近邻节点集合

其中r_RRT*为采样连接半径，并选取x_r的最近邻节点x_q和采样节点x_r尝试进行闭环预测连接；如果成功，则进入9-3)；如果失败则返回步骤9-1)重新采样；

9-3)将采样节点的近邻节点集合中每个节点x_w按照节点的到达路径长度cost_to-come(x_w)和节点x_w与采样节点x_r的欧式距离cost(x_w,x_r)之和从小到大排序，并依序访问δ(x_r)中的每个节点x_w，基于闭环预测尝试与采样节点x_r进行连接：如果连接成功，则更新采样节点x_r的父节点以及将连通轨迹中间点

作为新增节点加入当前生成树中；

9-4)对采样节点的近邻节点集合中除开步骤9-3)中实际选取的和采样节点x_r进行连接的近邻节点

之外所有节点依次进行访问，以访问节点为目标节点，使用闭环预测尝试和连通轨迹x(t)中的每个节点x_i进行连接，并生成对应轨迹；如果连接成功，则比较以节点x_i为父节点情况下访问节点的到达路径长度是否比当前维护的值小，如果是则更新访问节点的父节点以及该节点对应所有子树信息，并将连接的连通轨迹中间点作为新增节点加入当前生成树中；否则不进行操作；

9-5)检查步骤9-3)和9-4)的每个新增节点是否在目标区域δ(x_goal,r_goal)中，r_goal为预设目标区域半径；若是，则更新当前可行轨迹集合，记录当前最优轨迹或者终点距离目标节点最近的可行轨迹的叶子节点，保存相应的轨迹长度；

判定当前规划周期是否结束：若当前规划周期未结束，则重新返回步骤9-1)；否则结束树增长过程，得到更新完毕的当前可行轨迹集合，进入步骤10)；

10)当前规划周期结束时获取当前无人机实时状态并更新地图信息，将步骤7)中暂存的保护节点根据连接关系重新添加到当前生成树中；

根据可达性和最优性对所有可行轨迹进行优先级排序，并按照排序依次尝试连接节点和对应可行轨迹：如果节点和对应可行轨迹连接成功并且连接后的对应轨迹在更新后的地图中可行，则立即输出该可行轨迹作为新的当前最优轨迹，在下一个规划周期到来时，将新的规划周期作为当前规划周期，获取无人机实时位姿状态作为无人机当前规划周期的初始状态，重新返回步骤5)；如果节点和对应可行轨迹连接不成功或者连接后的对应轨迹在更新后的地图中不可行，则删除该可行轨迹在当前可行轨迹集合中的对应信息；如果节点和所有可行轨迹全部连接失败或者连接后的所有对应轨迹在更新后的地图中不可行，则输出安全停止轨迹，完全舍弃当前生成树，在下一个规划周期到来时，将新的规划周期作为当前规划周期，获取无人机实时位姿状态作为无人机当前规划周期的初始状态，以无人机初始状态为新的根节点并生成新的当前生成树，清除当前可行轨迹集合和最优轨迹信息，重新返回步骤4)。

Images