小型AUV動(dòng)力定位建模與控制仿真

蔡 煒，王俊雄

(上海交通大學(xué) 船舶海洋工程國(guó)家重點(diǎn)實(shí)驗(yàn)室，上海 200240)

0 引 言

水下自主航行器（AUV）廣泛應(yīng)用于水下監(jiān)測(cè)、偵察等工程作業(yè)，目前以軍用為主，但小型、民用的AUV產(chǎn)品需求已逐漸在市場(chǎng)上升溫。水下通信、路徑規(guī)劃、水動(dòng)力性能分析等成為小型AUV產(chǎn)品設(shè)計(jì)實(shí)現(xiàn)亟需解決的難題。相對(duì)于大型深海作業(yè)的AUV，小型民用AUV體積質(zhì)量小、航行速度慢、作業(yè)水深小、水環(huán)境多變，對(duì)水動(dòng)力性能試驗(yàn)和動(dòng)力定位控制的設(shè)計(jì)要求較高。

小型AUV的動(dòng)力定位系統(tǒng)要求能夠?qū)崟r(shí)地根據(jù)當(dāng)前的位置、姿態(tài)反饋信息發(fā)出控制信號(hào)，推進(jìn)器執(zhí)行信號(hào)產(chǎn)生抵抗風(fēng)、浪等外界干擾力的作用力并推動(dòng)AUV運(yùn)行，從而在不施加外力控制的條件下，沿預(yù)先規(guī)劃的路徑軌跡實(shí)現(xiàn)AUV的定位航行。

AUV動(dòng)力定位系統(tǒng)框圖如圖1所示。

1 AUV 數(shù)學(xué)模型建立

對(duì)于AUV動(dòng)力定位控制系統(tǒng)而言，首要工作是建立水環(huán)境、AUV自身以及推進(jìn)系統(tǒng)的數(shù)學(xué)模型。

圖 1 AUV 動(dòng)力定位系統(tǒng)框圖Fig. 1 The diagram of AUV dynamic positioning system

圖 2 AUV 固定坐標(biāo)系和運(yùn)動(dòng)坐標(biāo)系Fig. 2 Fixed coordinate and moving coordinate of AUV

由于小型民用AUV的作業(yè)水深小，姿態(tài)保持相對(duì)穩(wěn)定，橫傾和縱傾幅度很小，因此僅考慮三自由度（縱蕩、橫蕩、首搖）的運(yùn)動(dòng)情況。以位置向量表示固定坐標(biāo)系下AUV的位置及首向角，以速度向量表示運(yùn)動(dòng)坐標(biāo)系下AUV的三自由度運(yùn)動(dòng)速度。

1.1 隨機(jī)波浪模型

AUV在水下面臨的作業(yè)環(huán)境屬于不規(guī)則波浪環(huán)境，研究上經(jīng)常把僅存在于主風(fēng)方向且有無(wú)窮長(zhǎng)波線、單向波峰彼此保持平行的二次不規(guī)則波稱為不規(guī)則長(zhǎng)峰波[2]。

常用的波譜密度函數(shù)有P-M譜、紐曼（Newman）譜、JONSWAP譜、ITTC雙參數(shù)譜等。其中，ITTC雙參數(shù)譜以有義波高和海浪特征周期T1為譜參量，具體表達(dá)式如下：

波浪力包括2部分：1）1階波浪力引起的高頻往復(fù)運(yùn)動(dòng)；2）2階波浪力引起的慢漂運(yùn)動(dòng)，使AUV緩慢地漂離原來(lái)的位置。高頻運(yùn)動(dòng)位置變化頻繁，動(dòng)力定位系統(tǒng)很難也沒(méi)有必要對(duì)其進(jìn)行控制，因?yàn)檫@樣會(huì)大大加速推進(jìn)系統(tǒng)的磨損和能量損耗。

規(guī)則波的2階波浪力一般采用Daidola公式進(jìn)行計(jì)算[3]：

式中：ρ為水密度；L為船長(zhǎng)；χ為波向角，即航行方向與波浪方向的夾角；為波浪力計(jì)算系數(shù)。

波浪力系數(shù)的計(jì)算公式如下：

式中：λ為波長(zhǎng)；k為波數(shù)。

