本發(fā)明涉及水下航行器運(yùn)動控制，尤其是一種基于模型預(yù)測控制與代價勢場的水下航行器避障方法。

背景技術(shù)：

1、無人水下航行器是當(dāng)前海洋開發(fā)中使用的重要工具之一，能夠執(zhí)行水下目標(biāo)探查、處置作業(yè)、海底測繪、海洋科學(xué)調(diào)查等任務(wù)。無人水下航行器在航行的過程中，可能會遇到各種靜態(tài)與動態(tài)障礙物。

2、現(xiàn)有技術(shù)中，對于已知的靜態(tài)障礙物，無人水下航行器在規(guī)劃航線時會提前規(guī)避，以確保航程安全。然而，對于未知的靜態(tài)障礙物，或者動態(tài)、不確定的障礙物，傳統(tǒng)的無人水下航行器在執(zhí)行任務(wù)過程中難以進(jìn)行躲避，因而仍存在較大的損毀風(fēng)險，難以適用于當(dāng)前逐漸復(fù)雜的任務(wù)需求。

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

1、基于此，有必要針對現(xiàn)有技術(shù)中無人水下航行器在執(zhí)行任務(wù)過程中難以躲避未知障礙物、動態(tài)障礙物的問題，提供一種基于模型預(yù)測控制與代價勢場的水下航行器避障方法。

2、本發(fā)明所采用的技術(shù)方案如下：

3、一種基于模型預(yù)測控制與代價勢場的水下航行器避障方法，包括如下步驟：

4、s1.基于模型預(yù)測控制算法，建立水下航行器航跡跟蹤的第一最優(yōu)問題模型；

5、s2.在第一最優(yōu)問題模型中加入靜態(tài)代價函數(shù)和動態(tài)代價函數(shù)，所述靜態(tài)代價函數(shù)反映靜態(tài)障礙物形成的代價勢場，所述動態(tài)代價函數(shù)反映動態(tài)障礙物形成的代價勢場，從而在第一最優(yōu)問題模型中引入避障功能建立第二最優(yōu)問題模型；

6、s3.通過搭載于水下航行器上的探測組件，在探測范圍內(nèi)獲取航行方向上的初始障礙物信息；

7、根據(jù)水下航行器航行的任務(wù)要求，確定起始點(diǎn)和目標(biāo)點(diǎn)，結(jié)合探測組件獲取的初始障礙物信息，從而在起始點(diǎn)和目標(biāo)點(diǎn)之間規(guī)劃全局期望軌跡；

8、s4.所述水下航行器按照全局期望軌跡進(jìn)行航行，直至抵達(dá)終點(diǎn)；

9、同時，水下航行器航行過程中，通過探測組件獲取實時障礙物信息，當(dāng)探測組件獲取的實時障礙物信息顯示發(fā)現(xiàn)障礙物時，根據(jù)第二最優(yōu)問題模型進(jìn)行避障。

10、作為上述技術(shù)方案的進(jìn)一步改進(jìn)：

11、s1中，建立第一最優(yōu)問題模型時，包括如下步驟：

12、s1.1根據(jù)無人水下航行器的運(yùn)動學(xué)和動力學(xué)模型，建立離散的狀態(tài)空間模型；

13、s1.2.通過模型預(yù)測控制算法對離散的狀態(tài)空間模型進(jìn)行遞歸優(yōu)化，從而求解能使目標(biāo)函數(shù)最小的最優(yōu)控制序列，即第一最優(yōu)問題模型，由(式1)表示：

14、

15、(式1)中，j1(x(k),u(k))表示最優(yōu)函數(shù)；

16、x(·)表示水下航行器的輸入狀態(tài)；

17、u(·)表示水下航行器的控制輸入；

18、k表示時間步；

19、d(·)表示水下航行器受到的環(huán)境干擾；

20、y(·)表示水下航行器的輸出狀態(tài)；

21、np表示預(yù)測時域；

22、u表示控制輸入約束；

23、c表示狀態(tài)輸出矩陣；

24、f(·)為非線性函數(shù)，表示水下航行器輸入輸出關(guān)系的六自由度狀態(tài)空間模型；

25、i為非負(fù)整數(shù)。

26、s2中，建立第二最優(yōu)問題模型時，包括如下步驟：

27、s2.1.在k時間步的靜態(tài)代價函數(shù)由(式2)表示：

28、

29、(式2)中，costs(·)表示靜態(tài)避障函數(shù)；

30、k表示時間步；

31、(x,y,z)表示水下航行器在固定坐標(biāo)系的位置坐標(biāo)；

32、(xs,ys,zs)表示靜態(tài)障礙物在固定坐標(biāo)系中的位置坐標(biāo)；

33、σ1表示標(biāo)準(zhǔn)差；

34、c1為常數(shù)；

35、s2.2.在k時間步的動態(tài)代價函數(shù)由(式3)表示：

36、

37、(式3)中，costd(·)表示動態(tài)避障函數(shù)；

38、k表示時間步；

39、(x,y,z)表示水下航行器在固定坐標(biāo)系的位置坐標(biāo)；

40、(xd,yd,zd)表示動態(tài)障礙物在固定坐標(biāo)系中的位置坐標(biāo)；

41、σ2表示標(biāo)準(zhǔn)差；

42、c2為常數(shù)；

43、s2.3.將(式2)和(式3)引人(式1)中，從而得到第二最優(yōu)問題模型，由(式4)表示：

44、

