面向海底光學探測使命的自治水下機器人水平路徑跟隨控制

馬艷彤， 鄭榮， 韓曉軍

(1.中國科學院 沈陽自動化研究所 機器人學國家重點實驗室， 遼寧 沈陽 110016；2.東北大學 機械工程與自動化學院， 遼寧 沈陽 1108191)

面向海底光學探測使命的自治水下機器人水平路徑跟隨控制

馬艷彤1,2， 鄭榮1， 韓曉軍1

(1.中國科學院 沈陽自動化研究所 機器人學國家重點實驗室， 遼寧 沈陽 110016；2.東北大學 機械工程與自動化學院， 遼寧 沈陽 1108191)

針對利用光學探測設備對海底目標探測與搜索的使命，自治水下機器人(AUV)需具備更精確的航行控制性能，為此提出一種不依賴模型的改進型PID控制算法，通過對增設的左右水平推進器進行控制，以實現(xiàn)AUV低速時水平面的精確航行。將整個控制器分為兩層：內(nèi)層為偏航距離PID控制器，將輸出量轉化為所需偏轉角；外層為航向PID控制器，將內(nèi)層計算結果轉化為航向偏差，對其進行PID計算，輸出為偏轉所需推力和推力矩值。通過對設定探測路線進行路徑跟隨，以反映水平面航行控制精度。通過湖上試驗，得出精確的路徑跟隨航跡，實現(xiàn)了航向角偏差均值為0.09°、航向距離偏差均方差為0.29 m的航行穩(wěn)定控制，驗證了該控制方法的可行性。

控制科學與技術； 自治水下機器人； 海底光學探測； 改進型PID； 路徑跟隨； 水平推進器

0 引言

目前，利用自治水下機器人(AUV)搭載探測設備完成海底高精度探測和微小目標識別的需求日益強烈，而海底探測手段主要為聲學探測設備。但聲學存在諸多局限性，如聲納成像的像素信息少，近距離探測時易受環(huán)境噪聲和目標背景物的干擾等[1-2]。與聲學探測設備相比，光學設備則具有分辨率高、信息豐富的特點，但成像范圍較小，僅適用于米級的精確探測[1,3]。因此，若采用光學探測設備完整、高效地完成指定區(qū)域的目標探測與識別，則要求AUV具備良好的近底定高穩(wěn)定航行性能，包括：穩(wěn)定的高度和俯仰控制，以滿足海底近距離探測的需求；精確的分米級精度的路徑跟隨性能，即要求航行體航向偏差小和轉彎性能穩(wěn)定，以實現(xiàn)在指定區(qū)域的全覆蓋、無縫探測與目標搜索。對于前者，鄭榮等[4]已實現(xiàn)了AUV垂直面穩(wěn)定的近底定高航行，能夠很好地滿足要求，具體參見文獻[4]。因此，本文的重點是AUV水平面的精確路徑跟隨控制研究。

AUV的水下航行是一個復雜的空間六自由度運動，本身具備強耦合性和高度非線性，難以建立精確的數(shù)學模型，這使得AUV的精確航行控制問題非常復雜。對此，國內(nèi)外研究學者提出了多種智能算法。文獻[5-8]采用李亞普諾夫穩(wěn)定性理論和反步法設計了欠驅(qū)動AUV的路徑跟隨控制；文獻[9]針對傳統(tǒng)直線航跡控制力矩方法較為復雜的問題，提出基于級聯(lián)系統(tǒng)理論的控制力矩設計方法；文獻[10]提出自適應模糊反演滑模控制系統(tǒng)設計，以解決AUV水平面航跡跟蹤問題；文獻[11]針對AUV為多輸入多輸出系統(tǒng)特性，提出自適應模糊PID控制算法，用于AUV航向控制，以解決PID參數(shù)整定的不確定性問題。以上這些算法在仿真時實現(xiàn)了精確的路徑跟隨控制，驗證了其理論的可行性，但并未經(jīng)過實際航行試驗的檢驗，且以上這些航跡跟蹤控制算法對AUV的運動、動力模型精度要求較高，具有較強的模型針對性。對于載體模塊化的AUV，由于其可根據(jù)不同需求加載不同載荷，使得模型并不固定。因此基于模型的控制算法難以在工程上很好地應用于模塊化AUV。而PID控制算法具有原理簡單、使用方便、應用廣泛、魯棒性強等優(yōu)點，可實現(xiàn)不依賴對象模型的控制，被廣泛用于AUV工程實踐中。國內(nèi)外廣泛采用航向單閉環(huán)PID對AUV進行路徑跟隨控制，但航線偏差較大，路徑跟隨的控制精度難以滿足應用要求[12-13]。文獻[14]提出了基于雙閉環(huán)的矢量推進器的AUV路徑跟隨，做到了10 m以內(nèi)的航線偏差控制，其精度仍不能滿足光學探測需求。

