CN105487554A - 一种多旋翼无人机自动返航路径规划算法 - Google Patents

Abstract

本发明公开了一种多旋翼无人机自动返航路径规划算法，记录无人机在导航坐标系中的飞行参数，飞行参数包括无人机在各个记录点的实时位置、速度矢量；根据记录点的飞行参数搜索飞行轨迹交叉点，将无人机起飞点、飞行轨迹交叉点以及返航起始点统称为兴趣点，计算任意两个兴趣点之间的沿飞行轨迹的长度，作邻接矩阵；构建多叉树，在多叉树中运用深度优先方法搜索最短的返航路径；对最短的返航路径进行跟踪。本发明所规划出分返航路径为飞行中所记录路径的子集，能够有效避免直线返航方式可能碰上障碍物。

Description

一种多旋翼无人机自动返航路径规划算法

技术领域

本发明属于飞行器导航、制导与控制技术领域，具体涉及一种多旋翼无人机自动返航路径规划算法，适用于多旋翼无人机在较为复杂的环境中飞行时，正常以及故障条件下自动返航过程中的路径规划。

背景技术

多旋翼无人机在影视航拍、国土测绘等行业有着广泛的应用，由于其自动化程度高，操作简单，因此在非专业领域也有所应用。高度自动化的多旋翼无人机具有能够自动返回起飞点或设定点的能力，返航可以由操作者或故障处理程序所触发，当前多旋翼无人机在自动返航过程中，为了避免原路返航使得有效续航时间下降太多的缺点，通常采取返航起点到起飞点的直线返航方式，因此在返航过程中有可能撞上障碍物，即使产品采用了先升高飞行高度再返航的策略，但是这种策略在较为复杂的环境中仍然无法规避障碍物。

发明内容

本发明的目的是在于针对现有技术存在的上述问题，提供一种多旋翼无人机自动返航路径规划算法，当飞行范围较小且周围环境较为复杂时(如周围有多座高大建筑或高大树木)，该算法可以从原飞行路径中截取最短的部分作为返航路径，从而避免了直线返航无法规避障碍物的缺点。

本发明采用的技术方案如下：无人机飞行过程中实时记录每个时刻的位置和速度信息(经度、纬度、高度、速度方向)，在需要自动返航时，首先搜索飞行路径上的交叉点，然后在交叉点以及各交叉点之间的距离构成的无向图中搜索最短路径，并沿此为返航路径进行自动返航。

其具体步骤包括：

步骤1，以原点在起飞点，X,Y,Z轴分别指向北、东、地的当地铅锤坐标系为导航坐标系，在无人机正常飞行过程中在固定的时间间隔ΔT的记录点记录无人机在导航坐标系中的飞行参数，飞行参数包括无人机在各个记录点的实时位置、速度矢量，

步骤2，自动返航飞行触发后，根据记录点的飞行参数搜索飞行轨迹交叉点，

将无人机起飞点、飞行轨迹交叉点以及返航起始点统称为兴趣点，从起飞点开始沿着飞行轨迹计算任意两个兴趣点之间的沿飞行轨迹的长度，非直接连通的兴趣点之间的距离则记为∞，作邻接矩阵：

其中，邻接矩阵的第i行与兴趣点P_i对应，第i列与兴趣点P_i对应，其中i∈1,…,n，n为兴趣点总的个数，邻接矩阵中第i行的各个元素为对应的兴趣点P_i依次与各列对应的兴趣点P₁～P_n之间的飞行轨迹长度；

步骤3，从返航起始点开始，将返航起始点作为多叉树的根节点，其他的兴趣点作为多叉树的子节点构建多叉树，剔除多叉树中叶节点不是起飞点的分支，在多叉树中运用深度优先方法搜索最短的返航路径；

步骤4，将最短的返航路径对应的记录点组成返航路径点集，通过依次将返航路径点集中的点作为无人机的目标航路点进行跟踪，完成返航轨迹的跟踪。

如上所述的飞行轨迹交叉点的判别需要同时满足以下三个标准：

(1)两个记录点的无人机的实时位置的距离小于设定的距离Δl；

(2)两个记录点的时间差大于设定的时间差Δt；

(3)两个记录点的无人机的速度矢量夹角大于设定的速度矢量夹角Δα。

如上所述的步骤3包括以下步骤：

步骤3.1、选取返航起点作为多叉树的根节点，记为当前级节点；

