一種基于航路點(diǎn)的uuv轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法
【專利摘要】本發(fā)明公開了一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法。從使命文本讀取UUV的前一航路點(diǎn)、當(dāng)前航路點(diǎn)、下一航路點(diǎn)以及規(guī)劃速度，將規(guī)劃速度作為速度指令輸出；計(jì)算出當(dāng)前航路角；計(jì)算當(dāng)前航路的轉(zhuǎn)彎半徑；慣性導(dǎo)航儀實(shí)時(shí)采集UUV的實(shí)際位置；判斷UUV是否滿足啟用轉(zhuǎn)彎速度的條件，如果滿足條件，計(jì)算并輸出轉(zhuǎn)彎速度指令；光纖羅經(jīng)實(shí)時(shí)采集UUV的實(shí)際航向；判斷UUV是否到達(dá)當(dāng)前航路點(diǎn)，如果到達(dá)當(dāng)前航路點(diǎn)，判斷使命文本中的航路點(diǎn)是否都已到達(dá)，如果都到達(dá)，任務(wù)結(jié)束。本發(fā)明可以減少UUV轉(zhuǎn)彎時(shí)的航路超調(diào)，改善航路跟蹤效果。
【專利說明】一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于一種UUV速度調(diào)整方法，尤其涉及UUV航行轉(zhuǎn)彎時(shí)的一種基于航路點(diǎn) 的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法。

【背景技術(shù)】
[0002] 目前，水下無人航行器（UnmannedUnderwaterVehicle-UUV)執(zhí)行使命時(shí)最常用 的航行路徑是以多個(gè)航路點(diǎn)聯(lián)接而成的直線段路徑。當(dāng)UUV從一個(gè)航路轉(zhuǎn)換到另一個(gè)航路 時(shí)就必須要執(zhí)行轉(zhuǎn)彎動(dòng)作。目前UUV進(jìn)行水下航行任務(wù)時(shí)，接收的使命文本中僅包含航路 點(diǎn)的位置信息和規(guī)劃出的UUV速度信息，使命文本中并沒有包含轉(zhuǎn)彎速度信息，所以通常 的做法是設(shè)定一個(gè)固定的轉(zhuǎn)彎半徑，當(dāng)UUV與當(dāng)前航路點(diǎn)的距離小于等于轉(zhuǎn)彎半徑時(shí)，UUV 開始轉(zhuǎn)彎，但并不對(duì)轉(zhuǎn)彎時(shí)的速度進(jìn)行調(diào)整，主要是不降速，整個(gè)航行過程均采用統(tǒng)一的規(guī) 劃速度。這樣的做法存在以下兩個(gè)問題：（1)固定的轉(zhuǎn)彎半徑并不適合所有的轉(zhuǎn)彎角（兩 個(gè)航路之間的航路角）情況；（2)將會(huì)導(dǎo)致UUV以較大的速度進(jìn)行轉(zhuǎn)彎。以上兩個(gè)問題綜 合起來將會(huì)造成UUV出現(xiàn)急轉(zhuǎn)彎，使轉(zhuǎn)彎效果變差，能耗增加并且產(chǎn)生航路超調(diào)。特別地， 當(dāng)轉(zhuǎn)彎角度較大時(shí)（直角或鈍角度時(shí)），更加明顯。


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

[0003] 本發(fā)明的目的是提供一種能夠減少轉(zhuǎn)彎航路超調(diào)、改善航路跟蹤效果的基于航路 點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法。
[0004] 本發(fā)明是通過以下技術(shù)方案實(shí)現(xiàn)的：
[0005] 一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，包括以下幾個(gè)步驟：
[0006]步驟一：從使命文本讀取UUV的前一航路點(diǎn)Plrt的位置（Xη，yg)、當(dāng)前航路點(diǎn)pk 的位置（xk，yk)、下一航路點(diǎn)Pk+^位置（xk+1，yk+1)以及規(guī)劃速度信息，將規(guī)劃速度信息作為 UUV速度指令輸出，根據(jù)讀取的信息計(jì)算出當(dāng)前航路角ak;
[0007]步驟二：根據(jù)當(dāng)前航路角ak，計(jì)算UUV當(dāng)前航路的轉(zhuǎn)彎半徑Rk;
[0008] 步驟三：慣性導(dǎo)航儀實(shí)時(shí)采集UUV的實(shí)際位置（xuuv，yuuv);
[0009]步驟四：根據(jù)UUV的實(shí)際位置（xuuv,yuuv)和當(dāng)前航路點(diǎn)pk的位置（xk,yk)，判斷 UUV是否滿足啟用轉(zhuǎn)彎速度的條件，如果滿足條件，進(jìn)行步驟五，如果不滿足條件，重復(fù)步驟 三?步驟四；
[0010] 步驟五：根據(jù)當(dāng)前航路角ak，計(jì)算并輸出UUV轉(zhuǎn)彎速度指令；
[0011] 步驟六：光纖羅經(jīng)實(shí)時(shí)采集UUV的實(shí)際航向
[0012] 步驟七：根據(jù)UUV的實(shí)際航向Φuuv、實(shí)際位置（xuuv,yuuv)、當(dāng)前航路點(diǎn)Pk的位置 (xk，yk)、下一航路點(diǎn)Pk+^位置（Xk+1，yk+1)以及當(dāng)前航路的轉(zhuǎn)彎半徑Rk，判斷UUV是否到達(dá) 當(dāng)前航路點(diǎn)，如果到達(dá)當(dāng)前航路點(diǎn)，轉(zhuǎn)步驟八，如果沒有到達(dá)當(dāng)前航路點(diǎn)，重復(fù)步驟六?步 驟七；
[0013] 步驟八：判斷使命文本中的航路點(diǎn)是否都已到達(dá)，如果都到達(dá)，任務(wù)結(jié)束，如果沒 有，重復(fù)步驟一?步驟八。
[0014] 本發(fā)明一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，還可以包括：
[0015] 1、當(dāng)前航路角ak為：
[0016]