基于以上問題，本文在傳統(tǒng)PID控制的基礎上進行改進，提出雙層PID的控制算法，即通過雙層PID嵌套，控制AUV的航向偏差和轉彎穩(wěn)定性，以實現(xiàn)AUV水平面精確路徑跟隨控制。另外，為保證光學攝像機的成像效果，將AUV巡航探測速度設為1 kn以下。為解決低速下舵效不足[15]的問題，在載體重心左右對稱位置設置左右水平推進器。在此基礎上，通過湖上試驗驗證該控制方法的可行性。

1 問題描述

所謂路徑跟隨，是指為控制器設計一個控制輸入，使AUV能夠從給定的初始狀態(tài)出發(fā)，到達并跟隨指定坐標系下給定的參考路徑，而對到達目標位置的時間不做要求[16]。盡管AUV的實際水下航行為三維空間運動，但為突出研究重點，本文不討論航行體的深度控制，且忽略橫滾的影響，主要研究AUV水平面的路徑跟隨控制問題。

1.1 設定路徑描述

為實現(xiàn)對指定區(qū)域的全覆蓋探測與搜索，巡航路線應盡量密集。本文設計的“織網(wǎng)”式巡航路線，即在指定區(qū)域回紋穿梭航行，每條航線之間的距離略大于光學設備的視場寬度。“織網(wǎng)”式巡航路線如圖1所示。

圖1 “織網(wǎng)”式行進航線Fig.1 “weaving” cruise route

航行體按照：起點-1-2-3-…-16-a-b-c-…-h-終點的“套圈”式航行。每條航線間距5 m，通過東西向和南北向的交叉，可實現(xiàn)行進區(qū)域的全覆蓋搜索。同時，該航線的設計能夠很好地驗證航行體直航階段的航向偏差控制和轉彎的轉向控制。在直航階段，航向偏差可作為衡量其精確路徑跟隨的穩(wěn)定性指標；在轉彎階段，轉向性能主要體現(xiàn)在轉彎半徑的穩(wěn)定控制。

1.2 航行誤差分析

為便于問題描述，根據(jù)國際水池會議(ITTC)和造船與輪機工程學會(SNAME)術語體系，在水平面內(nèi)建立兩種坐標體系：固定坐標系Εηξ為慣性坐標系；運動坐標系OXY，將原點選在AUV重心處，為載體坐標系。在此坐標體系下分析航行體的路徑跟隨情況。

1.2.1 直航誤差分析

圖2 直航階段路徑跟隨示意圖Fig.2 Schematic diagram of AUV straight path following

1.2.2 轉彎誤差分析

由于航行體受水流和自身慣性影響，無法做到直角轉彎，其實際航線會呈弧線形。本文通過控制其實際的轉彎半徑來控制每條航跡間距離。根據(jù)前面的航線設計，為保證每條航線間距為5 m，此處應實現(xiàn)航行體轉向穩(wěn)定且轉彎半徑在2.5 m附近。其轉向示意圖如圖3所示。

圖3 轉彎示意圖Fig.3 Schematic diagram of AUV steering

2 水平面路徑跟隨航行控制

在水平面航行控制中，航行體的前向推力由主推進器提供，轉向力和力矩由左右水平推進器提供。針對實際航跡與目標航線偏差，設計控制器，以實現(xiàn)對設定航線的精確路徑跟隨控制。即由控制器控制左右水平電機轉速調(diào)節(jié)航線偏差和驅(qū)動轉彎。

