本發(fā)明屬于多智能體伴隨式飛行領(lǐng)域，更為具體地講，涉及一種基于位置信標(biāo)的多智能體伴隨與隊(duì)形保持方法及系統(tǒng)。

背景技術(shù)：

1、隨著現(xiàn)代科學(xué)技術(shù)的高速快速發(fā)展，智能化、無人化、低成本的智能體在各民用領(lǐng)域的伴隨與隊(duì)形保持已經(jīng)得到廣泛應(yīng)用，如物流車間機(jī)器人分配貨物、無人機(jī)表演等。由于其智能化、無人化與低成本的特定，這種智能體在任務(wù)執(zhí)行過程中的作用日益凸顯，可以伴隨執(zhí)行人員探測(cè)威脅其的目標(biāo)及完成系數(shù)較高的威脅任務(wù)，從而降低對(duì)執(zhí)行人員生命的影響。因此，研究基于位置信標(biāo)的多智能體伴隨與隊(duì)形保持方法對(duì)探測(cè)整體態(tài)勢(shì)，伴隨掩護(hù)執(zhí)行人員保障其生命安全尤其重要。

2、由于任務(wù)環(huán)境通常較為復(fù)雜、執(zhí)行人員行進(jìn)速度與方向不可控，這給智能體的伴隨掩護(hù)與隊(duì)形保持帶來了困難。為了更好地適應(yīng)任務(wù)環(huán)境、伴隨掩護(hù)執(zhí)行人員、探測(cè)威脅目標(biāo)以及保持隊(duì)形，需要從伴隨方式、隊(duì)形保持機(jī)理以及智能體穩(wěn)定性等方面進(jìn)行考慮，研究多維伴隨式掩護(hù)方案。然而，上述方面目前還沒有較優(yōu)的協(xié)同解決方案，大多是從控制協(xié)議、二維平面或已知主航線/行進(jìn)方向等方面研究。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的目的是提供一種基于位置信標(biāo)的多智能體伴隨與隊(duì)形保持方法及系統(tǒng)，具有c字型、ⅱ字型等不同伴隨隊(duì)形生成能力，具有按既定隊(duì)形伴隨被保護(hù)對(duì)象（執(zhí)行人員）的移動(dòng)能力。

2、實(shí)現(xiàn)本發(fā)明目的的技術(shù)解決方案為：

3、一種基于位置信標(biāo)的多智能體伴隨與隊(duì)形保持方法，包括：

4、步驟s101，獲取任務(wù)環(huán)境信息，構(gòu)建任務(wù)環(huán)境信息矩陣；

5、步驟s102，基于任務(wù)環(huán)境信息矩陣，確定多智能體編隊(duì)方位角；

6、步驟s103，基于任務(wù)環(huán)境信息矩陣，確定多智能體編隊(duì)的可行坐標(biāo)點(diǎn)；

7、步驟s104，基于任務(wù)環(huán)境信息矩陣，確定多智能體編隊(duì)中智能體的探測(cè)朝向與航行速度；

8、步驟s105，基于方位角、可行坐標(biāo)點(diǎn)和探測(cè)朝向與航行速度得到多智能體伴隨式飛行掩護(hù)編隊(duì)。

9、進(jìn)一步地，所述任務(wù)環(huán)境信息包括經(jīng)度、緯度和高。

10、進(jìn)一步地，所述步驟s102具體包括：

11、獲取執(zhí)行人員上一時(shí)刻坐標(biāo)與執(zhí)行人員的當(dāng)前坐標(biāo)；、為對(duì)應(yīng)坐標(biāo)的經(jīng)度，、為對(duì)應(yīng)坐標(biāo)的緯度，、為對(duì)應(yīng)坐標(biāo)的高；

12、將上一時(shí)刻坐標(biāo)和當(dāng)前時(shí)刻的坐標(biāo)經(jīng)緯度轉(zhuǎn)換為弧度；

13、采用反正切函數(shù)計(jì)算方位角，并轉(zhuǎn)化為度數(shù)；

