于 林，孟庆浩，刘科显，徐雪寒

(天津大学电气自动化与信息工程学院，天津 300072)

深海蕴藏着丰富的资源，深海探测是实现深海资源开发与利用的必要手段[1]。近些年，随着技术的快速发展，机器人在深海探测领域展现出了诱人的潜力。深海机器人也称深海潜水器，可以分为载人潜水器、无人潜水器和其它深海勘探设备(如深海滑翔机[2-3]等)，其中，无人潜水器按与母船之间是否有电缆连接分为有缆遥控潜水器(Remotely Operated Vehicle，ROV)[4]和无缆自主潜水器(Autonomous Underwater Vehicle，AUV)[5-6]。海底复杂的地形结构、未知的突发状况对AUV的机动性提出了更高的要求。现有的深海潜水器多为单刚体结构，采用尾舵和姿态调整系统控制运动方向，转弯半径大，机动性较差。蛇形机器人[7-8]、仿鳗机器人[9]和仿生鱼机器人[10]模仿生物行为方式，具有较高的灵活性，但一般用于水平方向运动，很少用于竖直方向探测。为实现深海纵向剖面探测，要求AUV具有灵活的纵向机动性，为此本文提出了一种新型多关节深海潜水器(Multi-Joint Autonomous Underwater Vehicle，MJ-AUV)。MJ-AUV由导流舱、导航控制舱、推进舱三部分组成，每两个舱段之间用正交关节连接，可以通过转动关节调整各舱段的相对姿态，改变MJ-AUV的水动力外形，进而调整机体姿态。MJ-AUV可以实现多种运动模式，比如水平蜿蜒运动，俯仰运动，螺旋上升、下潜运动等。

MJ-AUV属于多刚体无根系统，运动学和动力学建模是研究其运动和控制问题的前提。Kelasidi等人[11]使用力和力矩平衡理论建立了水下蛇形机器人在二维水平面上的运动学、动力学模型；
夏丹等人[12]提出了基于凯恩方法的仿生鱼模型；
唐敬阁等人[13]提出了一种水下滑翔蛇形机器人结构，使用动量定理建立机器人滑翔运动模型，使用递推牛顿-欧拉法建立机器人蛇形游动模型；
除此之外，欧拉-拉格朗日法[14]和Schiehlen[15]等方法也常用于处理多刚体建模问题。

近年来，水下机器人的控制研究越来越多，文献[16]针对多输入多输出AUV系统设计一种模糊自适应比例积分微分控制器，用于航向和深度控制。Sakiyama等人[17]提出了一种考虑建模误差的鲁棒控制器，实现对AUV的运动和姿态控制。Cui等人[18]采用干扰观测器对AUV扰动进行动态补偿。为解决水下滑翔蛇形机器人[13]纵倾控制过程中输入受限和扰动未知的问题，陈恩志等[19]将Nussbaum函数和双曲正切函数相结合，解决系统输入饱和问题，并通过非线性干扰观测器实现对外界扰动的观测和补偿，仿真结果表明控制器具有较强的鲁棒性。文献[20-23]分别在自抗扰控制器的基础上，做了不同的改进，例如结合滑模控制器、自搜索最优算法等方法，提高AUV运动控制的快速性、精确性、抗干扰性等性能。

MJ-AUV是一个高度非线性系统，具有强耦合、滞后、扰动未知等特性，不易建立准确的数学模型。此外，在俯仰控制过程中，MJ-AUV属于双输入单输出(即两个关节的角度变化作为系统输入，机体俯仰角度作为输出)的过驱动系统，这也给控制器的设计带来难题。本文参考AUV在机体坐标系中的建模方法，考虑重力和浮力不平衡的情况，使用牛顿第二定律和力矩平衡原理建立MJ-AUV在纵垂面上的运动学和动力学模型。以关节角度作为被控模型输入，符合MJ-AUV的实际操纵要求。为提高俯仰控制的精准性和抗干扰能力，本文提出一种将线性二次型最优控制器(linear quadratic regulator，LQR)与线性自抗扰控制器(linear active disturbance rejection control，LADRC)[24]相结合的线性二次型最优自抗扰控制器(linear quadratic regulator active disturbance rejection control，LQR-ADRC)，设计线性扩张状态观测器(linear extended state observer，LESO)估计系统总扰动，使用奇异值分解(singular value decomposition，SVD)法求解控制系数阵的伪逆矩阵，用于扰动补偿和输入分配，采用LQR优化线性反馈增益，使其按照期望输入输出效果达到最优。仿真结果表明LQR-ADRC控制器具有超调量小、抗干扰、精准度高等优点。