溝通交流是集體備課的核心，也是其區(qū)別于個(gè)人備課方式的最大特征.集體備課可以實(shí)現(xiàn)集中備課組所有老師的智慧與成果，實(shí)現(xiàn)取長(zhǎng)補(bǔ)短，共同提高.在主要發(fā)言人對(duì)整節(jié)課的教學(xué)環(huán)節(jié)進(jìn)行設(shè)計(jì)之后，備課組長(zhǎng)就可以組織所有老師開展交流，對(duì)方案的不足進(jìn)行補(bǔ)充，比如新課導(dǎo)入的方式、重難點(diǎn)的突破路徑、習(xí)題的設(shè)計(jì)與選擇等.

整理得到不規(guī)則2階隨機(jī)波浪力的計(jì)算公式：

1.2 操縱運(yùn)動(dòng)數(shù)學(xué)模型

在圖2所示的坐標(biāo)系下，AUV三自由度低頻運(yùn)動(dòng)學(xué)方程可表示為如下矩陣形式[4]：

均為水動(dòng)力導(dǎo)數(shù)。

1.3 基于粒子群算法（PSO）的PID參數(shù)優(yōu)化

20世紀(jì)70年代，F(xiàn)ossen提出非線性PID控制方法，實(shí)現(xiàn)了船舶自航控制。至今，PID控制以其算法成熟、結(jié)構(gòu)簡(jiǎn)單、控制效果好等優(yōu)點(diǎn)仍然成為工程項(xiàng)目控制系統(tǒng)的主要選擇。

PID控制器以目標(biāo)設(shè)定值與實(shí)際測(cè)量值的偏差作為輸入量，將偏差值的比例（P）、積分（I）和微分（D）分量通過(guò)線性組合構(gòu)成控制量，傳遞給執(zhí)行機(jī)構(gòu)，繼而作用于控制對(duì)象。PID系統(tǒng)原理如圖3所示。

圖 3 PID 系統(tǒng)原理圖Fig. 3 Schematic diagram of PID control system

AUV動(dòng)力定位系統(tǒng)的PID控制算法如下：

PID參數(shù)的整定與優(yōu)化是PID控制器設(shè)計(jì)實(shí)現(xiàn)的核心，隨著智能控制理論的發(fā)展，出現(xiàn)了粒子群算法、遺傳算法、神經(jīng)網(wǎng)絡(luò)算法等PID參數(shù)優(yōu)化方法。

粒子群算法（PSO）是從鳥群覓食行為特性中獲得啟發(fā)并研究發(fā)明的一種數(shù)值優(yōu)化算法。在PSO中，每個(gè)優(yōu)化問(wèn)題的潛在解都可以看成是搜索空間區(qū)域上的一個(gè)點(diǎn)，稱之為粒子，每個(gè)粒子都有一個(gè)被目標(biāo)函數(shù)所決定的適應(yīng)值，依據(jù)全局和個(gè)體適應(yīng)值的最優(yōu)值，修正更新每個(gè)粒子的飛行速度和飛行方向，直至搜尋到空間區(qū)域內(nèi)以目標(biāo)函數(shù)為導(dǎo)向的最優(yōu)解。

每次迭代過(guò)程中，依據(jù)粒子本身的位置速度信息和2個(gè)極值的大小對(duì)粒子的搜索速度和位置進(jìn)行修正，具體更新公式如下：其中：w為慣性權(quán)重；c1為粒子跟蹤自己最優(yōu)值的權(quán)重系數(shù)，稱之為“認(rèn)知”，通常設(shè)置為2；w2為粒子跟蹤全局最優(yōu)值的權(quán)重系數(shù)，稱之為“社會(huì)”，通常設(shè)置為2；表示[0，1]之間的隨機(jī)數(shù)；r為約束因子，通常設(shè)置為1。

選用PSO算法對(duì)PID參數(shù)進(jìn)行優(yōu)化，用三維空間中的粒子表示PID的3個(gè)參數(shù)，以加快系統(tǒng)響應(yīng)速度、減小超調(diào)為優(yōu)化目標(biāo)，選用誤差絕對(duì)值的時(shí)間積分函數(shù)，即ITAE函數(shù)作為目標(biāo)函數(shù)：