14、將方位角的度數(shù)轉(zhuǎn)換到0-360度范圍內(nèi)。

15、進(jìn)一步地，步驟s103中確定多智能體編隊(duì)的可行坐標(biāo)點(diǎn)具體包括：

16、通過二分查找法確定編隊(duì)最大可行水平半徑/隊(duì)長(zhǎng);

17、確定c型編隊(duì)或者ii型編隊(duì)時(shí)多智能體編隊(duì)可行坐標(biāo)點(diǎn)；

18、對(duì)編隊(duì)可行坐標(biāo)點(diǎn)進(jìn)行修正，得到最終的可行坐標(biāo)點(diǎn)。

19、進(jìn)一步地，所述通過二分查找法確定編隊(duì)最大可行水平半徑/隊(duì)長(zhǎng)具體包括：

20、確定可行水平半徑/隊(duì)長(zhǎng)范圍，最小水平半徑/隊(duì)長(zhǎng)，最大水平半徑/隊(duì)長(zhǎng)，為設(shè)定的執(zhí)行人員與多智能體的水平半徑或水平隊(duì)長(zhǎng)；

21、初始化最大可行水平半徑/隊(duì)長(zhǎng)；

22、計(jì)算最大可行水平半徑/隊(duì)長(zhǎng)為：

23、

24、

25、

26、其中，表示條件下的編隊(duì)不可行，表示條件下的編隊(duì)可行，為上一次計(jì)算的最大可行水平半徑/隊(duì)長(zhǎng)；

27、重復(fù)計(jì)算最大可行水平半徑/隊(duì)長(zhǎng)，當(dāng)時(shí)，結(jié)束計(jì)算，查找到最大可行水平半徑/隊(duì)長(zhǎng)。

28、進(jìn)一步地，確定c型編隊(duì)時(shí)多智能體編隊(duì)可行坐標(biāo)點(diǎn)具體包括：

29、基于最大可行水平半徑和設(shè)定的水平安全距離確定多智能體間的夾角，

30、

31、

32、其中，同一水平面下可容納的多智能體數(shù)量，為設(shè)定的多智能體數(shù)量，表示c型編隊(duì)隊(duì)形，表示半c型編隊(duì)隊(duì)形；

33、根據(jù)步驟s102的多智能體編隊(duì)方位角、多智能體間的夾角確定可行坐標(biāo)點(diǎn)方位角，

34、

35、其中表示第個(gè)多智能體，；

36、根據(jù)設(shè)定的執(zhí)行人員與多智能體的垂直高度、可行坐標(biāo)點(diǎn)方位角以及最大可行水平半徑，確定多智能體編隊(duì)可行坐標(biāo)點(diǎn)的初始坐標(biāo)，

37、

38、

39、

40、

41、

42、其中，表示地球每度的弧長(zhǎng)，單位是千米；表示最大可行水平半徑，單位是千米；單位是弧度，用于計(jì)算經(jīng)緯度；表示智能體的經(jīng)度坐標(biāo)；表示智能體的緯度坐標(biāo)。

43、進(jìn)一步地，確定ii型編隊(duì)時(shí)多智能體編隊(duì)可行坐標(biāo)點(diǎn)具體包括：

44、基于最大可行水平隊(duì)長(zhǎng)、多智能體編隊(duì)方位角，確定智能體的相對(duì)點(diǎn)和和相對(duì)點(diǎn)的相對(duì)方位角；

45、

46、

47、

48、

49、

50、

51、

52、其中表示地球每度的弧長(zhǎng)，單位是千米；

53、基于最大可行水平半徑和設(shè)定的水平安全距離確定多智能體間的間距；

54、

55、

56、根據(jù)設(shè)定的執(zhí)行人員與多智能體的垂直高度、可行坐標(biāo)點(diǎn)方位角以及最大可行水平半徑，確定多智能體編隊(duì)可行坐標(biāo)點(diǎn)，

57、

58、

59、

60、

61、

62、

63、其中表示第個(gè)智能體；表示ii型編隊(duì)一邊設(shè)置的智能體數(shù)量。

