作业条件下的UVMS垂直面姿态控制研究

Research on UVMS vertical attitude control method under operation conditions

摘要

针对水下机器人-机械臂系统（underwater vehicle-manipulator system，UVMS）水下作业的运动控制所存在的自身结构不确定性干扰、系统动力学耦合干扰以及海流干扰问题以及解决方法展开研究。对系统建立合适的坐标系，并依此得到系统的运动学模型；根据系统垂直面动力学方程和系统之间耦合影响，建立了垂直面欠驱动型水下机器人数学模型。针对作业条件下UVMS垂直面姿态进行控制研究，设计了一种基于模糊自适应反步控制器，通过仿真对不同运动条件下的姿态进行控制验证，结果表明文中方法能够使UVMS在不同运动条件下保持良好的控制姿态。

Abstract

The problems of structure uncertainty, system dynamics coupling and ocean current interference in motion control of underwater vehicle-manipulator system (UVMS) are studied. A suitable coordinate system is established for the system, and the kinematics model of the system is obtained. According to the dynamic equation of vertical plane and the coupling effect between systems, the mathematical model of underactuated underwater vehicle in vertical plane is established. Aiming at the control of UVMS vertical plane attitude under operating conditions, an adaptive backstepping controller based on fuzzy is designed. The attitude control under different motion conditions is verified by simulation. The results show that the method in this paper can better make UVMS maintain a good control attitude under different motion conditions.

Graphical abstract

关键词

水下作业 / UVMS / 模型解耦 / 姿态控制

Key words

underwater operation / UVMS / model decoupling / attitude control

随着社会的发展，陆地资源渐显匮乏，而海洋资源开发越来越得到人们的重视^[1]。随着海洋开发事业的日益发展，水下机器人-机械臂系统(UVMS)在海洋地质勘查、水底打捞救援、设备维护、样本采集等方面发挥着重要的作用。由于自治型UVMS无须配备海缆和大型的母船，节省成本又增加灵活性，而且体积小，质量轻，功耗低，故自治UVMS能够胜任多样化的海洋作业。

自主水下机器人(AUV)^[2]在水下定点作业过程中主要承担运载水下机械臂到达定点位置完成作业任务，因此，所设计的AUV控制器必须在有限时间内跟踪上UVMS任务规划层给出的期望轨迹。在整个运动过程当中，AUV会一直受到水下机械臂耦合干扰与AUV系统自身的结构性不确定性和海流干扰等因素所引起的非结构性不确定性的影响，需要通过具有强鲁棒性的抗扰控制方法来抑制这些影响。此外AUV在定点作业的过程中，会遭遇珊瑚礁群和海底狭湾等复杂海底地形结构，此时为了避免碰撞，需要对AUV在运动过程中的位置和姿态进行控制^[3]。

为了研究水下机器人-机械臂系统(UVMS)姿态控制过程中复杂的动态耦合效应，部分学者将UVMS视作一个整体进行集中控制，致力于UVMS模型的建立。集中式的控制器需要面对较大的系统模型参数和非参数的不确定性，而基于模型的控制器可以在一定程度上提升控制性能^[4]。文献[5]基于快速增量型模型预测控制器(MPC)，加入了扩展卡尔曼滤波器应对未建模动态、环境干扰和传感器噪声，证明了UVMS轨迹跟踪过程具备较好运动控制性能。文献[6]提出了一种非线性模型预测控制(NMPC)方法，以便在运输物体的同时避免重大约束和限制，如运动学和表示奇点、工作空间内的障碍、关节限制和控制输入饱和。文献[7]忽略不确定项和干扰，给出UVMS名义模型系统，基于名义系统提出了一种基于ESO的RMPC算法，并通过仿真验证其轨迹跟踪性能和抗扰动能力。文献[8]提出了一种基于扰动观测器的水下机器人系统模型预测控制方法。考虑未知项和外部干扰的集总不确定性表示，在制定标称MPC后，开发二阶滑模扰动观测器(SMDO)来估计集总扰动，MPC使用这些观测器产生无偏预测来应对运载体动态耦合。文献[1]提出任务优先级控制策略，并进行了相关的实验，实验表明与传统PID控制相比，其对干扰项的抑制更佳。

除此之外，将UVMS的本体和机械臂分为两个独立系统分别控制，利用解耦控制也是最常用到的方法。文献[9]提出了一种基于角度和线性动态耦合因子的评估框架，研究了关节构型与耦合效应之间的关系，得到了关节变量的耦合图。耦合图为机械手轨迹优化提供了理论依据，减少了动态耦合对系统的影响，数值仿真结果验证了所提出的评价框架的有效性。文献[10]提出了一种自适应干扰观测器的反步控制方法，来提高其抗干扰能力。Han等^[11]对水下机器人和机械臂之间的动态耦合进行了详细的建模和仿真，建模过程描述了最主要的流体动力效应，提出了一种模糊解耦控制器，采用了非对角元素来利用子系统自由度之间的动态耦合和两个子系统之间的动态耦合。

滑模控制得益于系统状态在滑模面上对扰动不敏感的特性，也被大量应用于UVMS的控制之中^[12-13]。文献[13]针对这种非线性强耦合系统，采用单位四元数反馈设计了滑模控制器，避免了系统的奇异性，获得了良好的动态控制性能。

1 UVMS运动学建模

1.1 AUV运动学建模

AUV的动力系统采用单推进器和十字舵组合的方式，是一种欠驱动的水下机器人系统。水下机械臂由两个回转关节、两个摆动关节和一个机械爪组成，是一个四自由度的水下机械臂。

