基于风场估计和利用的无人机抗干扰轨迹规划与控制方法

本发明涉及飞行器的轨迹规划和控制领域，具体涉及基于风场估计和利用的无人机抗干扰轨迹规划与控制方法。

背景技术：

1、当今世界中无人机技术正迅速推进，在巡航检测勘探、目标识别跟踪、物资运输投放以及危险环境作业等领域有着长足的发展。在这种应用背景及发展趋势下，对无人机的机动性、环境风险估计能力、规划方法智能性有着较高的要求。

2、在风场环境下无人机能够如何估计风场的信息，利用风场助力无人机飞行，并对轨迹规划的方向进行引导，实现避风与避障一体化，提高无人机机动性，保障无人机的安全性、机动性以及节省能量是一项值得研究的内容。现有的方法仅考虑风场环境下的控制精度，例如专利公开号为cn117270402a中的方法只考虑到了无人机收到风的测力干扰所带来的侧偏距误差问题，并没有考虑如何规划的问题；或是仅考虑环境有风险条件下的轨迹规划，并没有针对风场干扰的估计以及解决如何稳定控制的方法，例如专利公开号为cn117193370a中的方法通过将局部规划的起终点方向向量与原始轨迹的方向向量夹角变化去评估环境风险，但是并没有考虑风场的影响；或是基于深度学习，例如专利公开号为cn116774723a中的方法根据链路损耗确定无人机在不同空间位置的信干噪比；构建基于深度强化学习的轨迹规划框架；通过动态ε策略输出无人机动作；训练深度强化学习网络。但是该方法的性能严重依赖于学习的数据量以及网路建立的合理性，鲁棒性较差。

技术实现思路

1、为了解决上述问题，本发明提供了一种基于风场估计和利用的无人机抗干扰轨迹规划与控制方法，无人机在风场中建立更加精确的动力学模型，设计观测器对风场进行估计，并在规划层利用该风场信息，进行有向避障及御风飞行，提高其机动性和安全性并节省能量。

2、为实现上述目的，本发明采用的技术方案如下：

3、一种基于风场估计和利用的无人机抗干扰轨迹规划与控制方法，包括风场环境下无人机动力学建模，风场环境下的风速估计及安全控制基于条件在险价值cvar的风险量化和风能利用的飞行轨迹规划算法。

4、所述的风场环境下无人机动力学建模是对无人机在风场中进行受力分析，计算升力以及垂直于升力方向的切向力；随后，基于所述无人机动力学模型，设计基于时变多项式风速观测器和控制器；最后，利用风速观测器和控制器估计的风速以及无人机在风场中所受的切向力，基于cvar的风险量化的飞行轨迹规划算法和基于风能利用的飞行轨迹规划算法。

5、所述的无人机动力学建模使用叶片单元动量理论(bemt)计算风对螺旋桨的影响，综合考虑风对无人机机身和旋桨的力，建立精准统一的动力学模型，具体包括如下步骤：

6、步骤a1：将风分解为两个方向：升力方向和垂直于升力的切向。升力方向的风可以提高无人机所受的升力，切向的力则会导致无人机有接近障碍物的风险。考虑四旋翼无人机本身的动力学特征以及风场的影响，建立以下动力学模型：

7、

8、

9、其中，p为无人机的位置，为无人机的位置的导数即无人机的速度，m为无人机的质量，v为无人机的速度，为无人机速度的导数即无人机的加速度，g为重力加速度，e3＝[0，0，1]t为世界坐标系z轴方向的单位向量，上标t表示向量或矩阵转置，r为无人机的旋转矩阵，f为不考虑风场环境时无人机螺旋桨带来的原始升力，fw1为在风场存在的条件下，无人机垂直于机体平面的升力的提升数值，fw2为在风场存在的条件下，无人机机身所受到的切向力；为无人机的旋转矩阵的导数，ω为无人机的角速度，为无人机的角加速度，j为无人机的转动惯量矩阵，τ为无人机所受的外力矩，ω×为无人机的角速度ω所对应的反对称矩阵；

10、步骤a2：依据叶片单元动量理论bemt对升力增量和垂直于升力的切向力进行建模计算。将无人机每个螺旋桨的速度分为垂直于机体平面的速度分量和平行于机体平面的速度分量：

11、

