CN111207752B - 一种基于动态切点调整的无人机航迹规划方法 - Google Patents

Abstract

本发明属于无人机航迹规划技术领域，公开了一种基于动态切点调整的无人机航迹规划方法，基于动态切点调整的航迹规划方法，针对现有技术存在的问题，将航迹规划问题形式化为一个与定位误差校正次数、航迹长度和转弯半径相关的多目标优化问题，在保证定位误差得到有效校正的前提下优化航迹长度，并利用动态切点调整的方式来制定无人机的转弯策略。本发明可有效降低无人机航迹长度与算法复杂度并能符合无人机动力结构，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划。

Description

一种基于动态切点调整的无人机航迹规划方法

技术领域

本发明属于无人机航迹规划技术领域，尤其涉及一种基于动态切点调整的无人机航迹规划方法。

背景技术

无人机是一种有动力、可控制、能携带多种任务设备、执行多种任务并能重复使用的无人驾驶航空器。由于具备无人驾驶、尺寸小、成本低以及机动性强等优点，被广泛应用于各大军用和民用领域当中，如无人机通信、农业、火灾监控和检测和物联网等领域。无人机航迹规划技术是无人机应用的重要研究方向。自20世纪中期以来，一些西方国家已相继开始了无人机航迹规划技术的研究，并在20世纪70年代逐步构建了无人机飞行方面的一系列基础理论。而伴随着计算机技术的普遍应用和快速发展，从20世纪80年代中期以及后期开始，相关学者已将无人机航迹规划技术应用于遥控、遥感等领域，并在20世纪90年代，无人机航迹规划技术逐步趋于成熟。但现有的无人机航迹规划技术仍难以满足多样化的应用需求，无人机所用的航迹规划通常需要人工经验操纵处理，同时由于无人机系统结构限制，无人机的定位系统无法对自身进行精准定位，一旦定位误差积累到一定程度可能导致任务失败，并且无人机在转弯时受到结构和控制系统的限制，无法完成即时转弯。因此，在飞行过程中对定位误差进行校正并设计符合无人机动力结构的飞行轨迹是无人机航迹规划中的一项重要任务。

现有技术一提出运用几何数学的手段对无人机航迹进行规划，并在航迹规划过程中对转弯半径、转弯方向予以约束，该技术能快速规划出最短的无人机飞行路径，但该技术假设装载在无人机设备上的定位系统具有精准定位功能，不会产生定位误差。而在实际的无人机飞行过程中，定位误差是会逐渐累积的，一旦超过定位误差阈值则可能会导致无人机偏离预定轨迹，从而影响预期任务目标的实现。

现有技术二提出了与三维空间相结合的快速A*搜索算法，通过无源定位的方式对无人机航迹进行规划，该技术可以通过无源定位的方式来降低定位误差所带来的影响，但该技术所规划的航迹没有考虑无人机动力结构的限制。受限于无人机的动力结构，无人机在转弯时需有一定的转弯半径，无法实现即时转弯，因此该技术不能直接应用于实际的无人机航迹规划应用中。

现有技术三针对目前无人机在飞行任务中无法以最小路径代价实现航带间转弯飞行的问题，提出一种基于最小转弯半径的无人机转弯航迹规划算法。该技术依据无人机情报处理对于航片旁向重叠率的要求，确定航带间距，同时依据无人机飞行速度及姿态数据，确定无人机的最小转弯半径。该技术所规划的航迹不仅短而且符合无人机的动力结构，但该技术复杂程度较高，需根据不同的航带间距来制定不同的转弯策略，而现有无人机的计算能力有限，所规划的无人机航迹不能过于复杂，因此所规划的航迹不仅需符合无人机的动力结构还需尽可能的复杂度低。

综上所述，现有技术存在的问题是：

(1)现有技术一运用几何数学的手段对无人机航迹进行规划，没有考虑无人机定位存在误差的问题，而误差会逐步积累可能导致任务失败的情况发生。

(2)现有技术二通过无源定位的方式对无人机航迹进行规划，在航迹规划过程中没有考虑无人机动力结构的问题，无法满足无人机转弯半径所带来的约束。

(3)现有技术三基于最小转弯半径来对无人机航迹进行规划，但该技术复杂程度较高，针对不同的场景需制定不同的转弯策略，无法满足无人机航迹规划对低程度复杂度的要求。

解决上述技术问题的难度：现有技术一：该技术运用几何数学的手段对无人机航迹进行规划时需假设无人机时刻按预定路线进行飞行，而在飞行过程中产生定位误差是必然的，因此在规划航迹过程中需考虑定位误差所带来的影响。现有技术二：该技术采用快速A*搜索算法来规划航迹，并通过无源定位的方式来降低定位误差，但采用即时转弯的转弯策略不符合无人机的动力结构。现有技术三：该技术考虑无人机转弯半径，并根据不同的场景设计不同的转弯策略，但该技术复杂度较高。

