本發(fā)明涉及水下航行器技術(shù)，特別是一種基于flexray協(xié)議的auv狀態(tài)估計方法。

背景技術(shù)：

1、auv(水下航行器)在水下環(huán)境中執(zhí)行任務(wù)時，需要對自身的軌跡進(jìn)行精確估計，以確保能夠完成任務(wù)并返回預(yù)定位置。然而，由于水下環(huán)境具有特殊的挑戰(zhàn)，auv的軌跡估計面臨著一系列的技術(shù)難點，主要包括以下幾個方面：

2、水下環(huán)境中的水流、湍流、溫度變化等因素都對auv的航行造成影響。這些因素使得auv的運動軌跡非常難以預(yù)測；水下的光學(xué)信號衰減嚴(yán)重，限制了auv的視覺導(dǎo)航和成像系統(tǒng)的使用，這進(jìn)一步增加了軌跡估計的難度。在未知或動態(tài)變化的水下環(huán)境中，auv必須能夠?qū)崟r估計自身的軌跡，并根據(jù)環(huán)境變化進(jìn)行路徑規(guī)劃和動態(tài)避障。然而由于水下環(huán)境的復(fù)雜性和動態(tài)性，實時進(jìn)行精確的軌跡估計是一項巨大的挑戰(zhàn)。

3、集員估計已成為處理噪聲的中一種適當(dāng)而有效的狀態(tài)估計方法，其中考慮的是未知但有界的噪聲。但在經(jīng)典的集員濾波中考慮的是以橢球作為邊界的集員算法，而在橢球的集合運算中閔可夫斯基和的步驟往往需要一個更保守的外接橢球來包絡(luò)，這一定程度上導(dǎo)致估計精度的下降；而在另一種橢球算法中使用的線性矩陣不等式方法lmis，通常在高維度的系統(tǒng)中計算復(fù)雜，有實時響應(yīng)速度較慢的缺點。因此，設(shè)計一種估計集合更緊湊，響應(yīng)速度較快的集員估計算法成為了一個課題。

4、隨著auv在水下執(zhí)行復(fù)雜任務(wù)的需求增加，網(wǎng)絡(luò)技術(shù)的應(yīng)用在提升系統(tǒng)通信能力、數(shù)據(jù)傳輸效率等方面發(fā)揮著重要作用。然而，水下通信的特殊環(huán)境和資源的有限性，帶來了通信網(wǎng)絡(luò)上的挑戰(zhàn)，auv在水下航行時，通常需要進(jìn)行高精度的傳感器數(shù)據(jù)共享、實時狀態(tài)更新、路徑規(guī)劃與協(xié)同控制等操作。這些操作需要大量的數(shù)據(jù)傳輸，但由于水下資源有限，auv之間的通信帶寬和計算資源無法滿足需求，可能導(dǎo)致系統(tǒng)性能下降或任務(wù)無法按預(yù)期完成。為了有效解決數(shù)據(jù)沖突，必須設(shè)計合理的通訊調(diào)度協(xié)議，確保多個auv之間能夠有序地進(jìn)行數(shù)據(jù)交換，從而避免頻繁的信號干擾和沖突。

