本發(fā)明涉及的是一種自主水下航行器的控制方法，具體地說是一種欠驅(qū)動(dòng)自主水下航行器的對(duì)三維路徑理解和跟蹤控制方法。

背景技術(shù)：

auv所執(zhí)行的任務(wù)多為調(diào)查性質(zhì)。實(shí)際上，自主水下航行器對(duì)空間目標(biāo)的跟蹤控制形式是離散的、不連續(xù)的。即使面對(duì)解析形式的復(fù)雜曲線，對(duì)于控制系統(tǒng)而言也是將其以離散點(diǎn)的形式表示。在《一種基于電子海圖的欠驅(qū)動(dòng)auv區(qū)域搜索方法》(機(jī)器人，2014，第36卷第5期)、《dockingcontrolsystemfora54-cm-diameter(21-in)auv》(ieeejournalofoceanicengineering,2008,第33卷第4期)等所述的一些應(yīng)用中要求水下航行器具有很好的平面直線航線跟蹤能力，如覆蓋式水下目標(biāo)搜索、海底地形地貌勘測(cè)、自主水下對(duì)接等。根據(jù)潛艇操縱性知識(shí)，艇體關(guān)于xoz平面對(duì)稱的水下航行器在無操縱力作用下的運(yùn)動(dòng)具有直線穩(wěn)定性，無方向穩(wěn)定性和航線穩(wěn)定性。因此需要包含推進(jìn)器及操縱面等執(zhí)行機(jī)構(gòu)的控制系統(tǒng)介入，才能保證水下航行器的直線航線跟蹤從而進(jìn)行可靠地水下航行作業(yè)。

一方面，基于動(dòng)力學(xué)模型的控制器應(yīng)用困難：反步設(shè)計(jì)法、局部反饋線性化思想、神經(jīng)網(wǎng)絡(luò)控制等盡管在理論上有較大進(jìn)展，但多處于數(shù)值仿真研究的階段(《基于反步法的欠驅(qū)動(dòng)uuv空間目標(biāo)跟蹤非線性控制方法》，哈爾濱工程大學(xué)博士學(xué)位論文，2012)，未能有效應(yīng)用于載體。一方面，已經(jīng)應(yīng)用的多為航路點(diǎn)、及虛擬點(diǎn)跟蹤的控制方法，不能有效抵抗海流干擾(《海流環(huán)境下基于模糊理論的水下機(jī)器人區(qū)域搜索》，哈爾濱工程大學(xué)碩士學(xué)位論文，2005)。該發(fā)明采用的控制方法理論依據(jù)充分，簡單易形，環(huán)境適應(yīng)能力好：智能pid方法不依賴于數(shù)學(xué)模型，具有理論成熟、原理簡單、適用性好、使用方便等優(yōu)點(diǎn)，將其作為智能系統(tǒng)理性行為模型的規(guī)劃層，形成路徑跟蹤的制導(dǎo)控制器；另一方面，依據(jù)《多功能自主式水下機(jī)器人運(yùn)動(dòng)控制研究》(哈爾濱工程大學(xué)博士學(xué)位論文，2012)，s面控制方法成功應(yīng)用于auv運(yùn)動(dòng)控制，實(shí)現(xiàn)了干擾下的速度、艏向、縱傾等運(yùn)動(dòng)狀態(tài)的控制，在十余年的海洋探測(cè)試驗(yàn)及任務(wù)中表現(xiàn)穩(wěn)定，將其作為智能系統(tǒng)理性行為模型的執(zhí)行層，形成路徑跟蹤的狀態(tài)控制器。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種能夠使自主水下航行器有效抵抗海流干擾，對(duì)三維路徑穩(wěn)定、精確地跟蹤的欠驅(qū)動(dòng)自主水下航行器的對(duì)三維路徑進(jìn)行理解及跟蹤控制方法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：

步驟一：將全局路徑規(guī)劃所得的三維路徑理解為空間直線段序列；

步驟二：將空間直線段描述為自主水下航行器的目標(biāo)直線段，進(jìn)行慣性坐標(biāo)系下的投影，在水平面形成二維直線，在垂直方向形成深度和高度坐標(biāo)序列；

