基于航向補償的水下滑翔機路徑跟蹤控制方法

桑宏強, 于佩元, 孫秀軍

基于航向補償的水下滑翔機路徑跟蹤控制方法

桑宏強1,2, 于佩元1, 孫秀軍3,4*

(1. 天津工業大學 機械工程學院, 天津, 300387; 2. 天津市現代機電裝備技術重點實驗室, 天津, 300387; 3. 中國海洋大學 物理海洋教育部重點實驗室, 山東青島, 266100; 4. 青島海洋科學與技術國家實驗室 海洋動力過程與氣候功能實驗室, 山東 青島, 266237)

針對水下滑翔機在內部模型非線性和外界環境干擾下的水平路徑跟蹤控制問題, 文中以水下滑翔機Petrel-II 200動力學模型作為閉環控制系統仿真平臺, 提出一種包含積分視向導航(ILOS)、基于航向補償(HC)的滑模控制(SMC)及粒子濾波(PF)的路徑跟蹤控制方法。通過ILOS算法實時更新水下滑翔機的期望航向角, 基于航向補償的滑?？刂扑惴ㄓ糜谙较蚩刂浦械姆€態誤差, 在反饋回路引入粒子濾波器削弱過程噪聲及測量噪聲的干擾, 給出完整的路徑跟蹤控制模型, 并從不同方面進行了仿真驗證。由數值仿真結果可知, 與傳統的比例-積分-微分(PID)控制相比, 文中所提方法在方波航向跟蹤中航向平均誤差減小80.14%, 均方根誤差減小4.1%; 正弦航向中最大航向誤差減小40.9%, 標準差減小3.6%, 同時避免了舵角輸出的高頻震蕩, 有效地降低了能耗。在濾波仿真中, 粒子濾波可以濾除80%的固定航向噪聲與90%隨機航向噪聲。在路徑跟蹤仿真中, 所提方法能有效地對期望路徑進行跟蹤。上述仿真結果驗證了所設計路徑跟蹤控制方法的有效性。

水下滑翔機; 積分視向導航算法; 粒子濾波; 滑?？刂? 航向補償

0 引言

水下滑翔機廣泛應用于海洋環境觀測、軍事等領域, 具有噪音低、續航能力強等優點。實現高精度的路徑跟蹤控制是水下滑翔機完成如掃雷、水下觀察探測等任務的關鍵。由于水下滑翔機模型的不確定性、海洋環境的復雜性等問題的存在, 水下滑翔機的路徑跟蹤控制問題一直是該領域研究的難點[1]。

表1 路徑跟蹤控制方法特點

受上述文獻啟發, 文中以積分視向導航(in- tegral light-of-sight, ILOS)制導律和滑模控制(sli- ding model control, SMC)為基礎, 引入粒子濾波(particle filtering, PF)減小測量噪聲及外界環境干擾, 通過ILOS算法將路徑跟蹤問題轉化為航向控制問題; 引入航向補償(heading compensation, HC)算法消除航向的穩態誤差, 實現水下滑翔機對期望路徑的跟蹤, 通過數值仿真驗證了所提路徑跟蹤控制方法的有效性。

1 水下滑翔機動力學建模

圖1 Petrel-II 200坐標系

由于水下滑翔機的欠驅動特性, 其運動學與動力學可以描述為[8]

假設機體的質量和浮力相等, 浮力中心位于垂直平面內, 參考文獻[9]~[11]建立水下滑翔機的動力學模型, 具體的動力學參數與變量說明參考文獻[11], 式(1)中水下滑翔機的動力學模型可表示如下

2 路徑跟蹤控制方法

2.1 ILOS導航設計

圖2 路徑跟蹤控制方法框圖

圖3 積分視向導航原理示意圖

傳統的視向導航(light-of-sight, LOS)導航在海流等外界干擾下會產生側滑, 進而導致路徑偏移[13]。為了解決上述問題, 在LOS導航基礎上引入積分項, 使得水下滑翔機能夠在海流的影響下沿直線期望路徑航行。ILOS制導律可表示為

2.2 粒子濾波器設計

水下滑翔機工作環境復雜多變, 波浪擾動造成的測量噪聲和控制過程中產生的過程噪聲對滑翔機的航向控制產生不利影響, 這將引起控制輸入持續性的不必要振蕩, 降低控制性能。為克服噪聲擾動帶來的影響, 引入粒子濾波器[14-16]。通過粒子濾波器對航向進行估計, 將航向的估計值代入反饋項中, 濾去閉環反饋控制中的擾動項, 具體控制流程如圖4所示。

由滑翔機動力學模型可得航向狀態方程和測量方程

圖4 粒子濾波流程圖

根據航向狀態估計后驗概率

用蒙特卡洛采樣來代替計算后驗概率, 即

此時時刻的后驗概率密度近似表示為

