[發明專利]一種基于S57標準電子海圖的水下運載器多約束航路規劃方法

有效

• 申請號: 201410117057.X
• 申請日: 2014-03-27
• 公開（公告）號: CN103900573B
• 公開（公告）日: 2017-01-04
• 發明（設計）人: 劉廠;趙玉新;金娜;沈志峰;李剛;齊昭
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于S57標準電子海圖的水下運載器多約束航路規劃方法，其特征在于：

（1）確定規劃空間和要素提取空間：

（1.1）確定規劃空間：

確定規劃路徑的起點和終點，設為A(x_a,y_a,z_a)和G(x_g,y_g,z_g)，確定以AG為對角線的長方體ABCD-EFGH，即航行規劃任務的理論最小空間，設為空間V₀，長方體ABCD-EFGH的邊長為d，平面ABCD和平面EFGH的中心分別為O和O'，將ABCD面和EFGH面以O和O'為中心向外擴張成A'B'C'D'面和E'F'G'H'面，使擴張后平面邊長d'=d+kd,k∈[0,1]，設閾值d_min，當d<d_min時，令d'=d_min+kd_min航行最小深度h_min(h_min>0)和航行最大深度h_max(h_max>0)，確定h=h_max-h_min，立方體A'B'C'D'-E'F'G'H'是一個以h為高，上下平面邊長為d'的長方體；

（1.2）確定要素提取空間：

設最大水深為h'，使要素提取空間為以h'為高，上下平面邊長為d_min+k'd_min的長方體，其中k'∈[0,1]且k'>k；

（2）提取與轉換要素：

要素信息包括水深的經度、緯度、深度信息；點要素包括沉船、陸地區、暗礁適淹礁、傾廢場、障礙物，要素信息包括點要素數目，名稱，位置點個數以及每個位置點的經度、緯度、深度信息；線要素為陸地區，要素信息包括線要素數目，名稱，位置點個數以及每個位置點的經度、緯度、深度信息；面要素包括陸地區、障礙物、傾廢場、警告區，要素信息包括面要素數目，名稱，位置點個數以及每個位置點的經度、緯度、深度信息；

（3）航行空間生成：

（3.1）計算插值網格數

Ngrid=d1×d2,d1=abs(xs-xd)k1d2=abs(ys-yd)k2]]>

其中x_s，x_d，y_s，y_d分別為規劃空間的起點和終點的經度值、緯度值；

（3.2）讀取水深要素文件，提取水深要素位置數據；

（3.3）通過插值獲得網格水深：

對步驟（3.1）獲得的_Ngrid個網格按位置順序進行插值；

（3.4）對插值數據進行修正：

（3.4.1）疊加點要素：

找到點要素所在的網格，將網格的水深更改為0；

（3.4.2）疊加線要素：

讀取線要素中的位置點集p={p₁,p₂,...,p_n}，找到每一個位置點p_i所在的網格，將網格的水深更改為0，找到相鄰兩個位置點p_i與p_i+1連線經過的網格，將網格的水深更改為0；

（3.4.3）疊加面要素：

讀取面要素中的位置點集p={p₁,p₂,...,p_n}，連接點集的所有元素形成封閉圖形U，以水深數據網格點為中心，以2_dbuf為邊長生成判斷緩沖區S：

dbuf=abs(pd-ps)2×divNum,]]>

其中p_d和p_s為規劃空間的起點和終點在經度或緯度方向的位置數據，divNum為水深數據網格在經度或緯度方向的網格數，計算W=U∩S，如果W=U∩S≠θ則修改網格點的水深值為0；

（3.5）修補插值失敗網格：

檢測是否有插值失敗且沒有添加要素信息的區域，以檢測到的網格點為中心，d_buf為半徑的緩沖區搜索附近的水深值，如果緩沖區內沒有水深則增加緩沖區半徑使d_buf'=kd_buf，直到搜索到水深值，如果在緩沖區內搜索到多個水深值則通過求平均數對網格點進行賦值，搜索到的一組水深點為P={p₁,p₂,....p_n}，水深值P_hi為第i個水深點的水深值；