解决上述技术问题的意义：本发明采用基于动态切点调整的航迹规划方法，针对现有技术存在的问题，将航迹规划问题形式化为一个与定位误差校正次数、航迹长度和转弯半径相关的多目标优化问题，在保证定位误差得到有效校正的前提下优化航迹长度，并利用动态切点调整的方式来制定无人机的转弯策略。本发明可有效降低无人机航迹长度与算法复杂度并能符合无人机动力结构，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划。

发明内容

针对现有技术存在的问题，本发明提供了一种基于动态切点调整的无人机航迹规划方法。

本发明是这样实现的，一种基于动态切点调整的无人机航迹规划方法，所述基于动态切点调整的无人机航迹规划问题形式化为一个与定位误差校正次数、航迹长度和转弯半径相关的多目标优化问题，在保证定位误差得到有效校正的前提下优化航迹长度，并利用动态切点调整的方式来制定无人机的转弯策略。

进一步，所述基于动态切点调整的无人机航迹规划方法包括以下步骤：

第一步，构建由1个出发点A、1个目的地B、M个水平校正点、N个垂直校正点组成的无人机航迹规划区域；

第二步，基于无人机航迹规划区域，构建校正点选择策略；

第三步，基于校正点选择策略，构建航迹规划方案；

第四步，基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法。

进一步，所述第一步构建由1个出发点A、1个目的点B、R个水平校正点、L个垂直校正点组成的无人机航迹规划区域，具体包括：

步骤(1.1)，构建包含2+R+L点的无人机航迹规划区域，无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个专用单位，以下简称单位，其中δ为无人机飞行1m，所各增加的垂直误差和水平误差，到达目的点时垂直误差和水平误差均应小于θ个单位，当垂直误差和水平误差均小于θ个单位时，无人机仍能够按照规划航迹飞行；

步骤(1.2)，无人机在飞行过程中需要对定位误差进行校正，航迹规划区域内存在校正点可用于误差校正，当无人机到达校正点即能够根据校正点的误差校正类型进行误差校正，校正垂直和水平误差的位置可根据地形在航迹规划前确定，若垂直误差、水平误差都能得到及时校正，则无人机可以按照预定航线飞行，并通过若干个校正点进行误差校正后最终到达目的地；

步骤(1.3)，无人机在出发点的垂直和水平误差均为0，在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变，在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变；

步骤(1.4)，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正，其中α₁为无人机到达垂直校正点时所允许的最大垂直误差，α₂为无人机到达垂直校正点时所允许的最大水平误差；

步骤(1.5)，当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正，其中β₁为无人机到达水平校正点时所允许的最大垂直误差，β₂为无人机到达水平校正点时所允许的最大水平误差；

步骤(1.6)，无人机在转弯时受到结构和控制系统的限制，无法完成即时转弯，定义无人机的最小转弯半径为r_min；

进一步，所述第二步基于无人机航迹规划区域，构建校正点选择策略具体包括：

步骤(2.1)，经垂直/水平校正点交替校正后，无人机能按照规划路径飞行的最远距离D₁应满足以下关系：

应在该距离内寻找新的垂直校正点进行垂直校正，否则根据第一步所述约束条件无法按规划航迹飞行；

步骤(2.2)，经水平/垂直校正点交替校正后，无人机能按照规划路径飞行的最远距离D₂应满足以下关系：

应在该距离内寻找新的水平校正点进行水平校正，否则根据第一步所述约束条件无法按规划航迹飞行；

步骤(2.3)，若经垂直校正点校正后，无人机不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₃应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行垂直校正；

步骤(2.4)，若经水平校正点校正后，无人机不寻找垂直校正点进行校正，仍寻找水平校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₄应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行水平校正；

步骤(2.5)，综上所述，无人机选择垂直/水平交替校正方案可在飞行相同距离的条件下，取得最少校正次数，并且该方案在选择下一校正点时，仅需对不同组别的点进行计算即可，无需计算同组别的校正点，因此可以降低算法计算量，减少搜索次数；

进一步，所述第三步基于校正点选择策略，构建航迹规划方案具体包括代价函数构建、校正点的范围构建、循环体构建、终止条件构建和优化方案比较五部分：

步骤(3.1)，采用改进的A*算法进行航迹规划，首先构建代价函数，代价函数可表示为：

f(n)＝g(n)+h(n) (5)

其中，g(n)为备选校正点到出发点A的欧式距离，h(n)为备选校正点到目的点B的欧式距离，假设起点A周围存在m个同类型校正点可作为备选，将这些校正点编号为N₁,...,N_m，根据式(5)可计算每个点的代价函数，可表示为：

其中，

为起点A到校正点N₁之间的欧式距离，D_N1,B为校正点N₁到目的点B之间的欧式距离，

为起点A到校正点N_m之间的欧式距离，