5、在網(wǎng)絡(luò)通信和多任務(wù)調(diào)度領(lǐng)域，輪詢協(xié)議(rrp)和嘗試一次丟棄協(xié)議(todp)常常用來處理資源共享和數(shù)據(jù)傳輸?shù)恼{(diào)度問題。然而rrp作為一種固定的調(diào)度方式缺乏靈活性，尤其是當(dāng)某些設(shè)備長時間沒有數(shù)據(jù)需要傳輸時，輪詢協(xié)議仍然會浪費時間檢查這些設(shè)備；而todp雖然提高了調(diào)度的靈活性，但是在一定的競爭規(guī)則下保證不了估計\控制下系統(tǒng)的穩(wěn)定性。

6、因此，亟需研究一種面向auv在新的通信調(diào)度協(xié)議下的集員估計算法，實現(xiàn)auv在水下復(fù)雜環(huán)境中的軌跡精確估計。

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

1、針對上述缺陷，本發(fā)明的目的在于提出基于flexray協(xié)議的auv狀態(tài)估計方法，通過有效提高水下網(wǎng)絡(luò)資源的利用效率并實現(xiàn)靈活穩(wěn)定的數(shù)據(jù)傳輸，實現(xiàn)了auv在復(fù)雜未知但有界噪聲中的高精度定位。

2、為達(dá)此目的，本發(fā)明采用以下技術(shù)方案：

3、一種基于flexray協(xié)議的auv狀態(tài)估計方法，包括以下步驟：

4、s1、構(gòu)建6-dof模型的水下航行器系統(tǒng)，根據(jù)水下航行器的位置信息和歐拉角度的動力方程進(jìn)行建模，基于連續(xù)時間系統(tǒng)的動力學(xué)方程，通過零階保持器的方法將連續(xù)時間系統(tǒng)轉(zhuǎn)化為離散時間系統(tǒng)；

5、s2、基于flexray協(xié)議，對照其中傳輸協(xié)議的核心部分，將傳輸協(xié)議大致分為兩類，一類基于時間多址聯(lián)接方式，一類基于靈活時間多址聯(lián)接方式，以此設(shè)計針對水下航行器系統(tǒng)的flexray協(xié)議，將協(xié)議分為靜態(tài)段的輪詢協(xié)議和動態(tài)段的嘗試一次丟棄協(xié)議；

6、s3、由于使用flexray協(xié)議，數(shù)據(jù)包到達(dá)的時間存在一個單位的估計器工作間隔，該現(xiàn)象被視為一個部分時延問題；針對基于flexray協(xié)議的機制出現(xiàn)的部分時延問題，增廣系統(tǒng)狀態(tài)，變成一個更高維的離散系統(tǒng)，解決混合時延下一般的狀態(tài)估計算法同步困難、誤差累積、算法穩(wěn)定性下降的難題；

7、s4、由于增廣系統(tǒng)的維數(shù)很多，因此涉及到計算復(fù)雜度大的問題；建立全對稱多胞體的集員濾波，使用全對稱多胞體外殼提供更高精度的狀態(tài)集合估計。

8、較佳地，步驟s1中，auv的動態(tài)模型被描述為一個基于身體固定坐標(biāo)系和地球固定坐標(biāo)系的6-dof模型；

9、由于滾轉(zhuǎn)對平移運動的影響很小，因此不考慮滾轉(zhuǎn)速度；下面為auv在離散情況下的運動學(xué)模型和動力學(xué)模型：

10、

11、式1中，s＝[x,y,z]t和θ＝[φ,ψ]t分別表示auv的3d位置狀態(tài)和歐拉角度，并且和表示在相應(yīng)分量上的速度；定義系統(tǒng)狀態(tài)為；

12、

13、式2中，xk表示的是在k時刻系統(tǒng)的狀態(tài)參數(shù)；

14、由此，可以得到初始auv系統(tǒng)的動力學(xué)方程，

15、xk+1＝axk+wk?式(3)

16、并由傳感器量測得到原始量測

17、

18、其中，未知但有界的噪聲wk和vk受限于接下來的多胞體集合；c是量測矩陣，傳感器通常不能直接得到狀態(tài)x的數(shù)據(jù)，因此c是傳感器的物理參數(shù)：

19、

20、其中，<c，p>表示一個中心為向量c，生成矩陣為p的多胞體集合；所以wk和vk分別位于中心為原點，生成矩陣為w和v的多胞體集合中。

21、較佳地，在步驟2中，均勻量化器為設(shè)置一個標(biāo)量b，使得在[-b,b]的區(qū)間可以根據(jù)分配的比特rbps劃分成2r-1個量化區(qū)間，根據(jù)收到的信息大小，來判斷落在哪個小區(qū)間內(nèi)，輸出相應(yīng)的區(qū)間編碼；

22、我們定義量化后的量測為：

23、yk＝cxk+vk+dk?式(6)

24、式6中，表示量化誤差，并且量化誤差與數(shù)據(jù)包的數(shù)據(jù)位數(shù)有關(guān)；定義量化誤差為一個多胞體：

25、dk∈<0,dk〉；?式(7)

26、式7中，<0，dk>示中心為原點，生成矩陣為dk的多胞體集合；

27、在汽車網(wǎng)絡(luò)通信中，動態(tài)段被廣泛用于診斷刷寫；因此，動態(tài)的定時參數(shù)與靜態(tài)循環(huán)的定時參數(shù)不一致；在這里，將auv的歐拉角度的變化率定義為嘗試一次丟棄協(xié)議在動態(tài)段傳輸?shù)臄?shù)據(jù)包，即在動態(tài)段對進(jìn)行刷寫；考慮到傳輸周期只有一個數(shù)據(jù)包能訪問信道資源，我們將量測分為n個數(shù)據(jù)包并分為兩個集合和其中屬于集合l1的節(jié)點由輪詢協(xié)議調(diào)度，屬于l2的節(jié)點由嘗試一次丟棄協(xié)議調(diào)度，調(diào)度規(guī)則如下：

28、a.輪詢協(xié)議:

29、λ(j)＝mod(j-1,h)+1(8)