2.1 多关节深海潜水器坐标系建立

图1定义了MJ-AUV的惯性坐标系OEXEYEZE、机体坐标系(导航控制舱坐标系)OBXBYBZB、导流舱坐标系OHXHYHZH和推进舱坐标系OTXTYTZT。惯性系原点建立在海平面一点处，OEXE在海平面内指向东方，OEZE垂直于海平面并指向天空，OEYE在海平面内并垂直于OEXE，指定方向满足右手螺旋定则；
机体系原点固连在导航控制舱的形心位置，OBXB沿舱体的轴线方向向前，OBZB垂直于OBXB向上，OBYB的建立满足右手螺旋定则；
导流舱和推进舱的坐标系建立方式与机体坐标系建立方式类似。

2.2 多关节深海潜水器纵垂面运动学分析

图1 多关节深海潜水器坐标系定义

各舱体坐标系原点相对于机体坐标系的位置BPi如式(1)所示

(1)

其中，li为对应舱体长度的二分之一，i=H，B，T，下文同理；
BRH和BRT分别为导流舱坐标系和推进舱坐标系到机体坐标系的转换矩阵，分别表示为

(2)

式中，θ1为关节1(导流舱与导航控制舱之间的关节)的俯仰角度，θ2为关节2(导航控制舱与推进舱之间的关节)的俯仰角度，关节绕机体坐标系Y轴逆时针转动方向为正。

各舱体坐标系在机体坐标系下表示的角速度Bωi与线速度Bvi分别为

(3)

(4)

机体系速度在惯性系中表示为

(5)

(6)

各舱段坐标系在机体系下表示的角加速度Bαi与线加速度Bai分别为

(7)

(8)

2.3 多关节深海潜水器动力学分析

2.3.1 动力学分析

图2 多关节深海潜水器各舱段受力分析

根据牛顿第二定律，对MJ-AUV各舱段进行受力分析，为便于处理，所有的力将放在机体坐标系下进行计算，受力分析如式(9)所示

(9)

其中，mH、mB、mT分别表示各舱段的质量，I为3×3的单位矩阵。将(9)式中三个式子相加，可以消除关节间的相互作用力，得

BFbH+BFdH+BFaH-BGH+FbB+FdB+

FaB-GB+BFbT+BFdT+BFaT-BGT+

BFth-mHIBaH-mBIaB-mTIBaT=0

(10)

根据力矩平衡原理，力矩分析如下式所示

(11)

其中

Mgi是各舱段由于重心与形心不重合而产生的对各自舱段的重力矩；
HMB→H为控制舱对导流舱产生的力矩在导流舱坐标系中表示的结果，BMB→H、BMT→H、TMT→B的定义与HMB→H类似；
Ji为各舱段的转动惯量矩阵；
由于MJ-AUV只在垂直面运动，因此αH=BαH，αT=BαT，HM1=BM1，TM2=BM2。

MJ-AUV通过改变关节角度来控制系统姿态，将(11)中三式相加，消除关节扭矩，得

MaH-MgH+MaB-MgB+MaT-MgT+

HMB→H-BMB→H+BMT→B-TMB→T-

JHαH-JBαB-JTαT=0

(12)

2.3.2 水动力

MJ-AUV工作在深海环境中，运动速度较为缓慢，关节不会频繁摆动，可做如下假设：

假设1：水体流速相对于惯性坐标系为0；

假设2：各舱段水动力系数仅和自身形状尺寸有关。

2.3.2.1 阻力

MJ-AUV外部挂载多个传感器，并非规则的圆柱体，考虑压差阻力和尾涡脱落效应对其影响，阻力表达式如下

(13)

其中

2.3.2.2 附加质量效应