（3.6）將步驟（3.5）得到的網格水深寫入文件以供算法規劃使用；

（4）利用A*算法進行航路搜索：

（4.1）對水下運載器三維路徑規劃空間建模

進一步劃分規劃空間，形成最初的柵格網格，經度網格數為n，緯度網格數為m，深度網格數為h，對柵格網格添加障礙物信息，即計算每個網格點是否在陸地或障礙物禁航區內，計算直接定位柵格點對應平面：

n_X=floor((X(l,j)-start_X)/a_x)+1n_Y=floor((Y(l,j)-start_Y)/a_y)+1n_Z=floor((Z(l,j)-start_Z)/a_z)+1]]>

其中，start_X，start_Y和start_Z是地形數據在X和Y方向上的最小值，a_x、a_y和a_z是等分網格的平均距離，通過floor函數查找節點落在地形網格的位置序號n_X、n_Y和n_Z，確定X、Y、Z方向的序號，對每個節點進行編碼：

NodeNum=n×m×n_Z+n×n_Y+n_X

其中NodeNum為節點編碼序號，n為X方向序號的最大值，m為Y方向序號的最大值，

計算出節點距離平面的距離，垂直于平面的向量柵格點為O：

d=AO→·h→|h→|]]>

若d<D，則認為柵格點在禁航區內，其中D為安全距離，柵格網格形成后，對網格在X、Y、Z方向進行編碼，確定節點的位置；

（4.2）初始化OPEN表和CLOSED表：

把遍歷的節點放入一個表中保存，稱為OPEN表，比較、選擇后的節點放入另一個表中保存，為CLOSED表，表中的節點存有節點的信息，包括位置、啟發值、父節點，子節點；

（4.3）擴展子節點：

（4.3.1）按照三維空間的方向關系擴展出26個節點；

（4.3.2）判斷并削減超過航行深度范圍的節點，檢測擴展子節點是否達到空間縱向的邊界，最小下潛深度對應規劃空間Z方向編號為z₀，最大下潛深度為z_n，子節點位置中Z方向的編號為z_i，當z_i≤z₀或者z_i≥z_n時，刪除子節點；

（4.3.3）判斷并削減轉向角過大的節點，設當前擴展子節點為p_i，p_i的父節點為p_i-1，p_i-1的父節點為p_i-2，最大轉向角為θ_max，計算三個節點構成的兩條向量的夾角的余弦cosθ：

cosθ=pi-2pi-1→·pi-1pi→|pi-2pi-1→|·|pi-1pi→|]]>

若cosθ<cosθ_max，則轉向角過大，刪除該子節點；

（4.4）計算啟發值：

啟發值為f(s)=g(s)+h(s)，g(s)為從起點到當前點的實際距離，h(s)為估計值，h(s)為當前點到終點的直線距離，從父節點到擴展子節點的單位步長根據兩個點之間的方位關系應設為k,k為垂直方向單位步長的值：

g(s)=Σi=1mdih(s)=(xD-xm)×(yD-ym)×(zD-zm),]]>

其中d_i為每個節點距離其父節點的距離，即單位步長，m代表當前子節點，D代表終點，x、y、z分別為節點在X、Y、Z方向的序號；

（4.5）通過比較啟發值對OPEN表按照從小到大的順序進行排序，若新加入的節點在OPEN表和CLOSED表已經存在，則為舊節點，判斷新加入OPEN表和CLOSED表中的節點的g(s)值是否小于舊節點，如果小于則更新g(s)值，同時更新父節點，如果節點還在CLOSED表中則更新節點的子節點鏈表；

（4.6）選出OPEN表中啟發值最小的節點放入CLOSED表，若為終點則停止搜索，否則，返回步驟（4.3）；

（5）輸出規劃結果：將步驟四獲得的規劃結果存入文件。