基于雙閉環(huán)的矢量推進(jìn)器的AUV轉(zhuǎn)向控制方法

陳世利，衛(wèi) 民，李一博，張震宇(. 天津大學(xué)精密測試技術(shù)及儀器國家重點實驗室，天津 30007；. 威斯康辛州立大學(xué)拉克羅斯分校軟件工程學(xué)院，威斯康辛州 5460)

基于雙閉環(huán)的矢量推進(jìn)器的AUV轉(zhuǎn)向控制方法

陳世利1，衛(wèi) 民1，李一博1，張震宇2
(1. 天津大學(xué)精密測試技術(shù)及儀器國家重點實驗室，天津 300072；2. 威斯康辛州立大學(xué)拉克羅斯分校軟件工程學(xué)院，威斯康辛州 54601)

水下無人自航行器(autonomous underwater vehicle，AUV)在對海洋環(huán)境進(jìn)行勘查與測量時，實際航跡與目標(biāo)航跡的偏差要求控制在一定精度范圍內(nèi)．在傳統(tǒng)常采用的航向單閉環(huán)方法控制下，基于矢量推進(jìn)器的AUV在高速轉(zhuǎn)向時存在航跡跟蹤效果差的缺點．針對這一問題，設(shè)計了一種雙閉環(huán)自適應(yīng)轉(zhuǎn)向運(yùn)動控制方法．該方法基于矢量推進(jìn)器機(jī)動性強(qiáng)的特性，在AUV轉(zhuǎn)向時將航向控制閉環(huán)和航速控制閉環(huán)設(shè)計成雙閉環(huán)，調(diào)整航向的同時依據(jù)航向環(huán)偏航角差實時控制航速環(huán) AUV轉(zhuǎn)向目標(biāo)速度．理論分析和實驗證明：在AUV轉(zhuǎn)向時，該控制算法可以更好地實現(xiàn)航跡跟蹤，實際航行軌跡與目標(biāo)航跡的最大偏差可以控制在10,m以內(nèi)．

水下無人自航行器；矢量推進(jìn)器；轉(zhuǎn)向；航跡跟蹤；雙閉環(huán)

水下無人自航行器(autonomous underwater vehicle，AUV)是一種與母船沒有任何物理連接，依靠自身攜帶的動力來自主航行的一種全新水下機(jī)器人．它具有活動范圍廣、自動化能力強(qiáng)及運(yùn)行噪聲小等特點，在海洋環(huán)境測量和軍事方面具有重要應(yīng)用價值[1-3]．目前AUV推進(jìn)系統(tǒng)主要采用兩種推進(jìn)技術(shù)：基于非矢量技術(shù)和基于矢量技術(shù)．相對于傳統(tǒng)的、應(yīng)用較成熟的非矢量推進(jìn)器，矢量推進(jìn)器具有操縱性和機(jī)動性能強(qiáng)的特性．從國內(nèi)外AUV發(fā)展?fàn)顩r來看，矢量推進(jìn)器在AUV領(lǐng)域已被實際應(yīng)用，具有較好的應(yīng)用前景[4-5]．