建立AUV的惯性坐标系，如图1所示。这里取海面上的任意一点为原点建立惯性坐标系O-xyz，引入载体坐标系O_b-x_b{y}_b{z}_b，将载体坐标系与AUV相固连，伴随AUV在水下自由运动，取AUV的浮心位置为原点O_b。

{\left[\begin{array}{ccc}u& v& w\end{array}\right]}^{\mathrm{T}}是载体坐标系原点线速度在x_b、y_b和z_b轴上投影，{\left[\begin{array}{ccc}p& q& r\end{array}\right]}^{\mathrm{T}}是载体坐标系原点的角速度在x_b、y_b和z_b轴上投影。{\left[\begin{array}{ccc}x& y& z\end{array}\right]}^{\mathrm{T}}和{\left[\begin{array}{ccc}\varphi & \theta & \psi \end{array}\right]}^{\mathrm{T}}是AUV在惯性坐标系下的位置和姿态信息。

设{\mathit{\eta }}_{\mathrm{1}}={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^{\mathrm{T}}，{\mathit{\eta }}_{\mathrm{2}}={\left[\begin{array}{ccc}\varphi & \theta & \psi \end{array}\right]}^{\mathrm{T}}；{\mathit{v}}_{\mathrm{1}}=\left[uv\right.{\left.w\right]}^{\mathrm{T}}，{\mathit{v}}_{\mathrm{2}}={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^{\mathrm{T}}；\mathit{q}={\left[q_{\mathrm{1}},q_{\mathrm{2}},q_{\mathrm{3}},q_{\mathrm{4}}\right]}^{\mathrm{T}}。

定义惯性坐标系下UVMS的各个关节位置向量为\mathit{\eta }=[{\mathit{\eta }}_{\mathrm{1}}^{\mathrm{T}},{\mathit{\eta }}_{\mathrm{2}}^{\mathrm{T}},{\mathit{q}}^{\mathrm{T}}{]}^{\mathrm{T}}\in {\mathfrak{R}}^{\mathrm{6}+\mathrm{4}}和各个关节速度为\mathit{\zeta }=\left[{\mathit{\nu }}_{\mathrm{1}}^T,\right.{\left.{\mathit{\nu }}_{\mathrm{2}}^{\mathrm{T}},{\dot{\mathit{q}}}^{\mathrm{T}}\right]}^{\mathrm{T}}\in {\mathfrak{R}}^{\mathrm{6}+\mathrm{4}}，得到UVMS关节空间的运动学模型：

\mathit{\zeta }=\left[\begin{array}{c}{\mathit{\nu }}_{\mathrm{1}}\\ {\mathit{\nu }}_{\mathrm{2}}\\ \dot{\mathit{q}}\end{array}\right]=\left[\begin{array}{ccc}{\mathit{R}}_I^B& {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{3}\times \mathrm{4}}\\ {\mathrm{0}}_{\mathrm{3}\times \mathrm{3}}& {\mathit{J}}_{\mathrm{2}}^{-\mathrm{1}}\left({\mathit{\eta }}_{\mathrm{2}}\right)& {\mathrm{0}}_{\mathrm{3}\times \mathrm{4}}\\ {\mathrm{0}}_{\mathrm{4}\times \mathrm{3}}& {\mathrm{0}}_{\mathrm{4}\times \mathrm{3}}& {\mathit{I}}_{\mathrm{4}\times \mathrm{4}}\end{array}\right]\left[\begin{array}{c}{\dot{\mathit{\eta }}}_{\mathrm{1}}\\ {\dot{\mathit{\eta }}}_{\mathrm{2}}\\ \dot{\mathit{q}}\end{array}\right]={\mathit{J}}_k\left[\begin{array}{c}{\dot{\mathit{\eta }}}_{\mathrm{1}}\\ {\dot{\mathit{\eta }}}_{\mathrm{2}}\\ \dot{\mathit{q}}\end{array}\right](1)

式中：{\mathit{I}}_{\mathrm{4}\times \mathrm{4}}是单位矩阵；{\mathrm{0}}_{\mathrm{3}\times \mathrm{4}}和{\mathrm{0}}_{\mathrm{4}\times \mathrm{3}}是零矩阵。

{\mathit{R}}_I^B\left({\mathit{\eta }}_{\mathrm{2}}\right)=\left[\begin{array}{ccc}c\psi c\theta & c\theta s\psi & -s\theta \\ -c\varphi s\psi +s\varphi s\theta c\psi & c\varphi \mathrm{c}\psi +s\varphi s\theta s\psi & s\varphi c\theta \\ s\varphi s\psi +c\varphi s\theta c\psi & -s\varphi c\psi +c\varphi s\theta s\psi & c\varphi c\theta \end{array}\right](2)

{\mathit{J}}_{\mathrm{2}}{\left({\mathit{\eta }}_{\mathrm{2}}\right)}^{-\mathrm{1}}=\left[\begin{array}{ccc}\mathrm{1}& \mathrm{0}& -s\theta \\ \mathrm{0}& c\varphi & c\theta s\varphi \\ \mathrm{0}& -s\varphi & c\theta c\varphi \end{array}\right](3)

式中：s·=\mathrm{s}\mathrm{i}\mathrm{n}(·)；c·=\mathrm{c}\mathrm{o}\mathrm{s}(·)。

1.2 水下机械臂运动学建模

水下机械臂的运动学包括正运动学和逆运动学。前者通过各连杆的角度可以推导出末端执行器的位姿，后者则可以通过末端执行器位姿推导出各连杆的运动角度。推导的过程需要在各连杆上建立坐标系，然后再利用各个坐标系之间的转换关系计算^[8]。

1) 水下机械臂正运动学

{{}_{}{}^{\mathrm{0}}}_{\mathrm{4}}={\mathit{T}}_{\mathrm{1}}\cdot {\mathit{T}}_{\mathrm{2}}\cdot {\mathit{T}}_{\mathrm{3}}\cdot {\mathit{T}}_{\mathrm{4}}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right](4)

