[發(fā)明專利]一種基于改進A星算法的無人艇全局路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202010004408.1
• 申請日: 2020-01-03
• 公開（公告）號: CN111060109B
• 公開（公告）日: 2021-08-27
• 發(fā)明（設(shè)計）人: 張濤;秦彥樑;王立輝;夏茂棟;張佳宇;張晨;張江源
• 申請（專利權(quán)）人: 東南大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20;G05D1/02
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 蔣昱

1.一種基于改進A星算法的無人艇全局路徑規(guī)劃方法，具體包括如下步驟，其特征在于：

(1)獲取全局海圖信息，設(shè)置無人艇起點與終點位置的經(jīng)緯度信息，判斷無人艇起點或終點位置是否為不可通行區(qū)域，若是則重新設(shè)置起點與終點的經(jīng)緯度信息；

(2)構(gòu)建網(wǎng)格地圖，確定經(jīng)緯度范圍，以經(jīng)緯度縱橫比動態(tài)柵格細化海圖，形成M*N的網(wǎng)格海圖，將起點與終點的經(jīng)緯度坐標轉(zhuǎn)換為笛卡爾坐標；判斷起點與終點之間的連線是否通過不可通行區(qū)域，若不存在不可通行區(qū)域，則路徑即為起點與終點的連線，否則進行改進A星算法尋找路徑；

步驟(2)所采用的動態(tài)柵格細化，首先在整個海圖進行粗劃分，找到一條較為粗略的路徑，在此路徑的基礎(chǔ)上再次細劃分，M值為經(jīng)度劃分的網(wǎng)格數(shù)，N值為緯度劃分的網(wǎng)格數(shù)，比較海圖經(jīng)度范圍和緯度范圍，取較大者劃分為固定格數(shù)，再按比例劃分較小者，完成尋徑后，若未達到導(dǎo)航精度要求但找到路徑，按找到的路徑的范圍再次柵格化，若未達到導(dǎo)航精度要求也未找到路徑則將M與N乘以倍數(shù)，以原經(jīng)緯度范圍再次柵格化；

具體如下：

(a)確定柵格化區(qū)域的經(jīng)緯度范圍，LONMAX、LONMIN分別為經(jīng)度最大值、最小值，LATMAX、LATMIN分別為緯度最大值、最小值；

(b)比較經(jīng)度范圍和緯度范圍，確定劃分網(wǎng)格數(shù)，取較大者劃分為100格，再按比例劃分較小者，M值為經(jīng)度劃分的網(wǎng)格數(shù)，N值為緯度劃分的網(wǎng)格數(shù)；

(c)若前次柵格化網(wǎng)格圖未尋找到路徑，令M值與N值為原M值與原N值的2倍；

(d)運用map工具箱vec2mtx函數(shù)從西向開始將經(jīng)度范圍劃分網(wǎng)格數(shù)M，從南向開始將緯度范圍劃分網(wǎng)格數(shù)N，劃分M*N網(wǎng)格地圖并確定xstep與ystep，單位柵格橫向邊長實際代表的經(jīng)度度數(shù)xstep為柵格化區(qū)域經(jīng)度范圍差除以劃分網(wǎng)格數(shù)M，單位柵格縱向邊長實際代表的緯度度數(shù)ystep為柵格化區(qū)域緯度范圍差除以劃分網(wǎng)格數(shù)N；

(e)將起點與終點的經(jīng)緯度坐標轉(zhuǎn)換為笛卡爾坐標，經(jīng)緯度坐標(lon，lat)，橫坐標x為ceil((lon-LONMIN)/xstep)，縱坐標y為ceil((lat-LATMIN)/ystep)；

(f)判斷起點與終點之間的連線是否通過不可通行區(qū)域，若不存在不可通行區(qū)域，則路徑即為起點與終點的連線，否則進行改進A星算法尋找路徑；

(3)改進A星算法尋找路徑，創(chuàng)建OPEN集與CLOSE集，OPEN集用于存放待檢測的節(jié)點，CLOSE集用于存放已檢測過的節(jié)點；OPEN集與CLOSE集中每個節(jié)點有父節(jié)點、F值、G值、H值；父節(jié)點為路徑中通過該節(jié)點所經(jīng)過的上一節(jié)點，G值為從起點移動到當前節(jié)點的代價，H值為從當前節(jié)點移動到終點的估計代價，F(xiàn)值是G值與H值之和；遍歷OPEN集，查找F值最小的節(jié)點，將此節(jié)點作為當前要處理的節(jié)點；若將終點加入到OPEN集中，則路徑已經(jīng)找到，回溯路徑，將從終點開始沿著父節(jié)點移動直至起點的節(jié)點依次加入PATH集；若未將終點加入到OPEN集中且OPEN集已空，則未找到路徑；

