本發(fā)明涉及一種自主水下航行器的精確航路跟蹤控制方法,屬于自主水下航行器跟蹤控制技術(shù)領(lǐng)域。
背景技術(shù):
自主水下航行器(autonomous underwater vehicle,AUV)作為海洋開(kāi)發(fā)的一項(xiàng)重要工具,可以出色的完成多種民用與軍事任務(wù),近年來(lái)取得了長(zhǎng)足的發(fā)展。AUV的航路點(diǎn)跟蹤控制作為AUV研究中的一個(gè)關(guān)鍵問(wèn)題,引起了人們的廣泛關(guān)注。所謂航路點(diǎn)跟蹤是指AUV出發(fā)后,通過(guò)控制輸入作用使其向航路點(diǎn)運(yùn)動(dòng),當(dāng)與之距離小于某個(gè)閾值時(shí),認(rèn)為到達(dá)航路點(diǎn)并且開(kāi)始向下一個(gè)航路點(diǎn)運(yùn)動(dòng),直至完成所有航路點(diǎn)運(yùn)動(dòng)。
目前,AUV水下航路跟蹤控制方法主要有:
(1)視線法航路點(diǎn)導(dǎo)引(LOS)。LOS根據(jù)AUV位置和航路點(diǎn)的相對(duì)位置,計(jì)算視線的方位角作為參考航向角,然后基于線性化方法或反演方法,設(shè)計(jì)航向角控制器,并往往忽略參考航向角的動(dòng)態(tài)特性。LOS導(dǎo)引法通過(guò)航向角控制調(diào)整AUV的運(yùn)動(dòng)方向,使其始終向航路點(diǎn)運(yùn)動(dòng),而不考慮AUV與航路點(diǎn)連線的位置偏差。AUV的運(yùn)動(dòng)軌跡會(huì)由于初始位置和航向角偏差而偏差期望的航線,造成航程的損失。
(2)模糊航路點(diǎn)跟蹤控制。依據(jù)模糊控制理論的基礎(chǔ),并根據(jù)控制對(duì)象的特點(diǎn),制定模糊控制規(guī)則,設(shè)計(jì)出模糊控制器,對(duì)AUV的航路點(diǎn)跟蹤運(yùn)動(dòng)進(jìn)行控制。該方法的主要缺點(diǎn)是模糊控制規(guī)則的獲取以及輸入模糊變量論域和隸屬函數(shù)的確定都是實(shí)驗(yàn)分析和操作人員經(jīng)驗(yàn)的匯總,受人為主觀因素的影響較大。
技術(shù)實(shí)現(xiàn)要素:
為了避免現(xiàn)有技術(shù)的不足之處,本發(fā)明提出了一種自主水下航行器的精確航路跟蹤控制方法,這種航路跟蹤控制方法是直線航跡跟蹤控制。直線航跡跟蹤控制是使從任意初始狀態(tài)出發(fā)的AUV,收斂于給定直線,并沿該直線運(yùn)動(dòng),直線航跡跟蹤可以看作是一種特殊的路徑跟蹤。選擇期望的直線航跡為地面坐標(biāo)系的x軸,y軸與x軸垂直向右。
直線航跡跟蹤誤差用坐標(biāo)y描述,稱為橫向機(jī)動(dòng)誤差。欠驅(qū)動(dòng)AUV的水平面直線航跡跟蹤如圖1所示。
直線軌跡跟蹤控制的目標(biāo)是,設(shè)計(jì)偏航力矩N使AUV從任意初始狀態(tài)出發(fā),跟蹤誤差y漸近收斂到零。直線航跡跟蹤控制以AUV與期望航線的偏差為控制目標(biāo),使AUV跟蹤連接航路點(diǎn)的直線運(yùn)動(dòng),直到完成所有航路點(diǎn)。
一種自主水下航行器的精確航路跟蹤控制方法,步驟如下:
步驟1:建立AUV水平面數(shù)學(xué)模型:
步驟2:AUV以初始狀態(tài)開(kāi)始進(jìn)行直線跟蹤控制運(yùn)動(dòng),其中,y(0),ψ(0),v(0)和r(0)可在AUV航行之前進(jìn)行設(shè)定;
步驟3:假設(shè)點(diǎn)P1-Pn為指定的一系列航路點(diǎn);
步驟4:選擇以前一航路點(diǎn)Pi為坐標(biāo)原點(diǎn),x軸指向當(dāng)前目標(biāo)航路點(diǎn)Pi+1的航路點(diǎn)坐標(biāo)系{Wi},如圖2所示。
步驟5:AUV在{Wi}坐標(biāo)系中的坐標(biāo)(xi,yi)和航向角ψi可以通過(guò)下面的坐標(biāo)變換得到
ψi=ψ-ψpi
式中,(xpi,ypi)為航路點(diǎn)Pi的位置坐標(biāo),ψpi為向量的方位角,
為{Wi}到{U}的常值坐標(biāo)轉(zhuǎn)換矩陣。通過(guò)前面提出直線航跡跟蹤控制,使AUV沿直線PiPi+1運(yùn)動(dòng)。
步驟6:在模型式中,航向角ψ可以看作是航跡跟蹤誤差y的虛擬控制,選擇鎮(zhèn)定函數(shù)
ψd=α(y)=-atan(k1y)
顯然,ψd∈(-π/2,π/2),參考航向ψd可以看作是AUV以x軸上前方Δ=1/k1的點(diǎn)P為瞄準(zhǔn)點(diǎn)的一種視線導(dǎo)引法,如圖1所示。
步驟7:定義相應(yīng)的參考航向角速度
步驟8:定義航向角跟蹤誤差
步驟9:AUV沿直線PiPi+1運(yùn)動(dòng),AUV與Pi+1距離小于l(到達(dá)半徑)時(shí),建立下一個(gè)航路點(diǎn)坐標(biāo)系{Wi+1},使AUV沿Pi+1Pi+2運(yùn)動(dòng),直到完成所有航路點(diǎn)。
本發(fā)明的有益效果是:
直線航跡跟蹤控制以與期望航線的偏差為待控制量,從而保證了AUV沿期望的航線運(yùn)動(dòng),更加符合精確測(cè)量、偵察任務(wù)的要求。
附圖說(shuō)明
圖1欠驅(qū)動(dòng)AUV直線航跡跟蹤。
圖2基于直線航跡跟蹤控制的AUV航路點(diǎn)跟蹤圖。
圖3水平面航路點(diǎn)跟蹤運(yùn)動(dòng)軌跡;其中“CT”為直線航跡跟蹤控制軌跡,“LOS”為L(zhǎng)OS導(dǎo)引法軌跡。
圖4直線航跡跟蹤誤差曲線。
具體實(shí)施方式
下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步的說(shuō)明。
參照?qǐng)D1和圖2,一種自主水下航行器的精確航路跟蹤控制方法,步驟如下:
步驟1:建立AUV水平面數(shù)學(xué)模型:
步驟2:AUV以初始狀態(tài)開(kāi)始進(jìn)行直線跟蹤控制運(yùn)動(dòng),其中,y(0),ψ(0),v(0)和r(0)可在AUV航行之前進(jìn)行設(shè)定;
步驟3:假設(shè)點(diǎn)P1-Pn為指定的一系列航路點(diǎn);
步驟4:選擇以前一航路點(diǎn)Pi為坐標(biāo)原點(diǎn),x軸指向當(dāng)前目標(biāo)航路點(diǎn)Pi+1的航路點(diǎn)坐標(biāo)系{Wi},如圖2所示。
步驟5:AUV在{Wi}坐標(biāo)系中的坐標(biāo)(xi,yi)和航向角ψi可以通過(guò)下面的坐標(biāo)變換得到
ψi=ψ-ψpi
式中,(xpi,ypi)為航路點(diǎn)Pi的位置坐標(biāo),ψpi為向量的方位角,
為{Wi}到{U}的常值坐標(biāo)轉(zhuǎn)換矩陣。通過(guò)前面提出直線航跡跟蹤控制,使AUV沿直線PiPi+1運(yùn)動(dòng)。
步驟6:在模型式中,航向角ψ可以看作是航跡跟蹤誤差y的虛擬控制,選擇鎮(zhèn)定函數(shù)
ψd=α(y)=-atan(k1y)
顯然,ψd∈(-π/2,π/2),參考航向ψd可以看作是AUV以x軸上前方Δ=1/k1的點(diǎn)P為瞄準(zhǔn)點(diǎn)的一種視線導(dǎo)引法,如圖1所示。
步驟7:定義相應(yīng)的參考航向角速度
步驟8:定義航向角跟蹤誤差
步驟9:AUV沿直線PiPi+1運(yùn)動(dòng),AUV與Pi+1距離小于l(到達(dá)半徑)時(shí),建立下一個(gè)航路點(diǎn)坐標(biāo)系{Wi+1},使AUV沿Pi+1Pi+2運(yùn)動(dòng),直到完成所有航路點(diǎn)。
下述給出一個(gè)實(shí)施例具體予以說(shuō)明:航路點(diǎn)跟蹤
步驟1:以REMUS AUV為研究對(duì)象,建立數(shù)學(xué)模型。
步驟2:AUV運(yùn)動(dòng)的初始狀態(tài)為y(0)=10,ψ(0)=π/2,v(0)=1,r(0)=0,常值前向速度uc=1.0。
步驟3:為AUV設(shè)計(jì)所謂的“割草(Lawn mowing)”式搜索任務(wù),選擇如下航路點(diǎn)
P0(0,0),P1(200,0),P2(200,50),P3(0,50),P4(0,100),
P5(200,100),P6(200,150),P7(200,150),P8(0,150),P9(0,200),
P10(200,200),P11(200,250),P12(0,250),P13(0,300)
其中,P0為航行任務(wù)的起點(diǎn),P1~P13為待完成的航路點(diǎn)。
步驟4:選擇到達(dá)半徑l=10m。
步驟5:比較直線航跡跟蹤法和LOS導(dǎo)引法的性能,采用如下的LOS導(dǎo)引律
ψLOS=arctan2(yi+1-y,xi+1-x)
步驟6:采用航行角跟蹤航向角跟蹤控制
ψe=ψ-ψLOS
步驟7:當(dāng)AUV與Pi+1距離小于l(到達(dá)半徑)時(shí),建立下一個(gè)航路點(diǎn)坐標(biāo)系{Wi+1},使AUV沿Pi+1Pi+2運(yùn)動(dòng),直到完成所有航路點(diǎn)。
仿真結(jié)果如圖3、4所示,從航行軌跡看,直線航跡跟蹤控制以與期望航線的偏差為待控制量,從而保證了AUV沿期望的航線運(yùn)動(dòng),更加符合精確測(cè)量、偵察任務(wù)的要求,如海底勘查、對(duì)接回收等。而LOS導(dǎo)引法則以沿視線方向運(yùn)動(dòng)為控制目標(biāo),不考慮與期望航線的偏差,會(huì)由于航路點(diǎn)跟蹤的初始誤差而偏離期望航線,降低了航路點(diǎn)跟蹤的精度。