步骤3.2、若所有兴趣点均已纳入多叉树，执行步骤3.5；否则，执行步骤3.3；

步骤3.3、对当前级的各个节点依次进行如下处理：

在邻接矩阵对应节点的行中搜索大于0且小于∞的元素作为直通点元素，直通点元素所在列则对应了与节点直接连通的兴趣点，查找直通点元素中所在列对应的兴趣点不属于节点的所有父节点的元素作为子节点元素，将子节点元素的所在列对应的兴趣点作为节点的子节点并入多叉树；

步骤3.4、将当前节点级别递增，返回步骤3.2；

步骤3.5、将多叉树中叶节点不是起飞点的分支去除；

步骤3.6、在多叉树中运用深度优先方法搜索最短的返航路径。

本发明与现有技术相比，具有以下优点和效果：

在周围有高大建筑、树木等较复杂的飞行环境中，现有的直线返航的自动返航方法无法有效规避障碍物，存在较大的安全隐患，而本发明所规划出分返航路径为飞行中所记录路径的子集，能够有效避免直线返航方式可能碰上障碍物的缺点。

附图说明

图1无人机飞行轨迹和轨迹交叉点；

图2搜索得到的最短返航路径。

具体实施方式

实施例1：

如图1所示：一种无人机自动返航路径规划算法，包括以下几个步骤：

步骤1，以原点在起飞点，X,Y,Z轴分别指向北、东、地的当地铅锤坐标系为导航坐标系，在无人机正常飞行过程中在固定的时间间隔ΔT的记录点记录无人机在导航坐标系中的飞行参数，飞行参数包括无人机在各个记录点的实时位置、速度矢量。

步骤2，自动返航飞行可以由用户手动触发或遇到部件故障、电池电压过低或遥控器信号丢失故障自动触发，如图1所示，自动返航飞行触发后，从记录的飞行参数中将任意两个记录点的飞行参数进行比对，从而搜索出飞行轨迹交叉点Q₁,Q₂,…Q_m-1,Q_m，其中m为飞行轨迹交叉点总的个数，飞行轨迹交叉点的判别需要同时满足以下三个标准：

(1)两个记录点的无人机的实时位置的距离小于设定的距离Δl；

(2)两个记录点的时间差大于设定的时间差Δt；

(3)两个记录点的无人机的速度矢量夹角大于设定的速度矢量夹角Δα，此参数Δα与Δt共同能够保证两个点不是飞行轨迹上的相邻点。

将无人机起飞点(返航飞行终点)、飞行轨迹交叉点以及返航起始点统称为兴趣点P₁,P₂,…P_n-1,P_n，当所有符合要求的飞行轨迹交叉点被搜索完毕之后，则从起飞点开始沿着飞行轨迹计算任意两个兴趣点之间的沿飞行轨迹的长度，非直接连通的兴趣点之间的距离则记为∞，兴趣点之间的沿飞行轨迹的长度通过无人机在各个记录点的实时位置进行计算，假设兴趣点P_i与兴趣点P_i+1相连通，其中i∈1,…,n，n为兴趣点总的个数，n＝m+2，且两兴趣点的时间间隔为NΔT，则轨迹P_iP_i+1上具有N+1个记录点。记上述N+1个记录点在导航坐标系中的坐标为：

x_j,y_j,z_j,(j∈0,1,…,N)

则轨迹P_iP_i+1的总长度为：

$\left|P_i{P}_{i+1}\right|=\underset{j=1}{\overset{N}{\Σ}}\sqrt{{(x_j-x_{j-1})}^2+{(y_j-y_{j-1})}^2+{(z_j-z_{j-1})}^2}$

将各兴趣点与其之间的飞行轨迹长度构成无向图，并作邻接矩阵：

其中，邻接矩阵的第i行与兴趣点P_i对应，第i列与兴趣点P_i对应，邻接矩阵中第i行的各个元素为对应的兴趣点P_i依次与各列对应的兴趣点P₁～P_n之间的飞行轨迹长度，因此对角线元素均为0，非直接连通的兴趣点之间的距离则记为∞，例如邻接矩阵的第一行与兴趣点P₁对应，从左到右，第一个元素为兴趣点P₁到兴趣点P₁的距离(距离为0)，第二个元素|P₁P₂|为兴趣点P₁到兴趣点P₂之间的飞行轨迹长度，依次类推。