步驟三：欠驅(qū)動(dòng)控制形式的自主水下航行器，對(duì)單個(gè)的水平面目標(biāo)直線段進(jìn)行跟蹤控制，控制器采用分層的結(jié)構(gòu)，位于上層的制導(dǎo)控制器將位置偏差轉(zhuǎn)化為參考艏向角，位于下層的狀態(tài)控制器將艏向角偏差轉(zhuǎn)化為操縱面的轉(zhuǎn)動(dòng)執(zhí)行角度；

步驟四：進(jìn)行步驟二所述目標(biāo)直線段的更替，并依次運(yùn)行步驟三完成每個(gè)目標(biāo)直線段的跟蹤，最終完成空間直線段序列的跟蹤，實(shí)現(xiàn)三維路徑跟蹤。

本發(fā)明還可以包括如下特點(diǎn)：

1、對(duì)全局路徑的理解方式，具體為：

整體上將給定的三維路徑轉(zhuǎn)化為空間直線段序列，依據(jù)全局路徑的表示形式采用不同的理解方法，所述不同的理解方法包括：

(1)如果全局路徑以離散點(diǎn)序列p＝{p0,p1,p2,…,pm}表示，則依據(jù)三次b樣條理論進(jìn)行插值計(jì)算，首先求解離散點(diǎn)所定義的樣條曲線的解析表達(dá)式，依據(jù)公式：

其次按照一定的精細(xì)程度要求，選擇變量t的值，給出均勻的點(diǎn)序列p′＝{p′0,p′1,p′2,…,p′q}，q＝m·s，s是與t有關(guān)的變量表示分辨率，

有了均勻的點(diǎn)序列，則直線段序列的表達(dá)式為：

l＝{l0-1,l1-2,l2-3,…,li-i+1,…,lq-1-q},i＝0,1,2,…,q-1，

單個(gè)目標(biāo)直線段的表達(dá)式：

(2)如果全局路徑具有解析表達(dá)式，例如：l＝l(t),t∈(a,b)，那么均勻取t的值，得到點(diǎn)序列，后續(xù)步驟同(1)。

2、對(duì)單個(gè)的水平面目標(biāo)直線段進(jìn)行跟蹤控制的跟蹤控制器具體為：

跟蹤控制器具有分層的結(jié)構(gòu)，包含：制導(dǎo)控制器和狀態(tài)控制器，

制導(dǎo)控制器以位置偏差為控制輸入，以智能pid算法為控制率，輸出參考艏向角；狀態(tài)控制器以參考艏向角偏差為控制輸入，以自適應(yīng)s面算法為控制率，輸出操縱面的轉(zhuǎn)動(dòng)執(zhí)行角度。

3、制導(dǎo)控制器計(jì)算載體與目標(biāo)直線段的距離，采用智能pid算法為控制率，計(jì)算輸出參考艏向角。

4、制導(dǎo)控制器通過幾何計(jì)算的方式計(jì)算載體與目標(biāo)直線段之間的橫向偏差pe，以此作為智能pid制導(dǎo)控制器的輸入，其中三維空間的直線航線s在水平面e-ξη的投影為st0-t1|ζ＝0，一維平面上由點(diǎn)t0(ξ0,η0)和t1(ξ1,η1)所確定的方向線段(t0≠t1)，auv的位置ps(ξs,ηs)，航向?yàn)棣譻，包含以下步驟：

(1)：計(jì)算橫向距離，即偏差絕對(duì)值，公式為：

(2)：計(jì)算正負(fù)，規(guī)定自主水下航行器位于方向線段t0t1的左側(cè)時(shí)pe＞0：

sgn(pe)＝sgn[(ξ1-ξ0)·(η1-ηs)-(η1-η0)·(η1-ηs)]，

(3)：計(jì)算橫向偏差；

pe＝sgn(pe)·|pe|。

5、制導(dǎo)律采用智能pid算法，pid算法采用差分形式：

n為時(shí)鐘節(jié)拍，mi為積分開始的時(shí)鐘節(jié)拍，md為近幾個(gè)計(jì)算周期偏差平均變化率計(jì)算的開始節(jié)拍；kp，ki，kd分別為比例積分微分項(xiàng)系數(shù)，依據(jù)pe和的變化通過模糊自適應(yīng)的方式進(jìn)行調(diào)整：