步驟(3)所述改進A星算法尋找路徑，遍歷OPEN集，查找F值最小的節(jié)點，若出現(xiàn)多個節(jié)點F值最小值相同的情況，選擇H值較小者，若F值與H值皆相等，優(yōu)先選擇后加入OPEN集的節(jié)點；將此節(jié)點作為當前要處理的節(jié)點，依據(jù)擴展的16鄰域矩陣，以當前節(jié)點下方開始順時針順序查找該節(jié)點相鄰的節(jié)點，若當前節(jié)點為奇數(shù)節(jié)點，即橫縱坐標之和為奇數(shù)的節(jié)點，以相反的順序查找當前節(jié)點的鄰點，忽略已經(jīng)加入CLOSE集或者不可通行的節(jié)點，刪去受阻礙的位于對角線位置的鄰點，如當前節(jié)點的上方或右方不可通行，則右上方節(jié)點不可選；如果相鄰節(jié)點不在OPEN集中，將此相鄰節(jié)點加入OPEN集，并且把當前節(jié)點設(shè)置為節(jié)點的父節(jié)點，記錄該節(jié)點的F，G和H值；如果此相鄰節(jié)點已在OPEN集，并且經(jīng)由當前節(jié)點到達此相鄰節(jié)點G值小于原G值，則把此相鄰節(jié)點父節(jié)點設(shè)置為當前節(jié)點，并重新計算G值與F值；若將終點加入到OPEN集中，則路徑已經(jīng)找到，回溯路徑，將從終點開始沿著父節(jié)點移動直至起點的節(jié)點依次加入PATH集；若未將終點加入到OPEN集中且OPEN集已空，則未找到路徑；

步驟(3)中計算H值的啟發(fā)式函數(shù)的改進，曼哈頓距離是當前節(jié)點與終點橫縱坐標差的絕對值之和，是A星算法常用的代價估計，為避免鄰域中代價相同而造成的路徑不確定性和抖動現(xiàn)象，將橫縱坐標差加權(quán)后取和，X，Y為當前節(jié)點與終點橫坐標之差和縱坐標之差的絕對值；以終點為坐標原點，以X＝Y(jié)作為分割線，兩側(cè)分區(qū)對引導(dǎo)作用的要求相反，進一步將max(X,Y)與min(X,Y)加權(quán)求和，引入角度信息cross，即起點到終點的矢量與當前節(jié)點到終點的矢量的叉乘的模，X2，Y2為起點與終點橫坐標之差和縱坐標之差的絕對值，D為起點與終點的距離，cross＝abs(X*X2-Y*Y2)，結(jié)合距離和角度的新啟發(fā)式函數(shù)H＝a*max(X,Y)+b*min(X,Y)+c*cross/D；

(4)判斷是否達到導(dǎo)航精度要求，即單位網(wǎng)格邊長實際經(jīng)緯度數(shù)，若達到要求且找到路徑，則進行平滑路徑處理；若達到要求但未找到路徑，則表明當前分辨率下無法找到路徑；若未達到導(dǎo)航精度要求但找到路徑，按找到的路徑的范圍再次柵格化，若未達到導(dǎo)航精度要求也未找到路徑則將M與N乘以同一倍數(shù)，以原經(jīng)緯度范圍再次柵格化；

其中判斷是否達到導(dǎo)航精度要求，即xstep與ystep兩者都不大于要求的導(dǎo)航精度STEP，其中xstep為單位柵格橫向邊長實際代表的經(jīng)度度數(shù)，ystep為單位柵格縱向邊長實際代表的緯度度數(shù)，STEP按需求自己設(shè)定，則轉(zhuǎn)步驟(5)；若未達到要求，按以下步驟處理：

(a)查詢路徑查找失敗標志符以判斷是否尋找到路徑，若找到路徑，轉(zhuǎn)步驟(4)的(b)步驟，否則轉(zhuǎn)步驟(4)的(c)步驟；

(b)若找到路徑，依據(jù)PATH集中路徑節(jié)點橫縱坐標最大值與最小值XMAX、XMIN、YMAX、YMIN，重新確定柵格化區(qū)域，最小緯度LATMIN為原最小緯度LATMIN加ystep與(YMIN-1)的乘積，最大緯度LATMAX為原最大緯度LATMAX加ystep與YMAX的乘積，最小經(jīng)度LONMIN為原最小經(jīng)度LONMIN加xstep與(XMIN-1)的乘積，最大經(jīng)度LONMAX為原最大經(jīng)度LONMAX加xstep與XMAX的乘積，轉(zhuǎn)步驟(2)的(a)步驟，以此經(jīng)緯度范圍再次劃分網(wǎng)格；

(c)若未找到路徑，轉(zhuǎn)步驟(2)的(c)步驟，令M值與N值為原M值與原N值的2倍，以原經(jīng)緯度范圍再次劃分M*N網(wǎng)格；

(5)平滑路徑，刪去PATH集中保存的共線的節(jié)點以及多余轉(zhuǎn)折點，只保留必要轉(zhuǎn)折點的位置，并利用網(wǎng)格圖的經(jīng)緯度范圍大小，M值與N值，將PATH集中節(jié)點橫縱坐標轉(zhuǎn)換為經(jīng)緯度坐標。