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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

9、步驟6：以當(dāng)前航路點(diǎn)構(gòu)建los導(dǎo)引軌跡的期望時(shí)長軌跡，無人氣墊艇沿生成的軌跡運(yùn)行。

10、進(jìn)一步地，所述步驟1中的los導(dǎo)引軌跡生成具體為：

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

12、

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

14、

15、其中，的定義如下：

16、

17、πp與期望航向χd的關(guān)系為：

18、

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

20、

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

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

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

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

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

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

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

28、

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

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

31、

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

33、0≤tcpa≤trisk∩dcpa≤drisk

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

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

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

37、對(duì)遇：當(dāng)兩艘動(dòng)力船舶在相向或接近相向的航線上相遇，有碰撞危險(xiǎn)時(shí)，每一船應(yīng)向右舷改變航線，從另一船的左舷通過；

38、橫穿：當(dāng)兩艘動(dòng)力船舶橫越有碰撞危險(xiǎn)時(shí)，另一動(dòng)力船舶在其右舷的船舶應(yīng)避讓，在情況允許的情況下，應(yīng)避免橫穿另一動(dòng)力船舶的前方。

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

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

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

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

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