MJ-AUV在做变速运动时，会与周围流体形成相对加速运动，引起附加质量效应，产生与加速度方向相反的作用效果，可用下式表示

BFai=-BRi(λmiai)

Mai=-λJiαi

(14)

式中

2.3.2.3 浮力

浮力等于MJ-AUV所排开水的重力，假设海水不同深度密度变化不大，近似为常值，当MJ-AUV完全浸入海水中时，满足下式

(15)

式中，ρliquid为海水密度，Vi表示各舱段的体积，g表示重力加速度。

浮力作用在各舱段形心位置处，因此各舱段所受浮力对本舱段产生的力矩为0。

2.3.3 重力和重力矩

空气中，MJ-AUV各舱段所受重力在机体坐标系中表示为

式中mi为各舱段质量。

重心在形心正下方位置，因此各舱段会受到重力所产生的重力矩作用，如下式所示

(17)

式中，lc为各舱段重心与形心的距离。

2.3.4 推进器推力

推进器安装在MJ-AUV的推进舱尾部，且为单向力，方向沿着推进舱轴向方向向前，因此推力对推进舱不产生力矩作用。推进器推力在机体坐标系中表示为

BFth=BRTFth

(18)

2.3.5 各舱段间相互作用力和力矩

虽然在受力分析中消掉了各舱段间的相互作用力，但在式(12)中，各舱段间相互作用力矩是无法消掉的，根据(9)式，推导各舱段间相互作用力如下

(19)

式(12)中的相互作用力矩为

(20)

2.4 状态空间方程建立

将式(10)和(12)整理成状态空间方程形式：

(21)

上述动力学模型为解析模型，可用于基于模型的现代控制器设计。该模型将关节角度作为系统的输入量，符合实验样机设计要求。

通过调节关节1、2的俯仰角度调整各舱段间的相对姿态，改变MJ-AUV水动力外形，调整机体的俯仰角度，此时，系统属于过驱动系统。将LQR和LADRC相结合，并合理分配系统输入，提出一种LQR-ADRC俯仰控制方案，控制系统结构如图3所示。

图3 LQR-ADRC控制器结构图

3.1 LESO观测器设计

根据MJ-AUV动力学模型，其俯仰姿态模型可整理为

(22)

式中，fac1、fac2、bac1、bac2、bac3为关于X的非线性函数，wd为系统的外部干扰。

式(22)表明MJ-AUV是一个强耦合的高度非线性系统，很难将模型解耦，因此本文参考文献[21]的思想，将MJ-AUV所受的内力、外力定义为系统总扰动，取b1≈bac1，b2≈bac2，其中b1、b2为常值。式(22)可改写为

+(bac2-b2)θ2+bac3θ1θ2+b1θ1+b2θ2

=f+b1θ1+b2θ2

(23)

式中f=fac1θ+fac2q+wd+(bac1-b1)θ1+(bac2-b2)θ2+bac3θ1θ2，f为系统总扰动，b1、b2为关节1和关节2 的俯仰角控制系数。

(24)

(25)

(26)

(27)

3.2 LQR-ADRC观测器设计

引入误差e=x1d-x1，则式(27)可改写为

(28)

其中x1d为目标俯仰角值。

定义z1=e，z2=x2，则整理式(28)为

(29)

控制器设计主要包括两部分。第一部分为扰动补偿输入uf，第二部分采用LQR控制器设计u0，即u=u0+uf。

扰动分配补偿控制器为

(30)

将(30)带入(29)得

(31)

u0=-Kz，

(32)

K值有四个参数，本文通过LQR来调整。LQR可以降低调整参数难度，以最小化性能指标来获取适合控制目标的最优控制律。选取LQR性能指标函数为

(33)

式中Q为半正定矩阵，R为正定矩阵，前一项为系统状态误差的惩罚函数，后一项为系统输入状态的惩罚函数。该性能指标函数对应的Riccati方程组为

(34)

式中，P为(34)的解，为对称正定矩阵。此时线性反馈增益矩阵为

K=R-1BTP

(35)

3.3 稳定性分析

MJ-AUV的系统模型为

(36)

(37)

将(30)、(32)代入(37)得

