弱磁型四旋翼AUV 的深度與姿態(tài)控制

張寶泉，向 冰，余祖耀，曾雨昊

(1.華中科技大學(xué) 船舶與海洋工程學(xué)院，湖北 武漢 430074；2.中國船舶集團(tuán)有限公司第七二二研究所，湖北 武漢 430079)

0 引言

如今，人們?cè)诤Ｑ蟆⒑恿骱推渌颅h(huán)境中的活動(dòng)越來越廣泛[1]，水下探索的需求也日益增高，伴隨著這一現(xiàn)象，水下通信成為了這一目標(biāo)的難題之一。目前，大多數(shù)水下設(shè)備采用聲音通信，盡管聲學(xué)通信已被很好地應(yīng)用，但仍存在一些固有缺陷，如帶寬窄，高延遲等[2–3]。因此，近年來，水下天線實(shí)驗(yàn)（也稱射頻（RF）通信技術(shù)實(shí)驗(yàn)）成為海洋研究中必不可少的部分之一[4]。基于這種需求，本文提出一種應(yīng)用于水下探索的新型四旋翼自主水下航行器。

本文實(shí)驗(yàn)的關(guān)鍵部分是用于攜帶水下天線的弱磁型四旋翼水下自主航行器（Low-Magnetic Autonomous Underwater Vehicle,LM-AUV）。在此實(shí)驗(yàn)中，該AUV 的可控性起著關(guān)鍵作用，因?yàn)楸仨殞⑺绿炀€的深度和姿態(tài)限制在一個(gè)很小的范圍內(nèi)，這需要保持對(duì)AUV 的動(dòng)態(tài)控制。Li[5]設(shè)計(jì)了一種AUV 平臺(tái)用于克服強(qiáng)電流的同時(shí)使姿態(tài)穩(wěn)定。Santhakumar[6]提出一種應(yīng)用于長形AUV 的高效動(dòng)態(tài)控制方法。Koh[7]采用了一種特殊的推進(jìn)器配置，并使用了PD 控制器來解決水下機(jī)器人（ROV）的姿態(tài)穩(wěn)定問題。Serrani[8]在測(cè)量速度偏移量下，提出一種強(qiáng)魯棒性的姿態(tài)穩(wěn)定控制器。文獻(xiàn)[9]集中討論在未知的擾動(dòng)或波動(dòng)電流下，如何使AUV 姿態(tài)保持穩(wěn)定。但是，LM-AUV 僅允許弱電流通過推進(jìn)器，以避免對(duì)天線的電磁干擾。在這種情況下，上述文獻(xiàn)中的推進(jìn)器以及控制器布置都不是很可靠。

為解決上述問題，參考某些潛水式無人飛行器[10]和異型水下航行器[11–12]，本文提出一種新型LM-AUV設(shè)計(jì)方法，通過調(diào)整出最優(yōu)的PID 參數(shù)對(duì)該LM-AUV進(jìn)行深度以及姿態(tài)的穩(wěn)定控制，并驗(yàn)證該控制系統(tǒng)的穩(wěn)定性和魯棒性。

1 LM-AUV 外形設(shè)計(jì)

本文的LM-AUV 采用欠驅(qū)動(dòng)結(jié)構(gòu)，同時(shí)令其4 個(gè)推進(jìn)器垂直對(duì)稱地安裝在XY平面上，并且每個(gè)推進(jìn)器在向前和向后方向上都具有相同的推力動(dòng)力學(xué)特性。這種推力機(jī)構(gòu)可以用來分離控制力，從而方便地保持深度和防側(cè)傾。推進(jìn)器的選型上采用低電流型電機(jī)，因?yàn)榈碗娏魍七M(jìn)器所能提供的推力有限，因此對(duì)該AUV 的控制系統(tǒng)精度要求較高。這4 個(gè)推進(jìn)器的幾何布置相對(duì)分散，全部置于外拓展架上，并令其盡可能地遠(yuǎn)離搭載實(shí)驗(yàn)裝置的水密艙，同時(shí)采用并聯(lián)連接電路，從而減少內(nèi)部電流對(duì)射頻實(shí)驗(yàn)的干擾，達(dá)到弱磁性系統(tǒng)的目的。

