基于航向補(bǔ)償?shù)乃禄铏C(jī)路徑跟蹤控制方法

桑宏強(qiáng), 于佩元, 孫秀軍

基于航向補(bǔ)償?shù)乃禄铏C(jī)路徑跟蹤控制方法

桑宏強(qiáng)1,2, 于佩元1, 孫秀軍3,4*

(1. 天津工業(yè)大學(xué) 機(jī)械工程學(xué)院, 天津, 300387; 2. 天津市現(xiàn)代機(jī)電裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室, 天津, 300387; 3. 中國(guó)海洋大學(xué) 物理海洋教育部重點(diǎn)實(shí)驗(yàn)室, 山東青島, 266100; 4. 青島海洋科學(xué)與技術(shù)國(guó)家實(shí)驗(yàn)室 海洋動(dòng)力過程與氣候功能實(shí)驗(yàn)室, 山東 青島, 266237)

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

水下滑翔機(jī); 積分視向?qū)Ш剿惴? 粒子濾波; 滑?？刂? 航向補(bǔ)償

0 引言

水下滑翔機(jī)廣泛應(yīng)用于海洋環(huán)境觀測(cè)、軍事等領(lǐng)域, 具有噪音低、續(xù)航能力強(qiáng)等優(yōu)點(diǎn)。實(shí)現(xiàn)高精度的路徑跟蹤控制是水下滑翔機(jī)完成如掃雷、水下觀察探測(cè)等任務(wù)的關(guān)鍵。由于水下滑翔機(jī)模型的不確定性、海洋環(huán)境的復(fù)雜性等問題的存在, 水下滑翔機(jī)的路徑跟蹤控制問題一直是該領(lǐng)域研究的難點(diǎn)[1]。

表1 路徑跟蹤控制方法特點(diǎn)

受上述文獻(xiàn)啟發(fā), 文中以積分視向?qū)Ш?in- tegral light-of-sight, ILOS)制導(dǎo)律和滑模控制(sli- ding model control, SMC)為基礎(chǔ), 引入粒子濾波(particle filtering, PF)減小測(cè)量噪聲及外界環(huán)境干擾, 通過ILOS算法將路徑跟蹤問題轉(zhuǎn)化為航向控制問題; 引入航向補(bǔ)償(heading compensation, HC)算法消除航向的穩(wěn)態(tài)誤差, 實(shí)現(xiàn)水下滑翔機(jī)對(duì)期望路徑的跟蹤, 通過數(shù)值仿真驗(yàn)證了所提路徑跟蹤控制方法的有效性。

1 水下滑翔機(jī)動(dòng)力學(xué)建模

圖1 Petrel-II 200坐標(biāo)系

由于水下滑翔機(jī)的欠驅(qū)動(dòng)特性, 其運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)可以描述為[8]

假設(shè)機(jī)體的質(zhì)量和浮力相等, 浮力中心位于垂直平面內(nèi), 參考文獻(xiàn)[9]~[11]建立水下滑翔機(jī)的動(dòng)力學(xué)模型, 具體的動(dòng)力學(xué)參數(shù)與變量說明參考文獻(xiàn)[11], 式(1)中水下滑翔機(jī)的動(dòng)力學(xué)模型可表示如下

2 路徑跟蹤控制方法

2.1 ILOS導(dǎo)航設(shè)計(jì)

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

圖3 積分視向?qū)Ш皆硎疽鈭D

傳統(tǒng)的視向?qū)Ш?light-of-sight, LOS)導(dǎo)航在海流等外界干擾下會(huì)產(chǎn)生側(cè)滑, 進(jìn)而導(dǎo)致路徑偏移[13]。為了解決上述問題, 在LOS導(dǎo)航基礎(chǔ)上引入積分項(xiàng), 使得水下滑翔機(jī)能夠在海流的影響下沿直線期望路徑航行。ILOS制導(dǎo)律可表示為

2.2 粒子濾波器設(shè)計(jì)

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

由滑翔機(jī)動(dòng)力學(xué)模型可得航向狀態(tài)方程和測(cè)量方程

圖4 粒子濾波流程圖

根據(jù)航向狀態(tài)估計(jì)后驗(yàn)概率

