一种多旋翼无人机自动返航路径规划算法
【技术领域】
[0001] 本发明属于飞行器导航、制导与控制技术领域，具体涉及一种多旋翼无人机自动 返航路径规划算法，适用于多旋翼无人机在较为复杂的环境中飞行时，正常以及故障条件 下自动返航过程中的路径规划。
【背景技术】
[0002] 多旋翼无人机在影视航拍、国土测绘等行业有着广泛的应用，由于其自动化程度 高，操作简单，因此在非专业领域也有所应用。高度自动化的多旋翼无人机具有能够自动返 回起飞点或设定点的能力，返航可以由操作者或故障处理程序所触发，当前多旋翼无人机 在自动返航过程中，为了避免原路返航使得有效续航时间下降太多的缺点，通常采取返航 起点到起飞点的直线返航方式，因此在返航过程中有可能撞上障碍物，即使产品采用了先 升高飞行高度再返航的策略，但是这种策略在较为复杂的环境中仍然无法规避障碍物。
【发明内容】
[0003] 本发明的目的是在于针对现有技术存在的上述问题，提供一种多旋翼无人机自动 返航路径规划算法，当飞行范围较小且周围环境较为复杂时（如周围有多座高大建筑或高 大树木），该算法可以从原飞行路径中截取最短的部分作为返航路径，从而避免了直线返航 无法规避障碍物的缺点。
[0004] 本发明采用的技术方案如下:无人机飞行过程中实时记录每个时刻的位置和速度 信息(经度、炜度、高度、速度方向），在需要自动返航时，首先搜索飞行路径上的交叉点，然 后在交叉点以及各交叉点之间的距离构成的无向图中搜索最短路径，并沿此为返航路径进 行自动返航。
[0005] 其具体步骤包括：
[0006] 步骤1，以原点在起飞点，X，Y，Z轴分别指向北、东、地的当地铅锤坐标系为导航坐 标系，在无人机正常飞行过程中在固定的时间间隔AT的记录点记录无人机在导航坐标系 中的飞行参数，飞行参数包括无人机在各个记录点的实时位置、速度矢量，
[0007] 步骤2,自动返航飞行触发后，根据记录点的飞行参数搜索飞行轨迹交叉点，
[0008] 将无人机起飞点、飞行轨迹交叉点以及返航起始点统称为兴趣点，从起飞点开始 沿着飞行轨迹计算任意两个兴趣点之间的沿飞行轨迹的长度，非直接连通的兴趣点之间的 距离则记为~，作邻接矩阵：
[0010]其中，邻接矩阵的第i行与兴趣点Pi对应，第i列与兴趣点Pi对应，其中Κ1，···，η，η 为兴趣点总的个数，邻接矩阵中第i行的各个元素为对应的兴趣APi依次与各列对应的兴 趣点Pi-Pni间的飞行轨迹长度；
[0011] 步骤3，从返航起始点开始，将返航起始点作为多叉树的根节点，其他的兴趣点作 为多叉树的子节点构建多叉树，剔除多叉树中叶节点不是起飞点的分支，在多叉树中运用 深度优先方法搜索最短的返航路径；
[0012] 步骤4,将最短的返航路径对应的记录点组成返航路径点集，通过依次将返航路径 点集中的点作为无人机的目标航路点进行跟踪，完成返航轨迹的跟踪。
[0013] 如上所述的飞行轨迹交叉点的判别需要同时满足以下三个标准：
[0014] (1)两个记录点的无人机的实时位置的距离小于设定的距离Δ 1;
[0015] (2)两个记录点的时间差大于设定的时间差Δ t;
[0016] (3)两个记录点的无人机的速度矢量夹角大于设定的速度矢量夹角Δα。
[0017] 如上所述的步骤3包括以下步骤：
[0018] 步骤3.1、选取返航起点作为多叉树的根节点，记为当前级节点；
[0019] 步骤3.2、若所有兴趣点均已纳入多叉树，执行步骤3.5;否则，执行步骤3.3;
[0020] 步骤3.3、对当前级的各个节点依次进行如下处理：
[0021] 在邻接矩阵对应节点的行中搜索大于0且小于％的元素作为直通点元素，直通点 元素所在列则对应了与节点直接连通的兴趣点，查找直通点元素中所在列对应的兴趣点不 属于节点的所有父节点的元素作为子节点元素，将子节点元素的所在列对应的兴趣点作为 节点的子节点并入多叉树；
[0022] 步骤3.4、将当前节点级别递增，返回步骤3.2;
[0023] 步骤3.5、将多叉树中叶节点不是起飞点的分支去除；
[0024] 步骤3.6、在多叉树中运用深度优先方法搜索最短的返航路径。
[0025] 本发明与现有技术相比，具有以下优点和效果：
[0026] 在周围有高大建筑、树木等较复杂的飞行环境中，现有的直线返航的自动返航方 法无法有效规避障碍物，存在较大的安全隐患，而本发明所规划出分返航路径为飞行中所 记录路径的子集，能够有效避免直线返航方式可能碰上障碍物的缺点。
【附图说明】
[0027] 图1无人机飞行轨迹和轨迹交叉点；
[0028]图2搜索得到的最短返航路径。
【具体实施方式】 [0029] 实施例1:
[0030] 如图1所示:一种无人机自动返航路径规划算法，包括以下几个步骤：
[0031] 步骤1，以原点在起飞点，Χ，Υ，Ζ轴分别指向北、东、地的当地铅锤坐标系为导航坐 标系，在无人机正常飞行过程中在固定的时间间隔AT的记录点记录无人机在导航坐标系 中的飞行参数，飞行参数包括无人机在各个记录点的实时位置、速度矢量。
[0032] 步骤2，自动返航飞行可以由用户手动触发或遇到部件故障、电池电压过低或遥控 器信号丢失故障自动触发，如图1所示，自动返航飞行触发后，从记录的飞行参数中将任意 两个记录点的飞行参数进行比对，从而搜索出飞行轨迹交叉点QhQ% ???QnQm，其中m为飞 行轨迹交叉点总的个数，飞行轨迹交叉点的判别需要同时满足以下三个标准：
[0033] (1)两个记录点的无人机的实时位置的距离小于设定的距离Δ 1;
[0034] (2)两个记录点的时间差大于设定的时间差Δ t;
[0035] (3)两个记录点的无人机的速度矢量夹角大于设定的速度矢量夹角Δα，此参数△ α与A t共同能够保证两个点不是飞行轨迹上的相邻点。
[0036] 将无人机起飞点（返航飞行终点）、飞行轨迹交叉点以及返航起始点统称为兴趣点 Pi，P2，··％-!，Pn，当所有符合要求的飞行轨迹交叉点被搜索完毕之后，则从起飞点开始沿着 飞行轨迹计算任意两个兴趣点之间的沿飞行轨迹的长度，非直接连通的兴趣点之间的距离 则记为~，兴趣点之间的沿飞行轨迹的长