一种无碰撞空间参数化编队构型序列规划方法

本发明涉及智能体/无人机/航天器集群运动控制，具体涉及一种无碰撞空间参数化编队构型序列规划方法。

背景技术：

1、在无人机、航天器等真实集群编队重构/迁移运动控制研究中，为了实现与障碍物的有效规避，以保证集群运动的安全性，一种常用的手段是为集群各个体分别规划一条避开障碍物的目标轨迹。但该方法未充分考虑到集群整体的编队构型结构，易导致集群在障碍物附近短暂失去规则编队构型结构而影响其功能执行，并且容易随着集群规模的增大而在计算上变得越发困难。

2、一种更为合理的方式是基于参数化编队构型模型，通过调整目标编队构型的少量参数，为集群整体规划出避开障碍物的中间目标构型序列，从而间接确定各个体目标轨迹。该方法因目标编队构型参数独立于集群个体数，适用于大规模集群的整体重构或迁移。

3、但现有的参数化编队构型规划主要针对球面、线段等较为规则的目标编队构型，对于复杂障碍环境，不能对大小、位置、方位、形状等相关参数进行及时调整，不能够给出合适的目标编队构型序列规划策略，无法灵活调节目标编队构型形状来规避障碍物。

技术实现思路

1、针对现有技术中存在的参数化编队构型规划不能对相关参数进行及时调整，无法灵活调节目标编队构型形状来规避障碍物，不能够给出合适的目标编队构型序列规划策略的问题，本发明提供了一种无碰撞空间参数化编队构型序列规划方法，能够根据复杂障碍环境，及时对相关参数进行调整，灵活调整目标编队构型形状来规避障碍物。

2、为了实现上述目的，本发明提供了如下的技术方案。

3、一种无碰撞空间参数化编队构型序列规划方法，包括：

4、根据初始目标编队构型参数和最终目标编队构型参数，通过线性插值构造若干个中间目标编队构型参数；

5、将初始目标编队构型参数、中间目标编队构型参数及最终目标编队构型参数输入进目标编队构型模型中，之后目标编队构型模型根据集群个体数确定各个时刻下的个体目标位置；

6、依次检测各个时刻下的个体目标位置是否满足约束条件，若满足约束条件则根据对应的中间目标编队构型参数和目标编队构型模型，直接生成各个时刻下中间目标编队构型；

7、根据直接生成各个时刻下中间目标编队构型，完成无碰撞空间参数化编队构型序列规划。

8、作为本发明的进一步改进，所述目标编队构型模型以如下空间李萨如曲线给出

9、

10、其中，为目标编队构型中心c的位置坐标，a＝(ax,ay,az)t为主半轴矢量，b＝(bx,by,bz)t为副半轴矢量，ω＝(ωx,ωy,ωz)t为角频率矢量，u∈[0,umax)为曲线自变量，°为哈达玛积符号，rb为将x轴单位矢量旋转至b矢量单位矢量的坐标变换矩阵。

11、作为本发明的进一步改进，所述rb为将x轴单位矢量旋转至b矢量单位矢量的坐标变换矩阵，之后：

12、根据旋转公式计算如下：

13、

14、p为如下形式的目标编队构型参数

15、p＝(xc,yc,zc,ax,ay,az,bx,by,bz,ωx,ωy,ωz)t

16、其对于第j个目标编队构型(j＝1,2,...,nf)的具体值记作p(j)，其所对应的目标编队构型中心位置坐标记作ξc(j)、主半轴矢量记作a(j)、副半轴矢量记作b(j)、角频率矢量记作ω(j)，即

17、

18、初始目标编队构型参数p0＝p(1)、最终目标编队构型参数pf＝p(nf)均按上述公式形式给出。

19、作为本发明的进一步改进，所述根据初始目标编队构型参数和最终目标编队构型参数，通过线性插值构造若干个中间目标编队构型参数，包括：

20、中间目标编队构型参数按如下公式确定：

21、

22、其中，p(k)表示第k个目标编队构型的目标编队构型参数(k＝2,...,nf-1)。

23、作为本发明的进一步改进，所述将初始目标编队构型参数、中间目标编队构型参数及最终目标编队构型参数输入进目标编队构型模型中，之后目标编队构型模型根据集群个体数确定各个时刻下的个体目标位置，包括：