針對粒子濾波中出現的粒子退化現象, 引入重采樣思想, 舍棄權重小的粒子, 增加新的粒子。重采樣后的后驗概率密度為

2.3 基于航向補償的滑?？刂破髟O計

在實際的航向控制中, 期望航向與實際航向之間存在誤差, 即使在以后的路徑跟蹤過程中實際航向能夠完全跟蹤期望航向, 實際的路徑跟蹤軌跡也將與目標軌跡之間存在穩態誤差。為了消除穩態誤差, 文中設計了基于航向補償的滑模控制器[7]。航向補償算法

設計滑模面為

為了減小滑?？刂破鞯亩墩瘳F象, 采用指數趨近律為

利用李雅普諾夫函數進行穩定性分析, 定義李雅普諾夫函數為

將式(13)和式(14)帶入上式得

由上式可得, 閉環誤差收斂到零, 系統達到漸進穩定。

3 數值仿真

圖5 不同控制器航向跟蹤控制曲線

圖5為不同控制器對方波、正弦期望航向的航向跟蹤效果。與傳統PID控制相比, 在方波航向跟蹤中航向平均誤差減小80.14%, 均方根誤差減小4.1%; 在正弦航向中最大航向誤差減小40.9%, 標準差減小3.6%。從圖中可以看出, 基于航向補償的滑?？刂破飨啾容^于傳統的PID控制器和傳統滑?？刂破? 在航向控制上超調更小, 航向變化更加平滑, 避免了舵角輸出的高頻震蕩, 且有效地降低了能耗。

圖6 不同濾波器航向濾波曲線

表2 粒子濾波與擴展卡爾曼濾波偏差對比

在固定航向濾波中, 粒子濾波能濾除80%的噪聲信號, 而EKF的噪聲過濾只達到59%; 在隨機航向濾波中, 粒子濾波算法對噪聲的過濾高達75%以上, EKF對噪聲過濾僅達24%。仿真結果證明: 經過粒子濾波后的航向更加接近于實際航向, 也更加平滑, 可避免航向控制中因噪聲引起的頻繁操舵現象, 對減小舵機磨損和降低功耗具有重要意義。

4 結束語

文中針對混合驅動水下滑翔機的非線性動力學模型和外界環境干擾下的水平路徑跟蹤控制問題, 提出了一種包含ILOS導航、基于航向補償的滑模控制、粒子濾波的路徑跟蹤控制方法。通過航向補償算法消除航向跟蹤過程的穩態誤差, 在反饋回路中利用粒子濾波消除測量噪聲及過程噪聲。由數值仿真結果可得, 與PID控制相比, 文中所提方法在方波航向跟蹤中航向平均誤差減小80.14%, 均方根誤差減小4.1%; 正弦航向中最大航向誤差減小40.9%, 標準差減小3.6%。在濾波仿真中, 粒子濾波可以濾除80%的固定航向噪聲與75%的隨機航向噪聲。在路徑跟蹤仿真中, 所提方法也能有效對期望路徑進行跟蹤。在未來的研究工作中, 可以將水平路徑跟蹤拓展到三維路徑跟蹤, 并進行海試試驗驗證。

[1] Wang Y, Zhang Y, Zhang M, et al. Design and Flight Performance of Hybrid Underwater Glider with Controllable Wings[J]. International Journal of Advanced Robotic Systems, 2017, 14(3): 1-12.

[2] Bessa W M, Dutra M S, Kreuzer E. Depth Control of Remotely Operated Underwater Vehicles Using an Adaptive Fuzzy Sliding Mode Controller[J]. Robotics & Autonomous Systems, 2008, 56(8): 670-677.

[3] Xiang X, Yu C, Zhang Q, et al. Robust Fuzzy 3D Path Following for Autonomous Underwater Vehicle Subject to Uncertainties[J]. Computers and Operations Research, 2017, 84: 165-177.

[4] Sahu B K, Subudhi B. Potential Function-based Path- following Control of an Autonomous Underwater Vehicle in an Obstacle-rich Environment[J]. Transactions of the Institute of Measurement and Control, 2017, 39(8): 1236-1252.

[5] Liang X, Qu X, Hou Y, et al. Three-dimensional Path Following Control of Underactuated Autonomous Underwater Vehicle Based on Damping Backstepping[J]. International Journal of Advanced Robotic Systems, 2017, 14(4): 1-9.

[6] Peng Z, Wang J, Wang D. Distributed Containment Maneuvering of Multiple Marine Vessels via Neurodynamics-Based Output Feedback[J]. IEEE Transactions on In-dustrial Electronics, 2017, 64(5): 3831-3839.

[7] Xu D, Shi Y, Ji Z. Model Free Adaptive Discrete-time Integral Sliding Mode Constrained Control for Autonomous 4WMV Parking Systems[J]. IEEE Transactions on Indus- trial Electronics, 2017, 65(1): 834-843.