式中：

\begin{array}{l}{n}_x=s{\theta }_{\mathrm{1}}s{\theta }_{\mathrm{4}} +c{\theta }_{\mathrm{1}}c{\theta }_{\mathrm{4}}c({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}),\\ o_x=s{\theta }_{\mathrm{1}}c{\theta }_{\mathrm{4}} +c{\theta }_{\mathrm{1}}s{\theta }_{\mathrm{4}}c({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}) ,\\ n_y=s{\theta }_{\mathrm{1}}c{\theta }_{\mathrm{4}}c({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}})-c{\theta }_{\mathrm{1}}s{\theta }_{\mathrm{4}},\\ o_y=-c{\theta }_{\mathrm{1}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}) +s{\theta }_{\mathrm{4}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}),\\ n_z=-c{\theta }_{\mathrm{4}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}),\\ o_z=s{\theta }_{\mathrm{4}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}),\\ a_x=-c{\theta }_{\mathrm{1}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}),\\ a_y=-s{\theta }_{\mathrm{1}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}),\\ a_z=-c({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}}) ,\\ p_x=-\mathrm{0.07}c{\theta }_{\mathrm{1}}-\mathrm{0.4}c{\theta }_{\mathrm{1}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}})-\mathrm{0.48}c{\theta }_{\mathrm{1}}c{\theta }_{\mathrm{2}},\\ p_y=-d_{\mathrm{4}}s{\theta }_{\mathrm{1}}s({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}})+a_{\mathrm{2}}s{\theta }_{\mathrm{1}}c{\theta }_{\mathrm{2}}+\mathrm{0.07}s{\theta }_{\mathrm{1}},\\ p_z=-\mathrm{0.4}\mathrm{c}({\theta }_{\mathrm{2}}+{\theta }_{\mathrm{3}})-\mathrm{0.48}s{\theta }_{\mathrm{2}}\end{array}(5)

2) 水下机械臂逆运动学

通过依次左乘位姿变换矩阵的逆，可以依次得到机械臂各连杆需要运动的角度。

c{\theta }_{\mathrm{1}}{p}_y-s{\theta }_{\mathrm{1}}{p}_x=\mathrm{0}(6)

所以，可以求得{\theta }_{\mathrm{1}}为：

{\theta }_{\mathrm{1}}=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{2}\left(\frac{p_y}{p_x}\right)(7)

同理，可以求得{\theta }_{\mathrm{2}}~{\theta }_{\mathrm{4}}。

2 UVMS动力学模型

2.1 AUV动力学

在分析过程中，假设为刚体且水流流速为0，AUV在水下受到的力和力矩有推力\mathit{T}，重力\mathit{W}，浮力\mathit{B}和水动力{\mathit{F}}_f等，所以，合外力\mathit{F}可以定义为：

\mathit{F}=\mathit{W}+\mathit{B}+\mathit{T}+{\mathit{F}}_f(8)

水下机器人所受的合外力矩也可以表示为：

\mathit{M}={\mathit{M}}_W+{\mathit{M}}_B+{\mathit{M}}_f+{\mathit{M}}_T(9)

文献[3]中总结了AUV在动坐标系下六个自由度上所受的力和力矩。

2.2 机械臂动力学建模

对水下机器人动力学和水动力进行分析整理，可以将水下机械臂动力学方程简写为以下形式：

{\mathit{M}}_m{\dot{\mathit{v}}}_m+{\mathit{C}}_m\left({\mathit{v}}_m\right){\mathit{v}}_m+{\mathit{D}}_m\left({\mathit{v}}_m\right){\mathit{v}}_m+{\mathit{G}}_m({\mathit{v}}_m)={\mathit{\tau }}_m(10)

式中：{\mathit{v}}_m\in R^{n\times \mathrm{1}}表示各连杆转动角度；{\mathit{M}}_m\in R^{n\times n}表示有效惯量矩阵和耦合惯量矩阵；{\mathit{C}}_m\left({\mathit{v}}_m\right)\in R^{n\times n}表示向心加速度和哥氏加速度；{\mathit{D}}_m\left({\mathit{v}}_m\right)\in R^{n\times n}表示水阻力和附加质量力；{\mathit{G}}_m\left({\mathit{v}}_m\right)\in R^{n\times \mathrm{1}}是水下机械臂重力项；{\mathit{\tau }}_m表示关节的驱动力矩。

2.3 UVMS整体动力学模型

通过上述水下机器人动力学分析和水下机械臂动力学分析，可以将水下机械臂对水下机器人的耦合影响作为水下机器人的外加干扰来建立整体的模型，如下所示：