2.1 雙層PID控制器設計

AUV在水下航行過程中偏離目標航線時，按照偏航角φe和偏航距離l可將出現(xiàn)航向偏差的情形分為3種(見圖4)。

圖4 航向偏差示意圖Fig.4 Schematic diagram of course deviation

傳統(tǒng)的PID控制器多是航向單閉環(huán)PID控制器，即是將航向角偏差作為PID誤差反饋因子，在控制策略上未考慮由距離偏差造成的航線偏差問題，不能精確地實現(xiàn)路徑跟隨控制。因此，本文在航向角PID控制的基礎上，改善控制策略，將距離偏差引入到控制器中，采用雙層PID控制。即將航向角偏差和距離偏差作為反饋量同步關聯(lián)到航線調(diào)整中。另外，直接將AUV航向調(diào)整所需推力作為控制器的輸出量，以實現(xiàn)精確的路徑跟隨控制。雙PID控制框圖如圖5所示。圖5中：θp為要偏轉的角度；θt為目標航向角；Ht為目標航向；H為當前航向；e為航向偏差。

圖5 雙層PID控制框圖Fig.5 Block diagram of bilayer PID control strategy

2.2 控制器算法實現(xiàn)

整個控制器由兩部分組成：第1部分為內(nèi)層PID控制器，用于計算所需偏轉角，即對偏航距離進行PID處理，將輸出定為所需偏轉角；第2部分為外層PID控制器，用于計算AUV航向調(diào)整所需推力和推力矩，即將內(nèi)層PID計算結果轉換為目標航向，與實際航向偏差作為反饋因子，PID處理后將輸出作為推力和推力矩。

2.2.1 航向角偏差計算

如前面所述，當出現(xiàn)航向偏差時，航行體實際位置與設定路線存在一定距離l，通過l計算航行體所需偏轉角度。其中l(wèi)由AUV導航系統(tǒng)給出。

這里根據(jù)l的大小采用分段PID算法，具體調(diào)節(jié)過程為

l<10 m時，

(1)

l>10 m時，

θp=K′·l.

(2)

式中：lm表示第m個積分量；lk表示第k個微分量；T為采樣周期；Kp，θp、Ki,θp、Kd,θp為偏航角PID參數(shù)；K′為比例放大因子，可通過多次試驗獲得其準確值。

2.2.2 轉向控制

計算出所需偏轉角后，可由目標航向角計算出目標航向，再通過目標航向與當前航向的比較，得出航向偏差，其計算公式為

Ht=θt-θp，

(3)

e=Ht-H.

(4)

通過控制算法，求出AUV對應的轉向所需轉矩的控制量M，此處按航向偏差e大小進行分段控制：

式中：Kp、Ki、Kd為航向PID參數(shù)；Kδ為比例增益，可通過多次試驗獲得其準確值。以上參數(shù)均針對航行體具體轉向性能而定。如若實現(xiàn)轉彎半徑，調(diào)整控制參數(shù)即可。

由于采樣周期較短，微分項會產(chǎn)生較大的噪聲信號，影響控制效果，因此這里采用最小二乘法對角速度進行濾波平滑，以期獲得較好的控制效果[17]。

求出轉向力矩后，可計算出分配給左右推進器的轉向力為T=M/L，L為左右推進器距載體中軸線距離。計算出左右推進器轉速大小相等，方向相反。以此實現(xiàn)航行體的轉向控制。

本文中，推進器螺旋槳轉速與力的函數(shù)關系式采用參數(shù)辨識的方法獲得，具體不再贅述。

AUV航線偏差調(diào)整的控制流程圖如圖6所示。

圖6 AUV航向控制流程圖Fig.6 Flow chart of AUV course control

本文設計的雙層PID控制器完全獨立于控制模型。即無需對模型進行復雜的運動學、動力學分析，只需根據(jù)控制要求調(diào)整PID參數(shù)，即可實現(xiàn)控制器對模型的運動控制。

3 試驗結果分析