45、(式4)中，j2(x(k),u(k))表示最優(yōu)函數(shù)；

46、x(·)表示水下航行器的輸入狀態(tài)；

47、u(·)表示水下航行器的控制輸入；

48、k表示時間步；

49、d(·)表示水下航行器受到的環(huán)境干擾；

50、y(·)表示水下航行器的輸出狀態(tài)；

51、np表示預(yù)測時域；

52、u表示控制輸入約束；

53、c表示狀態(tài)輸出矩陣；

54、f(·)為非線性函數(shù)，表示水下航行器輸入輸出關(guān)系的六自由度狀態(tài)空間模型；

55、i為非負(fù)整數(shù)。

56、所述最優(yōu)函數(shù)j2(x(k),u(k))的表達(dá)式為：

57、

58、上式中，x(·)表示水下航行器的輸入狀態(tài)；

59、u(·)表示水下航行器的控制輸入；

60、k表示時間步；

61、np表示預(yù)測時域；

62、xref(·)表示參考狀態(tài)向量；

63、q表示狀態(tài)權(quán)重矩陣；

64、xf(·)表示終端狀態(tài)向量；

65、p表示終端狀態(tài)權(quán)重矩陣；

66、nc表示控制時域；

67、r表示控制變化量權(quán)重矩陣；

68、n表示靜態(tài)障礙物和動態(tài)障礙物的預(yù)測時域；

69、ns表示靜態(tài)障礙物的數(shù)量；

70、rs表示靜態(tài)障礙物的半徑；

71、costs(·)表示靜態(tài)避障函數(shù)；

72、nd表示動態(tài)障礙物的數(shù)量；

73、rd表示動態(tài)障礙物的半徑；

74、costd(·)表示動態(tài)避障函數(shù)；

75、i為非負(fù)整數(shù)；

76、j為非負(fù)整數(shù)。

77、所述全局期望軌跡包含未來一定時間內(nèi)各個時間點(diǎn)的期望位置和航向角。

78、所述探測組件包括雷達(dá)、聲納和水下相機(jī)。

79、s4.中，根據(jù)障礙物的位置信息和速度信息，判斷障礙物的種類，并將障礙物簡化成以中心為球心，距離中心最遠(yuǎn)點(diǎn)為半徑的球體。

80、所述障礙物的種類包括靜態(tài)障礙物和動態(tài)障礙物。

81、s4.中，根據(jù)第二最優(yōu)問題模型，所述水下航行器在航行過程中沿障礙物的代價勢場低的方向航行，從而實現(xiàn)避障。

82、本發(fā)明的有益效果如下：

83、本發(fā)明結(jié)構(gòu)緊湊、合理，操作方便，通過模型預(yù)測控制算法與代價函數(shù)相結(jié)合，從而得到第二最優(yōu)問題模型(式4)，能夠?qū)ξ粗系K物、動態(tài)障礙物進(jìn)行有效躲避，且原理清晰、形式簡潔、易于實現(xiàn)；同時，綜合考慮了水下航行器的動力學(xué)特性、模型不確定性、輸入飽和、環(huán)境干擾等因素，更加適用于海洋環(huán)境和水下航行器的運(yùn)動特征。

84、本發(fā)明根據(jù)障礙物的類型，能夠采取不同類型的避障策略，對于已知的靜態(tài)障礙物，在規(guī)劃航線時，會規(guī)避這些在路徑上的靜態(tài)障礙，以確保航程安全；對于未知的靜態(tài)障礙物或動態(tài)障礙物，水下航行器需要航行運(yùn)動控制過程中進(jìn)行實時躲避，一旦發(fā)現(xiàn)周圍存在碰撞風(fēng)險，水下航行器根據(jù)自身位置狀態(tài)、目標(biāo)狀態(tài)和環(huán)境條件，通過第二最優(yōu)問題模型進(jìn)行避障路徑規(guī)劃，計算出安全且優(yōu)化的航行軌跡，以避免發(fā)生碰撞。

85、本發(fā)明中，通過加入代價函數(shù)，能夠?qū)崟r預(yù)測碰撞風(fēng)險，從而方便水下航行器根據(jù)第二最優(yōu)問題模型計算出減速或轉(zhuǎn)向等避讓動作來優(yōu)化航線，保持在安全距離內(nèi)航行，有助于提高航運(yùn)效率，能夠減少因人為失誤而導(dǎo)致的事故風(fēng)險，是實現(xiàn)無人水下航行器航行的重要技術(shù)保障。

86、本發(fā)明中，第一最優(yōu)問題模型(式1)基于模型預(yù)測控制算法，能夠?qū)δＰ蛥?shù)的不確定性和外部干擾進(jìn)行一定程度的抑制，從而提高水下航行器的控制系統(tǒng)的魯棒性；還能夠同時考慮船舶的動力學(xué)約束、障礙物代價，并在優(yōu)化過程中平衡這些因素，得到綜合性能較優(yōu)的控制策略；此外，模型預(yù)測控制算法能夠根據(jù)實時感知的環(huán)境信息，如其他船舶的位置和運(yùn)動狀態(tài)，動態(tài)調(diào)整船舶的航行策略，從而更好地應(yīng)對動態(tài)和不確定的航行環(huán)境；模型預(yù)測控制能夠基于當(dāng)前狀態(tài)和未來預(yù)測的狀態(tài)，動態(tài)優(yōu)化船舶的航行軌跡，在滿足安全約束的前提下實現(xiàn)更加高效和優(yōu)化的航行。