AUV在進(jìn)行海底勘查與測量時，決策者需要制定合理的局部路徑規(guī)劃策略使AUV能夠按照目標(biāo)航跡進(jìn)行海底掃描測量．一旦控制策略不當(dāng)造成實際航跡與目標(biāo)航跡偏差過大，勘測時就會出現(xiàn)盲區(qū)，造成重大損失．筆者課題組在設(shè)計AUV時性能指標(biāo)要求實際航跡與目標(biāo)航跡偏差的最大偏差不能超過10,m．對于基于矢量推進(jìn)器的AUV，為控制其在固定深度或高度沿目標(biāo)航跡航行，國內(nèi)外廣泛采用航向閉環(huán)PID控制方法[6-7]．當(dāng)AUV直線行駛時，由于矢量推進(jìn)器水平擺角動作幅度較小，傳統(tǒng)采用的航向閉環(huán)控制器可以達(dá)到偏差控制要求．但當(dāng)AUV保持既定的高任務(wù)目標(biāo)速度航行轉(zhuǎn)向時，由于需要調(diào)節(jié)的偏航角較大，而矢量推進(jìn)器水平最大擺角有限，筆者課題組通過近些年實驗發(fā)現(xiàn)傳統(tǒng)采用的航向閉環(huán)PID控制方法不足以控制AUV較好的轉(zhuǎn)向，會導(dǎo)致轉(zhuǎn)向偏差d偏大，轉(zhuǎn)向時間t偏長，超出了最大允許范圍．因此，為解決在傳統(tǒng)的航向單閉環(huán)方法控制下AUV高速轉(zhuǎn)向時存在航跡跟蹤效果差的問題，針對矢量推進(jìn)器，筆者設(shè)計了一種可以大幅度減小AUV轉(zhuǎn)向偏差、轉(zhuǎn)向時間，從而可以改善轉(zhuǎn)向航跡跟蹤效果的新型雙閉環(huán)運(yùn)動控制算法．本文首先從理論上對矢量推進(jìn)器轉(zhuǎn)向特性進(jìn)行了分析，然后在此基礎(chǔ)上對雙閉環(huán)控制方法進(jìn)行了設(shè)計，最后在云南撫仙湖進(jìn)行了湖試實驗，檢驗該方法的轉(zhuǎn)向航跡跟蹤效果，并和常規(guī)航向單閉環(huán)控制算法進(jìn)行了對比，證明本方法在轉(zhuǎn)向時可以控制AUV獲得良好的航跡跟蹤效果．

1 矢量推進(jìn)器轉(zhuǎn)向特性分析

圖1為AUV轉(zhuǎn)向示意．

圖1 AUV轉(zhuǎn)向示意Fig.1 Schematic diagram of AUV steering

矢量推進(jìn)器的突出特點是靠轉(zhuǎn)向電機(jī)擺動從而改變主推進(jìn)器的推進(jìn)方向，使AUV獲得一定轉(zhuǎn)向力矩．圖2為筆者課題組設(shè)計的AUV矢量推進(jìn)器左右擺動圖．

為研究矢量推進(jìn)器的轉(zhuǎn)向特性，假設(shè)大地為慣性坐標(biāo)系，AUV為常質(zhì)量剛體，不考慮洋流影響，研究AUV在固定深度或高度水平轉(zhuǎn)向問題，故筆者只考慮AUV做水平面運(yùn)動情況，建立如圖3所示的坐標(biāo)系．

圖2 矢量推進(jìn)器擺動示意Fig.2 AUV vectored thruster swing figure

分析AUV的整體受力，可以推導(dǎo)出AUV轉(zhuǎn)向時動力學(xué)方程[8-9]為

式中：F(n)為矢量推進(jìn)器產(chǎn)生的推力；n為螺旋槳轉(zhuǎn)速；Φ為AUV矢量推進(jìn)器轉(zhuǎn)向擺角；L為主推力分量到AUV重心O的距離；ω為AUV轉(zhuǎn)向角速度；T(ω)為AUV轉(zhuǎn)向時所受到的阻力力矩；I為AUV的轉(zhuǎn)動慣量；β為AUV轉(zhuǎn)向角加速度；Iβ為轉(zhuǎn)向力矩．根據(jù)螺旋槳理論，螺旋槳在水下受到的推力為

式中：KT為推力系數(shù)，是與進(jìn)速比JO相關(guān)的函數(shù)；ρ為海水密度；D為槳葉直徑．

由式(1)和式(2)可知，矢量推進(jìn)器轉(zhuǎn)向時需要的轉(zhuǎn)向力矩和AUV線速度v無關(guān)．區(qū)別于傳統(tǒng)帶鰭葉靠流體動力轉(zhuǎn)向的非矢量推進(jìn)器，基于矢量推進(jìn)器的AUV在航行線速度很低時，也可以很好地完成轉(zhuǎn)向．

