飞行器航迹自适应规划的方法、装置、设备及存储介质与流程

1.本发明涉及无人机飞行管控技术领域，尤其涉及飞行器航迹自适应规划的方法、装置、设备及存储介质。
技术背景
2.以无人多旋翼飞行器通常要执行一些自主任务，比如航线巡检、平面测绘、三维建模等，而执行这些任务就需要自主导航，对运动轨迹进行制导。一般的对航迹规划都是以点到点的形式进行，到点后通过定点转向去往下一个点，以此往复来实现整个轨迹规划过程。这种方式在效率上不是很高，到点后会有减速停顿和等待，对于一些到点无动作的场景，如农田喷洒、等时等距拍照、路径跟踪等应用，由于执行周期时间长，导致飞行器执行工作时间长，效率不高。


技术实现要素：

3.本发明要解决的技术问题在于针对现有技术飞行器航迹规划方法执行周期时间长，导致飞行器执行工作时间长，效率不高的问题。
4.本发明解决其技术问题所采用的技术方案是：一种飞行器航迹自适应规划的方法，包括，
5.步骤a：构建飞行器的俯仰角、横滚角、方位角之间的关系式；
6.步骤b：获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件；
7.步骤c：构建飞行器的飞行轨迹需要满足的微分方程组；
8.步骤d：分段输入满足协调转弯条件以及俯仰角、横滚角、方位角之间的关系式的俯仰角、横滚角、方位角、纵轴向速度、纬度、经度、高度代入飞行器的飞行轨迹需要满足的微分方程组得到轨迹参数，从而建立分段轨迹，对各个分段轨迹进行拟合得到最终飞行器的飞行轨迹，飞行器根据飞行轨迹进行运动。
9.进一步地，所述构建飞行器的俯仰角、横滚角、方位角之间的关系式，包括：
10.飞行器飞行高度不变的情况下，纵轴向速度保持不变，俯仰角和横滚角也不变，仅存在方位角变化，则其中，表示俯仰角的变化率，为横滚角的变化率，为纵轴的速度的微分也即z方向的加速度；
11.其中，ω
ψ
表示方位角变化率大小；
12.方位角匀速变化时，通过公式构建飞行器的俯仰角、横滚角、方位角之间的关系式。
13.更进一步地，所述获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件，包括：
14.根据空气动力学知识，利用公式获取飞行器在进行方位转弯时，横
滚角需满足的协调转弯条件，其中，a
c
表示飞行器转弯时的向心加速度，g表示重力加速度，表示y方向的速度。
15.更进一步地，所述构建飞行器的飞行轨迹需要满足的微分方程组，包括：
16.构建飞行器的飞行轨迹需要满足的微分方程组
[0017][0018]
其中，表示欧拉角向量的微分且λ＝[θ γ ψ]
t
，θ，γ，ψ分别表示俯仰角、横滚角以及方位角，ω表示欧拉角速率向量且ω＝[ω
θ ω
γ ω
ψ
]
t
，ω
θ
表示俯仰角变化率的大小，ω
γ
表示横滚角变化率的大小，a
b
表示机体系加速度且a
b
＝[0 a
y 0]
t
，a
y
表示y方向的加速度，表示机体系转到地理系的旋转矩阵，v
b
表示机体系速度，v
n
表示地理系速度，m
pv
表示从位置映射到速度的矩阵，表示位置的微分；
[0019]
将上述微分方程组简化得到如下方程组
[0020][0021]
本发明还提供一种航迹自适应规划的装置，包括，
[0022]
关系模型，用于构建飞行器的俯仰角、横滚角、方位角之间的关系式；
[0023]
协调转弯条件获取模块，用于获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件；
[0024]
飞行轨迹方程构建模块，构建飞行器的飞行轨迹需要满足的微分方程组；
[0025]
飞行轨迹获取模块，用于分段输入满足协调转弯条件以及俯仰角、横滚角、方位角之间的关系式的俯仰角、横滚角、方位角、纵轴向速度、纬度、经度、高度代入飞行器的飞行轨迹需要满足的微分方程组得到轨迹参数，从而建立分段轨迹，对各个分段轨迹进行拟合得到最终飞行器的飞行轨迹，飞行器根据飞行轨迹进行运动。
[0026]
进一步地，所述构建飞行器的俯仰角、横滚角、方位角之间的关系式，包括：
[0027]
飞行器飞行高度不变的情况下，纵轴向速度保持不变，俯仰角和横滚角也不变，仅存在方位角变化，则其中，表示俯仰角的变化率，为横滚角的变化率，为纵轴的速度的微分也即z方向的加速度；
[0028]
其中，ω
ψ
表示方位角变化率大小；
[0029]
方位角匀速变化时，通过公式构建飞行器的俯仰角、横滚角、方位角之间的关系式。
[0030]
更进一步地，所述获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件，包括：
[0031]
根据空气动力学知识，利用公式获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件，其中，a
c
表示飞行器转弯时的向心加速度，g表示重力加速度，表示y方向的速度。
[0032]
更进一步地，所述构建飞行器的飞行轨迹需要满足的微分方程组，包括：
[0033]
构建飞行器的飞行轨迹需要满足的微分方程组
[0034][0035]
其中，表示欧拉角向量的微分且λ＝[θ γ ψ]
t
，θ，γ，ψ分别表示俯仰角、横滚角以及方位角，ω表示欧拉角速率向量且ω＝[ω
θ ω
γ ω
ψ
]
t
，ω
θ
表示俯仰角变化率的大小，ω
γ
表示横滚角变化率的大小，a
b
表示机体系加速度且a
b
＝[0 a
y 0]
t
，a
y
表示y方向的加速度，表示机体系转到地理系的旋转矩阵，v
b
表示机体系速度，v
n
表示地理系速度，m
pv
表示从位置映射到速度的矩阵，表示位置的微分；
[0036]
将上述微分方程组简化得到如下方程组
[0037][0038]
本发明还提供一种电子处理设备，包括至少一个处理器和存储有至少一个执行程序的存储装置，当所述至少一个执行程序被所述至少一个处理器执行，所述至少一个处理器实现如上述所述的方法。
[0039]
本发明还提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时能够实现上述所述的方法。
[0040]
本发明的有益效果是：本发明获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件，分段输入满足协调转弯条件以及俯仰角、横滚角、方位角之间的关系式的俯仰角、横滚角、方位角、纵轴向速度、纬度、经度、高度代入飞行器的飞行轨迹需要满足的微分方程组得到轨迹参数，从而获取飞行轨迹，不需要到点停顿，直接转弯运行，减少了转弯停留等待的时间，这大大的提高了作业效率。
附图说明
[0041]
下面将结合附图及实例对本发明作进一步说明。
[0042]
图1是现有技术航迹规划的示意图；
[0043]
图2是本发明的实施例1提供的飞行器航迹自适应规划的方法的效果示意图；
[0044]
图3是本发明的实施例1提供的飞行器航迹自适应规划的方法的流程图；
[0045]
图4是本发明的实施例1提供的飞行器航迹自适应规划的方法中飞行器进行方位转弯时力学分解示意图。
具体实施方式
[0046]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
[0047]
实施例1
[0048]
如图1所示，飞行器在执行一些航线巡检、平面测绘、三维建模等任务时，需要自主导航，对运动轨迹进行制导。一般的对航迹规划都是以点到点的形式进行，到点后通过定点转向去往下一个点，以此往复来实现整个轨迹规划过程，到点后需要定点转弯或等待式协调转弯，例如图1中从0点到1点到2点是以点到点的形式进行，到1点后通过定点转向去往2点，这种方式在效率上不是很高，到点后会有减速停顿和等待，效率不高。基于上述缺陷，本发明提供一种自适应航迹规划方法，如图2所示，从0点到1点到2点是自适应协调转弯的方式，在保持高度不变的情况下，只经过航迹中的起点和终点也即0点和2点，在中间航点也即1点采用平滑过渡的弧线轨迹飞行，这种方式到点后不会减速停顿和等待，提高飞行执行工作的效率。如图3所示，以下详细介绍本发明的算法过程：
[0049]
飞行器飞行高度不变的情况下，纵轴向速度保持不变，俯仰角和横滚角也不变，仅存在方位角变化，构建飞行器的俯仰角、横滚角、方位角之间的关系式如下：
[0050]
其中，表示俯仰角的变化率，为横滚角的变化率，为纵轴(z方向)的速度的微分也即z方向的加速度；
[0051]
其中，ω
ψ
表示方位角变化率大小；
[0052]
方位角匀速变化时，
[0053]
由于飞行器在纵轴向速度保持不变，俯仰角和横滚角也不变，仅存在方位角变化，故只在y方向速度有变化，因此为纵轴(y方向)的速度的微分也即y方向的加速度,a
y
表示y方向的加速度，当a
y
＜0时为减速运动，而当a
y
＝0时飞行器处于静止或匀速飞行状态。
[0054]
如图4所示，根据空气动力学知识，飞行器在进行方位转弯时，转弯的向心加速度a
c
由空气升力f
l
和重力g的合力提供，这时横滚角需满足如下协调转弯条件
[0055][0056]
构建飞行器的飞行轨迹需要满足的微分方程组
[0057][0058]
其中，表示欧拉角向量的微分且λ＝[θ γ ψ]
t
，ω表示欧拉角速率向量且ω＝[ω
θ ω
γ ω
ψ
]
t
，a
b
表示机体系加速度且a
b
＝[0 a
y 0]
t
，表示机体系转到地理系的旋转矩
阵，v
b
表示机体系速度，v
n
表示地理系速度，m
pv
表示从位置映射到速度的矩阵，表示位置的微分。
[0059]
将上述微分方程组简化得到如下方程组
[0060][0061]
通过分段设置飞行动作，即分段输入满足上述协调转弯条件以及俯仰角、横滚角、方位角之间的关系式的俯仰角、横滚角、方位角、纵轴向速度、纬度、经度、高度代入上述飞行轨迹需要满足的方程组得到轨迹参数λ、v
n
和p，对各分段轨迹拟合得到最终飞行轨迹，按照这个飞行轨迹实现飞行器航迹的自适应调整，其中输入的各初值为λ(t0)＝[θ
0 γ
0 ψ0]
t
，p(t0)＝[l
0 λ
0 h0]
t
，θ0，γ0，ψ0分别为俯仰角的初值、横滚角的初值以及方位角的初值，为y方向的速度的初值，l0，λ0，h0分别为纬度的初值、经度的初值、高度的初值。
[0062]
通过以上技术方案，本发明获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件，分段输入满足协调转弯条件以及俯仰角、横滚角、方位角之间的关系式的俯仰角、横滚角、方位角、纵轴向速度、纬度、经度、高度代入飞行器的飞行轨迹需要满足的微分方程组得到轨迹参数，从而获取飞行轨迹，不需要到点停顿，直接转弯运行，减少了转弯停留等待的时间，这大大的提高了作业效率。
[0063]
实施例2
[0064]
基于本发明实施例1，本发明实施例2还提供一种航迹自适应规划的装置，包括，
[0065]
关系模型，用于构建飞行器的俯仰角、横滚角、方位角之间的关系式；
[0066]
协调转弯条件获取模块，用于获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件；
[0067]
飞行轨迹方程构建模块，构建飞行器的飞行轨迹需要满足的微分方程组；
[0068]
飞行轨迹获取模块，用于分段输入满足协调转弯条件以及俯仰角、横滚角、方位角之间的关系式的俯仰角、横滚角、方位角、纵轴向速度、纬度、经度、高度代入飞行器的飞行轨迹需要满足的微分方程组得到轨迹参数，从而建立分段轨迹，对各个分段轨迹进行拟合得到最终飞行器的飞行轨迹，飞行器根据飞行轨迹进行运动。
[0069]
具体的，所述构建飞行器的俯仰角、横滚角、方位角之间的关系式，包括：
[0070]
飞行器飞行高度不变的情况下，纵轴向速度保持不变，俯仰角和横滚角也不变，仅存在方位角变化，则其中，表示俯仰角的变化率，为横滚角的变化率，为纵轴的速度的微分也即z方向的加速度；
[0071]
其中，ω
ψ
表示方位角变化率大小；
[0072]
方位角匀速变化时，通过公式构建飞行器的俯仰角、横滚角、方位角之间的关系式。
[0073]
更具体的，所述获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件，包
括：
[0074]
根据空气动力学知识，利用公式获取飞行器在进行方位转弯时，横滚角需满足的协调转弯条件，其中，a
c
表示飞行器转弯时的向心加速度，g表示重力加速度，表示y方向的速度。
[0075]
更具体的，所述构建飞行器的飞行轨迹需要满足的微分方程组，包括：
[0076]
构建飞行器的飞行轨迹需要满足的微分方程组
[0077][0078]
其中，表示欧拉角向量的微分且λ＝[θ γ ψ]
t
，θ，γ，ψ分别表示俯仰角、横滚角以及方位角，ω表示欧拉角速率向量且ω＝[ω
θ ω
γ ω
ψ
]
t
，ω
θ
表示俯仰角变化率的大小，ω
γ
表示横滚角变化率的大小，a
b
表示机体系加速度且a
b
＝[0 a
y 0]
t
，a
y
表示y方向的加速度，表示机体系转到地理系的旋转矩阵，v
b
表示机体系速度，v
n
表示地理系速度，m
pv
表示从位置映射到速度的矩阵，表示位置的微分；
[0079]
将上述微分方程组简化得到如下方程组
[0080][0081]
实施例3
[0082]
基于本发明实施例1，本发明实施例3还提供一种电子处理设备，包括至少一个处理器和存储有至少一个执行程序的存储装置，当所述至少一个执行程序被所述至少一个处理器执行，所述至少一个处理器实现如上述所述的方法。
[0083]
实施例4
[0084]
基于本发明实施例1，本发明实施例4还提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时能够实现上述所述的方法。
[0085]
本领域内的技术人员应明白，本发明的实施例可提供为方法、设备(系统)或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd
‑
rom、光学存储器等)上实施的计算机程序产品的形式。
[0086]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些
实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
[0087]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。