一種基于動(dòng)態(tài)規(guī)劃的水下航行器路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于水下航行器路徑規(guī)劃技術(shù)領(lǐng)域，具體涉及一種基于動(dòng)態(tài)規(guī)劃的水下航 行器路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 路徑規(guī)劃是一種在某一特定環(huán)境下，從設(shè)定的起始點(diǎn)到終點(diǎn)規(guī)劃出一條可行的路 徑的方法，這條路徑一定是在某種約束下的最優(yōu)或次優(yōu)解，這種約束可以是時(shí)間最優(yōu)或威 脅最小或能耗最少等條件。路徑規(guī)劃已經(jīng)應(yīng)用于各種場(chǎng)合，例如移動(dòng)機(jī)器人路徑規(guī)劃、無人 機(jī)路徑規(guī)劃，水下航行器路徑規(guī)劃。路徑規(guī)劃可以綜合考慮各種環(huán)境狀況，例如面臨的各種 障礙、威脅，規(guī)劃出一條時(shí)間最優(yōu)或者能耗最優(yōu)的路徑，對(duì)于水下航行器路徑規(guī)劃也已經(jīng)有 很多研究，然而對(duì)于低速水下航行器，例如水下滑翔機(jī)，在海洋中遠(yuǎn)比其它的水下航行器受 洋流的影響大，因此很有可能受洋流的運(yùn)動(dòng)而偏離既定的航跡很遠(yuǎn)，而且洋流速度的大小、 方向也不是固定不變的，而是隨時(shí)間及各種因素狀況不斷調(diào)整的。

【發(fā)明內(nèi)容】