为校正点N_m到目的点B之间的欧式距离；

根据式(6)的计算结果，可选出最优的校正点N_i，选择方案如下所示：

f(N_i)＝min{f(N₁),...,f(N_m)}i∈(1,m) (7)

步骤(3.2)，基于构建的代价函数和校正点选择策略，无人机单次飞行且不进行航迹校正的飞行距离是有限的，因此本方法通过确定搜索范围、计算梯度方向和预测未来轨迹来减少备选校正点个数；

步骤(3.2.1)，从出发点A出发先选择垂直校正点，无人机最远飞行距离D_Amax应满足以下条件：

D_Amax≤D₃ (8)

其中，D₃为无人机经垂直校正点校正后，不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，能按照规划路径飞行的最远距离；

若D_Amax＞D₃，即使找到垂直校正点也无法进行垂直校正，因此以出发点A为球心，以半径为D₃做球，在球内所有水平校正点可作为备选校正点，则在球外的校正点则舍去；

步骤(3.2.2)，在球内所有的点也非全是有效点，通过计算梯度方向来判断该点是否为有效点，点N_C、N_D、N_E均为出发点A的待选校正点，分别计算

与

之间梯度，此处梯度具体为向量夹角，若夹角＞90°则将该点舍去，夹角可采用如下的向量夹角公式进行计算：

其中，ω为向量夹角，

和

表示向量，具体指代为向量

与

即参数

可为向量

与

之一，参数

可为向量

与

之一，参数

和

不可相等，依次求解各向量之间夹角；

步骤(3.2.3)，采用垂直/水平交替校正的方案，但数据集中的校正点位置是随机布置的，若校正点N_C为垂直校正点，无人机经点N_C垂直校正后根据本方法应选择水平校正点，但若水平校正点附近无可达的垂直校正点时，应选择择次优的水平校正点N_E，并将点N_D舍去；

步骤(3.2.4)，通过确定搜索范围、计算梯度方向和预测未来轨迹可以减少备选校正点的个数，从而在下一步循环体构建中减少搜索次数；

步骤(3.3)，对目标函数和校正点的范围完成构建后，对循环体予以构建，假设无人机从出发点A出发后先选择的校正点为垂直校正点N_C，无人机到达N_C时的误差为：

其中，

为无人机到达校正点N_C后的垂直误差，

为无人机到达校正点N_C后的水平误差，

为起点A到校正点N_C之间的欧式距离；

经点N_C进行垂直校正后，无人机的垂直/水平误差为：

其中，

为无人机离开校正点N_C后的垂直误差，

为无人机离开校正点N_C后的水平误差；

之后将选择水平校正点进行水平校正，待选的水平校正点处于以点N_C为球心，半径为r′球内，r′满足以下约束条件：

再根据构建的校正点范围和目标函数，即可选出最优的下一校正点；

步骤(3.4)，因采用循环体结构，需对终止条件予以构建，由第一步可知当无人机飞到目的点B时的垂直/水平误差均小于预设值θ，因此在循环体部分选出校正点后，需计算该点是否能直接到达目的点B，若可以则终止循环，否则继续循环；

设点N_N为水平校正点，点N_M为垂直校正点，无人机由点N_M飞向点N_N，此时经点N_N校正后的无人机误差为：

其中，

为无人机离开校正点N_N后的垂直误差，

为无人机离开校正点N_N后的水平误差；

若无人机由点N_N直接飞至目的点B，则飞至目的点B时的误差为：

其中，

为校正点N_M与校正点N_N之间的欧式距离，

为校正点N_N与目的点B之间的欧式距离，

为无人机到达目的点B的水平误差，

为无人机到达目的点B的垂直误差；

若

则允许直接飞至目的点B，完成循环，否则在校正点N_N继续循环，直至达到终止条件；

步骤(3.5)，由于采用垂直/水平交替校正的方案，因此从出发点A出发的情况将有2种，分别是：

(a)先选择垂直校正点进行校正；

(b)先选择水平校正点进行校正；

因此将产生2种航迹规划方案，需对这2种方案进行比较，选择航迹更短的方案。

进一步，所述第四步基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法具体包括起点飞向校正点、校正点飞向校正点、校正点飞向终点三部分：

步骤(4.1)，由于无人机的初始飞行方向可自行设定，因此本方法使无人机的飞行方向dir_A与出发点A，校正点N_a构成的向量

方向一致即

步骤(4.2)，无人机经过校正点N_a后

与出发点A，校正点N_a构成的向量

方向应具有如下关系：

以N_a为切点做圆

圆

半径为，

无人机沿圆弧飞行，飞行的圆心角ξ应满足如下关系：

根据圆心角可计算无人机在N_a点沿圆弧飞行的航迹长度

为：

之后在N′_a点飞出，飞向方向

应与校正点N_a和校正点N_b构成的向量

方向一致，即

再沿