2 雙閉環(huán)控制方法設(shè)計

從矢量推進(jìn)器轉(zhuǎn)向特性分析可知，矢量推進(jìn)器轉(zhuǎn)向力矩與AUV線速度無關(guān)，低速轉(zhuǎn)向好．對比傳統(tǒng)僅靠航向PID控制器控制AUV轉(zhuǎn)向的方法，筆者設(shè)計如下自適應(yīng)雙閉環(huán)算法．該方法原理是利用矢量推進(jìn)器低速轉(zhuǎn)向好的性能優(yōu)勢，在AUV轉(zhuǎn)向時將航向控制閉環(huán)和航速控制閉環(huán)設(shè)計成雙閉環(huán)，通過航向環(huán)偏航角差去控制航速環(huán)AUV目標(biāo)速度，AUV轉(zhuǎn)向調(diào)整航向的同時依據(jù)偏航角差的大小實時調(diào)節(jié)AUV轉(zhuǎn)向目標(biāo)航速．雙閉環(huán)控制策略如圖4所示．

圖4 雙閉環(huán)控制策略示意Fig.4 Heading/speeding control strategy diagram

圖4 中航向環(huán)輸入變量為AUV期望的偏航角，輸出變量為AUV矢量推進(jìn)器轉(zhuǎn)向角，反饋變量為AUV當(dāng)前偏航角．航速環(huán)輸入量為AUV目標(biāo)線速度，輸出作用對象為AUV矢量推進(jìn)主電機(jī)，從而可以改變AUV線速度，反饋變量為AUV當(dāng)前線速度．轉(zhuǎn)向時航向環(huán)偏航角差和航速環(huán)輸入變量AUV目標(biāo)速度關(guān)系為

式中：vT為轉(zhuǎn)向前一時刻AUV任務(wù)目標(biāo)速度；Δθ為目標(biāo)航向角與當(dāng)前航向角的差值；vP是隨著航向偏差角差Δθ變化而不斷調(diào)整的AUV轉(zhuǎn)向目標(biāo)速度；c為速度調(diào)整系數(shù)，該系數(shù)大小與偏航角相關(guān)．為了使AUV轉(zhuǎn)向時速度逐漸下降，從而不損壞AUV的整體機(jī)械性能且不使問題復(fù)雜化，使轉(zhuǎn)向目標(biāo)速度隨著偏航角的變化呈線性變化．

圖5所示為Δθ和系數(shù)c的函數(shù)關(guān)系．

(1) 當(dāng)0＜|Δθ|≤5°時，由于偏航角比較小，轉(zhuǎn)向時AUV目標(biāo)速度不做改變，此時c=1．由式(3)計算可得AUV在此偏航角范圍內(nèi)轉(zhuǎn)向時航速環(huán)的輸入變量為轉(zhuǎn)向目標(biāo)速度，即

(2) 當(dāng)30°＜|Δθ|≤180°時，由于偏航角過大，此時根據(jù)偏航角差Δθ的變化，實時控制航速環(huán)主推電機(jī)，迅速降低AUV目標(biāo)速度．為使其低速轉(zhuǎn)向，定c= k，k在0～1范圍內(nèi)．由式(3)計算可得AUV在此偏航角范圍內(nèi)轉(zhuǎn)向時航速環(huán)輸入變量轉(zhuǎn)向目標(biāo)速度，即

(3) 當(dāng)5°＜|Δθ|≤30°時，偏航角超出5°，此時需要實時逐漸減小AUV目標(biāo)速度，為使航速隨著偏航角的增大線性減小，c將從1到k線性減?。墒?3)和圖5計算可得AUV在此偏航角范圍內(nèi)轉(zhuǎn)向時航速環(huán)輸入變量轉(zhuǎn)向目標(biāo)速度，即

