自主水下潜航器(AUV)具有智能、灵活和隐蔽等特性,在海洋科考、水下救援和军事侦察等领域得到了广泛应用[1 − 3]。然而AUV受自身携带能源和水下恶劣通信环境的限制[4],长时间作业后需要及时回收进行能源补充和数据回传等操作。近年来,水下对接已成为一种较为自主的回收方式[5 − 7],但水下环境复杂,近距离回收阶段对姿态的控制要求较为苛刻,AUV需要对目标姿态进行快速响应及跟踪才能实现可靠回收。
目前,实现AUV控制的主要方法包括PID控制、模糊控制、滑模控制和自抗扰控制等[8,9]。PID控制算法简单且比较容易实现,在水下机器人控制领域得到广泛应用[10]。但对于模型复杂、非线性的AUV控制对象,PID算法难以实现精确的运动控制和干扰补偿,实际应用中通常要与其他的控制方法相结合。模糊控制通过利用模糊规则和论域分割来简化复杂系统,能够有效地处理系统的非线性和不确定[11]。但由于无法获取较好的AUV水下操纵经验,模糊规则及隶属函数的设定存在困难。滑模控制能够根据状态变量信息不断优化系统结构,具有良好的响应特性和鲁棒性[12]。但滑模面的设计和参数调整相对复杂,且变结构控制律的不连续导致系统存在抖振缺陷。自抗扰控制是在PID基础上进行改进的控制方法[13,14],通过引入抗扰动观测器来实现对系统扰动的实时估计和补偿,不需要被控对象具有精确的数学模型,并且具有强抗扰性、高精度和自适应性。
本文采用自抗扰控制技术来实现AUV的姿态跟踪控制,降低控制系统对复杂运动模型的依赖。在构建AUV运动学和动力学模型的前提下,设计一种含平滑低通滤波的改进型自抗扰控制器,并采用无扰动和含阶跃、正弦波的扰动波形作为系统输入进行仿真。通过与PID的结果进行比对,验证了该改进型控制器的有效性。
1 水下航行器运动模型 1.1 坐标系构建为了分析AUV在整个海洋空间中的运动,按照ITTC推荐及SNAME术语体系的要求[15],建立了固定坐标系
![]() |
图 1 坐标系 Fig. 1 Coordinate system |
AUV的浮心为运动坐标系的原点,空间位置可以用x、y、z来表示。上述两坐标系中的
{η=[xyzϕθψ]T,v=[uvwpqr]T,τ=[XYZLMN]T。 | (1) |
AUV的刚体运动涉及6个自由度,分为沿x、y和
˙η=(J1(η)03×303×3J2(η))ν。 | (2) |
J1(η)=[cosψcosθsinψcosθ−sinθcosψsinθsinφ−sinψcosφsinψsinθsinφ+cosψcosφcosθsinφcosψsinθcosφ−sinψsinφsinψsinθcosφ−cosψsinφcosθcosφ], | (3) |
J2(η)=[100tanθsinφcosφsinφsecθtanθcosφ−sinφcosφsecθ]。 | (4) |
实际运动中,AUV姿态角的取值范围为:
{−π<ϕ⩽π,−π2<θ⩽π2,−π<ψ⩽π。 | (5) |
AUV水下运动时受到惯性力、静水力、流体力、螺旋桨推进力的共同作用,动力学呈现非线性、不确定性等复杂特性。根据AUV所受外力和力矩关系,并且依据牛顿定律和动量矩定理可推导出AUV的动力学方程式为:
M˙v+C(v)v+D(v)v+g(η)=τ。 | (6) |
式中:M为惯性矩阵,包括刚体惯性力矩阵MRB和附加质量系数力矩阵MA;C为科里奥利向心力矩阵,包括刚体科氏向心力矩阵CRB(v)和科氏向心力系数矩阵CA(v);D(v)为阻尼矩阵;g(η)为恢复力矩;τ为执行机构产生的控制力矩阵。
1.3 模型简化AUV的六自由度模型较为复杂,具有非线性和强耦合性。为了便于控制器系统的设计,针对本文AUV的特性及运动规律,可对模型进行合理简化。AUV空间运动可解耦成沿3个平面的运动,并且由于AUV四桨矢量推动器呈轴对称“X”型结构,自身动力抗横滚,因此将AUV模型简化成水平面与垂直面的运动模型。
1.3.1 水平面运动模型AUV在水平面的运动为沿
{˙x=ucosψ−vsinψ,˙y=usinψ+vcosψ,˙ψ=r。 | (7) |
AUV水平面的动力学方程为:
{(m−X˙u)˙u=(m−Y˙v)vr+(Xu+Xu|u||u|)u+X,(m−Y˙v)˙v=−(m−X˙u)ur+(Yv+Yv|v||v|)v,(Iz−N˙r)˙r=−(X˙u−Y˙v)uv+(Nr+Nr|r||r|)r+N。 | (8) |
AUV在垂直面的运动为沿
{˙x=ucosθ−wsinθ,˙z=−usinθ+wcosθ,˙θ=q。 | (9) |
AUV垂直面的动力学方程为:
{(m−X˙u)˙u=−(m−Z˙w)uw+(Xu+Xu|u||u|)u−(W−B)sinθ+X,(m−Z˙w˙w)˙w=Z˙q˙q+(m−X˙u)uw+(W−B)cosθ+(Zw+Zw|w||w|)w,(Iy−M˙q)˙q=M˙w˙w+(X˙u−Z˙w)uw++ZBBsinθ+(Mq+Mq|q||q|)q+M。 | (10) |
自抗扰控制的核心在于将非线性、不确定性与外部扰动视为系统的总扰动,并利用对象的输入输出信息进行实时估计。通过将控制系统补偿为标准的积分串联型,结合比例-微分控制部分实现精准而稳定的姿态跟踪控制。自抗扰控制器由跟踪微分器、扩张状态观测器和非线性状态误差反馈控制律3部分组成,本文根据其原理对系统各部件进行设计,并采用滑动平均低通滤波对干扰通道进行补偿优化。
2.1 跟踪微分器跟踪微分器是系统的重要的组成部分,负责安排过渡过程,防止输入信号的跳变。它通过建立无超调的快速跟踪,可有效解决响应速度与超调性之间的矛盾。跟踪微分器的公式为:
{fh=fhan(θ1−θ0,θ2,r,h0),θ1=θ1+hθ2,θ2=θ2+hfh, | (11) |
式中:
{d=h0r20,a0=h0θ2,y=θ1−θ0+a0,a1=√d(d+8|y|),a2=a0+sign(y)(a1−d)2,sy=(sign(y+d)−sign(y−d))2,a=(a0+y−a2)sy+a2,sa=(sign(a+d)−sign(a−d))2, fhan =−r0(ad−sign(a))sa−r0sign(a)。 | (12) |
非线性状态误差反馈控制律是系统的另一关键组成部分,用于制定被控对象的控制策略,它能够根据系统的状态误差来调整控制器的输出。非线性状态误差反馈控制律结合了跟踪微分器和扩张观测器的输出,通过非线性方式将信号误差和信号误差的微分进行组合,从而得到输出观测估计值误差。状态误差反馈控制律如下:
{e1=θ1−z1,e2=θ2−z2,u0=β0fal(e1,α0,δ)+β1fal(e2,α1,δ)。 | (13) |
式中:
fal(e,α,δ)={eδ1−α,|e|⩽δ,|e|αsign(e),|e|⩾δ。 | (14) |
扩张状态观测器是系统的核心部分,一方面它能够对系统输入信号的状态变量进行跟踪,实时了解系统的状态;另一方面它还能够识别和估计系统内部扰动和外部高频噪声干扰,将其扩张成额外的状态变量,并以反馈的形式对其加以及时补偿。扩张状态观测器通过对系统状态和扰动的估计和补偿,能够为系统提供准确的控制信息,从而实现对系统的主动干预和自适应调整。扩张状态观测器的公式为:
{e=z1−θ,fe=fal(e,α2,δ1),fe1=fal(e,α3,δ1),˙z1=z2−β01e,˙z2=z3+b0u−β02fe,˙z3=−β03fe1。 | (15) |
式中:e为跟踪误差;
由于
滑动平均低通滤波公式如下:
u1(k)=1N∑N−1i=0z3(k−i)。 | (16) |
式中:N为采样深度;i为求和计算工程中起索引作用的变量。
最终控制器通过补偿获得的输出为:
u=u0−u1b0。 | (17) |
自抗扰与滑动平均低通滤波控制器的系统框图如图2所示。
![]() |
图 2 改进型ADRC控制器框图 Fig. 2 Improved ADRC controller block diagram |
为验证上述改进型自抗扰控制器的有效性,在Matla/Simulink中搭建了控制器仿真模型。在仿真实验中,采用自定义的预设姿态角作为AUV回收时的航向和俯仰偏差姿态数据,并将改进的自抗扰控制器与传统PID姿态跟踪效果进行对比验证。
3.1 无扰动仿真实验无外界扰动环境下,将预设姿态角设定为固定值,此时预设姿态曲线为一条
![]() |
图 3 无扰动姿态跟踪仿真 Fig. 3 Undisturbed attitude tracking simulation |
从仿真对比实验结果可知,改进型自抗扰控制器的进入稳态的时间比PID提前约3 s,并且有着较高的姿态的跟踪精度,可以实现更快更精准跟踪;此外,其姿态控制调节时间比PID超前,可以实现更快的响应控制;并且姿态调节波动的峰值较低,姿态跟踪过程较为平滑,超调量由17.2%降低到9.4%。仿真结果对比说明,改进型自抗扰控制器比PID控制器的动态响应效果更好,可以实现对设定简单姿态的稳定跟踪。
3.2 有扰动仿真实验为有效验证在有干扰环境下的姿态跟踪效果,在设定姿态期望值为0的基础上,分别引入经典的阶跃扰动波形和高频正弦扰动波形进行实验,仿真实验结果如图4和图5所示。
![]() |
图 4 阶跃扰动姿态跟踪仿真 Fig. 4 Step disturbance attitude tracking simulation |
![]() |
图 5 高频正弦扰动姿态跟踪仿真 Fig. 5 High frequency sinusoidal disturbance attitude tracking simulation |
可以看出,改进型自抗扰控制器稳定回归到预设曲线的调整时间比PID要减少2.5 s左右,可以快速恢复到稳态,并且其超调波动控制在很小的范围内,姿态跟随效果更好。仿真结果对比说明,改进型自抗扰控制器比PID控制器具有更强的抗干扰能力和自适应力。
从仿真对比实验结果可知,在受到高频正弦波的干扰下,PID已发生明显震荡,导致姿态跟踪失去了稳定性。而改进型自抗扰控制器在2 s左右实现了平稳收敛,跟踪曲线与预设曲线贴合紧密,观测误差不存在振荡。说明采用改进型自抗扰控制器可以实现对设定较复杂姿态变化的稳定跟踪,具有较强的鲁棒性和稳定性。
4 结 语本文在建立AUV水平面和垂直面数学模型的基础上,构造了一款改进型自抗扰控制器进行对接姿态跟踪仿真研究。得到的实验数据表明,在无扰动情况下姿态控制可以进行快速响应并精确跟踪,在有扰动情况下姿态可以进行快速恢复并保持稳态。该改进型自抗扰控制器明显改善了传统PID的动静态响应特性和鲁棒性,具有较强的姿态控制优势,可以为水下机器人回收时的姿态控制提供技术支撑,具有实际的应用价值。
[1] |
JOE H, CHO H, SUNG M, et al. Sensor fusion of two sonar devices for underwater 3D mapping with an AUV[J]. Autonomous Robots, 2021, 45(4): 543-560. DOI:10.1007/s10514-021-09986-5 |
[2] |
MAHMOUDZADEH S, YAZDANI A. Distributed task allocation and mission planning of AUVs for persistent underwater ecological monitoring and preservation[J]. Ocean Engineering, 2023, 290: 116216. DOI:10.1016/j.oceaneng.2023.116216 |
[3] |
曹少华, 张春晓, 王广洲, 等. 智能水下机器人的发展现状及在军事上的应用[J]. 船舶工程, 2019, 41(2): 79-84,89. CAO S H, ZHANG C X, WANG G Z, et al. Development situation and military application of autonomous underwater vehicle[J]. Ship Engineering, 2019, 41(2): 79-84,89. |
[4] |
ZEREIK E, BIBULI M, MIŠKOVIĆ N, et al. Challenges and future trends in marine robotics[J]. Annual Reviews in Control, 2018, 46: 350-368. DOI:10.1016/j.arcontrol.2018.10.002 |
[5] |
PALOMERAS N, VALLICROSA G, MALLIOS A, et al. AUV homing and docking for remote operations[J]. Ocean Engineering, 2018, 154: 106-120. DOI:10.1016/j.oceaneng.2018.01.114 |
[6] |
袁学庆, 刁家宇, 李卫民, 等. AUV水下对接的发展与应用现状[J]. 舰船科学技术, 2023, 45(5): 1-8. YUAN X, DIAO J Y, LI W M, et al. Development and application status of AUV underwater docking[J]. Ship Science and Technology, 2023, 45(5): 1-8. DOI:10.3404/j.issn.1672-7649.2023.05.001 |
[7] |
VALLICROSA G, BOSCH J, PALOMERAS N, et al. Autonomous homing and docking for AUVs using Range-Only Localization and Light Beacons[J]. IFAC-Papers OnLine, 2016, 49(23): 54-60. DOI:10.1016/j.ifacol.2016.10.321 |
[8] |
王永鼎, 王鹏, 孙鹏飞. 自主式水下机器人控制技术研究综述[J]. 世界科技研究与发展, 2021, 43(6): 636-648. WANG Y D, WANG P, SUN P F. Review on research of control technology of autonomous underwater vehicle[J]. World Sci-tech R& D, 2021, 43(6): 636-648. |
[9] |
王芳, 万磊, 李晔, 等. 欠驱动AUV的运动控制技术综述[J]. 中国造船, 2010, 51(2): 227-241. WANG F, WAN L, LI Y, et al. A survey on development of motion control for underactuated AUV[J]. Shipbuilding of China, 2010, 51(2): 227-241. DOI:10.3969/j.issn.1000-4882.2010.02.030 |
[10] |
KIM M-J, BAEK W-K, HA K-N, et al. Implementation of hovering auv and its attitude control using PID controller[J]. Journal of Ocean Engineering and Technology, 2016, 30(3): 221-226. DOI:10.5574/KSOE.2016.30.3.221 |
[11] |
吕厚权, 郑荣, 杨斌, 等. 水下自主机器人航向控制算法应用研究[J]. 舰船科学技术, 2020, 42(3): 108-114. LV H Q, ZHENG R, YANG B, et al. Application research of heading control algorithm for AUV[J]. Ship Science and Technology, 2020, 42(3): 108-114. |
[12] |
饶志荣, 董绍江, 王军, 等. 基于干扰观测器的AUV深度自适应终端滑模控制[J]. 北京化工大学学报(自然科学版), 2021, 48(1): 103-110. RAO Z R, DONG S J, WANG J, et al. Adaptive terminal sliding mode control of autonomous underwater vehicle (AUV) depth based on a disturbance observer[J]. Journal of Beijing University of Chemical Technology(Natural Science Edition), 2021, 48(1): 103-110. |
[13] |
楼鉴路, 李文魁, 周铸, 等. 基于线性自抗扰控制的AUV深度控制研究[J]. 舰船科学技术, 2023, 45(6): 96-101. LOU J L, LI W K, ZHOU Z, et al. Research on AUV depth control based on linear active disturbance rejection control[J]. Ship Science and Technology, 2023, 45(6): 96-101. |
[14] |
FAREH R, KHADRAOUI S, ABDALLAH M Y, et al. Active disturbance rejection control for robotic systems: A review[J]. Mechatronics, 2021, 80: 102671. DOI:10.1016/j.mechatronics.2021.102671 |
[15] |
曾俊宝, 李硕, 刘鑫宇, 等. 便携式自主水下机器人动力学建模方法研究[J]. 计算机应用研究, 2018, 35(6): 1747-1750. ZENG J B, LI S, LIU X Y, et al. Research on dynamic modeling of portable autonomous underwater vehicle[J]. Application Research of Computers, 2018, 35(6): 1747-1750. |