本發(fā)明屬于船舶領域，具體涉及基于動態(tài)los的無人氣墊艇自主避障路徑規(guī)劃方法、系統(tǒng)、介質(zhì)及程序產(chǎn)品。

背景技術(shù)：

1、隨著海洋探索的發(fā)展需要，對擁有更高的航速的船舶的需要也更加迫切，以便于更高效的完成海上運輸?shù)热蝿?，傳統(tǒng)的船舶受其支撐方式的影響，很難擁有更高的航速。全墊升氣墊艇(air?cushion?vehicle,acv)通過墊升系統(tǒng)在船底產(chǎn)生的氣墊力作用下，能夠以墊態(tài)脫離航行表面，使得其航行阻力相比普通船舶要小得多，從而獲得更高的航速，可以更快地運輸人員和物資?，F(xiàn)如今，各種無人裝備井噴式發(fā)展，無人氣墊艇(unmannedaircushion?vehicle,uacv)的研究也受到了國內(nèi)外的廣泛關注，uacv在執(zhí)行任務時需要先運動到目標點附近，在這個過程中uacv可能會遇到障礙物，這就需要一種能夠順利躲避障礙物的路徑規(guī)劃算法，一種優(yōu)秀的路徑規(guī)劃方法有助于提高uacv智能化水平，使uacv更好的完成任務。

2、現(xiàn)有的路徑規(guī)劃方法主要分為三大類：基于圖搜索的路徑規(guī)劃算法、基于采樣的路徑規(guī)劃算法和基于優(yōu)化的路徑規(guī)劃算法?；趫D搜索的路徑規(guī)劃算法的代表算法有a*算法、jps算法、dijkstra算法等，這幾種算法的規(guī)劃速度相對較快，但是生成的路徑都是分段直線的形式，除了全向移動機器人以外，有非完整運動學特性的機器人都無法執(zhí)行這樣的路徑?；诓蓸拥穆窂揭?guī)劃算法的代表算法主要有prm算法、rrt算法，這些算法隨著采樣增加可以收斂到全局最優(yōu)，但這類算法搜索空間很大，規(guī)劃過程很耗費時間?；趦?yōu)化的路徑規(guī)劃算法通過構(gòu)建一個帶約束的最優(yōu)控制問題，最小化一個目標函數(shù)，能夠同時考慮多個約束，例如動力學約束、控制量約束等。這類算法在當前的自動駕駛領域比較受歡迎，但是因為現(xiàn)實中的優(yōu)化問題一般都是非線性的，所以需要一個好的初始解來進一步得到局部最優(yōu)解。這幾種方法都各有其優(yōu)缺點，因此為了實現(xiàn)更好的規(guī)劃效果，現(xiàn)階段的規(guī)劃算法主要是混合式方法，無人氣墊艇的路徑規(guī)劃方法也可以參考這個思路，本發(fā)明所提方法就是一種混合式方法。

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

1、本發(fā)明的目的在于提供一種基于動態(tài)los的無人氣墊艇自主避障路徑規(guī)劃方法、系統(tǒng)、介質(zhì)及程序產(chǎn)品。

2、本發(fā)明的目的通過如下技術(shù)方案來實現(xiàn)：

3、一種基于動態(tài)los的無人氣墊艇自主避障路徑規(guī)劃方法，具體步驟如下：

4、步驟1：環(huán)境建模與初始化；利用uacv感知設備獲取環(huán)境障礙物信息，建立點云數(shù)據(jù)和柵格地圖，確定起始點和目標點；初始化并動態(tài)維護航路點集合，生成los導引軌跡，包括附加時長和期望時長的軌跡；

5、步驟2：對生成的軌跡進行碰撞檢測；遍歷附加時長軌跡點集合中的點轉(zhuǎn)換成柵格索引，柵格地圖中的柵格如果被占用說明軌跡發(fā)生碰撞，進行步驟3；否則跳轉(zhuǎn)到步驟6；

6、步驟3：確定障礙運動方向并生成避障路徑；將穿入障礙物的點設為起點，穿出障礙物的點設為目標點，使用a*算法規(guī)劃起點到目標點最短的避障路徑，利用規(guī)劃出的軌跡中的航路點與起點的單位向量得到下一步的避障運動方向；以小步長的方式沿避障方向移動軌跡點，生成新的los導引軌跡并重復碰撞檢測，直至無碰撞；

7、步驟4：判斷是否有動態(tài)障礙物；設定一個安全距離閾值，利用cpa方法判斷無人氣墊艇和障礙船的最近距離點是否會發(fā)生碰撞，如果最近距離小于安全距離閾值即判定為發(fā)生碰撞，進行步驟5；否則跳轉(zhuǎn)步驟6；

8、步驟5：插入避障航路點；找到無人氣墊艇和障礙船的最近距離點處的無人氣墊艇自身的軌跡點，求出二者距離最近時無人氣墊艇位置到障礙船位置的向量，然后將軌跡點按向量的方向和長度先移動一次，最后沿著兩點連線的法線方向移動障礙物半徑長度，移動完成的軌跡點作為插入航路點，軌跡點移動的方向沿哪一側(cè)根據(jù)海事規(guī)則進行確定；