根據(jù)以上方法分析可知，當(dāng)AUV遇到轉(zhuǎn)彎開始轉(zhuǎn)向時，航向和航速控制形成雙閉環(huán)．轉(zhuǎn)向完成整個過程中，航向控制器的調(diào)節(jié)使偏航角差逐漸減小，系數(shù)c從k到1線性變化，相應(yīng)的航速閉環(huán)也在不斷調(diào)節(jié)航速，使矢量推進(jìn)器隨著偏航角差的變化以不同的低速轉(zhuǎn)向．轉(zhuǎn)向完成后，隨著偏航角差逐漸減小到零附近，航速環(huán)輸入量又相應(yīng)地調(diào)節(jié)到任務(wù)目標(biāo)速度．

圖5 系數(shù)c與|Δθ|的變化關(guān)系Fig.5 Relationship between c and |Δθ|

3 實驗驗證

本實驗AUV長度6.9,m，直徑800,mm，排水量1,400,kg，續(xù)航時間22,h(4節(jié)速度)，預(yù)計下水深度3,000,m．筆者課題組于2012年1月在云南撫仙湖進(jìn)行了湖試實驗，在實物控制系統(tǒng)上對上述雙閉環(huán)控制方法進(jìn)行了驗證．整個控制系統(tǒng)由甲板系統(tǒng)、主控系統(tǒng)、矢量推進(jìn)控制系統(tǒng)、導(dǎo)航控制系統(tǒng)以及傳感器系統(tǒng)組成[10]．圖6所示為湖試實驗現(xiàn)場．

圖6 AUV湖試實驗現(xiàn)場Fig.6 Lake experiment of AUV

3.1 AUV不同線速度轉(zhuǎn)向?qū)Ρ葘嶒?/p>

為了進(jìn)一步驗證基于矢量推進(jìn)器的AUV低速轉(zhuǎn)向效果更好，筆者進(jìn)行了如下實驗：使AUV在水面運(yùn)行，通過航速PID控制器調(diào)速，使用多普勒計程儀(DVL)來測量當(dāng)前AUV線速度，當(dāng)AUV達(dá)到不同的線速度要求后，使AUV勻速行駛，然后發(fā)出轉(zhuǎn)向命令使其在常用弓字型路徑90°彎轉(zhuǎn)向，其轉(zhuǎn)向偏差和轉(zhuǎn)向時間對比如表1所示．

表1 不同v值轉(zhuǎn)向偏差時間表Tab.1 Steering deviation and steering time of different v

從實驗結(jié)果看出，當(dāng)AUV航速從1.8,m/s降到0.3,m/s時，基于矢量推進(jìn)器的AUV不會因為速度過小而無法轉(zhuǎn)向，并且轉(zhuǎn)向偏差和轉(zhuǎn)向時間減小明顯．

3.2 弓字型路徑90°轉(zhuǎn)向?qū)Ρ葘嶒?/p>

圖7 雙閉環(huán)控制方法和常規(guī)航向PID方法對比Fig.7 Comparison of double-loop control method and traditional PID

為了驗證雙閉環(huán)控制方法轉(zhuǎn)向效果更好，筆者初步設(shè)定k=0.5，在路徑規(guī)劃常選用弓字型路徑上進(jìn)行了實驗對比．即使AUV先下潛到預(yù)定高度，然后按照圖7所示的弓字型路徑掃描海底地形地貌，選定航向PID控制器比例參數(shù)為1.0，積分參數(shù)為0.1，航速PID控制器比例參數(shù)為1.5，積分參數(shù)為0.5．分別使用常規(guī)航向PID控制方法和雙閉環(huán)控制方法使AUV在弓字型路線行駛，實驗運(yùn)行結(jié)果如圖7所示.