6、狀態(tài)控制器采用自適應(yīng)參數(shù)調(diào)整的s面控制方法，表達(dá)式如下：

e、為控制輸入分別表示艏向角偏差和偏差變化率，f為控制輸出、物理意義為歸一化的轉(zhuǎn)艏力矩，ke、kv為控制參數(shù)，依據(jù)模糊自適應(yīng)的方式進(jìn)行調(diào)整

7、依據(jù)自主水下航行器與目標(biāo)直線段的相對(duì)位置關(guān)系進(jìn)行目標(biāo)的更替，具體為：

計(jì)算向量與向量的夾角，如果模超過，則認(rèn)為應(yīng)該進(jìn)行目標(biāo)的更替。

本發(fā)明提供了一種能夠使自主水下航行器有效抵抗海流干擾，對(duì)三維路徑穩(wěn)定地、精確地跟蹤，進(jìn)而實(shí)現(xiàn)遠(yuǎn)程航行及調(diào)查類任務(wù)的三維全局路徑離散化理解和分層自適應(yīng)跟蹤控制方法。

本發(fā)明將曲線跟蹤離散化為直線段序列的跟蹤。這種處理方式簡單易行且安全可靠，便于應(yīng)對(duì)水下航行最常見的干擾形式——海流。另外，路徑的簡化、離散化過程帶來的精度損失是可控的。

本發(fā)明具有以下優(yōu)點(diǎn)及有益效果：

1.是一種完整的控制系統(tǒng)解決方案，具有模塊化特點(diǎn)。不依賴載體的數(shù)學(xué)模型及其它結(jié)構(gòu)因素，從對(duì)路徑的理解，到對(duì)路徑的跟蹤控制，輸入的是解析形式或離散點(diǎn)形式表示的全局三維路徑，輸出的是以角度為度量單位的操縱面轉(zhuǎn)動(dòng)幅度。方便移植及工程化應(yīng)用。

2.控制系統(tǒng)的魯棒性好。由于控制算法不依賴水下航行器的動(dòng)力學(xué)模型，且具有自適應(yīng)調(diào)整特性，已集成于多個(gè)異構(gòu)水下航行器平臺(tái)，于水池、湖泊、海上試驗(yàn)等不同環(huán)境下進(jìn)行航行作業(yè)試驗(yàn)，均達(dá)到了極好的控制效果。

3.路徑跟蹤控制的穩(wěn)定性好，精度高，抗海流干擾能力強(qiáng)。在所設(shè)計(jì)的直線段跟蹤控制器作用下，水下航行器能夠感受到海流給橫向偏差帶來的影響，在制導(dǎo)控制算法的作用下能夠調(diào)整參考艏向角輸出，經(jīng)狀態(tài)控制器調(diào)整操縱面角度實(shí)現(xiàn)auv轉(zhuǎn)艏，從而消除橫向偏差。以某80kg級(jí)水下航行器為例，在遠(yuǎn)程航行試驗(yàn)中，無論是靜水(湖泊)還是存在非定常海流干擾的海洋，均能保證跟蹤偏差≤2.0m(85％)，并且不存在穩(wěn)態(tài)偏差。

附圖說明

圖1為欠驅(qū)動(dòng)水下航行器外形圖；

圖2為分層的平面直線段跟蹤控制器結(jié)構(gòu)圖；

圖3為智能pid制導(dǎo)控制器的結(jié)構(gòu)圖；

圖4為欠驅(qū)動(dòng)水下航行器進(jìn)行直線航線跟蹤的可行性分析圖；

圖5為水下航行器對(duì)解析形式空間路徑的理解及跟蹤示意圖；

圖6為水下航行器對(duì)離散控制點(diǎn)形式空間路徑的理解示意圖；

圖7為海試試驗(yàn)數(shù)據(jù)之水平面路徑跟蹤偏差；

圖8為海試實(shí)驗(yàn)數(shù)據(jù)之垂直方向的坐標(biāo)跟蹤情況；

圖9為海試試驗(yàn)數(shù)據(jù)之參考艏向角、狀態(tài)艏向角的變化情況；

