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

背景技術：

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

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

技術實現思路

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

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

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

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

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

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

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

8、s4.所述水下航行器按照全局期望軌跡進行航行，直至抵達終點；

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

10、作為上述技術方案的進一步改進：

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

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

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

14、

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

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

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

18、k表示時間步；

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

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

21、np表示預測時域；

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

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

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

25、i為非負整數。

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

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

28、

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

30、k表示時間步；

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

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

33、σ1表示標準差；

34、c1為常數；

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

36、

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

38、k表示時間步；

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

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

41、σ2表示標準差；

42、c2為常數；

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

44、

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

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

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

48、k表示時間步；

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

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

51、np表示預測時域；

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

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

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

55、i為非負整數。

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

57、

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

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

60、k表示時間步；

61、np表示預測時域；

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

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

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

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

66、nc表示控制時域；

67、r表示控制變化量權重矩陣；

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

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

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

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

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

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

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

75、i為非負整數；

76、j為非負整數。

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

78、所述探測組件包括雷達、聲納和水下相機。

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

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

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

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

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

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

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

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