本試驗用AUV長度9.5 m，直徑為534 mm，空氣中凈質(zhì)量1.5 t，最大下潛深度為200 m，左右推進器最大轉速為1 800 r/min，其外形結構和左右推進器布放位置如圖7所示。在某湖泊進行了AUV的湖上試驗，設定航速為v=0.514 m/s. 通過試驗驗證航行體在低速下水平面路徑跟隨控制方案的可行性。

圖7 AUV外形結構俯視圖Fig.7 Top view of AUV shape structure

本文針對航行體在不考慮垂向高度和縱傾變化的情況，分析其水平面的航行控制穩(wěn)定性。將設定的“織網(wǎng)”式回紋行進航線作為目標航線。將實際航線與目標航線分析比較，得出AUV的路徑跟隨性能。這里主要對航向角偏差、轉彎角速度和轉彎半徑穩(wěn)定性進行分析。同時，由于為實現(xiàn)低速下航向偏差調(diào)整和轉彎的精確控制，增設了左右水平推進器，為此對其控制進行分析，以驗證推進器對航行體的驅(qū)動控制性能。

3.1 偏航誤差分析

受多種因素影響，AUV在航行中，會偏離設定航線。通過實際航向與目標航向的數(shù)據(jù)比較，可得出航向角偏差。通過試驗，驗證偏航誤差的調(diào)整和穩(wěn)定性控制性能。試驗結果如圖8所示。

圖8 航向誤差曲線Fig.9 Curve of AUV course deviation

從圖8中可以看出，航行體在航行至11 s附近出現(xiàn)了航向偏差，但隨后通過控制算法的調(diào)節(jié)，航向誤差值逐漸減小，最終在45 s附近趨于穩(wěn)定，且航向偏差穩(wěn)定在零值附近，均方差為0.09°，表明該控制系統(tǒng)的準確性與快速響應性。

3.2 轉彎穩(wěn)定性分析

3.2.1 轉彎角速度穩(wěn)定性

水平轉彎角速度ω由AUV上的慣性組合導航模塊采集得到。圖9為一組典型實航時轉彎角速度數(shù)據(jù)曲線。從圖9可以看出，水平角速度波動小，轉向平穩(wěn)。驗證了PID算法控制轉彎角速度的可行性與控制的精確性。

圖9 轉彎角速度曲線Fig.9 Curve of AUV turning angular velocity

3.2.2 轉彎半徑穩(wěn)定性分析

由于轉彎半徑無法通過傳感器直接采集，需對其進行計算。轉彎半徑r由弧長的計算公式推導得出：

(6)

式中：s為轉彎走過的弧長，由慣性組合導航單元與多普勒計程儀共同測得；Δt為轉彎航段所用時間。

表1為航行體在不同轉彎航段下的轉彎半徑。從試驗結果可以看出，轉彎半徑穩(wěn)定在2.5 m附近，表明了轉向控制的精確性和穩(wěn)定性。其中，南北航向均方差為0.099 m，東西航向轉彎半徑均方差為0.067 m，波動很小，表明方向的改變對轉彎半徑的大小和穩(wěn)定性影響很小。

表1 AUV不同航段下轉彎半徑Tab.1 AUV turning radii at different time

3.3 左右推進器驅(qū)動控制分析

AUV的精確轉向是通過PID控制左右推進器轉速實現(xiàn)的。通過不同的推進器轉速來調(diào)整相對應的航向偏差。

圖10反映了直航階段航向偏差調(diào)整時，航向角誤差和左右推進器轉速的變化關系。由圖10可以看出：左右電機轉速隨著航向偏差變化而變化，當航向誤差增大時，左右推進器轉速快速增加，以調(diào)節(jié)航向偏差值；同樣，當航向誤差減小時，轉速變低，很好地驗證了PID算法的準確性和快速響應性。

圖10 航向偏差與電機轉速關系曲線Fig.10 Course deviation vs. motor speed

圖11反映了轉彎階段航向角和左右推進器轉速的變化關系。由于轉彎時，航向角變化較大，通過轉向算法可知，當航向角大于15°時，左右推進器轉速達到最大值，以實現(xiàn)快速平穩(wěn)地轉向。