用蒙特卡洛采樣來代替計(jì)算后驗(yàn)概率, 即

此時(shí)時(shí)刻的后驗(yàn)概率密度近似表示為

針對(duì)粒子濾波中出現(xiàn)的粒子退化現(xiàn)象, 引入重采樣思想, 舍棄權(quán)重小的粒子, 增加新的粒子。重采樣后的后驗(yàn)概率密度為

2.3 基于航向補(bǔ)償?shù)幕？刂破髟O(shè)計(jì)

在實(shí)際的航向控制中, 期望航向與實(shí)際航向之間存在誤差, 即使在以后的路徑跟蹤過程中實(shí)際航向能夠完全跟蹤期望航向, 實(shí)際的路徑跟蹤軌跡也將與目標(biāo)軌跡之間存在穩(wěn)態(tài)誤差。為了消除穩(wěn)態(tài)誤差, 文中設(shè)計(jì)了基于航向補(bǔ)償?shù)幕？刂破鱗7]。航向補(bǔ)償算法

設(shè)計(jì)滑模面為

為了減小滑模控制器的抖振現(xiàn)象, 采用指數(shù)趨近律為

利用李雅普諾夫函數(shù)進(jìn)行穩(wěn)定性分析, 定義李雅普諾夫函數(shù)為

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

由上式可得, 閉環(huán)誤差收斂到零, 系統(tǒng)達(dá)到漸進(jìn)穩(wěn)定。

3 數(shù)值仿真

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

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

圖6 不同濾波器航向?yàn)V波曲線

表2 粒子濾波與擴(kuò)展卡爾曼濾波偏差對(duì)比

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

4 結(jié)束語

文中針對(duì)混合驅(qū)動(dòng)水下滑翔機(jī)的非線性動(dòng)力學(xué)模型和外界環(huán)境干擾下的水平路徑跟蹤控制問題, 提出了一種包含ILOS導(dǎo)航、基于航向補(bǔ)償?shù)幕？刂啤⒘Ｗ訛V波的路徑跟蹤控制方法。通過航向補(bǔ)償算法消除航向跟蹤過程的穩(wěn)態(tài)誤差, 在反饋回路中利用粒子濾波消除測(cè)量噪聲及過程噪聲。由數(shù)值仿真結(jié)果可得, 與PID控制相比, 文中所提方法在方波航向跟蹤中航向平均誤差減小80.14%, 均方根誤差減小4.1%; 正弦航向中最大航向誤差減小40.9%, 標(biāo)準(zhǔn)差減小3.6%。在濾波仿真中, 粒子濾波可以濾除80%的固定航向噪聲與75%的隨機(jī)航向噪聲。在路徑跟蹤仿真中, 所提方法也能有效對(duì)期望路徑進(jìn)行跟蹤。在未來的研究工作中, 可以將水平路徑跟蹤拓展到三維路徑跟蹤, 并進(jìn)行海試試驗(yàn)驗(yàn)證。

[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] 孫秀軍. 混合驅(qū)動(dòng)水下滑翔器動(dòng)力學(xué)建模及運(yùn)動(dòng)控制研究[D]. 天津: 天津大學(xué), 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

桑宏強(qiáng), 于佩元, 孫秀軍. 基于航向補(bǔ)償?shù)乃禄铏C(jī)路徑跟蹤控制方法[J]. 水下無人系統(tǒng)學(xué)報(bào), 2019, 27 (5): 541-547.

2019-10-30;

2019-11-15.

國(guó)家重點(diǎn)研發(fā)計(jì)劃重點(diǎn)專項(xiàng)(2017YFC0305902); 青島海洋科學(xué)與技術(shù)國(guó)家實(shí)驗(yàn)室“問海計(jì)劃”項(xiàng)目(2017WHZ ZB0101); 天津市自然科學(xué)基金重點(diǎn)項(xiàng)目(18JCZDJC40100); 天津市高等學(xué)校創(chuàng)新團(tuán)隊(duì)培養(yǎng)計(jì)劃(TD13-5037).

*孫秀軍(1981-), 男, 博士, 教授, 主要研究方向?yàn)楹Ｑ笠苿?dòng)觀測(cè)平臺(tái)技術(shù).

(責(zé)任編輯: 楊力軍)