圖10為pid控制參數(shù)的模糊調(diào)整之對(duì)比例項(xiàng)參數(shù)的調(diào)整表1；

圖11為pid控制參數(shù)的模糊調(diào)整之對(duì)積分項(xiàng)參數(shù)的調(diào)整表2；

圖12為pid控制參數(shù)的模糊調(diào)整之對(duì)微分項(xiàng)參數(shù)的調(diào)整表3；

圖13為本發(fā)明的流程圖。

具體實(shí)施方式

本發(fā)明的方法主要包括如下步驟：

步驟一：三維路徑的均勻離散化，將解析形式的三維路徑或點(diǎn)序列表示的三維路徑經(jīng)三次b樣條插值，并離散為均勻點(diǎn)序列p′＝{p′0,p′1,p′2,…,p′q}，認(rèn)為原三維路徑可由點(diǎn)序列相鄰兩點(diǎn)構(gòu)成的直線段首尾相連形成直線段序列l(wèi)＝{l0-1,l1-2,l2-3,…,li-i+1,…,lq-1-q}表示；

步驟二：目標(biāo)直線段的坐標(biāo)投影，將空間直線段描述為目標(biāo)直線段，即auv當(dāng)前的唯一目標(biāo)航線，依據(jù)弱機(jī)動(dòng)體的解耦控制理論，對(duì)目標(biāo)直線段進(jìn)行坐標(biāo)投影，在垂直方向形成深度/高度坐標(biāo)序列，在水平面形成二維直線段；

步驟三：對(duì)二維直線段的跟蹤控制，欠驅(qū)動(dòng)控制形式的auv通過前進(jìn)速度以及操縱面轉(zhuǎn)動(dòng)控制轉(zhuǎn)艏以及俯仰運(yùn)動(dòng)，與來流形成攻角從而改變位置，將對(duì)水平面投影直線的跟蹤控制器設(shè)計(jì)為分層結(jié)構(gòu)，制導(dǎo)控制器將位置偏差pe轉(zhuǎn)化為能夠?yàn)榍夫?qū)動(dòng)auv理解的艏向角偏差狀態(tài)控制器將艏向角偏差轉(zhuǎn)化為轉(zhuǎn)艏力矩，經(jīng)推力分配計(jì)算最終輸出操縱面轉(zhuǎn)動(dòng)角度δ。

步驟四：實(shí)現(xiàn)步驟二所述目標(biāo)直線段的更替，依據(jù)auv的位置ps(ξs,ηs)與t0(ξ0,η0)、t1(ξ1,η1)的位置關(guān)系實(shí)現(xiàn)平面目標(biāo)直線段的更新，依次運(yùn)行步驟三，完成每個(gè)目標(biāo)直線段的跟蹤，遍歷空間直線段序列，實(shí)現(xiàn)三維路徑跟蹤。

下面結(jié)合附圖舉例對(duì)本發(fā)明作進(jìn)一步描述：

本發(fā)明的實(shí)施平臺(tái)是一臺(tái)微小型自主水下航行器如圖1?？刂葡到y(tǒng)的執(zhí)行機(jī)構(gòu)采用欠驅(qū)動(dòng)的布置方案，在中心線的艉部布置一套推進(jìn)器1，由直流無刷電機(jī)和螺旋槳組成；艉部布置“十”字型操縱面，由步進(jìn)電機(jī)產(chǎn)生動(dòng)力，左右舷對(duì)稱布置升降舵3，上下對(duì)稱布置轉(zhuǎn)艏舵2。平臺(tái)搭載的運(yùn)動(dòng)傳感器包括gps、磁羅經(jīng)、加速度計(jì)、dvl、水壓力傳感器，水面運(yùn)動(dòng)狀態(tài)依賴gps、磁羅經(jīng)、dvl感知，水下運(yùn)動(dòng)狀態(tài)依賴磁羅經(jīng)、加速度計(jì)、dvl、水壓力傳感器感知并且進(jìn)行推算導(dǎo)航。

