基于航向补偿的水下滑翔机路径跟踪控制方法

桑宏强, 于佩元, 孙秀军

基于航向补偿的水下滑翔机路径跟踪控制方法

桑宏强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 基于航向补偿的滑模控制器设计

在实际的航向控制中, 期望航向与实际航向之间存在误差, 即使在以后的路径跟踪过程中实际航向能够完全跟踪期望航向, 实际的路径跟踪轨迹也将与目标轨迹之间存在稳态误差。为了消除稳态误差, 文中设计了基于航向补偿的滑模控制器[7]。航向补偿算法

设计滑模面为

为了减小滑模控制器的抖振现象, 采用指数趋近律为

利用李雅普诺夫函数进行稳定性分析, 定义李雅普诺夫函数为

将式(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-), 男, 博士, 教授, 主要研究方向为海洋移动观测平台技术.

(责任编辑: 杨力军)