\left\{\begin{array}{l}{\mathit{M}}_v\dot{\mathit{v}}+{\mathit{C}}_v(\mathit{v})\mathit{v}+{\mathit{D}}_v(\mathit{v})\mathit{v}+{\mathit{G}}_v(\mathit{v})={\mathit{\tau }}_v+\mathit{\Delta }\\ {\mathit{M}}_m{\dot{\mathit{v}}}_m+{\mathit{C}}_m\left({\mathit{v}}_m\right){\mathit{v}}_m+{\mathit{D}}_m\left({\mathit{v}}_m\right){\mathit{v}}_m+{\mathit{G}}_m({\mathit{v}}_m)={\mathit{\tau }}_m\end{array}\right.(11)

式中：\mathit{\Delta }包含了水下机械臂对水下机器人的耦合力和力矩，\mathit{\Delta }={[\begin{array}{cc}{}_{}{}^B{{\mathit{f}}_{M-A}}^{\mathrm{T}}& {}_{}{}^B{{\mathit{M}}_{M-A}}^{\mathrm{T}}\end{array}]}^{\mathrm{T}}，其中{{}_{}{}^B}_{M-A}是机械臂系统对AUV的作用力，{{}_{}{}^B}_{M-A}是机械臂系统对AUV的作用力矩。

2.4 UVMS耦合分析

为了抵消水下机械臂运动对本体位姿的耦合干扰，首先需要分析水下机械臂系统和水下机器人系统之间的耦合影响。耦合影响主要分为以下三种：水下机械臂基座对水下机器人本体的反作用力；水下机械臂运动造成重心和浮心变化导致的恢复力矩；机械臂运动导致的水阻力变化和引起流动区域的变化而导致水下机器人运动中产生的不同流体效应。

将对上述耦合进行详细分析：通过牛顿欧拉公式可以计算出对水下机器人机械臂基座的扰动；通过重心和浮心分析可以得到机械臂运动引起的恢复力；通过简单水动力分析可以得到机械臂不同姿态下的水阻力大小。

3 UVMS垂直面姿态控制

3.1 模糊自适应反步控制

忽略水下机器人水平面的运动，只考虑其垂直面，可以得到三自由度水下机器人运动学方程为：

\begin{array}{l}\dot{x}=u\mathrm{c}\mathrm{o}\mathrm{s}\theta +w\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ \dot{z}=-u\mathrm{s}\mathrm{i}\mathrm{n}\theta +w\mathrm{c}\mathrm{o}\mathrm{s}\theta \\ \dot{\theta }=q\end{array}(12)

式中：x表示横坐标位置；z表示纵坐标位置；u和w表示线速度；\theta表示俯仰角；q表示俯仰角速度。

同理，水下机器人垂直面动力学方程为：

\left\{\begin{array}{l}{m}_{\mathrm{11}}\dot{u}=-m_{\mathrm{22}}wq-c_{\mathrm{11}}u-f_u\left(u\right)+{\tau }_u+d_u+f_u\\ m_{\mathrm{22}}\dot{w}=m_{\mathrm{11}}uq-c_{\mathrm{22}}w-f_w\left(w\right)+d_w+f_w\\ \begin{array}{l}{m}_{\mathrm{33}}\dot{q}=\left(m_{\mathrm{22}}-m_{\mathrm{11}}\right)uw-c_{\mathrm{33}}q-f_q\left(q\right)\\ -\left(z_{g}W-z_{b}B\right)\mathrm{s}\mathrm{i}\mathrm{n}\theta +{\tau }_q+d_q+M_{M-A}\end{array}\end{array}\right.(13)

式中：水下机器人的惯性系数m_{\mathrm{11}}=m-X_{\dot{u}},m_{\mathrm{22}}=m-Z_{\dot{w}},m_{\mathrm{33}}=I_y-M_{\dot{q}}；水动力阻尼系数c_{\mathrm{11}}=X_u+X_{u\left|u\right|}\left|u\right|,c_{\mathrm{22}}=Y_v+Y_{v\left|v\right|}\left|v\right|,c_{\mathrm{33}}=M_q+M_{q\left|q\right|}\left|q\right|。定义{\mathit{d}}_x,{\mathit{d}}_z,{\mathit{d}}_q为定坐标系下的海流有界干扰力和干扰力矩，所以{\mathit{d}}_u={\mathit{d}}_x\mathrm{c}\mathrm{o}\mathrm{s}\theta -{\mathit{d}}_z\mathrm{s}\mathrm{i}\mathrm{n}\theta ,{\mathit{d}}_w={\mathit{d}}_x\mathrm{s}\mathrm{i}\mathrm{n}\theta +{\mathit{d}}_z\mathrm{c}\mathrm{o}\mathrm{s}\theta；定义f_u(u)、f_w(w)和f_q(q)为水下机器人动力学方程中的未建模动态和高阶非线性项，{\mathit{f}}_u和{\mathit{f}}_w分别表示水下机械臂对水下机器人的耦合力{\mathit{f}}_{M-A}在横坐标和纵坐标上的分量，{\mathit{M}}_{M-A}表示水下机械臂对水下机器人的耦合力矩。{\mathit{\tau }}_u和{\mathit{\tau }}_q表示动坐标系下水下机器人横向前进控制力和俯仰控制力矩。

为了应用反演控制，需要对UVMS垂直面模型进一步简化。假设\dot{u}=\mathrm{0}；水下机器人垂直速度w\approx \mathrm{0},\dot{w}\approx \mathrm{0}；纵倾角以较小的姿态变化，\mathrm{s}\mathrm{i}\mathrm{n}\theta \approx \theta。同时，令y_{\mathrm{1}}=x_{\mathrm{1}}=z,x_{\mathrm{2}}=\theta ,x_{\mathrm{3}}=q，有：

\left\{\begin{array}{l}{\dot{x}}_{\mathrm{1}}=b_{\mathrm{1}}{x}_{\mathrm{2}}\\ {\dot{x}}_{\mathrm{2}}=b_{\mathrm{2}}{x}_{\mathrm{3}}\\ {\dot{x}}_{\mathrm{3}}=b_{\mathrm{3}}{\tau }_q+f_{\mathrm{3}}+W_{\mathrm{3}}\end{array}\right.(14)

式中：

\begin{array}{l}{b}_{\mathrm{1}}=u,b_{\mathrm{2}}=\mathrm{1},b_{\mathrm{3}}=-\mathrm{1}/m_{\mathrm{33}},\\ f_{\mathrm{3}}=-\left(\left(z_{g}W-z_{b}B\right)x_{\mathrm{2}}+c_{\mathrm{33}}{x}_{\mathrm{3}}+f_q\left(q\right)\right)/m_{\mathrm{33}},\\ W_{\mathrm{3}}=\left(d_q+M_{M-A}+g(\eta )\right)/m_{\mathrm{33}}\mathrm{。}\end{array}

其中：系统的状态变量\mathit{x}=\left[x_{\mathrm{1}},x_{\mathrm{2}},x_{\mathrm{3}}\right]；系统控制输入为{\tau }_q；系统输出为y_{\mathrm{1}}=x_{\mathrm{1}}。

控制目的是在信号有界范围内输出y\left(t\right)跟踪期望轨迹y_d\left(t\right)。定义{\overline{x}}_i、y_{di}，其中{\overline{\mathit{x}}}_i={\left[x_{\mathrm{1}},x_{\mathrm{2}},x_{\mathrm{3}}\right]}^{\mathrm{T}}，{\mathit{y}}_{di}={\left[y_d,{\dot{y}}_d,{\ddot{y}}_d\right]}^{\mathrm{T}}。采用单值模糊器、乘积推理机和重心平均反模糊器：\mathrm{I}\mathrm{F}{x}_i\mathrm{i}\mathrm{s}{F}_{\mathrm{1}}^j\mathrm{a}\mathrm{n}\mathrm{d}\cdots \mathrm{a}\mathrm{n}\mathrm{d}{x}_n\mathrm{i}\mathrm{s}{F}_n^j\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{n}y\mathrm{i}\mathrm{s}{B}^{\hspace{0.17em}j}\left(j=\mathrm{1},\mathrm{2},\mathrm{3}\right)，则模糊系统的输出为：

\mathbf{y}\left(x\right)=\frac{{\displaystyle \sum _{j=\mathrm{1}}^{\mathrm{3}}}{\beta }_j{\prod }_i^{\mathrm{3}}{\mu }_i^{\hspace{0.17em}j}\left(x\right)}{{\displaystyle \sum _{j=\mathrm{1}}^{\mathrm{3}}}\left[{\prod }_i^{\mathrm{3}}{\mu }_i^{\hspace{0.17em}j}\left(x\right)\right]}={\mathit{\xi }}^{\mathrm{T}}\left(x\right)\mathit{\beta }(15)

式中：{\mu }_i^{\hspace{0.17em}j}\left(x_i\right)是隶属函数{\mu }_i^{\hspace{0.17em}j}；{\beta }_j=\mathrm{m}\mathrm{a}\mathrm{x}{B}^{\hspace{0.17em}j}\left(y\right)，B^{\hspace{0.17em}j}\left(y\right)是关于模糊系统输出y的定义在论域上的模糊语言值。

取\mathit{\xi }\left(x\right)={\left[{\xi }_{\mathrm{1}}\left(x\right),{\xi }_{\mathrm{2}}\left(x\right),{\xi }_{\mathrm{3}}\left(x\right)\right]}^{\mathrm{T}}，\mathit{\beta }={\left[{\beta }_{\mathrm{1}},{\beta }_{\mathrm{2}},{\beta }_{\mathrm{3}}\right]}^{\mathrm{T}}，则\mathit{y}\left(x\right)={\mathit{\xi }}^{\mathrm{T}}\left(x\right)\mathit{\beta }，其中\mathit{\xi }为矩阵系数，\mathit{\beta }为最优逼近向量。

根据模糊万能逼近定理^[1]，对于定义在紧集合\mathrm{\Omega }上的连续函数f\left(x\right)，都存在式(15)所示的模糊系统满足\mathrm{s}\mathrm{u}\mathrm{p}\left|f\left(x\right)-y\left(x\right)\right|\le \epsilon，其中\epsilon为大于零的任意常量。

3.2 模糊自适应反步控制设计

通过逐步构造中间变量e_i=x_i-{\alpha }_{i-\mathrm{1}}可以完成反步控制器的设计，其中{\alpha }_i是第i步的虚拟控制量，最后的虚拟控制量{\alpha }_n是施加于系统实际控制{\tau }_q的一部分。

采用反步法，对式(15)进行重新构造：

\left\{\begin{array}{l}{\dot{e}}_n=b_n{x}_n+b_n{\alpha }_n-{\dot{\alpha }}_{n-\mathrm{1}},n=\mathrm{1},\mathrm{2}\mathrm{ }{\alpha }_{\mathrm{0}}=z_d\\ {\dot{e}}_{\mathrm{3}}={\dot{x}}_{\mathrm{3}}-{\dot{\alpha }}_{\mathrm{2}}=b_{\mathrm{3}}{\tau }_q+f_{\mathrm{3}}\left({\overline{x}}_{\mathrm{3}}\right)+W_{\mathrm{3}}-{\dot{\alpha }}_{\mathrm{2}}\end{array}\right.(16)

为了使系统稳定，需要设计虚拟控制量{\alpha }_n和控制输入{\tau }_q。

对于第一个子系统，选择Lyapunov函数为：

V_{\mathrm{1}}=\frac{\mathrm{1}}{\mathrm{2}{b}_{\mathrm{1}}}{e}_{\mathrm{1}}^{\mathrm{2}}(17)

对其求导可得：

{\dot{V}}_{\mathrm{1}}=\frac{\mathrm{1}}{b_{\mathrm{1}}}{e}_{\mathrm{1}}{\dot{e}}_{\mathrm{1}}=e_{\mathrm{1}}\left(e_{\mathrm{2}}+{\alpha }_{\mathrm{1}}-\frac{\mathrm{1}}{b_{\mathrm{1}}}{\dot{z}}_d\right)(18)

选取控制信号{\alpha }_{\mathrm{2}}=-k_{\mathrm{2}}{e}_{\mathrm{2}}+{\dot{\alpha }}_{\mathrm{1}}/b_{\mathrm{2}},k_{\mathrm{2}}>\mathrm{0}，则式(18)变为：

{\dot{V}}_{\mathrm{1}}=-k_{\mathrm{1}}{e}_{\mathrm{1}}^{\mathrm{2}}+e_{\mathrm{1}}{e}_{\mathrm{2}}(19)

对于第二个子系统，选择Lyapunov函数为：

V_{\mathrm{2}}=V_{\mathrm{1}}+\frac{\mathrm{1}}{\mathrm{2}{b}_{\mathrm{2}}}{e}_{\mathrm{2}}^{\mathrm{2}}(20)

对其求导可得：

{\dot{V}}_{\mathrm{2}}={\dot{V}}_{\mathrm{1}}+\frac{\mathrm{1}}{b_{\mathrm{2}}}{e}_{\mathrm{2}}{\dot{e}}_{\mathrm{2}}=-k_{\mathrm{1}}{e}_{\mathrm{1}}^{\mathrm{2}}+e_{\mathrm{1}}{e}_{\mathrm{2}}+e_{\mathrm{2}}\left(e_{\mathrm{3}}+{\alpha }_{\mathrm{2}}-\frac{\mathrm{1}}{b_{\mathrm{2}}}{\dot{\alpha }}_{\mathrm{1}}\right)(21)

选取控制信号{\alpha }_{\mathrm{2}}=-k_{\mathrm{2}}{e}_{\mathrm{2}}+{\dot{\alpha }}_{\mathrm{1}}/b_{\mathrm{2}},k_{\mathrm{2}}>\mathrm{0}，则式(21)变为：

{\dot{V}}_{\mathrm{2}}=-k_{\mathrm{1}}{e}_{\mathrm{1}}^{\mathrm{2}}-k_{\mathrm{2}}{e}_{\mathrm{2}}^{\mathrm{2}}+e_{\mathrm{1}}{e}_{\mathrm{2}}+e_{\mathrm{2}}{e}_{\mathrm{3}}(22)

对于最后一个子系统，选择Lyapunov函数为：

V_{\mathrm{3}}=V_{\mathrm{2}}+\frac{\mathrm{1}}{\mathrm{2}{b}_{\mathrm{3}}}{e}_{\mathrm{3}}^{\mathrm{2}}(23)

对其求导可得：

\begin{array}{l}{\dot{V}}_{\mathrm{3}}={\dot{V}}_{\mathrm{2}}+\frac{\mathrm{1}}{b_{\mathrm{2}}}{e}_{\mathrm{3}}{\dot{e}}_{\mathrm{3}}=-k_{\mathrm{1}}{e}_{\mathrm{1}}^{\mathrm{2}}+e_{\mathrm{1}}{e}_{\mathrm{2}}+e_{\mathrm{2}}{e}_{\mathrm{3}}\\ -k_{\mathrm{2}}{e}_{\mathrm{2}}^{\mathrm{2}}+e_{\mathrm{3}}\left({\tau }_q+e_{\mathrm{2}}+{\widehat{f}}_{\mathrm{3}}\right)+\frac{e_{\mathrm{3}}}{b_{\mathrm{2}}}{W}_{\mathrm{3}}\end{array}(24)

式中：{\widehat{f}}_{\mathrm{3}}=\left(f_{\mathrm{3}}\left({\overline{x}}_{\mathrm{3}}\right)-{\dot{\alpha }}_{\mathrm{2}}\right)/b_{\mathrm{3}}，使用模糊系统{\phi }_{\mathrm{3}}逼近未知非线性函数{\widehat{f}}_{\mathrm{3}}，设计控制律为：

{\tau }_q=-k_{\mathrm{3}}{e}_{\mathrm{3}}-e_{\mathrm{2}}-{\phi }_{\mathrm{3}},k_{\mathrm{3}}>\mathrm{0}(25)

所以，有：

{\dot{V}}_{\mathrm{3}}=-k_{\mathrm{1}}{e}_{\mathrm{1}}^{\mathrm{2}}+e_{\mathrm{1}}{e}_{\mathrm{2}}+e_{\mathrm{2}}{e}_{\mathrm{3}}-k_{\mathrm{2}}{e}_{\mathrm{2}}^{\mathrm{2}}+e_{\mathrm{3}}\left({\widehat{f}}_{\mathrm{3}}-{\phi }_{\mathrm{3}}\right)+\frac{e_{\mathrm{3}}}{b_{\mathrm{2}}}{W}_{\mathrm{3}}(26)

采用{\phi }_{\mathrm{3}}={\mathit{\zeta }}^{\mathrm{T}}\left(\overline{x}\right)\mathit{\beta }逼近{\widehat{f}}_{\mathrm{3}}，存在最优向量{\mathit{\beta }}^{\mathrm{*}}，对于给定任意小的常量\left|{\widehat{f}}_{\mathrm{3}}-{\mathit{\beta }}_{}^{\mathrm{*}}\mathit{\zeta }\left({\overline{x}}_{\mathrm{3}}\right)\right|\le {\epsilon }_{\mathrm{3}}，{\epsilon }_{\mathrm{3}}>\mathrm{0}。取\tilde{\mathit{\beta }}={\mathit{\beta }}^{\mathrm{*}}-\mathit{\beta }，设计自适应律为：

\dot{\beta }=r{e}_{\mathrm{3}}{\zeta }_{\mathrm{3}}\left({\overline{x}}_{\mathrm{3}}\right)-\mathrm{2}\lambda \beta(27)

4 UVMS垂直面姿态控制仿真

4.1 水下机械臂控制器设计

设大臂关节质量m_{\mathrm{1}}=\mathrm{3}\mathrm{k}\mathrm{g}，连杆长度L_{\mathrm{1}}=\mathrm{0.4}\mathrm{m}；小臂关节质量m_{\mathrm{2}}=\mathrm{2}\mathrm{k}\mathrm{g}，连杆长度L_{\mathrm{2}}=\mathrm{0.2}\mathrm{m}；连杆质心到坐标原点的距离l_{\mathrm{1}}=\mathrm{0.2}\mathrm{m}，l_{\mathrm{2}}=\mathrm{0.1}\mathrm{m}；作业条件下假设末端抓取重物m_{\mathrm{3}}=\mathrm{0.5}\mathrm{k}\mathrm{g}；机械臂基座在动坐标系的位置为{{}_{}{}^B}_{\mathrm{0}}={\left[\begin{array}{ccc}\mathrm{0}& \mathrm{0}& \mathrm{0.1}\end{array}\right]}^{\mathrm{T}}，g=\mathrm{9.8}\mathrm{N}/\mathrm{k}\mathrm{g}。控制器参数\rho =\mathrm{1}，k_{\mathrm{1}}=\mathrm{3}，k_{\mathrm{2}}=\mathrm{2.5}，k_{\mathrm{3}}=\mathrm{1}，\lambda =\mathrm{1.5}，r=\mathrm{2}。取模糊隶属度函数为：

\begin{array}{l}{\mu }_i^{\mathrm{1}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i+\mathrm{2}\right)}^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{2}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i+\mathrm{1.5}\right)}^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{3}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i+\mathrm{1}\right)}^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{4}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i+\mathrm{0.5}\right)}^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{5}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{x}_i^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{6}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i-\mathrm{0.5}\right)}^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{7}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i-\mathrm{1}\right)}^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{8}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i-\mathrm{1.5}\right)}^{\mathrm{2}}\right),\\ {\mu }_i^{\mathrm{9}}\left(x_i\right)=\mathrm{e}\mathrm{x}\mathrm{p}\left(-\mathrm{0.6}{\left(x_i-\mathrm{2}\right)}^{\mathrm{2}}\right)\end{array}(28)