64、進(jìn)一步地，據(jù)執(zhí)行人員設(shè)置的垂直安全距離，在垂直方向?qū)庩?duì)可行坐標(biāo)點(diǎn)進(jìn)行修正，修正后的可行坐標(biāo)點(diǎn)為：

65、

66、

67、

68、

69、

70、其中表示同一水平面內(nèi)不能容納的智能體數(shù)量，表示同一水平面容納的智能體數(shù)量，為設(shè)定的多智能體數(shù)量；表示不能容納的智能體數(shù)量是同一水平面可行坐標(biāo)點(diǎn)的倍數(shù)，用與修正可行坐標(biāo)點(diǎn)的高度；表示不能容納的智能體數(shù)量除以同一水平面可行坐標(biāo)點(diǎn)的余數(shù)，用與修正可行坐標(biāo)點(diǎn)的高度；表示修正的可行坐標(biāo)點(diǎn)。

71、進(jìn)一步地，步驟s104中確定多智能體編隊(duì)中智能體的探測(cè)朝向與航行速度具體包括：

72、基于執(zhí)行人員的當(dāng)前坐標(biāo)以及智能體在編隊(duì)中的位置，采用步驟s102中確定多智能體編隊(duì)方位角的方法確定智能體探測(cè)朝向；

73、基于執(zhí)行人員速度以及智能體限制速度確定智能體航行速度。

74、一種基于位置信標(biāo)的多智能體伴隨與隊(duì)形保持系統(tǒng)，其特征在于，包括：

75、任務(wù)環(huán)境信息輸入單元，用于輸入任務(wù)環(huán)境信息和設(shè)定參數(shù)；

76、多智能體編隊(duì)方位角確定單元，基于任務(wù)環(huán)境信息矩陣，用于確定多智能體編隊(duì)方位角；

77、多智能體編隊(duì)的可行坐標(biāo)點(diǎn)確定單元，基于任務(wù)環(huán)境信息矩陣，用于確定多智能體編隊(duì)的可行坐標(biāo)點(diǎn)

78、多智能體編隊(duì)中智能體的探測(cè)朝向與航行速度確定單元，基于任務(wù)環(huán)境信息矩陣，用于確定多智能體編隊(duì)中智能體的探測(cè)朝向與航行速度

79、輸出單元，基于方位角、可行坐標(biāo)點(diǎn)和探測(cè)朝向與航行速度輸出多智能體伴隨式飛行掩護(hù)編隊(duì)信息。

80、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果為：本發(fā)明根據(jù)二分查找法以及位置信標(biāo)可以快速確定編隊(duì)最大安全偵察范圍，同時(shí)非網(wǎng)格化的處理可以更精確的確定多智能體編隊(duì)，實(shí)現(xiàn)三維環(huán)境下c型及ii型三維多智能體伴隨編隊(duì)敏捷規(guī)劃；本發(fā)明為具有空間（三維）、時(shí)間（速度）、智能體探測(cè)朝向等的隊(duì)形維持能力的伴隨式飛行掩護(hù)方法。

81、本發(fā)明是一種基于位置信標(biāo)的多智能體伴隨與隊(duì)形保持的方法，包括以下步驟：

82、（1）加載任務(wù)環(huán)境信息。

83、在伴隨掩護(hù)場(chǎng)景中，任務(wù)環(huán)境（自然障礙物、建筑物、禁飛區(qū)、雷達(dá)探測(cè)范圍等）對(duì)智能體的伴隨以及隊(duì)形維持尤為重要。首先構(gòu)建任務(wù)環(huán)境信息矩陣，任務(wù)環(huán)境信息指三維空間信息包括經(jīng)度、緯度和高。則任務(wù)環(huán)境信息矩陣可以表示為：

84、