本發(fā)明所述的對(duì)三維路徑的理解是指將全局路徑規(guī)劃給出的解析形式的三維路徑或經(jīng)介質(zhì)通訊接收到的航點(diǎn)序列表示的離散形式的三維路徑，經(jīng)插值計(jì)算，得到均勻分布的航點(diǎn)序列，最終得到能夠使控制系統(tǒng)直接理解計(jì)算的水平面直線段序列以及垂直方向的關(guān)鍵點(diǎn)序列的過程。具體如下：

如果全局路徑以離散點(diǎn)序列p＝{p0,p1,p2,…,pm}表示，那么依據(jù)三次b樣條理論進(jìn)行插值計(jì)算，首先求解離散點(diǎn)所定義的樣條曲線的解析表達(dá)式，依據(jù)公式：

其次按照一定的精細(xì)程度要求，選擇變量t的值，給出均勻的點(diǎn)序列p′＝{p′0,p′1,p′2,…,p′q}，在這里q＝m·s，s是與t有關(guān)的變量，表示分辨率，且以如下格式保存在mypathpt.dat文件中：

“index(long)latitude(double)longitude(double)depth(double)height(double)ispassed(bool)”。

有了均勻的點(diǎn)序列，可以給出直線段序列的表達(dá)式：

l＝{l0-1,l1-2,l2-3,…,li-i+1,…,lq-1-q},i＝0,1,2,…,q-1

同時(shí)，滿足|l0-1|＝|l1-2|＝…＝|lq-1-q|。

單個(gè)目標(biāo)直線段的表達(dá)式：

如果全局路徑具有解析表達(dá)式，例如：l＝l(t),t∈(a,b)，那么均勻取t的值，得到點(diǎn)序列，后續(xù)步驟同上。

本發(fā)明所述的對(duì)三維路徑的跟蹤控制方法指在水平面跟蹤上述獲得的目標(biāo)直線段li-i+1，在垂直方向上跟蹤高度值heighti-i+1或深度值depthi-i+1，這里詳述水平面目標(biāo)直線段的跟蹤控制方法：

跟蹤控制器具有分層的結(jié)構(gòu)，如圖2，包含：制導(dǎo)控制器和狀態(tài)控制器。

制導(dǎo)控制器具有圖3所示的結(jié)構(gòu)：以位置偏差為控制輸入，以智能pid算法為控制率，輸出參考艏向角。具體計(jì)算過程和依據(jù)為：

計(jì)算橫向位置偏差pe。

坐標(biāo)轉(zhuǎn)換，統(tǒng)一單位，將所有經(jīng)緯度表示的標(biāo)記位置信息的變量轉(zhuǎn)化為相對(duì)坐標(biāo)系下用度量單位“米”表示，依據(jù)公式：

其中，lat0與lon0表示相對(duì)坐標(biāo)系原點(diǎn)的經(jīng)緯度，一般為航行器初次上電后gps校準(zhǔn)時(shí)的位置；

航行器對(duì)目標(biāo)直線的跟蹤示意如圖4。設(shè)當(dāng)前目標(biāo)直線段為由點(diǎn)t0(ξ0,η0)和t1(ξ1,η1)所確定的方向線段(t0≠t1)，航行器的位置ps(ξs,ηs)，航向?yàn)棣譻，按照以下公式計(jì)算航行器距離目標(biāo)直線段的距離：

其正負(fù)利用向量和叉積的符號(hào)判斷，規(guī)定航行器位于方向線段t0t1的左側(cè)時(shí)pe＞0：

sgn(pe)＝sgn[(ξ1-ξ0)·(η1-ηs)-(η1-η0)·(η1-ηs)]

橫向偏差即pe＝sgn(pe)·|pe|；

設(shè)計(jì)制導(dǎo)律。

制導(dǎo)控制器的輸出為航行器的參考艏向角ψref，其整體上由兩部分組成：

ψref＝ψo(hù)+ψc(pe)

其中，ψo(hù)為目標(biāo)直線段自身的方向，依據(jù)下式計(jì)算：

ψc為由航行器和目標(biāo)直線段相對(duì)位置關(guān)系決定的相對(duì)艏向角，是制導(dǎo)器的核心內(nèi)容，計(jì)算過程描述為：