得到隶属度曲线图（图2）。

所以，有：

{\zeta }_{\mathrm{3}j}\left({\overline{x}}_{\mathrm{3}}\right)=\frac{{\mu }_{\mathrm{1}}^{\hspace{0.17em}j}\left(x_{\mathrm{1}}\right){\mu }_{\mathrm{2}}^{\hspace{0.17em}j}\left(x_{\mathrm{2}}\right){\mu }_{\mathrm{3}}^{\hspace{0.17em}j}\left(x_{\mathrm{3}}\right)}{{\displaystyle \sum _{j=\mathrm{1}}^{\mathrm{9}}}{\mu }_{\mathrm{1}}^{\hspace{0.17em}j}\left(x_{\mathrm{1}}\right){\mu }_{\mathrm{2}}^{\hspace{0.17em}j}\left(x_{\mathrm{2}}\right){\mu }_{\mathrm{3}}^{\hspace{0.17em}j}\left(x_{\mathrm{3}}\right)}(29)

{\mathit{\zeta }}_{\mathrm{3}}\left({\overline{x}}_{\mathrm{3}}\right)={\left[{\zeta }_{\mathrm{31}}\left({\overline{x}}_{\mathrm{3}}\right),{\zeta }_{\mathrm{32}}\left({\overline{x}}_{\mathrm{3}}\right),\cdots ,{\zeta }_{\mathrm{39}}\left({\overline{x}}_{\mathrm{3}}\right)\right]}^{\mathrm{T}}(30)