從圖1 可以看出，到LM-AUV 原型機(jī)的機(jī)械系統(tǒng)組成包括1 個(gè)防水外殼、2 個(gè)端蓋、2 個(gè)法蘭、1 組螺絲、1 組卡環(huán)、2 個(gè)框架和一些平衡組件。水密外殼和外部框架通過4 個(gè)半圓形卡環(huán)固定在一起。推進(jìn)器和平衡組件固定在外部框架的底部，以降低重心并增強(qiáng)自穩(wěn)定性，而電路板和其他電子組件則固定在內(nèi)部框架上。法蘭上的密封槽用于放置O 形密封圈，以實(shí)現(xiàn)軸向/徑向多次密封。螺紋螺釘也通過O 形圈密封件與端蓋密封，并使用環(huán)氧樹脂密封進(jìn)行一定程度的自密封。

圖1 LM-AUV 的3D 模型圖Fig.1 The 3D model of LM-AUV

表1 為總體設(shè)計(jì)參數(shù)。該實(shí)驗(yàn)中所有流體動(dòng)力學(xué)參數(shù)都是通過CFD 仿真獲得的，其余參數(shù)也是基于LM-AUV 原型機(jī)的實(shí)際測(cè)量所得。

表1 LM-AUV 各性能參數(shù)Tab.1 Parameter of LM-AUV

2 LM-AUV 數(shù)學(xué)模型

水下機(jī)器人的通用數(shù)學(xué)模型包括運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型[13]。運(yùn)動(dòng)模型描述了以地球?yàn)閰⒄盏膽T性固定坐標(biāo)系（I-FRAME）和以LM-AUV 自身為參照的坐標(biāo)系（B-FRAME）運(yùn)動(dòng)之間的幾何關(guān)系，可以寫為：

動(dòng)力學(xué)模型主要描述AUV 的速度矢量與力和力矩之間的關(guān)系，可以通過以下公式給出：

上面提到的物理符號(hào)遵循SNAME[14]指定的標(biāo)準(zhǔn)。其中 ξ=[X,Y,Z]T∈R3是I-FRAME 中LM-AUV 的位置矢量，J(Θ)為將B-FRAME 與基于I-FRAME 的轉(zhuǎn)換矩陣相關(guān)聯(lián)的變換矩陣。根據(jù)SNAME符號(hào)定義可得出以下2組向量：歐拉角Θ=[φ,?,ψ]T，速度向量ν=[u,v,w,p,q,r]T∈R6。其中，速度向量 ν中的6 個(gè)變量代表該AUV 運(yùn)動(dòng)的6 個(gè)自由度，分別是橫蕩、縱蕩、垂蕩、橫搖、縱搖、首搖。M=MRB+MA∈R6×6為該AUV 的慣性矩陣，C(ν)∈R6×6和D(ν)=D+D2(ν)∈R6×6分別表示科里奧利向心矩陣和阻尼矩陣，其中D是線性阻尼，D2(ν) 是二階阻尼。τ=Bcu是一個(gè)六維向量，包括B-FRAME 中的控制力和力矩，其中Bc∈R6×4是一個(gè)控制矩陣，u∈R4是一個(gè)分量與向量相關(guān)的向量，與該A U V 推進(jìn)器的角速度相關(guān)聯(lián)，w∈R6表示作用在該AUV 上的干擾矢量。

其中，笛卡爾坐標(biāo)系和運(yùn)動(dòng)的定義如圖2 所示。

圖2 基于LM-AUV 所建立的坐標(biāo)系Fig.2 Coordinate system established based on LM-AUV

上述左等式中向量的4 個(gè)項(xiàng)分別表示沿Z軸的力和圍繞X軸、Y軸和Z軸的轉(zhuǎn)矩。ωi(i=1,2,3,4)為每個(gè)推進(jìn)器的轉(zhuǎn)速，其定義為：若 ωi>0則推進(jìn)器沿順時(shí)針方向致動(dòng)，反之亦然。d為LM-AUV 的形心與推進(jìn)器之間的距離。Ct是積分推力系數(shù)，其滿足等式Ct=T/ω2，其中T該AUV 推進(jìn)系統(tǒng)的總推力。

通過上面的表達(dá)式，可以獲得下式：

3 LM-AUV 電氣及控制系統(tǒng)設(shè)計(jì)

3.1 電氣系統(tǒng)設(shè)計(jì)

