本發(fā)明屬于auv編隊控制，具體涉及一種基于動態(tài)拓撲切換策略下的auv編隊跟蹤控制方法、程序、設(shè)備及存儲介質(zhì)。

背景技術(shù)：

1、auv根據(jù)推力器的配置可以分為全驅(qū)動、過驅(qū)動和欠驅(qū)動。通常由機械設(shè)計者根據(jù)auv的期望功能并結(jié)合造價成本、重量和實現(xiàn)作業(yè)任務難度來決定配備推力器的數(shù)量，從而實現(xiàn)節(jié)約成本并能有效地完成控制目標?？紤]三自由度auv在水平面上的運動控制，auv水平面動態(tài)模型由于未安裝側(cè)向推進器，導致無側(cè)向驅(qū)動力產(chǎn)生，從而節(jié)省了動力負荷，但是對被控系統(tǒng)的算法提出了更高的要求。在以往水下多智能體編隊控制的設(shè)計中容易忽略水聲通信中斷的不可預見性，因此可以采用有向拓撲切換來解決這些問題。然而，當存在編隊內(nèi)智能體退出編隊或進入編隊，此時預先設(shè)定的拓撲結(jié)構(gòu)無法覆蓋編隊網(wǎng)絡，進而導致編隊穩(wěn)定性降低。

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

1、本發(fā)明的目的在于提供一種基于動態(tài)拓撲切換策略下的auv編隊跟蹤控制方法，可以在任意時刻生成當前位置拓撲結(jié)構(gòu)網(wǎng)絡，從而保證水下auv編隊在控制過程中是全連通的。

2、一種基于動態(tài)拓撲切換策略下的auv編隊跟蹤控制方法，包括以下步驟：

3、步驟1：初始時刻，auv編隊形成初始通信拓撲結(jié)構(gòu)；

4、所述auv編隊包括一臺領(lǐng)航auv，其余均為跟隨auv，通信拓撲結(jié)構(gòu)中的每一節(jié)點代表auv編隊中的一臺auv，且通信拓撲結(jié)構(gòu)中不存在孤立節(jié)點；

5、步驟2：當前時刻，領(lǐng)航auv獲取自身的位置信息；auv編隊中的跟隨auv獲取通信拓撲結(jié)構(gòu)中相鄰節(jié)點與自身的相對測距與相對角度作為量測信息，根據(jù)量測-通信聯(lián)合框架下的狀態(tài)空間模型，采用自適應卡爾曼濾波實現(xiàn)對過程噪聲和量測噪聲的自適應補償，估計得到自身的位置信息；

6、步驟3：獲取當前時刻auv編隊的期望位置信息，構(gòu)造期望生成樹通信拓撲結(jié)構(gòu)；

7、步驟4：計算當前時刻auv編隊的通信拓撲結(jié)構(gòu)與期望生成樹通信拓撲結(jié)構(gòu)之間的jaccard系數(shù)；若jaccard系數(shù)達到預設(shè)閾值，則當前時刻auv編隊的通信拓撲結(jié)構(gòu)不變；否則，基于2-delaunay生成新的通信拓撲結(jié)構(gòu)；

8、步驟5：根據(jù)當前時刻auv編隊的期望位置信息、通信拓撲結(jié)構(gòu)及狀態(tài)信息，通過分布式dmpc編隊控制器解算出各auv的最優(yōu)控制輸入，并反饋至auv編隊中各auv，完成當前時刻的跟蹤控制；

9、步驟6：若auv編隊未完成任務，則返回步驟2繼續(xù)執(zhí)行。

10、進一步地，所述步驟2中量測-通信聯(lián)合框架下的狀態(tài)空間模型為：

11、

12、其中，表示當前k時刻第i臺跟隨auv的位置信息；表示當前k時刻第i臺跟隨auv量測信息；φ為狀態(tài)轉(zhuǎn)移矩陣；h(·)為量測函數(shù)；wk-1和δk分別為過程噪聲和量測噪聲；