[8] Martinez A, Hernandez L, Sahli H, et al. Model-aided Navigation with Sea Current Estimation for an Autonomous Underwater Vehicle[J]. International Journal of Advanced Robotic Systems, 2015, 12(7): 1-14.

[9] Fossen T I. Handbook of Marine Craft Hydrodynamics and Motion Control[M]. Chichester: John Wiley & Sons, 2011.

[10] 孫秀軍. 混合驅動水下滑翔器動力學建模及運動控制研究[D]. 天津: 天津大學, 2011.

[11] Sang H, Zhou Y, Sun X, et al. Heading Tracking Control with an Adaptive Hybrid Control for Under Actuated Underwater Glider[J]. Isa Transactions, 2018, 80: 554-563.

[12] Lekkas A, Fossen T I. Line-of-Sight Guidance for Path Following of Marine Vehicles[M]. Saarbrcken: Lambert Academic Publishing, 2013.

[13] Borhaug E, Pavlov A, Pettersen K Y. Integral LOS Control for Path Following of Underactuated Marine Surface Ves- sels in the Presence of Constant Ocean Currents[C]//IEEE Conference on Decision & Control. Cancun, Mexico: IEEE, 2008: 4984-4991.

[14] Mahapatra S, Subudhi B. Design of a Steering Control Law for an Autonomous Underwater Vehicle Using Nonlinear State Feedback Technique[J]. Nonlinear Dynamics, 2017, 90(2): 837-854.

[15] Miao J, Wang S, Zhao Z, et al. Spatial Curvilinear Path Following Control of Underactuated AUV with Multiple Uncertainties[J]. Isa Transactions, 2017, 67: 107-130.

[16] Shojaei K, Dolatshahi M. Line-of-sight Target Tracking Control of Underactuated Autonomous Underwater Vehicles[J]. Ocean Engineering, 2017, 133: 244-252.

[17] Cui R, Zhang X, Cui D. Adaptive Sliding-mode Attitude Control for Autonomous Underwater Vehicles with Input Nonlinearities[J]. Ocean Engineering, 2016, 123: 45-54.

Path Tracking Control Method of Underwater Glider Based on Heading Compensation

SANG Hong-qiang1,2, YU Pei-yuan1, SUN Xiu-jun3,4*

（1. School of Mechanical Engineering, Tianjin Polytechnic University, Tianjin 300387, China; 2. Tianjin Key Laboratory of Advanced Mechatronic Equipment Technology, Tianjin 300387, China; 3. Physical Oceanography Laboratory, Ocean University of China, Qingdao 266100, China; 4. Laboratory of Marine Dynamics and Climate Function, Pilot National Laboratory for Marine Science and Technology(Qingdao), Qingdao 266237, China）

This paper focuses on the problem of horizontal path tracking control for underwater glider under internal model nonlinearity and external environment disturbances. A dynamic model of underwater glider Petrel-II 200 is established as the simulation platform of closed-loop control system, and a path tracking control method including integral light-of-sight(ILOS), sliding mode control(SMC) with heading compensation(HC), and particle filter(PF) is proposed. The desired heading angle of the underwater glider is updated in real time by the ILOS algorithm. The SMC algorithm based on HC is used to eliminate the steady state error in the heading control. The PF is introduced into the feedback loop to reduce the interference of process noise and measurement noise. The complete path tracking control model is verified by numerical simulation. According to the numerical simulation results, the proposed method reduces the mean heading error and the root mean square error in square wave heading tracking by 80.14% and 4.1%, respectively, compared with the traditional proportional-integral-derivative(PID) control. Also, the maximum heading error and the standard deviation in sinusoidal heading are reduced by 40.9% and 3.6%, respectively. The high frequency oscillation of the rudder angle output is also avoided, which effectively reduces the energy consumption. In the filtering simulation, PF can filter out 80% of fixed heading noise and 90% of random heading noise, and in the path tracking simulation, the proposed method can effectively track the desired path. These numerical simulation results verify the effectiveness of the proposed path tracking control method.

underwater glider; integral light-of-sight(ILOS); particle filter(PF); sliding mode control(SMC); heading compensation(HC)

U674.941; TP242

A

2096-3920(2019)05-0541-07

10.11993/j.issn.2096-3920.2019.05.009

桑宏強, 于佩元, 孫秀軍. 基于航向補償的水下滑翔機路徑跟蹤控制方法[J]. 水下無人系統學報, 2019, 27 (5): 541-547.

2019-10-30;

2019-11-15.

國家重點研發計劃重點專項(2017YFC0305902); 青島海洋科學與技術國家實驗室“問海計劃”項目(2017WHZ ZB0101); 天津市自然科學基金重點項目(18JCZDJC40100); 天津市高等學校創新團隊培養計劃(TD13-5037).

*孫秀軍(1981-), 男, 博士, 教授, 主要研究方向為海洋移動觀測平臺技術.

(責任編輯: 楊力軍)