85、其中表示任務(wù)環(huán)境信息矩陣；表示矩陣的行，指的是解析地圖后的行數(shù)；表示矩陣的列，指的是解析地圖后的列數(shù)；表示解析地圖后第行列的位置的地理信息高度值；其中禁飛區(qū)邊界對(duì)應(yīng)的行列高度值為30000表示智能體無法跨域的高度；雷達(dá)探測(cè)范圍內(nèi)的行列高度值為雷達(dá)探測(cè)實(shí)際范圍。

86、（2）確定多智能體編隊(duì)方位角。

87、方位角定義：表示多智能體編隊(duì)與正北方向的角度（單位：度），多智能體編隊(duì)與正北方向的夾角為0度。

88、多智能體編隊(duì)方位角是由執(zhí)行人員上一時(shí)刻坐標(biāo)與執(zhí)行人員的當(dāng)前坐標(biāo)確定。執(zhí)行人員的一般坐標(biāo)表示為，表示執(zhí)行人員的經(jīng)度，表示執(zhí)行人員的緯度，表示執(zhí)行人員相對(duì)地面的高度。多智能體編隊(duì)方位角的計(jì)算如下所示：

89、1）將經(jīng)緯度轉(zhuǎn)換為弧度

90、

91、

92、

93、

94、2）使用反正切函數(shù)計(jì)算方位角，并轉(zhuǎn)化為度數(shù)

95、

96、

97、

98、3）保證方位角的度數(shù)在0-360度范圍內(nèi)

99、

100、（3）確定多智能體編隊(duì)的可行坐標(biāo)點(diǎn)

101、在多智能體伴隨執(zhí)行人員進(jìn)行伴隨探測(cè)時(shí)，為了保證在形成編隊(duì)時(shí)多智能體間不產(chǎn)生碰撞，執(zhí)行人員會(huì)對(duì)編隊(duì)中多智能體間設(shè)置安全距離，包括多智能體間水平安全距離與垂直安全距離。并且執(zhí)行人員也會(huì)限制c型編隊(duì)探測(cè)時(shí)的執(zhí)行半徑、ii型編隊(duì)探測(cè)時(shí)的執(zhí)行隊(duì)長(zhǎng)，以及多智能體相對(duì)執(zhí)行人員的高度。這里本發(fā)明用表示水平安全距離，用表示垂直安全距離，用表示執(zhí)行人員與多智能體的垂直高度，用表示執(zhí)行人員與多智能體的水平半徑或水平隊(duì)長(zhǎng)，表示執(zhí)行人員與多智能體的直線半徑或直線隊(duì)長(zhǎng)。其中、與間存如一下關(guān)系。

102、

103、1）編隊(duì)可行半徑確定

104、執(zhí)行人員設(shè)置的僅僅是對(duì)編隊(duì)的最大執(zhí)行半徑進(jìn)行限制，但是由于任務(wù)環(huán)境的影響執(zhí)行人員設(shè)置的執(zhí)行半徑可能不可行，因此需要根據(jù)任務(wù)環(huán)境對(duì)編隊(duì)的執(zhí)行半徑進(jìn)行修正。

105、本發(fā)明最終將根據(jù)執(zhí)行人員設(shè)置的水平半徑/隊(duì)長(zhǎng)以及該水平半徑下滿足安全距離的多智能體編隊(duì)坐標(biāo)，通過二分查找法確定最大可行水平半徑/隊(duì)長(zhǎng)，確定多智能體伴隨坐標(biāo)時(shí)不需要進(jìn)行網(wǎng)格化等處理，可以提高伴隨飛行精度。

106、二分查找法通過比較數(shù)組中間元素和目標(biāo)值來縮小搜索范圍，從而提高查找效率，具體流程如下：

107、①確定可行水平半徑/隊(duì)長(zhǎng)范圍

108、為最小水平半徑/隊(duì)長(zhǎng)，為最大水平半徑/隊(duì)長(zhǎng)

109、②初始化最大可行水平半徑/隊(duì)長(zhǎng)

110、

111、③更新并確定最大可行水平半徑/隊(duì)長(zhǎng)

112、

113、

114、

115、其中表示條件下的編隊(duì)不可行，表示條件下的編隊(duì)可行。