在三維空間域內(nèi)搜尋使得ITAE函數(shù)獲得最小值的粒子參數(shù)值，作為優(yōu)化后的PID參數(shù)，并與常規(guī)PID控制方法進(jìn)行仿真比對(duì)。

2 仿真結(jié)果與分析

以小型民用AUV產(chǎn)品為研究對(duì)象，主要參數(shù)如表1所示?；贗TTC雙參數(shù)波浪譜和Daidola漂移力公式，在Matlab/Simulink中模擬AUV水環(huán)境，探究不施加作用力時(shí)，AUV的漂移運(yùn)動(dòng)情況。具體水況分布如表2所示。

表 1 AUV主參數(shù)Tab. 1 Principal parameters of AUV

表 2 水況分布Tab. 2 Water condition for calculation

在Simulink中搭建PSO優(yōu)化模型，設(shè)置粒子規(guī)模為100，最大迭代步數(shù)為1 000，慣性權(quán)重從1.0隨步數(shù)增長(zhǎng)逐漸遞減至0.1。優(yōu)化后的PID參數(shù)如下：KP=

在Simulink中搭建基于PID控制器的動(dòng)力定位控制系統(tǒng)，依據(jù)經(jīng)驗(yàn)選取一組常規(guī)PID控制參數(shù)：KP=。給定AUV的初始位置向量和目標(biāo)位置向量，2組控制器下的AUV運(yùn)動(dòng)仿真比對(duì)結(jié)果如圖6～圖9所示。

圖 4 隨機(jī)波浪波高時(shí)變曲線Fig. 4 Wave height time-varying curve of random waves

圖 5 模擬波浪作用下的 X–Y 運(yùn)動(dòng)軌跡圖Fig. 5 X–Y moving trace with the effect of simulated wave

圖 6 PID 控制下的橫向位移–時(shí)間變化曲線Fig. 6 Horizontal displacement curve with PID controller

圖 7 PID 控制下的縱向位移–時(shí)間變化曲線Fig. 7 Vertical displacement curve with PID controller

2組PID控制下，AUV均由定位原點(diǎn)抵達(dá)目標(biāo)位置。常規(guī)PID控制下，系統(tǒng)經(jīng)歷約100 s的調(diào)整達(dá)到穩(wěn)定，同時(shí)超調(diào)量達(dá)到約20%，控制效果較差；經(jīng)PSO優(yōu)化后的PID控制器，系統(tǒng)約40 s后快速達(dá)到目標(biāo)位置狀態(tài)附近進(jìn)行微調(diào)，整體超調(diào)量約5%，控制性能較常規(guī)PID控制得到大幅優(yōu)化。

圖 8 PID 控制下的首向角–時(shí)間變化曲線Fig. 8 Heading angle curve with PID controller

圖 9 PID 控制下的 X–Y 運(yùn)動(dòng)軌跡圖Fig. 9 X–Y moving trace with PID controller

3 結(jié) 語(yǔ)

AUV在軍用項(xiàng)目的成功推動(dòng)了將其向小型民用產(chǎn)品方向推廣的設(shè)想，用以實(shí)現(xiàn)例如湖底景色拍攝、水產(chǎn)養(yǎng)殖巡視、內(nèi)陸河水文信息采集等諸多功能。小型AUV產(chǎn)品設(shè)計(jì)的核心問(wèn)題有水動(dòng)力性能分析、路徑規(guī)劃、水下通信等。本文基于不規(guī)則隨機(jī)長(zhǎng)峰波浪理論，模擬小型AUV作業(yè)的水環(huán)境，獲取AUV的慢漂運(yùn)動(dòng)規(guī)律，并基于PID控制器搭建動(dòng)力定位控制仿真系統(tǒng)，利用粒子群算法（PSO）優(yōu)化PID參數(shù)，實(shí)現(xiàn)AUV的定位控制，獲取了較好的控制性能。為AUV水動(dòng)力性能試驗(yàn)及動(dòng)力定位系統(tǒng)的軟硬件設(shè)計(jì)創(chuàng)造了條件，為小型AUV產(chǎn)品的設(shè)計(jì)實(shí)現(xiàn)奠定基礎(chǔ)。