本發(fā)明涉及路徑規(guī)劃,更具體的說是涉及一種基于海洋數(shù)字運(yùn)載器的dwa和apf路徑規(guī)劃方法。
背景技術(shù):
1、在無人汽車發(fā)展的今天,海面領(lǐng)域的無人船路徑規(guī)劃在當(dāng)今無人駕駛汽車技術(shù)飛速發(fā)展的時(shí)代,無人駕駛技術(shù)的應(yīng)用已經(jīng)不再局限于陸地。海面領(lǐng)域的無人船技術(shù)同樣具有巨大的潛力和廣闊的應(yīng)用前景。因此,加速無人船路徑規(guī)劃技術(shù)的發(fā)展,對于推動(dòng)海洋經(jīng)濟(jì)的轉(zhuǎn)型升級和海洋資源的開發(fā)利用具有重要意義。無人船路徑規(guī)劃技術(shù)的進(jìn)步,能夠?yàn)楹Q笥^測、監(jiān)測和勘探等領(lǐng)域提供高效、精確的解決方案。通過路徑規(guī)劃和自主導(dǎo)航,無人船可以在復(fù)雜的海洋環(huán)境中完成數(shù)據(jù)采集、環(huán)境監(jiān)測等任務(wù),大大降低人力成本和風(fēng)險(xiǎn)。高效的路徑規(guī)劃技術(shù)可以提高無人船的運(yùn)輸效率,降低運(yùn)營成本,同時(shí)也能在緊急情況下迅速抵達(dá)目的地,提高救援效率。路徑規(guī)劃是無人船自主任務(wù)面對的核心問題,一個(gè)合格的路徑規(guī)劃算法需要滿足無人船高效、安全的工作需求。路徑規(guī)劃算法希望能夠在有限的時(shí)間內(nèi)找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。路徑規(guī)劃算法能夠避免無人車與其他車輛、行人、障礙物發(fā)生碰撞。安全性是無人車自主駕駛的首要要求,路徑規(guī)劃算法必須能夠充分考慮周圍環(huán)境的變化,及時(shí)規(guī)劃出避障路徑。
2、局部路徑規(guī)劃算法負(fù)責(zé)在實(shí)時(shí)環(huán)境中規(guī)劃避障路徑,通常采用dwa(動(dòng)態(tài)窗口算法),人工勢場算法等。面對障礙物能夠很好的進(jìn)行避障,再融合導(dǎo)入到數(shù)字平臺(tái)中,進(jìn)行較好的路徑的規(guī)劃,dwa算法能夠在每個(gè)控制周期內(nèi)快速計(jì)算出最佳航速,使得運(yùn)載器能夠?qū)崟r(shí)響應(yīng)環(huán)境變化,適合在動(dòng)態(tài)環(huán)境中使用。apf算法通常能夠生成連續(xù)和平滑的運(yùn)動(dòng)路徑,減少了運(yùn)載器的突然轉(zhuǎn)向和加速,提高了行駛的平穩(wěn)性,兩者結(jié)合形成完善的避障措施。然而,現(xiàn)有的dwa算法容易出現(xiàn)陷入局部最優(yōu)解,在目標(biāo)點(diǎn)附近有障礙物時(shí),運(yùn)載器可能會(huì)陷入局部最小值,無法找到直接到達(dá)目標(biāo)點(diǎn)的路徑。因此,如何提供一種基于海洋數(shù)字運(yùn)載器的dwa和apf路徑規(guī)劃方法是本領(lǐng)域技術(shù)人員亟需解決的問題。
技術(shù)實(shí)現(xiàn)思路
1、有鑒于此,本發(fā)明提供了一種基于海洋數(shù)字運(yùn)載器的dwa和apf路徑規(guī)劃方法,解決dwa算法在進(jìn)行局部路徑規(guī)劃容易出現(xiàn)陷入局部最優(yōu)解的缺陷,提高運(yùn)載器路徑規(guī)劃的可行性和安全性。
2、為了實(shí)現(xiàn)上述目的,本發(fā)明提供如下技術(shù)方案:
3、一種基于海洋數(shù)字運(yùn)載器的dwa和apf路徑規(guī)劃方法,包括以下步驟:
4、s1、根據(jù)海洋數(shù)字運(yùn)載器接口的輸入信息,初始化海洋數(shù)字運(yùn)載器和地圖信息,利用柵格法建造地圖,仿真模擬地形和障礙物信息;
5、s2、采用改進(jìn)dwa算法進(jìn)行局部路徑規(guī)劃,提取并更新路徑和運(yùn)載器的狀態(tài)信息;
6、s3、判斷海洋數(shù)字運(yùn)載器是否達(dá)到目標(biāo)點(diǎn)附近,如果達(dá)到則調(diào)用apf算法進(jìn)行路徑規(guī)劃,否則繼續(xù)采用改進(jìn)dwa算法進(jìn)行局部路徑規(guī)劃;
7、s4、海洋數(shù)字運(yùn)載器到達(dá)目標(biāo)點(diǎn)后,將路徑和海洋數(shù)字運(yùn)載器參數(shù)信息輸入到海洋數(shù)字運(yùn)載器平臺(tái),進(jìn)行路徑顯示。
8、可選的,s1中海洋數(shù)字運(yùn)載器基于海圖數(shù)據(jù)轉(zhuǎn)化成的地圖信息制作一個(gè)新的地圖信息用于后續(xù)的路徑規(guī)劃,規(guī)劃完成后到處所需要的數(shù)據(jù)。
9、可選的,改進(jìn)dwa算法具體為:設(shè)置海洋數(shù)字運(yùn)載器的初始狀態(tài),包括初始位置、航速和目標(biāo)點(diǎn)位置,設(shè)定改進(jìn)dwa算法的參數(shù),包括航速和航向角的范圍、時(shí)間步長、預(yù)測窗口,在當(dāng)前航速的基礎(chǔ)上,在預(yù)設(shè)的航速和航向角范圍內(nèi)進(jìn)行采樣,生成一系列局部路徑,對每條采樣路徑進(jìn)行評價(jià),根據(jù)評價(jià)結(jié)果,選擇綜合評價(jià)最高的路徑,確定下一時(shí)刻海洋數(shù)字運(yùn)載器的航速和航向角,海洋數(shù)字運(yùn)載器根據(jù)選定的航速和航向角執(zhí)行動(dòng)作。
10、可選的,改進(jìn)dwa算法中計(jì)算動(dòng)態(tài)窗口具體為:
11、vs={(v,w)|v∈[vmin,vmax],w∈[wmin,wmax]}
12、vd={(v,w)|v∈[vc-vadt,vc+vadt],w∈[wc-wadt,wc+wadt]}
13、其中,vmin、vmax、wmin、wmax、vc、va、wc、wa和dt分別為海洋數(shù)字運(yùn)載器所允許的最小航速,最大航速,最小角速度,最大角速度,當(dāng)前航速,最大加速度,當(dāng)前航向角,最大角加速度和時(shí)間步長。
14、可選的,改進(jìn)dwa算法中生成一系列局部路徑具體為:
15、在改進(jìn)dwa算法的動(dòng)態(tài)窗口中均勻采樣,可選航速組合(v,w)的數(shù)量n為:
16、n=[(vmax-vmin)/mv]·[(wmax-wmin)/mw]
17、式中,mv、mw分別為航速分辨率和角速度分辨率;
18、xk+1=xk+v·cosθ·δt
19、yk+1=xk+v·sinθ·δt
20、θk+1=θk+w·δt
21、式中,(x,y,θ)為當(dāng)前海洋數(shù)字運(yùn)載器的位置以及航向角。
22、可選的,改進(jìn)dwa算法的評價(jià)函數(shù)由航向評價(jià)函數(shù),航速評價(jià)函數(shù),障礙物評價(jià)函數(shù)加權(quán)組成,其中,航向評價(jià)函數(shù)基于海洋數(shù)字運(yùn)載器與目標(biāo)之間的航向差評估,航速評價(jià)函數(shù)根據(jù)海洋數(shù)字運(yùn)載器未來時(shí)間內(nèi)最高航速進(jìn)行評價(jià),障礙物評價(jià)函數(shù)基于海洋數(shù)字運(yùn)載器與障礙物之間最大距離進(jìn)行評價(jià)。
23、可選的,改進(jìn)dwa算法的評價(jià)函數(shù)feval(v,w)具體為:
24、
25、式中,goal(v,w)為航向評價(jià)函數(shù),vel(v)為航速評價(jià)函數(shù),obs(v,w)為障礙物評價(jià)函數(shù),cgoal、cvel、cobs分別為航向評價(jià)函數(shù)、航速評價(jià)函數(shù)、障礙物評價(jià)函數(shù)所對應(yīng)的權(quán)重系數(shù),t為步長時(shí)間。
26、可選的,apf算法具體為:
27、計(jì)算當(dāng)前點(diǎn)與障礙物、目標(biāo)點(diǎn)之間的連線平均角度theta:
28、
29、式中,x1為當(dāng)前點(diǎn)的位置,x2為障礙物與目標(biāo)點(diǎn)的位置,t(x1,x2)計(jì)算兩點(diǎn)之間的向量,進(jìn)行向量和得出航向信息;
30、計(jì)算當(dāng)前點(diǎn)與目標(biāo)點(diǎn)之間的引力勢能ua:
31、
32、式中,ka為引力增益函數(shù),p1為當(dāng)前點(diǎn),p2為目標(biāo)點(diǎn),f(p1,p2)為當(dāng)前點(diǎn)與目標(biāo)點(diǎn)之間的距離;
33、計(jì)算障礙物與當(dāng)前點(diǎn)之間的斥力勢能ur:
34、
35、式中,kr為斥力增益函數(shù),dis為障礙物影響范圍的閾值,q1為障礙物坐標(biāo),q2為當(dāng)前點(diǎn)坐標(biāo);
36、疊加引力勢能和斥力勢能得到總勢場,根據(jù)總勢場力進(jìn)行方向和航速的變化,得到路徑信息。
37、可選的,海洋數(shù)字運(yùn)載器平臺(tái)在全局態(tài)勢界面展示出自動(dòng)避碰算法輸出的避碰航路,在實(shí)景感知界面展示期望航路與避碰航路的輸出,并展示出該期望航路的期望航向與航速。
38、經(jīng)由上述的技術(shù)方案可知,與現(xiàn)有技術(shù)相比,本發(fā)明提供了一種基于海洋數(shù)字運(yùn)載器的dwa和apf路徑規(guī)劃方法,具有以下有益效果:
39、1.本發(fā)明在傳統(tǒng)dwa算法中,增加距離自適應(yīng)權(quán)重調(diào)節(jié)各項(xiàng)評價(jià)影響,有效改善了dwa算法在面對障礙物時(shí)的軌跡與航速振蕩;
40、2.本發(fā)明將海圖與模擬圖進(jìn)行結(jié)合,能夠輸入經(jīng)緯度進(jìn)行柵格地圖的模擬和生成,也能節(jié)省部分內(nèi)存空間,提高算法效率;
41、3.本發(fā)明將改進(jìn)的dwa算法和apf算法進(jìn)行融合,對評價(jià)函數(shù)進(jìn)行進(jìn)一步的調(diào)節(jié),使dwa算法能夠高效的躲避障礙物,融合apf算法避免dwa陷入局部最小值情況。