13、對于auv編隊中的每一臺跟隨auv，獲取通信拓撲結(jié)構(gòu)中相鄰節(jié)點與自身的相對測距與相對角度，作為量測信息采用自適應卡爾曼濾波方法，對量測-通信聯(lián)合框架下的狀態(tài)空間模型的過程噪聲和量測噪聲進行自適應補償，得到自身的位置信息

14、進一步地，所述步驟2中采用自適應卡爾曼濾波方法，對量測-通信聯(lián)合框架下的狀態(tài)空間模型的過程噪聲和量測噪聲進行自適應補償?shù)姆椒ň唧w為：

15、步驟2.1：計算過程噪聲均值矩陣和誤差協(xié)方差矩陣預測

16、

17、其中，dk-1＝(1-b)/(1-bk)，b為遺忘因子，0＜b＜1；為k-1時刻計算得到的第i臺跟隨auv的位置信息；為k-1時刻計算得到的誤差協(xié)方差矩陣；為k-1時刻計算得到的過程噪聲方差矩陣；

18、步驟2.2：計算位置預測

19、

20、步驟2.3：計算量測噪聲均值矩陣

21、

22、其中，h為量測函數(shù)h(·)在原點的雅可比矩陣；

23、步驟2.4：計算量測預測

24、

25、步驟2.5：計算量測噪聲的方差矩陣

26、

27、步驟2.6：計算自適應卡爾曼濾波增益

28、

29、步驟2.7：計算誤差協(xié)方差矩陣

30、

31、其中，i為單位矩陣；

32、步驟2.8：計算當前k時刻第i臺跟隨auv的位置信息和過程噪聲的方差矩陣

33、

34、進一步地，所述步驟4中計算當前時刻auv編隊的通信拓撲結(jié)構(gòu)與期望生成樹通信拓撲結(jié)構(gòu)之間的jaccard系數(shù)具體為：

35、

36、其中，gσ(t)表示當前時刻auv編隊的通信拓撲結(jié)構(gòu)；gσ(e)表示當前時刻期望生成樹通信拓撲結(jié)構(gòu)。

37、進一步地，所述步驟4中基于2-delaunay生成新的通信拓撲結(jié)構(gòu)的方法具體為：

38、步驟4.1：令auv編隊內(nèi)各節(jié)點位置標記為v＝{n1,n2,…,nn}，n1為領(lǐng)航auv，n為auv編隊中auv的總數(shù)；設(shè)超級三角形三個頂點a,b,c使得co(n)＝{a,b,c,v}為頂點集v的凸包，即將超級三角形的頂點加入到頂點鏈表中，頂點集v的三角剖分表示為δ(cn)；

39、對于三角形表鏈中的所有三角形，計算出其外接圓的圓心和半徑，若頂點鏈表中的點在這個外接圓內(nèi)，則把這個三角形的三條邊加入邊集合中，最后將所有重復的邊和包含超級三角形頂點的邊刪除，完成初步拓撲網(wǎng)絡的生成，即頂點集為v＝{1,2,…,n}，邊集為e＝{(i,j),i,j∈v,i≠j}的德勞內(nèi)三角剖分g＝(v,e)后的拓撲網(wǎng)絡結(jié)構(gòu)圖；

40、步驟4.2：設(shè)置最優(yōu)通信距離ro和最大通信距離rm作為初始參數(shù)，ro＜rm，將邊集e中大于最優(yōu)通信距離的元素||eij||＞ro刪除，得到通信拓撲網(wǎng)絡g*＝(v,e*)。

41、進一步地，所述步驟4.2中得到通信拓撲網(wǎng)絡g*＝(v,e*)后，若g*＝(v,e*)中存在孤立點，則以孤立點位置為圓心，以最大通信距離rm為半徑進行搜索；若搜索到節(jié)點，則建立該孤立點與最近節(jié)點之間的邊；若未搜索到節(jié)點，則建立該孤立點與領(lǐng)航auv節(jié)點之間的邊。

42、進一步地，所述步驟5中根據(jù)當前時刻auv編隊的期望位置信息、通信拓撲結(jié)構(gòu)及狀態(tài)信息，解算出各auv的最優(yōu)控制輸入的方法具體為：