從AUV采用兩種控制方法在弓字規(guī)劃路徑行駛情況可以看出，采用雙閉環(huán)控制方法在90°轉(zhuǎn)向時航跡跟蹤效果明顯得到改善．

3.3 系數(shù)c選定實驗

雙閉環(huán)控制方法中設(shè)計30°＜|Δθ|≤180°時，令c=k，k在0～1范圍內(nèi)．為了驗證k取何值時，AUV轉(zhuǎn)向效果最好，筆者進(jìn)行了如下實驗：采用雙閉環(huán)控制方法，當(dāng)AUV在弓字型路徑上90°轉(zhuǎn)彎且k取不同值時，記錄其轉(zhuǎn)向偏差和轉(zhuǎn)向時間．不同k值轉(zhuǎn)向偏差時間表如表2所示．

表2 不同k值轉(zhuǎn)向偏差時間表Tab.2 Steering deviation and steering time of different k

從實驗結(jié)果可以看出，當(dāng)k=0.8時，由于AUV速度比較大，導(dǎo)致其轉(zhuǎn)向偏差和轉(zhuǎn)向時間比較大．當(dāng)k逐漸減小時，轉(zhuǎn)向偏差和轉(zhuǎn)向時間也逐漸減小．但k從0.1變化為0時，其轉(zhuǎn)向偏差和轉(zhuǎn)向時間變化比較小，考慮到k=0時，需要關(guān)閉主推進(jìn)器，造成推進(jìn)器的頻繁啟動，會導(dǎo)致能源浪費，故從工程上考慮取k=0.1．

3.4 多角度轉(zhuǎn)向?qū)Ρ葘嶒?/p>

取k=0.1．雙閉環(huán)控制算法在常用弓字型路徑90°轉(zhuǎn)向時航跡跟蹤效果明顯改善后，筆者將雙閉環(huán)控制算法應(yīng)用在多角度轉(zhuǎn)向，兩種算法在不同轉(zhuǎn)向角度轉(zhuǎn)向偏差和轉(zhuǎn)向時間對比如表3所示．

表3 多角度轉(zhuǎn)向偏差時間對比Tab.3 Comparison of steering deviation and steering time at different steering angles

圖8給出了雙閉環(huán)控制方法和常規(guī)控制方法下轉(zhuǎn)向偏差的統(tǒng)計結(jié)果．

實驗結(jié)果表明，應(yīng)用雙閉環(huán)控制算法在不同角度轉(zhuǎn)向時轉(zhuǎn)向偏差和轉(zhuǎn)向時間都得到了大幅度的減少；并且隨著轉(zhuǎn)向角度的增加，應(yīng)用雙閉環(huán)算法時轉(zhuǎn)向偏差減小得更明顯．

圖8 轉(zhuǎn)向偏差統(tǒng)計結(jié)果Fig.8 Chart of steering deviation

4 結(jié) 語

理論分析和實驗證明，雙閉環(huán)自適應(yīng)運(yùn)動控制方法可以有效地解決基于矢量推進(jìn)器的AUV在高速轉(zhuǎn)向時轉(zhuǎn)向偏差大、轉(zhuǎn)向時間長的問題．對不同期望轉(zhuǎn)向角，該算法都可以控制AUV很好地實現(xiàn)軌跡跟蹤，并且轉(zhuǎn)向角度越大，使用該控制算法效果越好．

［1］ Blidberg D R. The development of autonomous underwater vehicle(AUV)：A brief summary[C]//International Conference on Robotics and Automation. USA，2001：3-4.

［2］ Kirkwood W J. AUV technology and application basics[C]//OCEANS 2008-MTS/IEEE Kobe Techno-Ocean. Kobe，Japan，2008：15-30.

［3］ 蔣新松，封錫盛，王棣棠. 水下機(jī)器人[M]. 沈陽：遼寧科學(xué)技術(shù)出版社，2000. Jiang Xinsong，F(xiàn)eng Xisheng，Wang Ditang. Underwater Robots[M]. Shenyang：Liaoning Science and Technol ogy Publishing House，2000(in Chinese).

