一种用于环境动态变化情况下的无人机飞行系统

本发明涉及无人机系统领域，具体为一种用于环境动态变化情况下的无人机飞行系统。

背景技术：

1、无人机飞行系统主要包括无人机飞行轨迹规划方法、无人机控制模块、无人机飞行模块、无人机/地面站反馈四个部分，而无人机飞行轨迹规划是无人机飞行系统中的核心技术，在复杂、动态的地图中进行轨迹规划成为当前无人机领域一项研究的热点。为解决无人机的航迹规划问题，国内外学者已经提出大量航迹规划算法，其中局部规划算法拥有更广泛的应用领域。传统prm、rrt算法往往需要在一个确定的空间内对障碍物进行建模，计算复杂，因而不适用于无人机在复杂环境中的规划；dijkstra算法实现简单，但在大范围高维空间中搜索时间长，对内存要求高，所以多用于二维静态航迹规划；人工势场法有着实时性强、计算量低等优点，适用于无人机存在一定条件限制情况下的轨迹规划任务，然而人工势场法规划轨迹时容易产生在搜索局部最小值时导致在最小值周围徘徊、斥力方向变化过大导致飞行抖动情况，导致无人机飞行目标不可达、控制效率低下等问题，这些问题亟待解决。

2、综上所述，无人机飞行效率以及局部规划问题对无人机轨迹局部规划方法提出了许多挑战；一方面，完成轨迹规划之后，无人机根据预先轨迹飞行途中，目标点发生变化、环境中障碍物状态信息发生变化、执行轨迹与规划轨迹有所偏差时，此时需要快速重新规划一条从当前状态到目标状态的可行轨迹，这样对无人机自身轨迹重规划方法提出实时性问题；另一方面，轨迹规划方法要减弱或者解决轨迹规划中出现的局部最小值震荡、徘徊问题。因此研究一种满足实时性、高效的无人机轨迹规划方法对无人机飞行系统以及无人机自主飞行任务是十分必要的。

技术实现思路

1、为解决上述提出的问题，本发明公开了一种用于环境动态变化情况下的无人机飞行系统。

2、一种用于环境动态变化情况下的无人机飞行系统，包括发布信息、改进人工势场轨迹重规划方法、无人机控制模块、无人机飞行模块、无人机/地面站反馈五个部分，通过改进人工势场轨迹重规划方法获取目标信息、无人机状态信息、环境状态信息，构建改进人工势场，预测状态轨迹，对改进人工势场进行重规划，计算期望传递给无人机飞行控制模块，结合反馈计算无人机飞行控制量，实现无人机在环境动态变化情况下飞行的任务；

3、改进人工势场轨迹重规划方法包括以下步骤：

4、步骤1：信息获取，获取目标点、状态信息，目标点信息为世界坐标系下目标点三维位置，状态信息包含无人机在世界坐标系下三维位置坐标数据，无人机在世界坐标系下速度数据，机体坐标系下的姿态角度数据，机体坐标系下无人机的角速度数据以及无人机飞行过程中环境信息状态数据，为后续步骤做数据准备；

5、四旋翼无人机模型为x型无人机，定义 x， y， z表示无人机在世界坐标系下三维位置坐标， x与 z分别为无人机坐标与目标点坐标， v x， v y， v z表示无人机在世界坐标系下速度值， v=[ v x v y v z] t； φ， θ， ψ分别表示机体坐标系下的姿态角， φ是滚转角， θ是俯仰角， ψ是偏航角， p， q， r分别是机体坐标系下无人机的角速度；

6、从机体坐标系到世界坐标系的旋转矩阵 r为：

7、

8、无人机飞行过程中环境信息状态数据，定义为世界坐标系下的障碍物状态，以及上述数据的历史信息。

9、步骤2：建立势场函数，据步骤1中数据信息更新期望目标点，构建改进人工势场法规划虚拟势场函数与虚拟力函数；

10、定义在 t时刻，四旋翼无人机位置坐标为 x t，目标点坐标为 z t， z t根据与 x t的欧氏距离小于一定值后更新，使得在无人机没有到达目标点之前始终受当前目标点引力影响， x t与 z t之间的引力随着距离的缩减而减小， x t与 z t重合则引力为0，引力场函数为：

11、 u att( t)= k att d2( x t, z t)

12、其中， d( x t, z t)=|| z t- x t||为无人机与目标之间的欧氏距离， k att为已知的引力场常量，引力场中无人机所受吸引力 f att( t)与 u att( t)关系为：

13、 f att( t)= du att( t)/ dt

14、其中， f att( t)为无人机所受规划目标点 z t的牵引力， f att( t)方向为 x t与 z t之间连接线，方向指向 z t。

15、定义障碍物状态 o t与 v ot， o t是在 t时刻障碍物在地球坐标系的位置信息， v ot是 t时刻障碍物在地球坐标系的速度状态信息；

16、改进斥力场函数表达式为：

17、，

18、其中， d( x t, o t)=|| o t- x t||为无人机与障碍物之间的欧氏距离， k r1为斥力场位置分量常数， k r2为斥力场速度分量常数， d o为障碍物斥力场范围半径， v r为 t时刻无人机与障碍物动态信息中速度的相对差值，表达式为：