【權(quán)利要求】
1. 一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，其特征在于，包括以下幾個(gè)步驟： 步驟一：從使命文本讀取UUV的前一航路點(diǎn)Ph的位置（xH，yg)、當(dāng)前航路點(diǎn)pk的位 置（xk，yk)、下一航路點(diǎn)？1;+1的位置（xk+1，yk+1)以及規(guī)劃速度信息，將規(guī)劃速度信息作為UUV速度指令輸出，根據(jù)讀取的信息計(jì)算出當(dāng)前航路角ak; 步驟二：根據(jù)當(dāng)前航路角ak，計(jì)算UUV當(dāng)前航路的轉(zhuǎn)彎半徑Rk; 步驟三：慣性導(dǎo)航儀實(shí)時(shí)采集UUV的實(shí)際位置（xuuv，yuuv); 步驟四：根據(jù)UUV的實(shí)際位置（xuuv，yuuv)和當(dāng)前航路點(diǎn)pk的位置（xk，yk)，判斷UUV是 否滿足啟用轉(zhuǎn)彎速度的條件，如果滿足條件，進(jìn)行步驟五，如果不滿足條件，重復(fù)步驟三? 步驟四； 步驟五：根據(jù)當(dāng)前航路角ak，計(jì)算并輸出UUV轉(zhuǎn)彎速度指令； 步驟六：光纖羅經(jīng)實(shí)時(shí)采集UUV的實(shí)際航向ituuv; 步驟七：根據(jù)UUV的實(shí)際航向步uuv、實(shí)際位置（xuuv,yuuv)、當(dāng)前航路點(diǎn)pk的位置（xk,yk)、 下一航路點(diǎn)Pk+1的位置（xk+1，yk+1)以及當(dāng)前航路的轉(zhuǎn)彎半徑Rk，判斷UUV是否到達(dá)當(dāng)前航路 點(diǎn)，如果到達(dá)當(dāng)前航路點(diǎn)，轉(zhuǎn)步驟八，如果沒有到達(dá)當(dāng)前航路點(diǎn)，重復(fù)步驟六?步驟七； 步驟八：判斷使命文本中的航路點(diǎn)是否都已到達(dá)，如果都到達(dá)，任務(wù)結(jié)束，如果沒有，重 復(fù)步驟一?步驟八。
2. 根據(jù)權(quán)利要求1所述的一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，其特征在 于： 所述的當(dāng)前航路角ak為：
3. 根據(jù)權(quán)利要求1所述的一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，其特征在 于： 所述的UUV當(dāng)前航路的轉(zhuǎn)彎半徑Rk為：
其中，Rmax為UUV轉(zhuǎn)彎半徑的最大限值，Rmin為UUV轉(zhuǎn)彎半徑的最小限值，〇 半徑因 子。
4. 根據(jù)權(quán)利要求1所述的一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，其特征在 于： 所述的判斷UUV是否滿足啟用轉(zhuǎn)彎速度的條件為：
5. 根據(jù)權(quán)利要求1所述的一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，其特征在 于： 所述的轉(zhuǎn)彎速度指令Vcmd_k為：
其中，Vmax為UUV轉(zhuǎn)彎速度指令的最大限值，vmin為UUV轉(zhuǎn)彎速度指令的最小限值，Cv 為速度因子。
6.根據(jù)權(quán)利要求1所述的一種基于航路點(diǎn)的UUV轉(zhuǎn)彎速度自適應(yīng)調(diào)整方法，其特征在 于： 所述的判斷UUV是否到達(dá)當(dāng)前航路點(diǎn)的條件為：
其中，屯為航向偏差閾值。
【文檔編號(hào)】G05D1/10GK104483975SQ201410617653
【公開日】2015年4月1日 申請(qǐng)日期:2014年11月5日 優(yōu)先權(quán)日:2014年11月5日
【發(fā)明者】陳濤, 張耕實(shí), 嚴(yán)浙平, 徐達(dá), 張宏瀚 申請(qǐng)人:哈爾濱工程大學(xué)