方向飞行l长度，l可由下式进行计算：

此时无人机的位置为点N′_b，以点N′_b为切点做圆

圆

半径仍为

无人机沿圆

飞行即可到达下一校正点N_b；

因此点N_a飞向点N_b采用航迹平滑方案所航行的航迹长度

与

之间的关系如下所示：

在上式中

是一个定值，为点N_a与N_b之间的欧式距离，圆心角ξ也是一个定值，由

与

决定，因此若要使航迹最短，即使

最小；因此，无人机按最小转弯半径进行转弯，即满足以下关系：

其中，

为无人机在校正点N_i时的转弯半径；

无人机此时离开点N_a时的飞向方向

与校正点N_a和校正点N_b构成的向量

垂直；

步骤(4.3)，在圆

上重新寻找一个切点N″_m使之满足以下条件：

其中O_m为圆

的圆心，

为圆

的圆心O_m与切点N″_m所组成的向量；

由点N_m″直接飞向目的点B，到达终点；该段航迹长度可表示为：

其中，

为切点N″_m与目的点B之间的欧式距离，φ为无人机在圆

处转过的圆心角，可由下式进行计算：

其中，

为圆

的圆心O_m与无人机经过的最后一个校正点N_m所组成的向量；

因此规划后的航迹长度可由式(24)进行表示；在式(24)中，点N_a为第一个校正点，点N_m为最后一个校正点，无人机共经过k个校正点；

其中

为起点A到校正点N_a之间的欧式距离，

为校正点N_i与校正点N_j之间的欧式距离。

综上所述，本发明的优点及积极效果为：本发明采用基于动态切点调整的方法，针对现有技术存在的问题，将无人机航迹规划问题形式化为一个与定位误差校正次数、航迹长度和转弯半径相关的多目标优化问题，在保证定位误差得到有效校正的前提下优化航迹长度，并利用动态切点调整的方式来制定无人机的转弯策略。本发明可在保证无人机定位精度的前提下缩短航迹长度和校正次数，同时满足无人机的动力结构。

本发明与现有技术相比，可有效减少定位误差所带来的影响，同时满足无人机的动力结构，平滑航迹，并且本发明方法简单，易于实现，使之能实际运用于无人机通信、农业、火灾监控和检测和物联网等领域。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1是本发明实施例提供的一种基于动态切点调整的无人机航迹规划方法的流程图。

图2是本发明实施例提供的由出发点飞向校正点航迹平滑方案。

图3是本发明实施例提供的由校正点飞向校正点航迹平滑方案。

图4是本发明实施例提供的由校正点飞向目的点航迹平滑方案。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种基于动态切点调整的无人机航迹规划方法，下面结合附图对本发明作详细的描述。

如图1所示，本发明实施例提供的一种基于动态切点调整的无人机航迹规划方法包括以下步骤：

S101：构建由1个出发点A、1个目的地B、M个水平校正点、N个垂直校正点组成的无人机航迹规划区域；

S102：基于无人机航迹规划区域，构建校正点选择策略；

S103：基于校正点选择策略，构建航迹规划方案；

S104：基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法。

下面结合具体实施例对本发明的技术方案作进一步的描述。

本发明实施例提供的一种基于动态切点调整的无人机航迹规划方法包括以下步骤：

第一步，构建由1个出发点A、1个目的点B、R个水平校正点、L个垂直校正点组成的无人机航迹规划区域，具体包括：

步骤(1.1)，构建包含2+R+L点的无人机航迹规划区域，无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个专用单位，以下简称单位，其中δ为无人机飞行1m，所各增加的垂直误差和水平误差，到达目的点时垂直误差和水平误差均应小于θ个单位，当垂直误差和水平误差均小于θ个单位时，无人机仍能够按照规划航迹飞行；

步骤(1.2)，无人机在飞行过程中需要对定位误差进行校正，航迹规划区域内存在校正点可用于误差校正，当无人机到达校正点即能够根据校正点的误差校正类型进行误差校正，校正垂直和水平误差的位置可根据地形在航迹规划前确定，若垂直误差、水平误差都能得到及时校正，则无人机可以按照预定航线飞行，并通过若干个校正点进行误差校正后最终到达目的地；

步骤(1.3)，无人机在出发点的垂直和水平误差均为0，在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变，在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变；

步骤(1.4)，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正，其中α₁为无人机到达垂直校正点时所允许的最大垂直误差，α₂为无人机到达垂直校正点时所允许的最大水平误差；

步骤(1.5)，当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正，其中β₁为无人机到达水平校正点时所允许的最大垂直误差，β₂为无人机到达水平校正点时所允许的最大水平误差；

步骤(1.6)，无人机在转弯时受到结构和控制系统的限制，无法完成即时转弯，定义无人机的最小转弯半径为r_min；

第二步，基于无人机航迹规划区域，构建校正点选择策略具体包括：