19、 v r=|| v t/( v t- v ot)|| v ot

20、斥力场中无人机所受斥力 f r( t)与 u r( t)关系表达式为：

21、 f r( t)= -du r( t)/ dt

22、其中， f r( t)为无人机所受规划目标点 o t的斥力， f r( t)方向为 x t与 o t之间连接线，方向指向 x t；

23、在无人机飞行轨迹规划过程中，考虑无人机进入到斥力场时， u r( t)产生突变，斥力 f r( t)会导致无人机飞行轨迹震荡；为减弱飞行震荡，平滑飞行轨迹，定义切向函数表达式为：

24、，

25、其中， k q为切向函数常数， d q为虚拟切向力函数范围半径，中心为 o t。 f q( t)与 u q( t)关系为：

26、 f q( t)= du q( t)/ dt

27、其中， f q( t)为所受规划切向力，无人机自身位置与障碍物连线的垂线，方向偏向 z t。

28、于是，无人机飞行轨迹规划过程中由引力场、斥力场、切向函数共同作用，合成势场函数以及合力表达式为：

29、 u( t)= u att( t)+ u r( t)+ u q( t)

30、 f( t)= f att( t)+ f r( t)+ f q( t)

31、步骤3：高斯过程回归预测轨迹，据势场函数局部最小值以及步骤1获取的有限组历史数据构建稀疏性高斯分布函数，通过高斯过程回归来预测无人机轨迹与障碍物状态信息。

32、针对步骤2的无人机轨迹规划中收敛到局部最小点时的情况，无人机运动轨迹状态在任意有限的时刻集，其对应状态服从联合高斯分布；设定高斯分布的均值向量和高斯分布的协方差矩阵，将轨迹状态高斯分布公式化，利用高斯分布的稀疏性将其扩展为第一个轨迹状态量的先验状态分布，末尾轨迹状态量的先验状态分布，相邻轨迹状态量先验状态分布，根据先验函数以及贝叶斯推理获取后验函数表示为：

33、，

34、其中，{ s}为飞行事件集合， p( x)为先验概率；似然函数设计如下：

35、，

36、其中，条件1是针对运动事件的满足条件，条件2是针对运动事件不满足条件；根据无人机环境感知系统获取飞行状态{ x t}，利用高斯过程回归算法预测无人机的下一时刻无人机的飞行轨迹状态{ x gp}，并且根据障碍物状态信息，同时预测障碍物状态信息为{ o gp}。

37、步骤4：重规划势场函数，按步骤3中预测的轨迹预测次目标点，在期望目标点与无人机之间插值次目标点，重规划虚拟势场函数与虚拟力函数；

38、在已经完成轨迹规划的基础上，无人机根据预先轨迹飞行途中，目标点发生变化、环境中障碍物位置发生变化、执行轨迹与规划轨迹有所偏差，此时需要在线快速重新规划一条从当前状态到目标状态的可行轨迹。

39、步骤4.1：获取次目标点信息，根据步骤3高斯过程回归算法预测无人机下一时刻飞行轨迹，计算插值，获取次目标点信息，同时获取障碍物预测信息，为步骤4.2做数据准备。

40、高斯过程回归算法预测无人机下一时刻飞行轨迹{ x gp}，{ x gp}中位置信息为 x gp，{ x gp}中速度预测信息为 v gp，在预测轨迹与 z t坐标点之间选获取轨迹规划次目标点 z * t；预测障碍物状态信息为{ o gp}，{ o gp}中位置信息为 o gp，{ o gp}中速度信息为 v ogp。

41、步骤4.2：重规划虚拟力函数，根据步骤4.1获取的次目标点信息规划次目标引力场函数，障碍物预测信息规划改进斥力场函数与切向函数，将步骤2中引力场函数与次目标引力场函数相结合，与改进斥力场函数与切向函数共同计算合力函数。

42、根据此目标点信息，构建此目标引力场函数为：

43、，

44、其中， p( s| x t)为次目标点概率模型，为中引力常数；

45、次目标引力函数为：

46、

47、目标引力与次目标引力合力表示为：

48、

49、根据障碍物状态信息，对改进斥力场进行重规划：

50、带有预测的斥力场速度补偿函数表示为：

51、

52、重规划的改进斥力场表示为：

53、

54、对斥力场进行重规划：

55、

56、对切向函数进行重规划：

57、

58、对切向力函数进行重规划：

59、

60、最终重规划后的合力函数表示为：

61、

62、步骤5：计算期望，根据步骤4重规划的虚拟力函数 f*( t)，根据 f*( t)的大小以及方向，计算无人机控制系统中状态的期望速度与期望加速度。

63、与现有技术相比，本发明的有益效果是：本发明对无人机的改进人工势场轨迹规划方法，利用高斯过程回归预测下一时刻无人机飞行轨迹的概率，通过概率分布实时对规划轨迹进行插值处理，对无人机轨迹进行重规划，减弱无人机在飞行过程中因搜索局部最小值产生的抖动问题，优化人工势场法规划无人机飞行任务过程中飞行轨迹平滑度。