43、步驟5.1：建立分布式dmpc編隊控制器，跟蹤控制動態(tài)過程的離散時間系統(tǒng)表示為：

44、ξi(k+1)＝aiξi(k)+biui(k)

45、ξi(k)＝cξi(k)

46、其中，ξi(k)為當前k時刻第i臺auv的狀態(tài)信息，包括位置、姿態(tài)和速度信息；ui(k)為待求解的當前k時刻第i臺auv的控制輸入；ξi(k)為預測輸出；c＝[i3×3o3×3]，矩陣ai與bi根據(jù)第i臺auv的運動模型獲取：mi與di分別為第i臺auv的慣性矩陣與阻尼矩陣；

47、設(shè)置預測區(qū)間np和控制區(qū)間nc，定義預測控制序列預測輸出序列為：

48、

49、ξi(k)和滿足：

50、

51、其中，

52、步驟5.2：將編隊跟蹤控制問題轉(zhuǎn)化為以下優(yōu)化問題：

53、

54、約束條件為：

55、(1)

56、(2)δymin≤||ξi(k)-ξj(k)||＜δymax；

57、(3)s∈[k+1,k+nc-1]；

58、其中，np和nc為預測區(qū)間和控制區(qū)間，nc≤np；為當前k時刻auv編隊的通信拓撲結(jié)構(gòu)對應的鄰接矩陣中第i行第j列的元素；為第i臺auv與第j臺auv之間的期望距離；δymin與δymax分別表示auv編隊中各auv之間距離的下限與上限；umin與umax分別表示控制輸入的下限與上限；

59、通過求解優(yōu)化問題得到最優(yōu)預測控制序列：

60、

61、取中的第一項，即令即得到第i臺auv的最優(yōu)控制輸入。

62、一種計算機裝置/設(shè)備/系統(tǒng)，包括存儲器、處理器及存儲在存儲器上的計算機程序，所述處理器執(zhí)行所述計算機程序以實現(xiàn)上述基于動態(tài)拓撲切換策略下的auv編隊跟蹤控制方法的步驟。

63、一種計算機可讀存儲介質(zhì)，其上存儲有計算機程序/指令，該計算機程序/指令被處理器執(zhí)行時實現(xiàn)上述基于動態(tài)拓撲切換策略下的auv編隊跟蹤控制方法的步驟。

64、一種計算機程序產(chǎn)品，包括計算機程序/指令，該計算機程序/指令被處理器執(zhí)行時實現(xiàn)上述基于動態(tài)拓撲切換策略下的auv編隊跟蹤控制方法的步驟。

65、本發(fā)明的有益效果在于：

66、本發(fā)明采用動態(tài)拓撲切換策略下的auv編隊跟蹤控制，基于此設(shè)計的auv編隊控制系統(tǒng)能夠在出現(xiàn)連通分量不唯一的情況時，擬通過求解不同集合之間的最短路徑作為新的通信鏈路，完成拓撲網(wǎng)絡優(yōu)化；本發(fā)明利用2-delaunay生成新的通信拓撲結(jié)構(gòu)，并結(jié)合最優(yōu)通信距離和最大通信距離，通過求解不同集合之間最短路徑作規(guī)避了連通分量不唯一的問題，從而完成拓撲網(wǎng)絡優(yōu)化。本發(fā)明采用基于jaccard系數(shù)的動態(tài)拓撲切換方案控制，當編隊內(nèi)智能體通信拓撲中斷或復通時，切換拓撲網(wǎng)絡結(jié)構(gòu)覆蓋編隊網(wǎng)絡，從而達到利用期望拓撲與自身拓撲之間的度量實現(xiàn)對拓撲的實時監(jiān)測與動態(tài)管理的目的。本發(fā)明采用了dmpc作為編隊控制器，充分發(fā)揮mpc在系統(tǒng)動態(tài)特性建模及未來行為預測方面的卓越能力，從而實現(xiàn)對多個auv在復雜環(huán)境中進行高效、精確的編隊控制。