步骤(2.1)，经垂直/水平校正点交替校正后，无人机能按照规划路径飞行的最远距离D₁应满足以下关系：

应在该距离内寻找新的垂直校正点进行垂直校正，否则根据第一步所述约束条件无法按规划航迹飞行；

步骤(2.2)，经水平/垂直校正点交替校正后，无人机能按照规划路径飞行的最远距离D₂应满足以下关系：

应在该距离内寻找新的水平校正点进行水平校正，否则根据第一步所述约束条件无法按规划航迹飞行；

步骤(2.3)，若经垂直校正点校正后，无人机不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₃应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行垂直校正；

步骤(2.4)，若经水平校正点校正后，无人机不寻找垂直校正点进行校正，仍寻找水平校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₄应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行水平校正；

步骤(2.5)，综上所述，无人机选择垂直/水平交替校正方案可在飞行相同距离的条件下，取得最少校正次数，并且该方案在选择下一校正点时，仅需对不同组别的点进行计算即可，无需计算同组别的校正点，因此可以降低算法计算量，减少搜索次数；

第三步，基于校正点选择策略，构建航迹规划方案具体包括代价函数构建、校正点的范围构建、循环体构建、终止条件构建和优化方案比较五部分：

步骤(3.1)，采用改进的A*算法进行航迹规划，首先构建代价函数，代价函数可表示为：

f(n)＝g(n)+h(n) (5)

其中，g(n)为备选校正点到出发点A的欧式距离，h(n)为备选校正点到目的点B的欧式距离，假设起点A周围存在m个同类型校正点可作为备选，将这些校正点编号为N₁,...,N_m，根据式(5)可计算每个点的代价函数，可表示为：

其中，

为起点A到校正点N₁之间的欧式距离，

为校正点N₁到目的点B之间的欧式距离，

为起点A到校正点N_m之间的欧式距离，

为校正点N_m到目的点B之间的欧式距离；

根据式(6)的计算结果，可选出最优的校正点N_i，选择方案如下所示：

f(N_i)＝min{f(N₁),...,f(N_m)}i∈(1,m) (7)

步骤(3.2)，基于构建的代价函数和校正点选择策略，无人机单次飞行且不进行航迹校正的飞行距离是有限的，因此本方法通过确定搜索范围、计算梯度方向和预测未来轨迹来减少备选校正点个数；

步骤(3.2.1)，从出发点A出发先选择垂直校正点，无人机最远飞行距离D_Amax应满足以下条件：

D_Amax≤D₃ (8)

其中，D₃为无人机经垂直校正点校正后，不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，能按照规划路径飞行的最远距离；

若D_Amax＞D₃，即使找到垂直校正点也无法进行垂直校正，因此以出发点A为球心，以半径为D₃做球，在球内所有水平校正点可作为备选校正点，则在球外的校正点则舍去；

步骤(3.2.2)，在球内所有的点也非全是有效点，通过计算梯度方向来判断该点是否为有效点，点N_C、N_D、N_E均为出发点A的待选校正点，分别计算

与

之间梯度，此处梯度具体为向量夹角，若夹角＞90°则将该点舍去，夹角可采用如下的向量夹角公式进行计算：

其中，ω为向量夹角，

和

表示向量，具体指代为向量

与

即参数

可为向量

与

之一，参数

可为向量

与

之一，参数

和

不可相等，依次求解各向量之间夹角；

步骤(3.2.3)，采用垂直/水平交替校正的方案，但数据集中的校正点位置是随机布置的，若校正点N_C为垂直校正点，无人机经点N_C垂直校正后根据本方法应选择水平校正点，但若水平校正点附近无可达的垂直校正点时，应选择择次优的水平校正点N_E，并将点N_D舍去；

步骤(3.2.4)，通过确定搜索范围、计算梯度方向和预测未来轨迹可以减少备选校正点的个数，从而在下一步循环体构建中减少搜索次数；

步骤(3.3)，对目标函数和校正点的范围完成构建后，对循环体予以构建，假设无人机从出发点A出发后先选择的校正点为垂直校正点N_C，无人机到达N_C时的误差为：

其中，

为无人机到达校正点N_C后的垂直误差，

为无人机到达校正点N_C后的水平误差，

为起点A到校正点N_C之间的欧式距离；

经点N_C进行垂直校正后，无人机的垂直/水平误差为：

其中，

为无人机离开校正点N_C后的垂直误差，

为无人机离开校正点N_C后的水平误差；

之后将选择水平校正点进行水平校正，待选的水平校正点处于以点N_C为球心，半径为r′球内，r′满足以下约束条件：

再根据构建的校正点范围和目标函数，即可选出最优的下一校正点；

步骤(3.4)，因采用循环体结构，需对终止条件予以构建，由第一步可知当无人机飞到目的点B时的垂直/水平误差均小于预设值θ，因此在循环体部分选出校正点后，需计算该点是否能直接到达目的点B，若可以则终止循环，否则继续循环；