電氣系統(tǒng)的示意圖如圖3 所示，包括一個(gè)控制子系統(tǒng)，一個(gè)通信子系統(tǒng)和電源，控制電路采用STM32F1系列微控制器（MCU）作為中央處理單元。由深度傳感器和姿態(tài)航向參考系統(tǒng)（AHRS）收集的深度和姿態(tài)信息，用作控制系統(tǒng)的數(shù)字輸入。控制器產(chǎn)生的PWM 波是雙向無刷電子調(diào)諧器的數(shù)字輸出，其通過電調(diào)連接到無刷直流（BLDC）推進(jìn)器，從而改變推力，調(diào)整姿態(tài)和深度并實(shí)現(xiàn)動(dòng)態(tài)定位平衡。LM-AUV 原型機(jī)通過無浮力電纜與主機(jī)進(jìn)行實(shí)時(shí)通信與數(shù)據(jù)反饋，并通過上位機(jī)自帶的通信軟件對(duì)LM-AUV 的運(yùn)行參數(shù)、控制參數(shù)等進(jìn)行實(shí)時(shí)調(diào)控。當(dāng)LM-AUV 運(yùn)行時(shí)，深度、姿態(tài)和PWM 值可實(shí)時(shí)顯示在上位機(jī)的通信軟件中，同時(shí)該原型機(jī)帶有TF 卡模塊，可以將工作運(yùn)行時(shí)的數(shù)據(jù)以及日志存儲(chǔ)在TF 卡中備份，以便后續(xù)分析數(shù)據(jù)時(shí)使用。最后，LM-AUV 原型機(jī)使用集成在其機(jī)艙內(nèi)的5 塊DC7.2V 模型電池進(jìn)行供電。其中4 塊電池分別對(duì)應(yīng)4 個(gè)推進(jìn)器獨(dú)立供電，剩余1 塊電池則負(fù)責(zé)MCU 等電氣系統(tǒng)的供電.通信協(xié)議采用TTL-422，該協(xié)議降低了遠(yuǎn)距離傳輸難度，從而實(shí)現(xiàn)上位機(jī)對(duì)LMAUV 的實(shí)時(shí)監(jiān)控以及控制。

圖3 LM-AUV 電氣系統(tǒng)組成簡述圖Fig.3 The composition of LM-AUV electrical system

3.2 控制系統(tǒng)設(shè)計(jì)

LM-AUV 的運(yùn)動(dòng)控制僅包含升沉運(yùn)動(dòng)、橫搖運(yùn)動(dòng)和縱搖運(yùn)動(dòng)這3 個(gè)自由度的控制，所涉及的參數(shù)有深度、橫傾角、縱傾角以及電機(jī)PWM 值等。其中，位移、角位移控制回路主要實(shí)現(xiàn)的是對(duì)該無人水下試驗(yàn)平臺(tái)進(jìn)行自動(dòng)定位控制，即輸入一個(gè)預(yù)定深度或角度值。通過控制系統(tǒng)的調(diào)整后輸出每個(gè)推進(jìn)器所需分配的PWM 波，從而實(shí)現(xiàn)這3 個(gè)自由度的定位控制。其控制回路圖如圖4 所示。

圖4 LM-AUV 的定位閉環(huán)控制示意圖Fig.4 The diagram of positioning closed-loop control of LM-AUV

圖中定深減搖控制器控制系統(tǒng)采用比例—積分—微分（Proportional-integral-differential,PID）控制。其是一種基于線性模型設(shè)計(jì)的控制算法，其結(jié)構(gòu)簡單、參數(shù)性質(zhì)明確、便于現(xiàn)場調(diào)試[15]。PID 控制器是線性的，即誤差量輸入與控制量輸出是線性相關(guān)的。將誤差量輸入到控制器后，通過控制STM32 單片機(jī)（主控MCU）向LM-AUV 的4 個(gè)無刷直流推進(jìn)器輸出PWM 波，以達(dá)到控制LM-AUV 的運(yùn)動(dòng)狀態(tài)，從而實(shí)現(xiàn)定深減搖的目的。

在弱磁型無人水下航行器的控制系統(tǒng)設(shè)計(jì)中，PID 控制器擁有結(jié)構(gòu)簡單、控制性優(yōu)、魯棒性強(qiáng)等特點(diǎn)被廣泛應(yīng)用[16]。將PID 控制器中的比例、微分、積分環(huán)節(jié)進(jìn)行拉普拉斯變換后可得出該控制器的控制算法為：

式中：Kp為比例系數(shù)；Ki為積分系數(shù)；Kd為微分系數(shù)；u為輸出控制量。