控制律设计为：

{\tau }_q=-k_{\mathrm{3}}\left(x_{\mathrm{3}}-{\alpha }_{\mathrm{2}}\right)-\left(x_{\mathrm{2}}-{\alpha }_{\mathrm{1}}\right)-{\mathit{\zeta }}_{\mathrm{3}}^{\mathrm{T}}\left({\overline{x}}_{\mathrm{3}}\right)\mathit{\beta }(31)

欠驱动型水下机器人仿真相关参数如表1所示。

4.2 定深和变深水下作业仿真实验

为了验证作业条件下UVMS姿态控制的有效性，分别对垂直面UVMS定深作业中姿态和变深作业中姿态进行仿真。在仿真中，假设水下机械臂运动对本体的耦合力矩{{}_{}{}^B}_{M-A}为常值，仿真时长150 s。在仿真中，水下机械臂在第50 s进行展开作业，大臂关节作业期望角度为-0.8 rad，小臂关节作业期望角度为-1.57 rad。

1) 定深水下作业仿真

设置水下机器人沿x轴线速度u=\mathrm{1.4}\mathrm{m}/\mathrm{s}，初始深度5.6 m，期望跟踪作业深度为6 m。当t=50 s时，机械臂关节在夹持重物的情况下开始运动，水下机械臂总质量为5.5 kg，其造成的耦合影响不可忽略。