设点N_N为水平校正点，点N_M为垂直校正点，无人机由点N_M飞向点N_N，此时经点N_N校正后的无人机误差为：

其中，

为无人机离开校正点N_N后的垂直误差，

为无人机离开校正点N_N后的水平误差；

若无人机由点N_N直接飞至目的点B，则飞至目的点B时的误差为：

其中，

为校正点N_M与校正点N_N之间的欧式距离，

为校正点N_N与目的点B之间的欧式距离，

为无人机到达目的点B的水平误差，

为无人机到达目的点B的垂直误差；

若

则允许直接飞至目的点B，完成循环，否则在校正点N_N继续循环，直至达到终止条件；

步骤(3.5)，由于采用垂直/水平交替校正的方案，因此从出发点A出发的情况将有2种，分别是：

(a)先选择垂直校正点进行校正；

(b)先选择水平校正点进行校正；

因此将产生2种航迹规划方案，需对这2种方案进行比较，选择航迹更短的方案；

第四步，基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法具体包括起点飞向校正点、校正点飞向校正点、校正点飞向终点三部分：

步骤(4.1)，由于无人机的初始飞行方向可自行设定，因此本方法使无人机的飞行方向dir_A与出发点A，校正点N_a构成的向量

方向一致即

步骤(4.2)，无人机经过校正点N_a后

与出发点A，校正点N_a构成的向量

方向应具有如下关系：

以N_a为切点做圆

圆

半径为，

无人机沿圆弧飞行，飞行的圆心角ξ应满足如下关系：

根据圆心角可计算无人机在N_a点沿圆弧飞行的航迹长度

为：

之后在N′_a点飞出，飞向方向

应与校正点N_a和校正点N_b构成的向量

方向一致，即

再沿

方向飞行l长度，l可由下式进行计算：

此时无人机的位置为点N′_b，以点N′_b为切点做圆

圆

半径仍为

无人机沿圆

飞行即可到达下一校正点N_b；

因此点N_a飞向点N_b采用航迹平滑方案所航行的航迹长度

与

之间的关系如下所示：

在上式中

是一个定值，为点N_a与N_b之间的欧式距离，圆心角ξ也是一个定值，由

与

决定，因此若要使航迹最短，即使

最小；因此，无人机按最小转弯半径进行转弯，即满足以下关系：

其中，

为无人机在校正点N_i时的转弯半径；

无人机此时离开点N_a时的飞向方向

与校正点N_a和校正点N_b构成的向量

垂直；

步骤(4.3)，在圆

上重新寻找一个切点N″_m使之满足以下条件：

其中O_m为圆

的圆心，

为圆

的圆心O_m与切点N″_m所组成的向量；

由点N_m″直接飞向目的点B，到达终点；该段航迹长度可表示为：

其中，

为切点N″_m与目的点B之间的欧式距离，φ为无人机在圆

处转过的圆心角，可由下式进行计算：

其中，

为圆

的圆心O_m与无人机经过的最后一个校正点N_m所组成的向量；

因此规划后的航迹长度可由式(24)进行表示；在式(24)中，点N_a为第一个校正点，点N_m为最后一个校正点，无人机共经过k个校正点；

其中

为起点A到校正点N_a之间的欧式距离，

为校正点N_i与校正点N_j之间的欧式距离。

综上，本发明实施例提供的一种基于动态切点调整的无人机航迹规划方法，针对现有技术存在的问题，将航迹规划问题形式化为一个与定位误差校正次数、航迹长度和转弯半径相关的多目标优化问题，在保证定位误差得到有效校正的前提下优化航迹长度，并利用动态切点调整的方式来制定无人机的转弯策略。本发明可有效降低无人机航迹长度与算法复杂度并能符合无人机动力结构，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于动态切点调整的无人机航迹规划方法，其特征在于，所述一种基于动态切点调整的无人机航迹规划方法将航迹规划问题形式化为一个与定位误差校正次数、航迹长度和转弯半径相关的多目标优化问题，在保证定位误差得到有效校正的前提下优化航迹长度，并利用动态切点调整的方式来制定无人机的转弯策略；本方法可有效降低无人机航迹长度与算法复杂度并能符合无人机动力结构，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划；

所述一种基于动态切点调整的无人机航迹规划方法包括以下步骤：

第一步，构建由1个出发点A、1个目的地B、M个水平校正点、N个垂直校正点组成的无人机航迹规划区域；

第二步，基于无人机航迹规划区域，构建校正点选择策略；

第三步，基于校正点选择策略，构建航迹规划方案；

第四步，基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法；

所述第一步构建由1个出发点A、1个目的点B、R个水平校正点、L个垂直校正点组成的无人机航迹规划区域，具体包括：