30、式8中，λ(j)∈l1表示在時刻j獲得傳輸權(quán)的節(jié)點，mod()為求余函數(shù)；

31、b.嘗試一次丟棄協(xié)議：

32、

33、式9中，y′j，k表示在k時刻前最后一次傳輸?shù)墓?jié)點j的信息，αk∈l2表示在時刻k在“最大誤差優(yōu)先”的規(guī)則下最需要傳輸?shù)墓?jié)點；是一個給定的正定權(quán)重矩陣；

34、因此，基于在高速率網(wǎng)路的flexray協(xié)議下，量測可分為靜態(tài)段和動態(tài)段待調(diào)度的數(shù)據(jù)包：

35、

36、同樣，定義量測矩陣：

37、

38、將接收端收到的量測定義為：

39、基于高速率網(wǎng)絡(luò)的假設(shè)，我們將一個flexray協(xié)議的通信周期可以分為個傳輸周期，這是因為傳輸速度關(guān)于硬件技術(shù)的更新而大大提升；同時，狀態(tài)更新周期并未因為傳輸速度的變化而改變，這使得在每一個狀態(tài)更新周期都可以收到至多個數(shù)據(jù)包；更進(jìn)一步，接收量測被定義為：

40、

41、其中，和分別表示接收到數(shù)據(jù)包的量化誤差和量測噪聲。

42、進(jìn)一步地，在步驟3中，使用flexray協(xié)議的通信調(diào)度下自然會出現(xiàn)靜態(tài)段數(shù)據(jù)包和動態(tài)段數(shù)據(jù)包所在時刻不一樣的問題，即接收端面對了瞬時時刻的量測和帶有一個單位時延的量測混合在一塊，這使得在估計之前得先進(jìn)行預(yù)處理，使之轉(zhuǎn)化為標(biāo)準(zhǔn)無時延的系統(tǒng)；

43、我們設(shè)置增廣后的狀態(tài)為我們有接下來的增廣系統(tǒng)：

44、

45、其中，

46、

47、進(jìn)一步地，在步驟4中，利用全對稱多胞體作為集合外殼，我們基于先驗知識定義初始狀態(tài)位于多胞體相較于現(xiàn)有的橢球算法，多胞體的閔可夫斯基和滿足集合運算的封閉性，這意味著運算后依然是一個多胞體，即：

48、

49、這減少了估計在各種步驟中的保守性，使得估計的集合的尺寸更小，使得最終的估計結(jié)果誤差更??；

50、對于k時刻的狀態(tài)，首先對增廣后的系統(tǒng)進(jìn)行一步預(yù)測得到一步預(yù)測多胞體其中：

51、

52、式14中，和分別表示一步預(yù)測狀態(tài)的中心和生成矩陣；

53、下一步，估計的多胞體其中：

54、

55、式15中，和分別表示最終估計狀態(tài)集合的中心和生成矩陣。

56、進(jìn)一步地，在步驟4中，具體步驟為：

57、s41、初始化，利用先驗知識輸入初始狀態(tài)生成矩陣

58、s42、計算一步預(yù)測多胞體集合，根據(jù)下面的公式求出一步預(yù)測集合的集合中心和生成矩陣

59、

60、從而得到：

61、

62、s43、計算估計多胞體集合，根據(jù)下面的公式求出一步預(yù)測集合的集合中心和生成矩陣

63、

64、從而得到：

65、

66、s44、基于矩陣的f半徑最小化設(shè)計增益gk+1，為了方便最小化矩陣的f半徑我們定義生成矩陣的協(xié)方差矩陣為：

67、p＝cov(<c，r>)＝rrt?式(20)

68、因此，類似的協(xié)方差矩陣可以表示成：

69、

70、根據(jù)最優(yōu)性定理可以得到，

71、

72、s45、迭代，最終我們根據(jù)auv的動力學(xué)模型和基于flexray協(xié)議得到的量測得到關(guān)于每一步auv在三維空間中的航行軌跡。

73、上述技術(shù)方案中的一個技術(shù)方案包括以下有益效果：

74、本專利設(shè)計的基于flexray協(xié)議在高速率網(wǎng)絡(luò)下的3d軌跡估計算法，可以解決在水下網(wǎng)絡(luò)中資源有限的問題，通過一次只在共享網(wǎng)絡(luò)信道中傳輸一個包來實現(xiàn)，有效避免了數(shù)據(jù)沖突，并且由于混合協(xié)議的使用，既保留了rrp的穩(wěn)定性優(yōu)點又由于todp有一定的靈活性。與現(xiàn)有方法相比，可以有效提高水下網(wǎng)絡(luò)資源的利用效率并實現(xiàn)靈活穩(wěn)定的數(shù)據(jù)傳輸。