pe為制導(dǎo)器的狀態(tài)變量，依據(jù)lyapunov穩(wěn)定性定理設(shè)計(jì)控制率，取lyapunov函數(shù)系統(tǒng)的唯一平衡狀態(tài)為原點(diǎn)pe＝0。標(biāo)量函數(shù)v＞0，為正定的，并且pe→∞有v→∞；由于pe為空間相對(duì)位置的真實(shí)度量，其值連續(xù)變化并且對(duì)時(shí)間的導(dǎo)數(shù)為靠近目標(biāo)直線段的速度，因而函數(shù)v具有連續(xù)導(dǎo)數(shù)，表示為：解釋為：當(dāng)偏差pe＞0時(shí)，控制航行器使之產(chǎn)生即pe減小的狀態(tài)(由正方向靠近目標(biāo)航線)；當(dāng)偏差pe＜0時(shí)，控制航行器使之產(chǎn)生即pe增大的狀態(tài)(由負(fù)方向靠近目標(biāo)航線)。依據(jù)圖4所示，欠驅(qū)動(dòng)航行器是通過調(diào)整艏向而達(dá)到調(diào)整自身與目標(biāo)航線之間距離的，因而控制率如下：

n為時(shí)鐘節(jié)拍，mi為積分開始的時(shí)鐘節(jié)拍取值為5～10s，md為近幾個(gè)計(jì)算周期偏差平均變化率計(jì)算的開始節(jié)拍取值為3～6s；ψ′歸一化的角度系數(shù)，可以經(jīng)非線性sigmoid函數(shù)映射到角度空間{ψ|-π/2≤ψ≤π/2}

kp，ki，kd分別為比例、積分、微分項(xiàng)系數(shù)。在應(yīng)用時(shí)采用模糊控制的方式依據(jù)偏差pe以及偏差變化率調(diào)整三個(gè)控制參數(shù)：

三個(gè)參數(shù)調(diào)整的模糊規(guī)則如附圖10-12的表1-3所示。

狀態(tài)控制器以制導(dǎo)控制器輸出的參考艏向角ψref以及航行器導(dǎo)航傳感器磁羅經(jīng)給出的狀態(tài)艏向角ψauv的差δψ為控制輸入，以自適應(yīng)s面算法為控制率，計(jì)算輸出轉(zhuǎn)艏力矩。

控制率為：

其中ke、kv為控制參數(shù)，經(jīng)模糊控制的方式進(jìn)行自適應(yīng)調(diào)整：

模糊控制規(guī)則與制導(dǎo)控制kp，ki，kd的調(diào)整類似，這里不贅述。

f0為轉(zhuǎn)艏力矩的歸一化值，經(jīng)公式mz＝f0·mδ-z計(jì)算轉(zhuǎn)艏力矩，mδ-z為垂直舵在最大航速、最大舵角下產(chǎn)生的轉(zhuǎn)艏力矩。

計(jì)算舵的垂直舵的升力，由于產(chǎn)生轉(zhuǎn)艏力矩的艉垂舵有兩個(gè)且上下對(duì)稱布置，因此：lδ-z表示由舵的幾何位置引起的力臂；

依據(jù)公式計(jì)算垂直舵的舵角，fδ-z表示舵的升力；βe為舵翼有效攻角；ve為舵翼有效進(jìn)速；afin為舵翼側(cè)投影面積；cl(δe,z)舵翼的升力系數(shù)，都是關(guān)于有效攻角的函數(shù)，可以通過經(jīng)驗(yàn)公式估算、cfd數(shù)值計(jì)算以及試驗(yàn)獲得。

依據(jù)航行器與目標(biāo)直線段的相對(duì)位置關(guān)系進(jìn)行目標(biāo)直線段的更替，具體判據(jù)為：

計(jì)算向量與向量的夾角，如果模超過，則認(rèn)為應(yīng)該進(jìn)行目標(biāo)的更替。

海上試驗(yàn)驗(yàn)證與分析：