步骤(1.1)，构建包含2+R+L点的无人机航迹规划区域，无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个专用单位，以下简称单位，其中δ为无人机飞行1m，所各增加的垂直误差和水平误差，到达目的点时垂直误差和水平误差均应小于等于θ个单位，当垂直误差和水平误差均小于等于θ个单位时，无人机仍能够按照规划航迹飞行；

步骤(1.2)，无人机在飞行过程中需要对定位误差进行校正，航迹规划区域内存在校正点可用于误差校正，当无人机到达校正点即能够根据校正点的误差校正类型进行误差校正，校正垂直和水平误差的位置可根据地形在航迹规划前确定，若垂直误差、水平误差都能得到及时校正，则无人机可以按照预定航线飞行，并通过若干个校正点进行误差校正后最终到达目的地；

步骤(1.3)，无人机在出发点的垂直和水平误差均为0，在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变，在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变；

步骤(1.4)，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正，其中α₁为无人机到达垂直校正点时所允许的最大垂直误差，α₂为无人机到达垂直校正点时所允许的最大水平误差；

步骤(1.5)，当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正，其中β₁为无人机到达水平校正点时所允许的最大垂直误差，β₂为无人机到达水平校正点时所允许的最大水平误差；

步骤(1.6)，无人机在转弯时受到结构和控制系统的限制，无法完成即时转弯，定义无人机的最小转弯半径为r_min＝200m；

所述第二步基于无人机航迹规划区域，构建校正点选择策略具体包括：

步骤(2.1)，经垂直/水平校正点交替校正后，无人机能按照规划路径飞行的最远距离D₁应满足以下关系：

应在该距离内寻找新的垂直校正点进行垂直校正，否则根据第一步约束条件无法按规划航迹飞行；

步骤(2.2)，经水平/垂直校正点交替校正后，无人机能按照规划路径飞行的最远距离D₂应满足以下关系：

应在该距离内寻找新的水平校正点进行水平校正，否则根据第一步约束条件无法按规划航迹飞行；

步骤(2.3)，若经垂直校正点校正后，无人机不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₃应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行垂直校正；

步骤(2.4)，若经水平校正点校正后，无人机不寻找垂直校正点进行校正，仍寻找水平校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₄应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行水平校正；

步骤(2.5)，综上所述，无人机选择垂直/水平交替校正方案可在飞行相同距离的条件下，取得最少校正次数，并且该方案在选择下一校正点时，仅需对不同组别的点进行计算即可，无需计算同组别的校正点，因此可以降低算法计算量，减少搜索次数；

所述第三步基于校正点选择策略，构建航迹规划方案具体包括代价函数构建、校正点的范围构建、循环体构建、终止条件构建和优化方案比较五部分：

步骤(3.1)，采用改进的A*算法进行航迹规划，首先构建代价函数，代价函数可表示为：

f(n)＝g(n)+h(n) (5)

其中，g(n)为备选校正点到出发点A的欧式距离，h(n)为备选校正点到目的点B的欧式距离，假设起点A周围存在m个同类型校正点可作为备选，将这些校正点编号为N₁,...,N_m，根据式(5)可计算每个点的代价函数，可表示为：

其中，

为起点A到校正点N₁之间的欧式距离，

为校正点N₁到目的点B之间的欧式距离，

为起点A到校正点N_m之间的欧式距离，

为校正点N_m到目的点B之间的欧式距离；

根据式(6)的计算结果，可选出最优的校正点N_i，选择方案如下所示：

f(N_i)＝min{f(N₁),...,f(N_m)}i∈(1,m) (7)

步骤(3.2)，基于构建的代价函数和校正点选择策略，无人机单次飞行且不进行航迹校正的飞行距离是有限的，因此本方法通过确定搜索范围、计算梯度方向和预测未来轨迹来减少备选校正点个数；

步骤(3.2.1)，从出发点A出发先选择垂直校正点，无人机最远飞行距离D_Amax应满足以下条件：

D_Amax≤D₃ (8)

其中，D₃为无人机经垂直校正点校正后，不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，能按照规划路径飞行的最远距离；

若D_Amax＞D₃，即使找到垂直校正点也无法进行垂直校正，因此以出发点A为球心，以半径为D₃做球，在球内所有水平校正点可作为备选校正点，则在球外的校正点则舍去；

步骤(3.2.2)，在球内所有的点也非全是有效点，通过计算梯度方向来判断该点是否为有效点，点N_C、N_D、N_E均为出发点A的待选校正点，分别计算

与

之间梯度，此处梯度具体为向量夹角，若夹角＞90°则将该点舍去，夹角可采用如下的向量夹角公式进行计算：

其中，ω为向量夹角，

和

表示向量，具体指代为向量

与

即参数

可为向量

与

之一，参数

可为向量

与

之一，参数

和

不可相等，依次求解各向量之间夹角；

