一种固定翼集群队形生成方法与流程

1.本发明属于无人机技术领域的一种固定翼无人机飞行控制方法，尤其是一种利用地速分配和等长dubins算法的固定翼无人机协同路径规划的方法。


背景技术：

2.随着无人机技术的发展，固定翼编队在作战中发挥的作用越来越多。固定翼编队执行协同探测、侦察、作战等任务的整体性相比单个无人机有了很大程度的提高。
3.而队形生成是固定翼集群中的一个重要环节。在固定翼队形生成过程中，固定翼一般会面临风速的扰动，掉速和动力学约束等问题，所以对于多无人机队形生成问题，目前尚无较好的解决方法。


技术实现要素：

4.本发明所要解决的技术问题是，在有限的平台计算资源，风速扰动和动力学约束下，快速规划固定翼集群的路径，并沿路径进行速度动态分配，实现固定翼集群队形生成。
5.为了克服上述问题，本发明提出了一种利用速度分配和等长dubins算法实现固定翼集群队形生成的方法。
6.为了实现上述技术目的，本发明的技术方案为：
7.步骤s1：实时获取每架固定翼的当前位姿p
s
，设定每架固定翼在固定翼集群上的会合点位姿p
f
；位姿包括位置和姿态。
8.步骤s2：设置dubins路径计算模块，根据各个固定翼的当前位姿p
s
和会合点位姿p
f
以及固定翼的转弯半径区间[r
min
，r
max
]，r
min
表示固定翼最小转弯半径，r
max
表示固定翼的最大转弯半径，通过dubins路径计算模块计算出从固定翼的当前位姿p
s
到会合点位姿p
f
之间的dubins曲线作为该固定翼的最优dubins路径，并选取所有固定翼的最优dubins路径中最长的一条最优dubins路径的长度作为参考等长路径长度，参考等长路径长度对应的固定翼作为参考固定翼；
[0009]
步骤s3：dubins路径计算模块中，根据固定翼的转弯半径区间[r
min
，r
max
]和参考等长路径长度，搜索获得除了参考固定翼以外的剩余各个固定翼的最优dubins路径在不同dubins曲线类型下达到参考等长路径长度所需的最优半径r
*
，用最优半径r
*
代替固定翼最小转弯半径r
min
回到步骤s2重新获得剩余各个固定翼的最优dubins路径；
[0010]
由此，将参考固定翼的最优dubins路径保持始终不变，其余固定翼的最优dubins路径进行优化重新设置。
[0011]
步骤s4：设定每架固定翼的地速相同，通过地速控制器控制各个固定翼沿自身的最优dubins路径飞行移动，使得保持所有固定翼同时到达固定翼集群的会合点。
[0012]
本发明中，布置各个固定翼集群在平行于地面的上空平面上进行飞行，固定翼在同一飞行高度上在平面进行集群队形生成，飞行高度所在的平面上不具有障碍物。
[0013]
所述步骤s3中，设置约束条件：
[0014]
|dubins(r
*
,mode)
‑
l
ref
|＜＝1m
[0015]
其中，dubins()表示dubins函数，mode表示dubins曲线类型，l
ref
表示参考等长路径的长度。
[0016]
所述步骤s3中，固定翼的最大转弯半径r
max
采用以下公式设置：
[0017][0018]
在所述步骤s3的dubins路径计算模块中，根据输入的固定翼的转弯半径区间[r
min
，r
max
]，在不同的每个dubins曲线类型下，均通过二分搜索法在转弯半径区间[r
min
，r
max
]范围内改变转弯半径r来达到参考等长路径长度，从各种获得的dubins曲线中寻找，固定翼之间相互不想交且转弯半径r最小的dubins曲线作为最优dubins路径，以最优dubins路径的转弯半径r作为该固定翼的最优半径r
*
，最优dubins路径的类型作为该固定翼的dubins曲线类型。
[0019]
所述的通过地速控制器控制各个固定翼沿自身的最优dubins路径飞行移动，具体为：
[0020]
通过固定翼的实际地速v
g
和外部输入的固定翼的期望地速v
gref
相减获得地速偏差e，由地速偏差e经过pd控制运算处理后叠加上固定翼的实际空速v
a
，进而输出作为固定翼的期望空速v
aref
，发送到固定翼的空速控制器；
[0021]
所述的地速控制器建立表示为：
[0022]
e＝v
g
‑
v
gref
[0023][0024]
式中，v
g
表示固定翼的实际地速，v
gref
表示固定翼的期望地速，v
aref
表示固定翼的期望空速，v
a
表示固定翼的实际空速，e表示地速偏差，表示地速偏差e的导数，p表示pd控制器的比例系数，d表示pd控制器的微分系数。
[0025]
所述的固定翼的空速控制器根据固定翼的期望空速v
aref
结合固定翼的实际空速v
a
处理输出速度控制量，速度控制量叠加上风速干扰量后发送到固定翼上控制飞行速度。
[0026]
本发明中，所述的地速为相对于地面的速度，空速为相对于空气的速度。
[0027]
通常的固定翼无人机都是采用空速进行飞行控制，固定翼控制速度为空速，地速会受到风速的干扰，这样会带来控制的空速与期望的地面速度不一致，导致固定翼集群队形不能同时生成的问题。
[0028]
由此本发明在现有空速控制器控制固定翼飞行的基础上添加增设了地速控制器来控制飞行，采用本发明方法的速控制器能够维持固定翼实际地速相同，保持准确的固定翼集群队形生成，具有计算量小，容易部署在嵌入式设备上等优点。
[0029]
本发明提供的技术方案带来的益处：
[0030]
本发明通过二分搜索法实现的等长dubins路径，相较于其他算法，其优势在于时间复杂度只有log(n)，可以在嵌入式设备上无压力运行。利用二分搜索法实现等长dubins路径规划，可以快速进行规划，可以在计算资源有限的平台实现该算法。
[0031]
本发明因为固定翼上改变速度范围有限，所以等长路径规划进一步减轻了速度分配的难度。
[0032]
本发明通过增设构建了地速控制器，控制固定翼地速匀速，就能实现固定翼集群队形生成，进一步消除了风速干扰对无人机队形形成的影响。
附图说明
[0033]
图1是本发明方法的流程图；
[0034]
图2是dubins曲线类型示意图；
[0035]
图3是地速控制器框图；
[0036]
图4是二分搜索法算法框图；
[0037]
图5是二分搜索法模块的示意图；
[0038]
图6是dubins算法生成的路径示意图；
[0039]
图7是等长dubins算法生成的路径示意图；
[0040]
图8是dubins算法生成的路径长度统计结果图；
[0041]
图9是等长dubins算法生成的路径长度统计结果图；
[0042]
图10是无地速控制器实现集群队形生成的效果图；
[0043]
图11是有地速控制器实现集群队形生成的效果图。
具体实施方式
[0044]
下面结合说明书附图，对本发明的具体工作过程进一步详细说明。
[0045]
如图1所示，本发明的实施例及其实施过程如下：
[0046]
步骤s1：获取每架固定翼当前位姿p
s
，并设定每架无人机在固定翼集群上的会合点位姿p
f
：
[0047]
p
s
(x
s
,y
s
,z
s
,χ
s
)
[0048]
p
f
(x
f
,y
f
,z
f
,χ
f
)
[0049]
其中，x,y,z表示北东地导航坐标系下的三轴方向坐标，χ表示固定翼的航向角。
[0050]
根据固定翼自身属性预先已知获得固定翼的最小转弯半径r
min
，预先设置固定翼的最大转弯半径r
max
，构建固定翼的转弯半径区间[r
min
，r
max
]。
[0051]
步骤s2：设置dubins路径计算模块，根据步骤s1所获得的各个固定翼的当前位姿p
s
和会合点位姿p
f
以及固定翼的转弯半径区间[r
min
，r
max
]，r
min
表示固定翼最小转弯半径，r
max
表示固定翼的最大转弯半径，通过dubins路径计算模块计算出从固定翼的当前位姿p
s
到会合点位姿p
f
之间的dubins曲线作为该固定翼的最优dubins路径，即选取最短的dubins曲线作为最优dubins路径。
[0052]
如图2所示，dubins路径由起始圆弧、直线、终止圆弧组成。一般有四种dubins类型：lsl、lsr、rsl、rsr。具体实施中，dubins路径算法会算出lsl、lsr、rsl、rsr四种类型各自对应的长度，选择四种类型中dubins曲线长度最短的作为最优dubins路径。
[0053]
然后，选取所有固定翼的最优dubins路径中最长的一条最优dubins路径的长度作为参考等长路径长度，参考等长路径长度对应的最优dubins路径作为参考等长路径，参考等长路径长度对应的固定翼作为参考固定翼；
[0054]
多架固定翼形成多条不等长的最优dubins路径，可以用一个序列表示所有固定翼的生成的最优dubins路径的长度集合{l1,l2,
…
,l
n
},l
n
第n架固定翼生成最短dubins路径
的长度，选取最长的最优dubins路径的长度l
ref
＝max{l1,l2,
…
,l
n
}作为参考等长路径长度。
[0055]
步骤s3：dubins路径计算模块中，根据输入固定翼的转弯半径区间[r
min
，r
max
]和参考等长路径长度，通过二分搜索算法搜索获得除了参考固定翼以外的剩余各个固定翼的最优dubins路径在不同dubins曲线类型下达到参考等长路径长度所需的最优半径r
*
，用最优半径r
*
代替固定翼最小转弯半径r
min
回到步骤s2重新获得剩余各个固定翼的最优dubins路径；来实现所有dubins路径长度相等的目的。
[0056]
如图5所示，在所述步骤s3的dubins路径计算模块中，根据输入的固定翼的转弯半径区间[r
min
，r
max
]，在不同的每个dubins曲线类型下，均通过二分搜索法在转弯半径区间[r
min
，r
max
]范围内改变转弯半径r来达到参考等长路径长度，从各种获得的dubins曲线中寻找，固定翼之间相互不想交且转弯半径r最小的dubins曲线作为最优dubins路径，以最优dubins路径的转弯半径r作为该固定翼的最优半径r
*
，最优dubins路径的类型作为该固定翼的dubins曲线类型。
[0057]
如图4所示，具体实施中，首先设定转弯半径r的转弯半径区间[r
min
，r
max
]和dubins曲线类型mode。其次，取该转弯半径r的转弯半径区间[r
min
，r
max
]的中值r
mid
，计算出dubins曲线的长度，与参考等长路径的长度做比较。若相差小于1m以内，即取该中值作为该无人机dubins路径的最优半径r
*
；若相差大于等于1m，更新r区间范围，并不断重复直至达到约束条件。
[0058]
设置约束条件：
[0059]
|dubins(r
*
,mode)
‑
l
ref
|＜＝1m
[0060]
其中，dubins()表示dubins函数，mode表示dubins曲线类型，l
ref
表示参考等长路径的长度，mode为dubins曲线类型序列{lsl,lsr,rsl,rsr}中的一种。lsl表示由左手圆弧，直线和左手圆弧组成，lsr表示由左手圆弧，直线和右手圆弧组成，rsr表示由右手圆弧，直线和右手圆弧组成，rsl表示由右手圆弧，直线和左手圆弧组成，如图2所示。
[0061]
若起始位姿和终止位姿已知，即当前位姿p
s
到会合点位姿p
f
姿已知，dubins曲线的长度只取决于半径r和dubins曲线类型mode。由此能够约束条件寻找到固定翼之间相互不想交且转弯半径r最小的dubins曲线作为最优dubins路径。
[0062]
步骤s4：设定每架固定翼的地速相同，通过地速控制器控制各个固定翼沿自身的最优dubins路径飞行移动，使得保持所有固定翼同时到达固定翼集群的会合点。
[0063]
如图3所示，利用实际地速和期望地面速度做偏差，根据pd控制得出一个速度的增量去补偿空速和地速之间的风速的干扰，并且加入实际空速作为前馈来加快控制器反应。
[0064]
通过固定翼自身上的传感器采集检测获得的实际地速v
g
和外部输入的固定翼的期望地速v
gref
相减获得地速偏差e，由地速偏差e经过pd控制运算处理后叠加上固定翼自身上的传感器采集检测获得的实际空速v
a
，进而输出作为固定翼的期望空速v
aref
，发送到固定翼的空速控制器；表示为：
[0065]
e＝v
g
‑
v
gref
[0066][0067]
式中，v
g
表示固定翼的实际地速，v
gref
表示固定翼的期望地速，v
aref
表示固定翼的期望空速，v
a
表示固定翼的实际空速，e表示地速偏差，表示地速偏差e的导数，p表示pd控
制器的比例系数，d表示pd控制器的微分系数。
[0068]
固定翼的空速控制器根据固定翼的期望空速v
aref
结合固定翼的实际空速v
a
处理输出速度控制量，速度控制量叠加上固定翼自身的传感器采集检测获得的风速干扰量后发送到固定翼上控制飞行速度。
[0069]
本发明方法为了验证可行性，借助matlab进行仿真，同时加入了各种扰动来增加仿真的可信度。
[0070]
随机初始化三架固定翼无人机，随机设定集群队形会合点。
[0071]
未用本发明dubins路径计算模块前的结果和用本发明dubins路径计算模块后的三个固定翼飞行路径的结果分别如图6和图8所示。
[0072]
未用本发明dubins路径计算模块前的结果和用本发明dubins路径计算模块后的三个固定翼飞行路径的长度分别如图7和图9所示。
[0073]
结果显示，本发明可实现dubins路径等长的目的。
[0074]
如图10和图11所示，图中的线条为固定翼飞行路径，末端的三个飞机形态位置为会合点位置。如图10所示，没有地速控制器的情况下，不能达到每架无人机同时到达固定翼集群会合点。如图11所示，通过添加地速控制器后，每架无人机可以同时到达固定翼集群会合点。