12、其中，表示第i个螺旋桨垂直于机体平面的速度分量，i＝1，2，3，4，表示第i个螺旋桨平行于机体平面的速度分量；b3表示升力方向上所对应的单位向量，为第i个螺旋桨的速度相对于风速在机体坐标系下的数值，中间参数其中i为单位矩阵，vw为风速，上标t表示矩阵的转置，li为第i个螺旋桨中心到无人机中心的向量，设第i个螺旋桨中心到无人机中心的距离远大于第i个螺旋桨本身的半径，即螺旋桨上的每个点在机体坐标系下的速度相等；

13、步骤a3：设螺旋桨具有足够的刚性，使其扑动角度小，水平诱导速度为0，根据叶片单元动量理论bemt可以得到风场存在条件下的第i个螺旋桨拉力ti表达式：

14、

15、其中，k1，k2，ks，k4为根据测量值回归计算得到的常数，r为无人机螺旋桨的半径，ωi为无人机第i个螺旋桨的角速度，||·||为对向量进行取模计算，kω，kz，kh为简化后的常数项；

16、在风存在的条件下，在相同的转速下螺旋桨所提供的升力会受到影响。水平风及纵向风存在时会产生增加的升力。综合四个螺旋桨所提供的拉力，与传统的拉力模型相对比，得到f，fw1和fw2的表述：

17、

18、其中，d为无人机的阻尼系数对角矩阵，li×为li所对应的反对称矩阵，螺旋桨的速度相对于风速在机体坐标系下的数值va＝v-vw，中间变量a＝4d，

19、步骤a4：将无人机机身所受的切向力fw2在世界坐标系下进行分解。当无人机的速度v小于风速vw时，此时风对无人机的力与无人机的速度方向是相同的，对无人机而言变为了推力，不再是阻力，切向力fw2用来提升前进的加速度；而当无人机速度超过风速时，切向力fw2作为阻力可以放宽减速的加速度(非均匀放宽，取决于无人机速度和相对速度)：

20、

21、其中，δax，δay代表在世界坐标系下x，y方向上放宽的加速度数值，e1＝[1 0 0]t为世界坐标系x轴的方向的单位向量；e2＝[0 1 0]t为世界坐标系y轴的方向的单位向量。

22、进一步，设计基于时变多项式风场估计器，具体包括如下步骤：

23、步骤a1：根据无人机机身所受的切向力fw2建立新的动力学模型：

24、

25、其中，vw为风速，假设风场中的风速是连续的，可以考虑用多项式对其进行拟合，此处以二次多项式来拟合风速，以此为假设条件设计观测器及控制器。

26、步骤a2：规划层提供规划的期望位置和速度，分别记为pd，vd，基础的pi控制器模型建立如下：

27、

28、其中，vr为经过位置误差pd-p修正的期望速度，记为参考速度，为参考速度的导数即参考加速度，kp为给定的各元素均大于0的第一对角矩阵；

29、针对当前建立的动力学模型设计如下风速观测器对使用多项式拟合的风速进行观测：

30、

31、其中，z为中间变量，为z的导数，为风速vw的观测值，为世界坐标系下x，y，z三个方向观测器观测到的风速值，中间参量u＝(f+fw1)b3，l2，l1，l0为给定的不同系数的对角矩阵；

32、利用风速观测值设计风场下的控制器：

33、

34、其中，kv为给定的各元素均大于0的第二对角矩阵。

35、当取给定的不同系数对角矩阵l2，l1，l0使得特征方程λ3+l2λ2+l1λ+l0＝0的根均为相异负实根时，且最大特征根的绝对值与阻力系数矩阵a的特征值μ以及无人机质量m满足时观测器以及控制器渐进稳定指数收敛于0，λ表示使特征方程满足条件的所有根μmin(a)为矩阵a的最小特征值，μmax(a)为矩阵a的最大特征值λmin为特征方程的最小特征根绝对值；

36、记风速观测器的误差为：建立李雅普诺夫函数如下：

37、

38、其中，v1，v2，v为中间变量，速度误差

39、先证明v1成立，已知风速干扰为二次多项式，其三阶导数为0，即vw(3)＝0，对观测器中得到的风速估计值求导三次，在左侧加上vw(3)，然后进行移项可以得到：

40、

41、由于特征方程的根均为相异负实根，因此该三阶微分方程的解为：

42、

43、其中，为任意3×3维矩阵，-λij<0 i，j＝1，2，3为xyz维度上三个特征方程所对应的9个相异负实根；

44、对其求导可以得到：

45、