从图3中可以看出，大臂关节和小臂关节可以快速达到关节期望角度，且小臂关节运动速度更快。此时，机械臂运动造成的恢复力矩成为影响水下机器人姿态的主要因素。

在垂直面UVMS作业条件下姿态控制中，主要考虑水下机械臂对俯仰角\theta以及俯仰角速度q和沿z轴的线速度的影响。如图4所示，水下机器人在10 s内跟踪上期望的深度，且在此期间俯仰角、俯仰角速度和其线速度变化较快。之后保持稳定，直到50 s时，水下机械臂开始运动，此时，线速度有了微弱的变化，但水下机器人俯仰角及角速度几乎没有变化，证明了控制器在UVMS垂直面作业过程中可以保持姿态的稳定，且有较强鲁棒性。

在水下机器人作业过程中，可以得到其姿态的变化曲线，在跟踪目标深度过程中俯仰角\theta最大值为15°左右；在机械臂作业条件下，水下机器人姿态为俯仰角\theta保持0°；沿z轴线速度w\approx \mathrm{0}，符合建模时提出的假设条件。可以看出，在整个作业过程中，水下机器人姿态符合设计要求，始终保持在理想姿态。

2) 变深水下作业仿真

为了进一步验证作业过程中姿态控制的稳定性，设计对变化的深度轨迹进行跟踪，进行水下机械臂展开作业条件下的姿态研究。设水下机器人沿x轴速度u=\mathrm{1.4}\mathrm{m}/\mathrm{s}，初始深度z=\mathrm{9}\mathrm{m}，期望跟踪作业深度为z_d=\mathrm{10}+\mathrm{s}\mathrm{i}\mathrm{n}(\mathrm{\pi }t/\mathrm{10})。同样，当t=\mathrm{50}\mathrm{s}时大臂关节和小臂关节在夹持重物m_{\mathrm{3}}=\mathrm{0.5}\mathrm{k}\mathrm{g}的情况下开始运动，大臂关节作业期望角度为-0.8 rad，小臂关节作业期望角度为-1.57 rad。