(38)

将(35)代入(38)得

(39)

4.1 模型搭建

4.2 控制参数

根据MJ-AUV的数学模型，选取系统控制系数b1=0.1，b2=0.1。考虑系统俯仰控制性能要求，选取权重矩阵为Q=diag(600，300)，R=diag(0.1，0.1)，diag表示对角矩阵。利用MATLAB的lqr()函数得到反馈增益矩阵为

参考文献[26]，ω0的选取影响观测器的精度，ω0越大，观测精度越高，但过大容易引起观测器不稳定，通过观察仿真结果对其进行调整，最终选取ω0=80。

4.3 仿真结果

图4-图7为无干扰情况下MJ-AUV的俯仰角控制效果。图4表明系统遇到阶跃信号能够快速做出调整，且低超调，控制效果良好，图4 (b)为0-15s过程的放大图，出现这种现象的原因是MJ-AUV的净浮力为正，各舱段间的合力矩不平衡，初始关节角度为0时，机体不为平衡状态，因此系统通过调整关节角度控制机体俯仰角度为0。图5为系统在无干扰输入情况下观测器对总扰动的估计结果，证明LESO对扰动观测的有效性。图7为参考信号与实际俯仰角度之间的误差值，系统稳定后差值为10-8rad，说明控制系统具有较高的准确性。

图5 无干扰情况下总扰动估计

图6 无干扰情况下MJ-AUV输入

图7 无干扰情况下MJ-AUV俯仰控制误差

图8 有输入干扰情况下MJ-AUV俯仰控制

图9 有输入干扰情况下总扰动估计

图10 有输入干扰情况下MJ-AUV输入

图8-图11为有输入干扰情况下的俯仰控制仿真结果，干扰加在关节1的输入位置，在70s时，输入0.1rad的阶跃干扰信号，90s时输入-0.2rad的阶跃干扰信号。仿真结果表明系统输入在受到干扰情况下能够迅速调整，使系统稳定下来，表现良好的抗干扰特性。

图11 有输入干扰情况下MJ-AUV俯仰控制误差

本文提出一种具有正交关节结构的新型多关节深海潜水器(MJ-AUV)，考虑重力和浮力不平衡的情况，根据牛顿第二定律和力矩平衡原理建立潜水器纵垂面运动学、动力学模型，提出一种将LQR与LADRC相结合的LQR-ADRC控制器，在SIMULINK中进行MJ-AUV俯仰姿态仿真与分析，得出以下结论：

1)所建立的模型为解析模型，可用于对MJ-AUV运动姿态的研究和基于模型的控制器的设计。

2)采用线性状态观测器观测系统总扰动，采用SVD法求解控制系数矩阵的伪逆矩阵，实现对总扰动的合理补偿与分配，有效解决了系统过驱动的问题。

3)采用LQR实现线性反馈控制增益按照期望输入输出效果达到最优，使参数整定工作更加高效。

4)仿真结果表明LQR-ADRC控制器具有低超调、抗干扰、控制精确等优势，同时该控制器具有计算量小、不依赖系统精确模型的特点，具有很好的工程应用价值。

在未来工作中，计划将该控制算法应用于MJ-AUV实际控制器系统中，并完成实验验证。

猜你喜欢 舱段潜水器深海 近年“国际空间站”美国舱段环控生保系统事件回顾国际太空(2021年8期)2021-11-05《无人潜水器检验指南》将于2019年7月1日生效中国船检(2019年6期)2019-11-29深海特“潜”队小哥白尼(趣味科学)(2019年7期)2019-11-16某型号舱段复杂环框类工件整体数控加工工艺研究制造技术与机床(2019年10期)2019-10-26基于TwinCAT的舱段数字化柔性自动对接平台控制系统设计制造技术与机床(2019年4期)2019-04-04我国首艘载人潜水器支持母船“深海一号”在武船开工中国军转民(2017年9期)2017-12-19隐藏在深海里的神秘生物学苑创造·A版(2017年4期)2017-05-13深海里的神秘生物学苑创造·A版(2017年5期)2017-05-09多舱段圆柱壳振动特性研究振动工程学报(2014年4期)2014-03-01