PID 控制器中的比例、積分、微分部分都起著不同作用[17–18]：

1）比例部分可成比例反映出控制系統(tǒng)的偏差值，當(dāng)輸出值與輸入值有誤差時(shí)，可以快速將控制其偏差，以減小輸出值的波動(dòng)。但是，如果Kp過大，會(huì)增加輸出值的超調(diào)，使系統(tǒng)的整體穩(wěn)定性降低。

2）積分部分可利用誤差值的積分量進(jìn)行控制，消除該控制系統(tǒng)的穩(wěn)態(tài)誤差，提高該控制系統(tǒng)的無差度。但是Ki過大會(huì)導(dǎo)致該控制系統(tǒng)穩(wěn)定性下降，導(dǎo)致其動(dòng)態(tài)響應(yīng)變慢，故Ki值一般取適當(dāng)大小即可。

3）微分部分可利用誤差值的微分量進(jìn)行控制，提高該控制系統(tǒng)的響應(yīng)速度，同時(shí)提高其穩(wěn)定性，加快輸出值達(dá)到預(yù)定值（即輸入值）的時(shí)間。與前2 項(xiàng)不同的是，微分部分不可以單獨(dú)存在，它經(jīng)常與比例部分并存。若Kd過大，會(huì)導(dǎo)致該控制系統(tǒng)波動(dòng)時(shí)間變長，不能穩(wěn)定在理想值之間。故Kd值和Kp值的調(diào)試需要同時(shí)進(jìn)行，以達(dá)到對(duì)該系統(tǒng)的最優(yōu)控制效果。

4 實(shí)驗(yàn)及數(shù)據(jù)處理

對(duì)LM-AUV 進(jìn)行一系列實(shí)驗(yàn)，該AUV 通過深度傳感器、姿態(tài)傳感器對(duì)深度信息和姿態(tài)角信息進(jìn)行捕獲、并交由集成與AUV 控制系統(tǒng)中的MCU 進(jìn)行處理，以PWM 波形式傳遞給4 個(gè)推進(jìn)器，實(shí)現(xiàn)其在水下的定深、減搖等功能。本文所搭建的四旋翼式小型LM-AUV 在華中科技大學(xué)船舶水池實(shí)驗(yàn)室完成了水下3 m 的定深減搖實(shí)驗(yàn)，深度誤差控制在0.06 m（深度傳感器精度誤差0.01 m）、姿態(tài)角誤差控制在5°，并進(jìn)一步完成了水下3 m 的基礎(chǔ)上進(jìn)行一系列的姿態(tài)角穩(wěn)態(tài)控制、所達(dá)成的誤差控制在10°以內(nèi)。特別地，為了增強(qiáng)該LM-AUV 的穩(wěn)定性，需要在下水前使用鉛塊進(jìn)行重量配平，使該LM-AUV 在水中處于正浮力狀態(tài)，并采用無浮力電纜，避免在該LM-AUV 工作過程中因?yàn)榫€纜的因素而降低控制系統(tǒng)的穩(wěn)定性。

由于四旋翼式水下機(jī)器人正常工作與調(diào)試均需要與串口進(jìn)行大量交互，在保留通用串口助手基本功能的基礎(chǔ)上，開發(fā)了DEBUG 模式、IMU 控制命令模式、深度解算板配置模式、電調(diào)配置模式，以及四旋翼式水下機(jī)器人系統(tǒng)狀態(tài)輸出界面等，用于實(shí)現(xiàn)該LM-AUV 的各種功能并實(shí)時(shí)監(jiān)測(cè)該LM-AUV 的工作狀態(tài)。

雙向電調(diào)器的有效PWM 占空比在0.05～0.1 之間（即本實(shí)驗(yàn)中相應(yīng)的PWM 值為1 000～2 000），中間值為0.75（PWM 值為1 500）。為了進(jìn)一步增強(qiáng)該LM-AUV 的續(xù)航性，MCU 對(duì)這4 個(gè)推進(jìn)器所輸出的PWM 占空比被限制在0.065～0.085（即PWM 值在1 300～1 700）。

圖5 為期望深度3 m，俯仰角以及橫搖角均設(shè)為0°的定深實(shí)驗(yàn)數(shù)據(jù)。

圖5 LM-AUV 定深實(shí)驗(yàn)數(shù)據(jù)曲線Fig.5 LM-AUV depth determination experiment data curve