步骤3，从返航起始点开始，将返航起始点作为多叉树的根节点，并将与返航起始点距离在开区间(0,∞)内的兴趣点作为其子节点构建多叉树，剔除多叉树中叶节点不是起飞点的分支，在多叉树中运用深度优先方法搜索最短的返航路径，其具体步骤如下：

步骤3.1、选取返航起点作为多叉树的根节点，记为当前级节点；

步骤3.2、若所有兴趣点均已纳入多叉树，则表明多叉树的建立已完成，执行步骤3.5，否则，继续构建多叉树，执行步骤3.3；

步骤3.3、对当前级的各个节点依次进行如下处理：

在邻接矩阵对应节点的行中搜索大于0且小于∞的元素作为直通点元素，直通点元素所在列则对应了与节点直接连通的兴趣点，查找直通点元素中所在列对应的兴趣点不属于节点的所有父节点的元素作为子节点元素，将子节点元素的所在列对应的兴趣点作为节点的子节点并入多叉树；

步骤3.4、将当前节点级别递增，返回步骤3.2，即新的当前节点级别中的各个节点为步骤3.3中获得的子节点；

步骤3.5、将多叉树中叶节点不是起飞点的分支去除；

步骤3.6、如图2所示，在多叉树中运用深度优先方法搜索最短的返航路径。

步骤4，得到最短的返航路径之后，则将最短的返航路径对应的记录点组成返航路径点集，通过依次将返航路径点集中的点作为无人机的目标航路点进行跟踪，即可完成返航轨迹的跟踪。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (3)

1.一种多旋翼无人机自动返航路径规划算法，其特征在于，包括以下步骤：

步骤1，以原点在起飞点，X,Y,Z轴分别指向北、东、地的当地铅锤坐标系为导航坐标系，在无人机正常飞行过程中在固定的时间间隔ΔT的记录点记录无人机在导航坐标系中的飞行参数，飞行参数包括无人机在各个记录点的实时位置、速度矢量，

步骤2，自动返航飞行触发后，根据记录点的飞行参数搜索飞行轨迹交叉点，

将无人机起飞点、飞行轨迹交叉点以及返航起始点统称为兴趣点，从起飞点开始沿着飞行轨迹计算任意两个兴趣点之间的沿飞行轨迹的长度，非直接连通的兴趣点之间的距离则记为∞，作邻接矩阵：

其中，邻接矩阵的第i行与兴趣点P_i对应，第i列与兴趣点P_i对应，其中i∈1,…,n，n为兴趣点总的个数，邻接矩阵中第i行的各个元素为对应的兴趣点P_i依次与各列对应的兴趣点P₁～P_n之间的飞行轨迹长度；

步骤3，从返航起始点开始，将返航起始点作为多叉树的根节点，其他的兴趣点作为多叉树的子节点构建多叉树，剔除多叉树中叶节点不是起飞点的分支，在多叉树中运用深度优先方法搜索最短的返航路径；

步骤4，将最短的返航路径对应的记录点组成返航路径点集，通过依次将返航路径点集中的点作为无人机的目标航路点进行跟踪，完成返航轨迹的跟踪。

2.根据权利要求1所述的一种多旋翼无人机自动返航路径规划算法，其特征在于，所述的飞行轨迹交叉点的判别需要同时满足以下三个标准：

(1)两个记录点的无人机的实时位置的距离小于设定的距离Δl；

(2)两个记录点的时间差大于设定的时间差Δt；

(3)两个记录点的无人机的速度矢量夹角大于设定的速度矢量夹角Δα。

3.根据权利要求1所述的一种多旋翼无人机自动返航路径规划算法，其特征在于，所述的步骤3包括以下步骤：

步骤3.1、选取返航起点作为多叉树的根节点，记为当前级节点；

步骤3.2、若所有兴趣点均已纳入多叉树，执行步骤3.5；否则，执行步骤3.3；

步骤3.3、对当前级的各个节点依次进行如下处理：

在邻接矩阵对应节点的行中搜索大于0且小于∞的元素作为直通点元素，直通点元素所在列则对应了与节点直接连通的兴趣点，查找直通点元素中所在列对应的兴趣点不属于节点的所有父节点的元素作为子节点元素，将子节点元素的所在列对应的兴趣点作为节点的子节点并入多叉树；

步骤3.4、将当前节点级别递增，返回步骤3.2；

步骤3.5、将多叉树中叶节点不是起飞点的分支去除；

步骤3.6、在多叉树中运用深度优先方法搜索最短的返航路径。