［4］ Le Page Y G，Holappa K W. Simulation and control of an autonomous underwater vehicle equipped with a vectored thruster[C] //OCEANS 2000 MTS/IEEE Conference and Exhibition. Providence，RI，USA，2000：2129-2134.

［5］ Cavallo E，Michelini R C. Conceptual design of an AUV equipped with a three degrees of freedom vectored thruster [J]. Journal of Intelligent and Robotic Systems，2004，39(4)：365-391.

［6］ Wang Bo，Su Yumin，Wan Lei，et al. Adaptive PID control system for an autonomous underwater vehicle [J]. High Technology Letter，2011，17(1)：7-12.

［7］ Chen Qiang，Chen Tao，Zhang Yong. Research of GA-based PID for AUV motion control[C]//Proceedings of the 2009 IEEE International Conference on Mechatronics and Automation. Changchun，China，2009：4446-4451.

［8］ 潘存云，郭克希，楊 俊，等. 水下矢量推進(jìn)器的動力學(xué)分析[J]. 機(jī)械設(shè)計與研究，2010，26(5)：20-23. Pan Cunyun，Guo Kexi，Yang Jun，et al. Dynamics analysis and research of vectored thruster with propeller under the deep sea[J]. Machine Design and Research，2010，26(5)：20-23(in Chinese).

［9］ 李殿璞. 船舶運(yùn)動與建模[M]. 北京：國防工業(yè)出版社，2008. Li Dianpu. Ship Motion and Modeling[M]. Beijing：National Defense Industry Press，2008(in Chinese).

［10］ 蔣 辰，靳世久，李一博，等. 嵌入式ARM在基于以太網(wǎng)的AUV控制系統(tǒng)中的應(yīng)用[J]. 現(xiàn)代科學(xué)儀器，2011(6)：74-77. Jiang Chen，Jin Shijiu，Li Yibo，et al. The application of ARM in the control system of AUV based on Ethernet[J]. Modern Scientific Instruments，2011(6)：74-77(in Chinese).

(責(zé)任編輯：趙艷靜)

Steering Control Strategy of AUV with Vectored Thruster Based on Double-Loop Mode

Chen Shili1，Wei Min1，Li Yibo1，Zhang Zhenyu2
.(1. State Key Laboratory of Precision Measuring Technology and Instruments，Tianjin University，Tianjin 300072，China；2. School of Software Engineering，University of Wisconsin-Lacrosse，Wisconsin 54601，The United States)

Autonomous underwater vehicle(AUV)should have the ability to control the deviation of the actual navigation trajectory and target track within a limited range when surveying the seabed. Based on a lot of AUV running experiments in the past，it is known that with the traditional PID control method, AUV often has such shortcomings as large steering deviation and long-time navigation when turning along the target track. So according to AUV steering characteristics，this paper proposes a new method of adaptive double-loop PID. This method is based on the lowspeed steering characteristic of vectored thruster. It can adjust the AUV speed according to different yaw anglesin real time when AUV turns the rudder angle. Theoretical analysis and experiments show that this method can effectively settle the problems of large steering deviation and long-time navigation when AUV turns along the target track. The maximum deviation of the actual navigation trajectory and target track can be controlled within 10,m.

autonomous underwater vehicle；vectored thruster；steering；path tracking；double-loop

TP242.3

A

0493-2137(2014)06-0530-05

10.11784/tdxbz201209062

2012-09-22；

2012-10-17.

國家自然科學(xué)基金資助項目(60974110，61201039)；國家自然科學(xué)基金青年科學(xué)基金資助項目(51004076)；天津市應(yīng)用基礎(chǔ)及前沿技術(shù)研究資助項目(10JCYBJC07100).

陳世利（1973— ），男，副教授，slchen@tju.edu.cn.

李一博，slyb@tju.edu.cn.