[0003] 為了避免現(xiàn)有技術(shù)的不足之處，本發(fā)明提出一種基于動(dòng)態(tài)規(guī)劃的水下航行器路徑 規(guī)劃方法，針對(duì)傳統(tǒng)水下航行器路徑規(guī)劃未能考慮低速水下航行器受洋流影響較大的情 況，根據(jù)可預(yù)測(cè)或已知的時(shí)變洋流設(shè)計(jì)了適用于單個(gè)水下航行器路徑規(guī)劃方法。
[0004] 一種基于動(dòng)態(tài)規(guī)劃的水下航行器路徑規(guī)劃方法，其特征在于步驟如下：
[0005] 步驟1、將路徑規(guī)劃空間柵格化：將規(guī)劃區(qū)域的橫向等間距分為n份，縱向等間距 分為m份，這樣就形成了nXm個(gè)點(diǎn)，以每列的點(diǎn)為一個(gè)搜索階段，按順序組成n個(gè)階段；以 第一個(gè)階段的第一個(gè)點(diǎn)為搜索起始點(diǎn)，以最后階段最后一個(gè)點(diǎn)為終點(diǎn)；
[0006] 步驟2 :在航行器起始點(diǎn)開始，根據(jù)已知的洋流模型，獲取在初始時(shí)刻tstart、初始 點(diǎn)Search_start_point所在位置的洋流速度利用速度合成模型判斷兩個(gè) 搜索點(diǎn)之間能否通過；
[0007] 速度合成模型為：
【主權(quán)項(xiàng)】
1. 一種基于動(dòng)態(tài)規(guī)劃的水下航行器路徑規(guī)劃方法，其特征在于步驟如下： 步驟1、將路徑規(guī)劃空間柵格化：將規(guī)劃區(qū)域的橫向等間距分為η份，縱向等間距分為m 份，這樣就形成了 nXm個(gè)點(diǎn)，以每列的點(diǎn)為一個(gè)搜索階段，按順序組成η個(gè)階段；以第一個(gè) 階段的第一個(gè)點(diǎn)為搜索起始點(diǎn)，以最后階段最后一個(gè)點(diǎn)為終點(diǎn)；
步驟2 :在航行器起始點(diǎn)開始，根據(jù)已知的洋流模型，獲取在初始時(shí)刻tstart、初始點(diǎn) Search_start_point所在位置的洋流速度利用速度合成模型判斷兩個(gè)搜 索點(diǎn)之間能否通過； 速度合成模型為 如果Λ < 0,兩點(diǎn)之間路徑不可通； 如果Λ >0,計(jì)算兩個(gè)搜索點(diǎn)之間的速度
如果彡〇,兩點(diǎn)之間路徑不可通； 式中：表示兩個(gè)搜索點(diǎn)之間的單位矢量；Vvdlic^為航行器的速度，V 是洋 流在某一時(shí)刻的速度矢量； 如果'_> 〇, V 為兩個(gè)搜索點(diǎn)之間的速度； 步驟3 :計(jì)_
，以為粗略估計(jì)的時(shí)間tMUgh_MUte; 步驟4 :以tMUgh MUte+tstart為時(shí)間值從洋流模型中獲取在初始點(diǎn)的下一個(gè)搜索點(diǎn)的 V〇cean-current-en d速度； 少馬來 5 :以(V〇cean-current-start+V〇cean-current-end) Z2 作為 V〇c旦an' 再次計(jì)算
，得到兩個(gè)階段搜索點(diǎn)之間的相對(duì) trough_"ute更精確的時(shí)間t _U MUte，以tacxu _為搜索權(quán)重；以此計(jì)算一個(gè)階段的所有點(diǎn)到下 一個(gè)階段的所有點(diǎn)之間的時(shí)間，并把該時(shí)間作為兩點(diǎn)間的搜索權(quán)重；然后以tacxuMUte+tstart 作為Search_end_point這一點(diǎn)的時(shí)刻值，在起始時(shí)刻為0的條件下，應(yīng)用動(dòng)態(tài)規(guī)劃算法算 出最初起始點(diǎn)到這一搜索點(diǎn)的最早時(shí)刻，然后將其作為下一個(gè)搜索點(diǎn)的t start，以此作為獲 取時(shí)變洋流速度的變量之一；在下一步搜索中，以Search_end_point為下一步的初始點(diǎn)， 以 1^。。11_1^+1^￡^作為初始點(diǎn)所在時(shí)亥 |1，直至363^11_611(^〇;[111:為航行器的規(guī)劃終點(diǎn)； 步驟6、應(yīng)用動(dòng)態(tài)規(guī)劃算法找出從初始點(diǎn)到第i階段所有點(diǎn)之間的距離：f [SiJ表示從 第i-Ι階段所有點(diǎn)中找出使SiJ最小的點(diǎn)，作為f[Si，j]: f [si；J] = min{f[Si_1；k]+w(Si^ljk, si；J)} 其中：《(s^k，Si，P表示第i-1階段的第k個(gè)點(diǎn)到第i階段的第j個(gè)點(diǎn)所耗費(fèi)時(shí)間； 然后開始進(jìn)行第i到i+Ι階段的搜索，首先通過2-5步驟計(jì)算出i階段所有點(diǎn)到i+1 階段所有點(diǎn)之間的時(shí)間，然后找出初始點(diǎn)到第i+1階段所有點(diǎn)之間的距離，往后遞推計(jì)算， 直至到最后階段，最后f[S4(U(1]即為所求最短路徑； 步驟7:將每個(gè)搜索階段中找出的一個(gè)點(diǎn)，從起始點(diǎn)到最終點(diǎn)首位順序連接出一條線 段，為規(guī)劃路徑，然后根據(jù)步驟2-5求得兩個(gè)點(diǎn)之間所需時(shí)間。
【專利摘要】本發(fā)明涉及一種基于動(dòng)態(tài)規(guī)劃的水下航行器路徑規(guī)劃方法，對(duì)于低速水下航行器受到洋流影響較大，很大程度上會(huì)收到洋流的影響而偏離預(yù)定航跡很遠(yuǎn)，而現(xiàn)有的水下航行器都沒能夠考慮這一影響，而且洋流是復(fù)雜、時(shí)變的。本發(fā)明在洋流的未來狀況已知的情況下，先將所要規(guī)劃的區(qū)域柵格化然后利用速度合成模型來計(jì)算搜索點(diǎn)之間所耗費(fèi)的時(shí)間值，并以時(shí)間值作為權(quán)重，應(yīng)用動(dòng)態(tài)規(guī)劃算法在二維平面內(nèi)規(guī)劃出一條可達(dá)最優(yōu)路徑。本發(fā)明能夠根據(jù)已知的洋流的情況，利用動(dòng)態(tài)規(guī)劃快速規(guī)劃出一條可行路徑，仿真驗(yàn)證了該算法具有良好的效果。
【IPC分類】G05D1-02
【公開號(hào)】CN104536442
【申請(qǐng)?zhí)枴緾N201410766346
【發(fā)明人】劉明雍, 許寶貴, 董婷婷, 高沛林, 武小金, 閆闊
【申請(qǐng)人】西北工業(yè)大學(xué)
【公開日】2015年4月22日
【申請(qǐng)日】2014年12月11日