在定深實(shí)驗(yàn)完成的基礎(chǔ)上，進(jìn)行將橫搖角和俯仰角分別定為25°和15°的定姿態(tài)角實(shí)驗(yàn)。相關(guān)實(shí)驗(yàn)結(jié)果如圖6 所示。

圖6 LM-AUV 定深定姿態(tài)角實(shí)驗(yàn)數(shù)據(jù)曲線Fig.6 LM-AUV fixed depth and attitude angle experimental data curve

根據(jù)2 次實(shí)驗(yàn)整理關(guān)鍵性能參數(shù)，如表2 所示。

表2 關(guān)鍵性能參數(shù)對(duì)比表Tab.2 Key performance parameter comparison table

LM-AUV 在接收到定深3 m 的信號(hào)后，由MCU輸出的PWM 波驅(qū)動(dòng)4 個(gè)推進(jìn)器達(dá)到下潛的效果。由于該LM-AUV 在下潛的過程中受到水中暗流擾動(dòng)，會(huì)導(dǎo)致其姿態(tài)角產(chǎn)生變化，由AHRS 模塊收集姿態(tài)角誤差傳入MCU 中，對(duì)4 個(gè)推進(jìn)器分別進(jìn)行PWM 波的調(diào)整，以達(dá)到在運(yùn)動(dòng)中姿態(tài)穩(wěn)定的效果。從圖表可知，該LM-AUV 在定深的過程中俯仰角和橫傾角都得到了穩(wěn)定控制，均在0°上下波動(dòng)，深度超調(diào)量控制在0.2 m 以內(nèi)，并在30 s 內(nèi)穩(wěn)定在指定深度。因此可得出結(jié)論：該控制系統(tǒng)對(duì)深度控制具有魯棒性，并且控制過程相對(duì)穩(wěn)定，結(jié)果可靠。

同樣分析可知，該LM-AUV 在3 m 定深的前提下，由上位機(jī)向其發(fā)送將橫搖角和俯仰角分別穩(wěn)定在25°和15°的指令后，在20 s 內(nèi)完成對(duì)橫傾角以及俯仰角的控制，使其進(jìn)入相對(duì)應(yīng)的穩(wěn)定區(qū)間。由于水池中存在水流干擾以及使用了低成本的AHRS，導(dǎo)致數(shù)據(jù)噪聲較多。當(dāng)LM-AUV 進(jìn)入穩(wěn)態(tài)狀態(tài)后，橫搖角以及俯仰角分別存在10°以及7°的動(dòng)態(tài)誤差，同時(shí)由于PID 控制器調(diào)整得當(dāng)，橫搖角以及俯仰角分別存在1°以及-1°的穩(wěn)態(tài)誤差，超調(diào)量均控制在10°～15°以內(nèi)。由此得出結(jié)論，該LM-AUV 控制系統(tǒng)對(duì)橫搖角與俯仰角的z姿態(tài)控制具有穩(wěn)定性，系統(tǒng)的魯棒性較強(qiáng)，且結(jié)果可靠。

5 結(jié)語

本文提出一種將PID 用于LM-AUV 深度控制和姿態(tài)控制的穩(wěn)態(tài)系統(tǒng)，首先使用數(shù)值分析的方法對(duì)其有效性進(jìn)行證明，并設(shè)計(jì)實(shí)驗(yàn)以驗(yàn)證所提出的控制系統(tǒng)穩(wěn)定性。同時(shí)將該P(yáng)ID 控制器應(yīng)用于LM-AUV 實(shí)體模型中，并完成了相對(duì)應(yīng)的深度控制實(shí)驗(yàn)以及姿態(tài)控制實(shí)驗(yàn)，得出結(jié)果均為可靠。未來將繼續(xù)改良該LMAUV 原型機(jī)的控制系統(tǒng)，進(jìn)一步運(yùn)用模糊PID 控制、滑模控制等現(xiàn)代控制算法進(jìn)行優(yōu)化，使其控制效果更優(yōu)，響應(yīng)速度加快同時(shí)壓低超調(diào)量，減少穩(wěn)態(tài)誤差并降低其波動(dòng)性。將對(duì)該LM-AUV 硬件進(jìn)一步升級(jí)，如使用高性能的傳感器增強(qiáng)其數(shù)據(jù)回傳的精確度，減少各狀態(tài)量回傳的噪點(diǎn)，并增加數(shù)據(jù)傳輸頻率等。