9、步驟6：以當前航路點構(gòu)建los導引軌跡的期望時長軌跡，無人氣墊艇沿生成的軌跡運行。

10、進一步地，所述步驟1中的los導引軌跡生成具體為：

11、通過兩個航路點和構(gòu)建一個路徑切向坐標系{p}，{p}的原點為xp軸沿著指向的方向，該方向和坐標系{n}的xn軸夾角為πp，πp的計算方法如下：

12、

13、通過夾角πp計算出無人氣墊艇當前位置在路徑切向坐標系{p}上的縱向跟蹤誤差和橫向跟蹤誤差計算過程如下：

14、

15、其中，的定義如下：

16、

17、πp與期望航向χd的關系為：

18、

19、其中，kp是比例增益，由前視距離δ計算得到，即：

20、

21、進一步地，所述步驟3將穿過障礙物的一段los軌跡沿a*算法確定的避障方向直接移出障礙物范圍，并將移出障礙物范圍后這段軌跡的兩端點作為插入航路點并生成los導引軌跡進行碰撞檢測，若有碰撞將繼續(xù)以小步長的方式，將插入的航路點沿避障方向移動，根據(jù)插入的航路點生成los導引軌跡，再進行碰撞檢測，直到生成的導引軌跡無碰撞為止。

22、進一步地，所述步驟3中以穿入障礙物點a為起點，以穿出障礙物點b為終點，搜索從a到b的最短距離，利用代價函數(shù)：

23、f(n)＝g(n)+h(n)

24、其中，g(n)為從起點a到終點b的距離，h(n)為啟發(fā)式估計距離；

25、根據(jù)歐氏距離作為估計距離，構(gòu)建一個空的open?list和closed?list，open?list用于存放擴展的鄰居節(jié)點，closed?list存放open?list中彈出的節(jié)點和障礙節(jié)點；從起點開始，擴展八個鄰居節(jié)點并放入open?list，彈出代價f(n)最小的鄰居節(jié)點并放入closedlist，繼續(xù)擴展彈出節(jié)點的鄰居節(jié)點并加入open?list，不斷重復上述過程直到目標節(jié)點被彈出為止。

26、進一步地，所述步驟4根據(jù)雷達或視覺信息預測障礙船的航向與航速，再經(jīng)過一定時間的遍歷。

27、進一步地，所述步驟4中通過計算到cpa的時間tcpa和到cpa的距離dcpa得到，計算方法如下：

28、

29、dcpa＝||pos+vostcpa-(pts+vtstcpa)||

30、其中，p和v是自船os和目標船ts的位置和速度向量，范數(shù)為二范數(shù)，定義如下，

31、

32、通過檢查是否可能發(fā)生碰撞或者在未來一段時間內(nèi)可能接近來判斷碰撞；通過以下條件來判定：

33、0≤tcpa≤trisk∩dcpa≤drisk

34、其中，trisk和drisk是自定義的參數(shù)，作為判斷碰撞的安全閾值。

35、進一步地，所述步驟5中海事規(guī)則包括兩船交會時的規(guī)則，具體包括以下三種情況：

36、超車：船舶超車時，應當避開被超車船舶；從船尾22.5度以上的方向接近另一艘船的船只是超車船；其后兩艘船只之間的航向發(fā)生任何改變，并不免除超車船只與被超車船只保持距離的責任，直至該船最終超車并離開為止；

37、對遇：當兩艘動力船舶在相向或接近相向的航線上相遇，有碰撞危險時，每一船應向右舷改變航線，從另一船的左舷通過；

38、橫穿：當兩艘動力船舶橫越有碰撞危險時，另一動力船舶在其右舷的船舶應避讓，在情況允許的情況下，應避免橫穿另一動力船舶的前方。

39、一種計算機裝置/設備/系統(tǒng)，包括存儲器、處理器及存儲在存儲器上的計算機程序，所述處理器執(zhí)行所述計算機程序以實現(xiàn)基于動態(tài)los的無人氣墊艇自主避障路徑規(guī)劃方法的步驟。

40、一種計算機可讀存儲介質(zhì)，其上存儲有計算機程序/指令，該計算機程序/指令被處理器執(zhí)行時實現(xiàn)基于動態(tài)los的無人氣墊艇自主避障路徑規(guī)劃方法的步驟。

41、一種計算機程序產(chǎn)品，包括計算機程序/指令，該計算機程序/指令被處理器執(zhí)行時實現(xiàn)基于動態(tài)los的無人氣墊艇自主避障路徑規(guī)劃方法的步驟。

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

43、本發(fā)明相比傳統(tǒng)的a*算法運算量更少，運行速度更快，相比于單一的los導引方法能夠?qū)崿F(xiàn)避障功能。同時本發(fā)明可以在無人氣墊艇路徑規(guī)劃領域廣泛推廣。