下面舉例說明，為驗(yàn)證自主水下航行器對(duì)三維路徑的理解及跟蹤控制方法的有效性，進(jìn)行了給定三維路徑下的路徑理解及跟蹤試驗(yàn)：按照發(fā)明內(nèi)容中步驟1所述，對(duì)兩種不同形式的三維路徑進(jìn)行理解。首先，在給定初始點(diǎn)(lat.135296.3，lon.440174.2，ang.90°)及終止點(diǎn)(lat.135263.9，lon.440174.2，ang.180°)的情況下，全局路徑規(guī)劃器給出解析形式的路徑，本專利所述的路徑理解方法將解析形式的路徑表示理解為目標(biāo)直線段序列，表示為圖5；其次在給定關(guān)鍵路徑點(diǎn)p0(308，416，40)、p1(297.4，156.6，40)、p2(275.3，189.9，40)、p3(220.8，207.8，36)、p4(182.8，200.7，32)、p5(93.5，141.7，20)、p6(114.6，-84.1，8)、p7(84.9，-103.7，4)、p8(38.6，-98.4，40)、p9(-2.5，-40，0)、p10(-9.2，-15，40)、p11(0，0，0)、的情況下，本專利所述的路徑理解方法將離散點(diǎn)形式的路徑理解為目標(biāo)直線段序列，表示為圖6；

以圖5的路徑為例，在海上對(duì)其進(jìn)行路徑跟蹤控制，設(shè)置航行器的巡航速度為1.2m/s，路徑的垂向坐標(biāo)為深度2.0m。圖5的藍(lán)色實(shí)線為航行器的航跡平面投影，可以看出航行器能夠較好地跟蹤目標(biāo)直線段，并且目標(biāo)直線段能夠依據(jù)航行器的前進(jìn)進(jìn)行更替，最終航行器能夠遍歷組成路徑的直線段序列；

圖7是橫向偏差的隨時(shí)間的變化，可以看出在每次目標(biāo)直線段的更替后，特別是路徑曲率較大的位置，橫向偏差會(huì)有一個(gè)階躍，隨后會(huì)減小最終收斂在零點(diǎn)；

圖8是航行器的深度變化情況，可以看出垂向坐標(biāo)控制器能夠穩(wěn)定地、無偏的跟蹤垂向坐標(biāo)；

圖9為路徑跟蹤過程中，制導(dǎo)控制器計(jì)算所得的參考艏向角與航行器的狀態(tài)艏向角的對(duì)比，可以看出參考艏向角作為制導(dǎo)控制器的輸出是在不斷變化的，而狀態(tài)艏向角能夠在狀態(tài)控制器的作用下較好地達(dá)到與參考艏向角一致。

海試結(jié)果表明，本發(fā)明對(duì)三維路徑的理解及跟蹤控制方法具有全局漸進(jìn)穩(wěn)定特性，并且能夠保證無偏差的跟蹤，適用于欠驅(qū)動(dòng)自主水下航行器在復(fù)雜海況下執(zhí)行遠(yuǎn)程航行及調(diào)查型任務(wù)。

本發(fā)明根據(jù)需求按照一定的分辨率將三維路徑離散為直線段序列，認(rèn)為跟蹤這個(gè)直線段序列即完成路徑跟蹤。跟蹤每一個(gè)目標(biāo)直線段并實(shí)現(xiàn)目標(biāo)的更替能夠完成直線段序列的跟蹤。在水平面上進(jìn)行目標(biāo)直線的投影直線段的跟蹤，同時(shí)在垂直面進(jìn)行深度或高度的跟蹤控制。水平面控制采用分層的控制系統(tǒng)結(jié)構(gòu)：規(guī)劃層依據(jù)橫向位置偏差經(jīng)智能pid控制率計(jì)算輸出參考艏向角，形成制導(dǎo)控制器；執(zhí)行層依據(jù)艏向角偏差通過自適應(yīng)s控制律計(jì)算輸出轉(zhuǎn)艏力矩并經(jīng)推力分配計(jì)算給出操縱面轉(zhuǎn)動(dòng)角度，形成狀態(tài)控制器。本發(fā)明適用于欠驅(qū)動(dòng)自主水下航行器進(jìn)行遠(yuǎn)程航行及調(diào)查作業(yè)，具有一定的抗海流干擾能力，不具有模型依賴性，且控制參數(shù)數(shù)量少、容易調(diào)整，方法的準(zhǔn)確性和可靠性已經(jīng)過數(shù)個(gè)異構(gòu)航行器的數(shù)次海試驗(yàn)證，因而可以得到廣泛應(yīng)用。