24、第j个时刻(j＝1,2,...,nf)下的个体i(i＝1,2,...,n)目标位置表示为如下形式

25、

26、其中，p(j)表示第j个目标编队构型的目标编队构型参数，p(1)＝p0，p(nf)＝pf。

27、作为本发明的进一步改进，依次检测各个时刻下的个体目标位置是否满足约束条件，包括：

28、对于第k个目标编队构型(k＝2,3,...,nf-1)，其对应的个体目标位置(i＝1,2,...,n)应满足如下约束条件：

29、

30、使得

31、并且对于第(nf-1)个目标编队构型其对应的个体目标位置(i＝1,2,...,n)还应满足如下约束

32、使得

33、其中，为障碍物模型。

34、作为本发明的进一步改进，依次检测各个时刻下的个体目标位置是否满足约束条件，包括：

35、若目标编队构型对应的个体目标位置满足约束，则根据对应的中间目标编队构型参数和目标编队构型模型，直接生成对应的中间目标编队构型。

36、作为本发明的进一步改进，所述依次检测各个时刻下的个体目标位置是否满足约束条件，包括：

37、若不满足约束条件，按策略一调整目标编队构型(k＝2,...,nf-1)的目标编队构型参数p(k)；然后继续检查下一个目标编队构型直至所有中间目标编队构型满足约束条件，则检查完毕，输出中间目标编队构型序列。

38、作为本发明的进一步改进，所述按策略一调整目标编队构型(k＝2,...,nf-1)的目标编队构型参数p(k)，其中：

39、策略一为目标编队构型参数调整，包括：

40、确定优化变量：

41、优化变量取为

42、q＝(xc,yc,zc,ax,ay,az,bx,by,bz)t

43、即目标编队构型参数p的前9个分量构成的向量；

44、为第k个目标编队构型的参数优化问题构造np个初始解(ip＝1,2,...,np)，其中第ip个初始解的初始值取为

45、

46、其中，q(k)为由目标编队构型的目标编队构型参数p(k)的前9个分量构成的向量；rand(9,1)为9行1列随机数矩阵，其每个元素在-1～1之间均匀分布；°为哈达玛积符号；

47、然后依次验证每个是否满足如下约束条件

48、

49、其中，ω(k)为目标编队构型所对应的角频率矢量的具体值；

50、当依次验证每个都满足以上约束条件，则利用遗传算法等全局性优化算法，并以得到的作为初始解的集合，求解如下关于变量q的优化问题

51、

52、得到最优解

53、然后在将p(k)临时替换为的条件下，判断以下约束

54、约束1：使得

55、约束2：使得

56、是否满足；

57、若不满足，则重新求解上述关于变量q的优化问题，直至以上约束满足为止；

58、满足约束以后，令

59、

60、作为调整后的第k个目标编队构型的目标编队构型参数。

61、作为本发明的进一步改进，所述依次验证每个是否满足如下约束条件

62、

63、其中：

64、若不满足，则进行如下替换

65、

66、得到新的其中和分别为所对应的主半轴矢量和副半轴矢量；后回到上述约束条件的检查过程，如此往复，直至满足约束条件为止。

67、与现有技术相比，本发明具有以下有益效果：

68、本发明提供一种无碰撞空间参数化编队构型序列规划方法。首先基于满足约束的初始和最终目标编队构型参数，用线性插值初步建立由初始目标编队构型平缓过渡到最终目标编队构型的中间目标编队构型参数序列，然后利用先优化后检验的两步优化策略进行中间目标编队构型参数调整，避免了计算量较大的遮挡性约束对优化问题求解进度的影响，同时利用全局优化算法的多解性更快地找到满足遮挡性约束的解。本发明的目标编队构型参数含义明晰，便于调整不同参数来灵活改变目标编队构型位置、大小、方位、形状，因而适用于一大类不同编队构型间的重构或迁移，并增强了对复杂障碍环境和任务需求的适应能力。由于目标编队构型参数个数的恒定性，本方法适用于大规模集群的编队重构或迁移。通过平缓过渡的中间目标编队构型规划，本方法能够在引导集群实现障碍物规避的同时，保证编队构型形状的连续性，以使集群在编队运动过程中更好地执行目标任务。