圖11 轉彎時電機轉速Fig.11 Motor speed when turning

圖12為AUV實際的航跡和設定航線對比圖，其中實線為實際航跡，虛線為設定航線。該設定航線即為前面所述的間隔為5 m的“織網(wǎng)”式巡航路線。

圖12 AUV實際航跡和設定航線對比圖(實線為實際航跡，虛線為設定航線)Fig.12 Comparison of AUV actual tracking and set routes (solid line: actual path; dotted line: set path)

由圖12可以看出，實際航跡能夠很好地跟隨設定航線，主要體現(xiàn)在：其水平角速度均方差為0.131°/s；轉彎半徑穩(wěn)定在2.5 m；偏航角均值為0.09°；偏航距離均方差為0.29 m，實現(xiàn)了精確的分米級路徑跟隨。

4 結論

本文針對AUV搭載水下光學探測設備進行目標探測時航行控制精度要求高的問題，提出采用雙層PID控制算法。并通過湖上試驗對該控制方案的可行性進行了驗證，在對試驗結果進行分析后，可得出以下結論：

1)利用左右水平推進器提供航行體低速下的轉向驅(qū)動力，通過雙層PID控制器實現(xiàn)了精確的航向偏差調(diào)整和轉彎穩(wěn)定性控制。

2)該控制方案提高了路徑跟隨的控制精度，實現(xiàn)了AUV分米級跟隨，滿足AUV搭載光學設備高效精細探測海底的要求。

References)

[1] 徐玉如, 李彭超. 水下機器人發(fā)展趨勢[J]. 自然雜志, 2011, 33(3):125-132. XU Yu-ru, LI Peng-chao. The development of underwater robots[J]. Chinese Journal of Nature, 2011, 33(3):125-132.(in Chinese)

[2] Fillinger L, Hunter A J, Zampolli M, et al. Passive acoustic detection of closed-circuit underwater breathing apparatus in an operational port environment[J]. Acoustical Society of America, 2012, 132(4):310-315.

[3] 孫傳東, 陳良益, 高立民. 水的光學特性及其對水下成像的影響[J]. 應用光學, 2000, 21(4):39-46. SUN Chuan-dong, CHEN Liang-yi, GAO Li-min. Water optical properties and their effect on underwater imaging[J]. Journal of Applied Optics, 2000, 21(4):39-46. (in Chinese)

[4] 鄭榮, 馬艷彤, 張斌. 基于垂向推進方式的AUV低速近底穩(wěn)定航行[J]. 機器人, 2016, 38(5):588-592. ZHENG Rong, MA Yan-tong, ZHANG Bin. AUV’s near-bottom and low-speed sailing stability control based on vertical thruster[J]. Robot, 2016, 38(5):588-592. (in Chinese)

[5] Lionel L,Didik S. Nonlinear path-following control of an AUV[J]. Ocean Engineering, 2007, 34(11/12):1734-1744.

[6] Silvestre C, Pascoal A. Control of the INFANTE AUV using gain scheduled static output feedback[J]. Control Engineering Practice, 2004, 12(12):1501-1509.

[7] Repoulias F, Papadopoulos E. Planar trajectory planning and tracking control design for underactuated AUVs[J]. Ocean Engineering, 2007, 34(11/12):1650-1667.

[8] Bidyadhar S. Debabrata A. Design of a path following controller for an underactuated AUV[J]. Archives of Control Sciences, 2009, 19( 3): 245-259.

[9] Khodayari M H, Balochian S. Modeling and control of autonomous underwater vehicle (AUV) in heading and depth attitude via self-adaptive fuzzy PID controller[J]. Journal of Marine Science and Technology, 2015, 20(3):559-578.

[10] 付江鋒, 嚴衛(wèi)生, 趙濤. 欠驅(qū)動AUV的直線航跡跟蹤控制[J]. 計算機仿真, 2009, 26(10):145-147. FU Jiang-feng, YAN Wei-sheng, ZHAO Tao. Line tracking control of underactuated AUV[J]. Computer Simulation, 2009, 26(10):145-147. (in Chinese)