116、④確定最大可行水平半徑/隊(duì)長(zhǎng)終止條件

117、重復(fù)步驟③中的賦值操作，當(dāng)時(shí)，步驟③結(jié)束，查找到最大可行水平半徑/隊(duì)長(zhǎng)。

118、2）c型編隊(duì)時(shí)多智能體編隊(duì)可行坐標(biāo)點(diǎn)確定

119、確定多智能體c型編隊(duì)可行坐標(biāo)點(diǎn)需要獲取最大可行水平半徑、執(zhí)行人員與多智能體的垂直高度、多智能體編隊(duì)方位角、多智能體數(shù)量以及可行坐標(biāo)點(diǎn)的方位角等信息。其中執(zhí)行人員與多智能體的垂直高度已知、最大可行水平半徑與多智能體編隊(duì)方位角可通過上述過程獲取，可行坐標(biāo)點(diǎn)的方位角以及多智能體c型編隊(duì)可行坐標(biāo)點(diǎn)計(jì)算過程如下。

120、①確定多智能體間的夾角

121、多智能體間的夾角由多智能體數(shù)量和水平安全距離計(jì)算得出，計(jì)算公式如下。

122、

123、

124、其中表示最大可行水平半徑、水平安全距離時(shí)同一水平面下可容納的多智能體數(shù)量；當(dāng)執(zhí)行人員貼著墻走時(shí)完整的c型編隊(duì)將無法保持，因此本發(fā)明考慮這種情況也可以首先半c型編隊(duì)，保證執(zhí)行人員貼墻走時(shí)也可以收到多智能體的伴隨保護(hù)，表示c型編隊(duì)隊(duì)形，表示半c型編隊(duì)隊(duì)形。

125、②確定多智能體編隊(duì)可行坐標(biāo)點(diǎn)方位角

126、根據(jù)多智能體編隊(duì)方位角（就是c型編隊(duì)中心的朝向）、多智能體間的夾角可以確定可行坐標(biāo)點(diǎn)方位角，計(jì)算公式如下所示。

127、

128、其中表示第個(gè)多智能體，。

129、③確定多智能體編隊(duì)可行坐標(biāo)點(diǎn)

130、根據(jù)執(zhí)行人員與多智能體的垂直高度、可行坐標(biāo)點(diǎn)方位角以及最大可行水平半徑，可以確定多智能體編隊(duì)可行坐標(biāo)點(diǎn)的初始坐標(biāo)，計(jì)算過程如下所示。

131、

132、

133、

134、

135、

136、

137、其中表示地球每度的弧長(zhǎng)，單位是千米；表示最大可行水平半徑，單位是千米；單位是弧度，用于計(jì)算經(jīng)緯度；表示智能體的經(jīng)度坐標(biāo)；表示智能體的緯度坐標(biāo)。

138、3）ii型編隊(duì)時(shí)多智能體編隊(duì)可行坐標(biāo)點(diǎn)確定

139、確定多智能體ii型編隊(duì)可行坐標(biāo)點(diǎn)需要獲取最大可行水平隊(duì)長(zhǎng)、執(zhí)行人員與多智能體的垂直高度、多智能體編隊(duì)方位角、多智能體數(shù)量、多智能體相對(duì)點(diǎn)和以及相對(duì)方位角等信息。其中執(zhí)行人員與多智能體的垂直高度已知、最大可行水平隊(duì)長(zhǎng)與多智能體編隊(duì)方位角可通過上述過程獲取，相對(duì)方位角、多智能體相對(duì)點(diǎn)和以及多智能體ii型編隊(duì)可行坐標(biāo)點(diǎn)計(jì)算過程如下。

140、①確定多智能體相對(duì)點(diǎn)與相對(duì)方位角

141、不同于c型編隊(duì)需要計(jì)算夾角，ii型編隊(duì)需要計(jì)算的是智能體的相對(duì)點(diǎn)和以及相對(duì)于相對(duì)點(diǎn)的相對(duì)方位角。

142、

143、

144、

145、

146、

147、

148、

149、

150、