水下机器人初始深度为9 m，在10 s左右跟踪上目标深度轨迹，在追踪目标轨迹过程中，水下机器人俯仰角\theta以及俯仰角速度q变化较快，俯仰角\theta最大为25°左右，由于目标深度为正弦函数，在跟踪上期望轨迹后，俯仰角也呈现正弦规律的变化，幅值为15°左右。当t=\mathrm{50}\mathrm{s}时，大臂关节和小臂关节在夹持重物m_{\mathrm{3}}的情况下开始运动，俯仰角\theta以及俯仰角速度q几乎没有受到影响，沿z轴线速度w\approx \mathrm{0}。证明了控制器在跟踪给定深度变化的目标轨迹时，在机械臂展开作业过程中保持姿态的稳定，且有较强鲁棒性。

5 结论

本文首先对UVMS进行建模，将机械臂对水下机器人本体的耦合看作干扰，然后对垂直面作业条件下UVMS姿态进行控制研究，根据简化垂直面动力学方程和系统之间耦合干扰，设计了一种基于模糊自适应反步控制器，分别对定深作业条件和变深作业条件下的UVMS姿态进行控制，通过仿真分析，证明了控制器有良好的姿态控制效果。本文对UVMS建模、UVMS耦合分析、水下机器人姿态控制进行了初步的研究。由于水下环境复杂多变，本文仅对机械臂的水阻力进行了简单计算，需要进一步对机械臂水动力和其运动对本体的水动力影响进行研究，建立更加准确的数学模型，提高控制效果来完成复杂的海洋作业任务。

参考文献

[1] DI VITO D, DE PALMA D, SIMETTI E, et al. Experimental validation of the modeling and control of a multibody underwater vehicle manipulator system for sea mining exploration[J]. Journal of Field Robotics, 2021, 38(2): 171-191.

[2] 蒋新松, 封锡盛, 王棣棠. 水下机器人[M]. 沈阳: 辽宁科学技术出版社, 2000.

[3] 黄琰, 李岩, 俞建成, 等. AUV智能化现状与发展趋势[J]. 机器人, 2020, 42(2): 215-231.

[4] KARIMI H R, LU Y A. Guidance and control methodologies for marine vehicles: a survey[J]. Control Engineering Practice, 2021, 111: 104785.

[5] DAI Y, YU S H, YAN Y. An adaptive EKF-FMPC for the trajectory tracking of UVMS[J]. IEEE Journal of Oceanic Engineering, 2020, 45(3): 699-713.

[6] HESHMATI-ALAMDARI S, KARRAS G C, KYRIAKOPOULOS K J. A predictive control approach for cooperative transportation by multiple underwater vehicle manipulator systems[J]. IEEE Transactions on Control Systems Technology, 2022, 30(3): 917-930.

[7] 王红都, 高枫, 黎明, 等. 基于ESO的水下机器人机械臂系统鲁棒模型预测控制[J]. 水下无人系统学报, 2023, 31(6): 827-838.

[8] OLIVEIRA É L, ORSINO R M M, DONHA D C. Disturbance-observer-based model predictive control of underwater vehicle manipulator systems[J]. IFAC-PapersOnLine, 2021, 54(16): 348-355.

[9] XIONG X Y, XIANG X B, WANG Z, et al. On dynamic coupling effects of underwater vehicle-dual-manipulator system[J]. Ocean Engineering, 2022, 258: 111699.

[10] 侯冬冬, 都军民, 秦丽萍, 等. 基于自适应干扰观测器的水下机器人-机械臂系统反步控制[J]. 水下无人系统学报, 2023, 31(3): 413-420.

[11] HAN H, WEI Y, YE X, et al. Motion planning and coordinated control of underwater vehicle-manipulator systems with inertial delay control and fuzzy compensator[J]. Applied Sciences, 2020, 10(11): 3944.

[12] 经慧祥, 侯冬冬, 王凯, 等. 水下作业机器人滑模自抗扰控制方法研究[J]. 舰船科学技术, 2023, 45(1): 101-107.

[13] DU X W, XIAO H Y, HOU D D, et al. A quaternion-based sliding mode controller for UVMS[C]//2020 Chinese Automation Congress (CAC). Shanghai, China: IEEE, 2020: 3552-3557.

基金资助

海南省科技计划三亚崖州湾科技城联合项目(2021CXLH0001)