[11] 萬磊, 崔士鵬, 張國成, 等. 欠驅(qū)動水下機器人航跡跟蹤控制[J]. 電機與控制學報, 2013, 17(2):103-110. WANG Lei, CUI Shi-peng, ZHANG Guo-cheng, et al. Path following control of underactuated autonomous underwater vehicles [J]. Electric Machines and Control, 2013, 17(2):103-110. (in Chinese)

[12] Wang B, Su Y M, Wan L, et al. Adaptive PID control system for an autonomous underwater vehicle[J]. High Technology Letter, 2011, 17(1):7-12.

[13] Chen Q, Chen T, Zhang Y. Research of GA based PID for AUV motion control[C]∥Proceedings of the 2009 IEEE International Conference on Mechatronics and Automation.Changchun: IEEE, 2009:4446-4451.

[14] 陳世利, 衛(wèi)民, 李一博. 基于雙閉環(huán)的矢量推進器的AUV 轉向控制方法[J]. 天津大學學報:自然科學與工程技術版, 2014, 47(6):530-534. CHEN Shi-li, WEI Min, LI Yi-bo. Steering control strategy of AUV with vectored thruster based on double-loop mode [J]. Journal of Tianjin University :Science and Technology, 2014, 47(6):530-534. (in Chinese)

[15] 晏亮, 趙琳, 程建華. 潛艇操縱運動的舵效仿真研究[J]. 計算機仿真, 2009, 26(7):12-15. YAN Liang, ZHAO Lin, CHENG Jian-hua. Rudder effects simulation of submarine maneuvering motion [J].Computer Simulation, 2009, 26(7):12-15. (in Chinese)

[16] 王芳, 萬磊, 李曄. 欠驅(qū)動AUV的運動控制技術綜述[J].中國造船, 2010, 51(2):227-241. WANG Fang, WAN Lei, LI Ye. Asurvey on development of motion control for underactuated AUV[J]. Shipbuilding of China, 2010, 51(2):227-241. (in Chinese)

[17] 劉健, 于闖, 劉愛民. 無纜自治水下機器人控制方法研究[J].機器人, 2004, 26(1):7-10. LIU Jian, YU Chuang, LIU Ai-min. Research on untethered autonomous underwater vehicle control method[J]. Robot, 2004, 26(1):7-10.(in Chinese)

Horizontal Trajectory Tracking Control of Autonomous UnderwaterVehicle Based on Seabed Optical Detection Mission

MA Yan-tong1,2， ZHENG Rong1， HAN Xiao-jun1

(1.State Key Laboratory of Robotics, Shenyang Institute of Automation, Chinese Academy of Sciences,Shenyang 110016, Liaoning, China;2.School of Mechanical Engineering and Automation, Northeastern University, Shenyang 110819, Liaoning, China)

Autonomous underwater vehicles (AUVs) need to be provided with more accurate navigation control due to the limited operating range of optical detection device for detecting the seabed. An improved bilayer PID control algorithm which is independent of the object model is proposed. By controlling the channel propellers which are retrofitted to flanks around AUV, the controller can achieve AUV precise navigation control at low speed. The controller consists of two layers: the inner layer is a PID controller for distance deviation computation, which converts the output to desired deflection angle; the outer layer is a PID controller which computes the desired thrust and torque by inputting the output value of inner layer. The horizontal navigation precision can be reflected by tracking the planned detection route. The precise tracking path was obtained in lake trial. The mean value of course angular deviation is 0.09 °, and the mean square errors of deviation from the course is 0.29 m, which verifies the feasibility of the control scheme.

control science and technology; autonomous underwater vehicle; seabed optical detection; improved PID; trajectory tracking; horizontal propeller

2016-10-11

中國科學院國防科技創(chuàng)新基金項目(CXJJ-16M221)

馬艷彤(1991—)，女，碩士研究生。E-mail: mayantong@sia.cn

鄭榮(1963—)， 男， 研究員， 博士生導師。 E-mail: zhengr@sia.cn

TP273+.1

A

1000-1093(2017)06-1147-07

10.3969/j.issn.1000-1093.2017.06.014