步骤(3.2.3)，采用垂直/水平交替校正的方案，但数据集中的校正点位置是随机布置的，若校正点N_C为垂直校正点，无人机经点N_C垂直校正后根据本方法应选择水平校正点，但若水平校正点附近无可达的垂直校正点时，应选择择次优的水平校正点N_E，并将点N_D舍去；

步骤(3.2.4)，通过确定搜索范围、计算梯度方向和预测未来轨迹可以减少备选校正点的个数，从而在下一步循环体构建中减少搜索次数；

步骤(3.3)，对目标函数和校正点的范围完成构建后，对循环体予以构建，假设无人机从出发点A出发后先选择的校正点为垂直校正点N_C，无人机到达N_C时的误差为：

其中，

为无人机到达校正点N_C后的垂直误差，

为无人机到达校正点N_C后的水平误差，

为起点A到校正点N_C之间的欧式距离；

经点N_C进行垂直校正后，无人机的垂直/水平误差为：

其中，

为无人机离开校正点N_C后的垂直误差，

为无人机离开校正点N_C后的水平误差；

之后将选择水平校正点进行水平校正，待选的水平校正点处于以点N_C为球心，半径为r′球内，r′满足以下约束条件：

再根据构建的校正点范围和目标函数，即可选出最优的下一校正点；

步骤(3.4)，因采用循环体结构，需对终止条件予以构建，由第一步可知当无人机飞到目的点B时的垂直/水平误差均小于预设值θ，因此在循环体部分选出校正点后，需计算该点是否能直接到达目的点B，若可以则终止循环，否则继续循环；

设点N_N为水平校正点，点N_M为垂直校正点，无人机由点N_M飞向点N_N，此时经点N_N校正后的无人机误差为：

其中，

为无人机离开校正点N_N后的垂直误差，

为无人机离开校正点N_N后的水平误差；

若无人机由点N_N直接飞至目的点B，则飞至目的点B时的误差为：

其中，

为校正点N_M与校正点N_N之间的欧式距离，

为校正点N_N与目的点B之间的欧式距离，

为无人机到达目的点B的水平误差，

为无人机到达目的点B的垂直误差；

若

则允许直接飞至目的点B，完成循环，否则在校正点N_N继续循环，直至达到终止条件；

步骤(3.5)，由于采用垂直/水平交替校正的方案，因此从出发点A出发的情况将有2种，分别是：

(a)先选择垂直校正点进行校正；

(b)先选择水平校正点进行校正；

因此将产生2种航迹规划方案，需对这2种方案进行比较，选择航迹更短的方案；

所述第四步基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法具体包括起点飞向校正点、校正点飞向校正点、校正点飞向终点三部分：

步骤(4.1)，由于无人机的初始飞行方向可自行设定，因此本方法使无人机的飞行方向dir_A与出发点A，校正点N_a构成的向量

方向一致即

步骤(4.2)，无人机经过校正点N_a后

与出发点A、校正点N_a构成的向量

方向应具有如下关系：

以N_a为切点做圆

圆

半径为，

无人机沿圆弧飞行，飞行的圆心角ξ应满足如下关系：

根据圆心角可计算无人机在N_a点沿圆弧飞行的航迹长度

为：

之后在N′_a点飞出，飞向方向

应与校正点N_a和校正点N_b构成的向量

方向一致，即

再沿

方向飞行l长度，l可由下式进行计算：

此时无人机的位置为点N′_b，以点N′_b为切点做圆

圆

半径仍为

无人机沿圆

飞行即可到达下一校正点N_b；

因此点N_a飞向点N_b采用航迹平滑方案所航行的航迹长度

与

之间的关系如下所示：

在式(19)中，

中ξ为弧度，

中ξ为角度；

在上式中

是一个定值，为点N_a与N_b之间的欧式距离，圆心角ξ也是一个定值，由

与

决定，因此若要使航迹最短，即使

最小；因此，无人机按最小转弯半径进行转弯，即满足以下关系：

其中，

为无人机在校正点N_i时的转弯半径；

无人机此时离开点N_a时的飞向方向

与校正点N_a和校正点N_b构成的向量

垂直；

步骤(4.3)，在圆

上重新寻找一个切点N″_m使之满足以下条件：

其中O_m为圆

的圆心，

为圆

的圆心O_m与切点N″_m所组成的向量；

由点N_m″直接飞向目的点B，到达终点；该段航迹长度可表示为：

其中，

为切点N″_m与目的点B之间的欧式距离，φ为无人机在圆

处转过的圆心角，可由下式进行计算：

其中，

为圆

的圆心O_m与无人机经过的最后一个校正点N_m所组成的向量；

因此规划后的航迹长度可由式(24)进行表示；在式(24)中，点N_a为第一个校正点，点N_m为最后一个校正点，无人机共经过k个校正点；

其中

为起点A到校正点N_a之间的欧式距离，

为校正点N_i与校正点N_j之间的欧式距离。

Images