46、其中，为λij i，j＝1，2，3组成的矩阵，代表矩阵之间的点乘。展开v1并对v1求导可得，比较二者进行放缩可以得到：

47、

48、其中，λmin＝min(λij)i，j＝1，2，3

49、以此为基础证明v成立：

50、

51、其中，

52、进一步设计基于cvar的a*路径规划算法：

53、步骤a1：设通过相机得到的障碍物位置坐标在世界坐标系x，y，z三个方向独立地满足截断正态分布：

54、

55、其中，γ表示在风场中障碍物的分布位置，μγ为截断正态分布，σγ表示关联风速和障碍物的函数，将风速对应到障碍物的分布方差，为随着世界坐标系下x，y方向观测到的风速单调递增的函数，为标准正态分布函数pdf，t表示时间，为标准正态分布函数的累计分布函数cdf，aγ，bγ表示在风场中障碍物的分布范围：

56、

57、其中，固定常数ξ1＝0.2，ξ2＝1.5；为xy方向上观测到的总风速；

58、步骤a2：在cvar风险的考虑下，当一些位置点的风险值过大的时需要避开。设障碍物的分布范围内任一位置pos＝[px，py，pz]t，设置障碍物的风险函数为：

59、

60、其中，poγ为障碍物位置坐标在x，y方向上的位置分量，pγ为任一位置pos在x，y方向上的位置分量；

61、给定置信度β计算风险函数的cvar数值，最终pos处的风险risk(pos)为xy两个方向上风险的最大值：

62、

63、其中，为变量的累计分布函数，为变量的分布函数，代表的概率，z为所有满足这一公式的解，均为中间变量。

64、障碍物的风险函数为负，cvar越大风险值越高(cvar接近0代表离障碍物越近，越小代表离障碍物越远)。

65、最后设计基于风能利用的飞行轨迹规划算法，具体步骤如下：

66、采用b样条对轨迹进行表述：

67、

68、

69、

70、其中，n为控制点的个数，代表第i个控制点，t表示时间，为n个控制点组成的列向量，p(t)为b样条轨迹数值，k为b样条曲线所采用的阶数，为权重系数，对确定阶数的样条曲线该值固定，p(n)(t)代表对p(t)求n阶导数，代表控制点qi的n阶导数，δt为常数，ψ(n)为由计算公式得到的对应阶数的矩阵；

71、最优化飞行轨迹的计算包含能量项、碰撞项以及可行性项。能量项为最小化升力的模，使用公式表述如下：

72、

73、

74、(f+fw1)2＝m2[qt(ψ(2))tψ(2)q-2gtψ(2)q+gtg]-||a||2[qt(ψ(1))tψ(1)q-2wtψ(1)q+wtw]

75、其中，

76、第i个控制点qi的碰撞函数fc设计为：

77、

78、其中，sf为设定的安全距离，dij为当前位置到最近障碍物的距离ci＝sf-di为中间变量；

79、对所有控制点的碰撞函数求和得到碰撞项：

80、

81、可行性项ff包括对速度的限制ff1和加速度的限制ff2，使用动力学建模中计算的切向力对加速度的约束上限进行放宽：

82、

83、

84、其中，δa为放宽的加速度向量，其中δax，δay代表在世界坐标系下x，y方向上放宽的加速度数值，amax为不考虑切向力时设置的最大加速度向量，为考虑到切向力后的最大加速度向量，vmax为设置的最大无人机的速度向量。

85、本发明与现有技术相比的优点在于：

86、本发明公开的一种基于风场估计和利用的无人机抗干扰轨迹规划与控制方法中综合考虑风对无人机机身和旋桨的力，建立精准统一的动力学模型，使用多项式对外界变化的风场进行建模，建立风速观测器对外界风场的大小和方向进行估计，并基于此设计控制器，为轨迹规划提供风场参考值。证明了在适当选取多项式观测器增益的条件下无人机的控制环可以保持全局稳定性。考虑横向的风使得无人机在接近障碍物时的危险增加，利用障碍物的信息和特点，降低能量消耗，选择避风端进行飞行，将无人机的尺寸和风场的风险结合，通过cvar对风险进行量化处理，提升无人机飞行的安全性；采用均匀样条b曲线参数化轨迹，优化轨迹的目标为能量项、碰撞项和动力可行性，前进方向的风可以为无人机提供推力，迎面的风可以为无人机提供更大的阻力，利用风力实现对无人机机动性的提升并节省能量。