151、

152、

153、②確定多智能體間的間距

154、多智能體間的間距由多智能體數(shù)量和水平安全距離計(jì)算得出，計(jì)算公式如下。

155、

156、

157、③確定多智能體編隊(duì)可行坐標(biāo)點(diǎn)（全部）

158、根據(jù)執(zhí)行人員與多智能體的垂直高度、相對(duì)方位角以及多智能體與相對(duì)點(diǎn)距離，可以確定多智能體編隊(duì)可行坐標(biāo)點(diǎn)的初始坐標(biāo)，計(jì)算過程如下所示。

159、

160、

161、

162、

163、

164、

165、

166、其中表示第個(gè)智能體；表示ii型編隊(duì)一邊設(shè)置的智能體數(shù)量。

167、4）多智能體編隊(duì)可行坐標(biāo)點(diǎn)修正

168、當(dāng)智能體數(shù)量較多，同一水平面無法容納時(shí)，通過上面的步驟得出的可行坐標(biāo)點(diǎn)將不足，這時(shí)將智能體分配到可行坐標(biāo)點(diǎn)會(huì)出現(xiàn)多個(gè)智能體在同一個(gè)位置的情況。因此，這里將根據(jù)執(zhí)行人員設(shè)置的垂直安全距離，在垂直方向修正可行坐標(biāo)點(diǎn)，計(jì)算公式如下所示。

169、

170、

171、

172、

173、

174、其中表示同一水平面內(nèi)不能容納的智能體數(shù)量；表示同一水平面可以容納的智能體數(shù)量；表示不能容納的智能體數(shù)量是同一水平面可行坐標(biāo)點(diǎn)的倍數(shù)，用與修正可行坐標(biāo)點(diǎn)的高度；表示不能容納的智能體數(shù)量除以同一水平面可行坐標(biāo)點(diǎn)的余數(shù)，用與修正可行坐標(biāo)點(diǎn)的高度；表示修正的可行坐標(biāo)點(diǎn)。

175、（4）確定多智能體編隊(duì)中智能體的探測(cè)朝向與航行速度

176、在確定智能體的探測(cè)朝向和航行速度前，需要確定智能體的坐標(biāo)點(diǎn)，本發(fā)明通過任務(wù)分配方法確定智能體的坐標(biāo)點(diǎn)，以此計(jì)算初多智能體的探測(cè)朝向和航行速度。

177、任務(wù)分配的就是根據(jù)多智能體信息，以就近原則為標(biāo)準(zhǔn)，將可行坐標(biāo)點(diǎn)分配給多智能體，使智能體可以快速前往附近可行坐標(biāo)點(diǎn)，以更好地保持隊(duì)形。

178、1）探測(cè)朝向確定

179、智能體的探測(cè)朝向與執(zhí)行人員的當(dāng)前坐標(biāo)以及智能體在編隊(duì)中的位置有關(guān)，智能體探測(cè)朝向的計(jì)算公式如下所示。

180、①將經(jīng)緯度轉(zhuǎn)換為弧度

181、

182、

183、

184、

185、②使用反正切函數(shù)計(jì)算方位角，并轉(zhuǎn)化為度數(shù)

186、

187、

188、

189、③保證方位角的度數(shù)在0-360度范圍內(nèi)

190、

191、2）航行速度確定

192、為了保證航行速度的穩(wěn)定性，智能體的航行速度將會(huì)限制在一定的范圍內(nèi)，由設(shè)定的執(zhí)行人員速度以及智能體限制速度決定，航行速度的確定如下公式所示。

193、

194、

195、其中表示執(zhí)行人員行進(jìn)距離，表示兩次伴隨編隊(duì)更新時(shí)間，表示執(zhí)行人員的實(shí)際行進(jìn)速度。

196、（5）多智能體伴隨式飛行掩護(hù)編隊(duì)

197、通過上述過程多智能體伴隨式飛行掩護(hù)編隊(duì)序列為，分別表示智能體編號(hào)，經(jīng)度，緯度，高度，方位角，速度。