水下机器人桨鳍协同推进姿态控制

夏明海; 尚建忠; 殷谦; 曾潇丰; 徐毓泽; 罗自荣; XIA Minghai; SHANG Jianzhong; YIN Qian; ZENG Xiaofeng; XU Yuze; LUO Zirong

水下机器人桨鳍协同推进姿态控制

doi: 10.11887/j.cn.202406020

夏明海¹ ，尚建忠¹ ，殷谦² ，曾潇丰¹ ，徐毓泽¹ ，罗自荣¹

1. 国防科技大学智能科学学院，湖南长沙 410073

2. 长沙理工大学能源与动力工程学院，湖南长沙 410076

基金项目: 国家自然科学基金面上资助项目（52075537）；国家自然科学基金青年科学基金资助项目（52105289）

详细信息

作者简介

夏明海（1995—），男，湖北黄冈人，博士研究生，E-mail: xiaminghai@nudt.edu.cn

通讯作者

罗自荣（1974—），男，湖南邵阳人，教授，博士，博士生导师，E-mail: luozirong@nudt.edu.cn

中图分类号: TP24

文献标识码: A

文章编号: 1001-2486(2024)06-184-10

Attitude control of propeller-fin cooperative propulsion for underwater robot

XIA Minghai¹ ， SHANG Jianzhong¹ ， YIN Qian² ， ZENG Xiaofeng¹ ， XU Yuze¹ ， LUO Zirong¹

1. College of Intelligence Science and Technology, National University of Defense Technology, Changsha 410073 , China

2. College of Energy and Power Engineering, Changsha University of Science & Technology, Changsha 410076 , China

摘要

为提高水下机器人的机动性与稳定性，提出一种四旋翼波动鳍混合驱动的新型水下机器人及其姿态控制算法。建立了机器人桨鳍协同推进的运动学与动力学模型，通过流体数值仿真，获取了水动力系数，构建了机器人六自由度动力方程。提出螺旋桨与波动鳍协同推进的姿态控制策略，设计了一种四自由度串级比例-积分-微分控制器与控制分配算法。在MATLAB/Simulink中建立了仿真模型，仿真分析了机器人定深前进、升沉运动、原地转向中的机器人位姿曲线，以及随机干扰对姿态控制的影响。实验结果证明，机器人具备良好的姿态控制性能，最大推进频率下样机运动姿态角波动误差小于±4°，深度波动误差小于±5 cm。仿真及实验结果验证了所设计的水下机器人与桨鳍协同推进的姿态控制算法的可行性。

关键词

水下机器人 / 波动鳍 / 四旋翼 / 姿态控制 / 串级比例-积分-微分控制

Abstract

In order to improve the maneuverability and stability of underwater robots, a novel underwater robot driven by quadrotor and undulating fin was developed and its attitude control algorithm was proposed. The kinematics and dynamics model of cooperative propulsion by the propellers and the fin was established. The resistance coefficient was obtained by fluid numerical simulation. And the six-degree-of-freedom dynamic model of the robot was constructed. The attitude control strategy using hybrid drive of propellers and fin was proposed. A four degrees of freedom cascade proportional-integral-derivative controller and a control allocation algorithm were designed. The simulation model was established in MATLAB/Simulink software, in which pose curves in fixed depth cruise motion, heave motion and pivot steering motion were simulated and analysed. In addition, the influence of random interference on attitude control was simulated. Experimental results show that the robot has good attitude control performance. At the maximum undulating frequency, the attitude angle error of the prototype is less than ±4°; and the depth error is less than ±5 cm. The simulation and experimental results verify the feasibility of the novel underwater robot and the attitude control algorithm of propeller-fin cooperative propulsion.

Keywords

underwater robot / undulating fin / quadrotor / attitude control / cascade proportional-integral-derivative control

1 控制模型 1.1 机器人设计方案 1.2 运动学与动力学模型 1.3 控制力模型 1.4 机器人水动力模型 2 控制系统设计与仿真 2.1 控制器设计 2.2 仿真模型 2.3 仿真结果 2.3.1 升沉运动 2.3.2 原地转向运动 2.3.3 定深前进运动 2.3.4 抗干扰运动仿真 3 实验验证 3.1 实验平台 3.2.1 升沉运动 3.2.2 原地转向运动 3.2.3 定深前进运动 4 结论

传统的水下机器人多是螺旋桨产生推力，配合舵面控制转向与平衡，其缺点是低速和狭小空间下机动性与稳定性不足。随着无人系统在作战打击、抢险救灾等军民事应用的需求越来越高，需要能够实现多自由度运动、定点悬停、稳定性好的新型水下机器人平台。

受旋翼飞行器的启发，出现了多旋翼布局方式的水下机器人，展现出良好的机动性与可控性。文献^[1]提出一种四旋翼航行器，推进器水平环绕在艇身周围，可完成四个自由度运动。文献^[2]针对一种欠驱动四旋翼水下航行器，基于非奇异终端滑模控制器仿真研究了五自由度轨迹跟踪问题。文献^[3]提出了一种四旋翼蝶形自主水下航行器，仿真研究了在不同的攻角下的流体特性。文献^[4-5]设计了一种X型布局的四旋翼水下航行器，并针对运动控制与编队控制展开了研究。文献^[6]提出一种可倾转四旋翼水下机器人，基于分解补偿控制实现了六自由度悬停控制。文献^[7]使用可倾转四旋翼设计了一种跨介质航行器，控制旋翼倾角实现水空两栖运动。

鱼类具有非凡的水下运动能力，其推进方式具有效率高、机动性强、隐蔽性好等突出优势。国内外学者相继研制出模拟鱼类中央鳍-对鳍（median and/or paired fin，MPF）推进模式的仿生长鳍水下航行器。文献^[8]设计了一种仿鱿鱼机器人，双侧波动鳍各由17个电机驱动，配合尾舵可以灵活实现六自由度动作。文献^[9]设计了一种仿刀鱼机器人、文献^[10]提出一种仿魔鬼鱼水下推进器，均采用一组波动长鳍推进。文献^[11]设计了一种仿生刀鱼波动推进水下机器人，并对比分析了有无两侧胸鳍对推进效率与机动性能的影响。文献^[12]研制了一种液压驱动仿生波动鳍推进器，文献^[13]研制了基于胸鳍波动与仿生喷射系统混合推进的仿生墨鱼机器人。文献^[14]在新一代仿鳐鱼机器人上实现了定深定向、路径跟踪等闭环控制。文献^[15]提出了一种环形长鳍推进的水下机器人，并针对中枢模式发生器（central pattern generator，CPG）控制展开了研究。文献^[16]设计了一种四波动鳍前后串联排列的新型仿生水下机器人，并研究了波数、相位差等参数对推进效率的影响。

研究表明，仿生波动鳍推进模式机动性与抗干扰能力强、环境适应性好，同时具有较高的推进效率^[17]，多旋翼的控制策略灵活、控制精度高、响应速度快。针对现有水下机器人机动性与稳定性不足、低速与狭小空间下适应能力差的问题，结合波动鳍的推进优势与螺旋桨的控制优势，设计了一种桨鳍协同推进的水下机器人，并针对姿态控制问题展开研究。该机器人具备前进/后退、上升/下潜、偏航、滚转、俯仰等五自由度运动能力，并且可以通过运动解耦实现上述任一单自由度运动，从而具有极强的适应能力。针对机器人动力学模型，设计了一种基于串级比例-积分-微分控制器的多自由度姿态控制算法。通过Simulink仿真验证了机器人的多模态运动能力与控制器效果，实验结果显示机器人具有良好的控制性能。

1 控制模型

1.1 机器人设计方案

桨鳍协同推进的水下机器人设计模型如图1（a）所示。机器人采用模块化设计，由四旋翼子系统、波动鳍驱动子系统、波动鳍摆动子系统、波动鳍子系统组成。控制舱位于四旋翼子系统内部，安装电池、微控制器、传感器等电气部件。波动鳍驱动子系统内部设置一个直流电机，通过齿轮系统将动力传递到波动鳍。波动鳍如图1（b）所示，通过机械拟合生成正弦波形，使用8根鳍条夹持波动鳍面，驱动电机的连续旋转通过凸轮传动转化为鳍条的正弦摆动；相邻鳍条相位差为90°，从而形成两个完整波形。波动鳍摆动单元内部设置一个舵机，经过齿轮传动，控制波动鳍整体绕机体中轴线左右偏转，从而可以形成偏航力矩，如图1（c）所示。

螺旋桨为X型布局，垂直安装于机体对角线四处，通过控制四个螺旋桨的转速，快速补偿运动过程中的流体阻力与外部扰动，从而调节机体姿态角。在运动控制中，通过螺旋桨与波动鳍不同的运动组合，可以生成直线巡游、上浮下沉、原地转向、行进转弯、空间平移等多模式运动，并且具备良好的稳定性。

图1水下机器人原理样机

Fig.1Principle prototype of the underwater robot

1.2 运动学与动力学模型

水下机器人受到波动鳍推进力、螺旋桨推进力及流体阻力等复合作用，在机体坐标系下进行动力学分析更为简洁，其通用动力学模型为：

\{\begin{matrix} τ = M (v) \dot{v} + C (v) v + D (v) v + g (η) \\ τ = B u \end{matrix}

(1)

其中：

M （ v ）

为惯性矩阵；

C （ v ）

为离心力与科氏力矩阵；

D （ v ）

为阻尼矩阵；

g （ η ）

为恢复力矩阵；

τ =

{(τ_{X} ， τ_{Y} ， τ_{Z} ， τ_{K} ， τ_{M} ， τ_{N})}^{T}

为六自由度力与力矩；u为控制力输入，即四个螺旋桨与波动鳍产生的推力；B为控制矩阵，表示从控制力输入到六自由度力和力矩的转换关系；

η = （ X ， Y ， Z ， ϕ ， θ ， ψ ）^{T}

为地理坐标系下的位姿信息；

v = （ u ， v ， w ， p ， q ， r ）^{T}

为机体坐标系下的线速度与角速度。

水下机器人的运动学模型描述了机器人从机体坐标系到地理坐标系的转换关系，表示为：

\dot{η} = J (η) v

(2)

式中，

J （ η ）

为坐标系旋转矩阵，其定义为：

J (η) = (\begin{matrix} J_{1} (η) & 0 \\ 0 & J_{2} (η) \end{matrix})

(3)

J_{1} （ η ）

与

J_{2} （ η ）

为线速度和角速度旋转矩阵，形式如下：

J_{1} (η) = (\begin{matrix} c_{ψ} c_{θ} & c_{ψ} s_{θ} s_{ϕ} - s_{ψ} c_{ϕ} & c_{ψ} s_{θ} c_{ϕ} + s_{ψ} s_{ϕ} \\ s_{ψ} c_{θ} & s_{ψ} s_{θ} s_{ϕ} + c_{ψ} c_{ϕ} & s_{ψ} s_{θ} c_{ϕ} - c_{ψ} s_{ϕ} \\ - s_{θ} & c_{θ} s_{ϕ} & c_{θ} c_{ϕ} \end{matrix})

(4)

J_{2} (η) = (\begin{matrix} 1 & s_{ϕ} t_{θ} & c_{ϕ} t_{θ} \\ 0 & c_{ϕ} & - s_{ϕ} \\ 0 & s_{ϕ} / c_{θ} & c_{ϕ} / c_{θ} \end{matrix})

(5)

其中，c_（·）、s_（·）、t_（·）分别为cos（·）、sin（·）、tan（·）的缩写。

1.3 控制力模型

机器人的控制变量为螺旋桨的转速、波动鳍的频率和波动鳍的偏转角度，动力输入为四个螺旋桨的垂直推力和波动鳍波动生成的水平推力和侧向力，输出为六自由度力与力矩，其受力简图如图2所示。

图2水下机器人控制力分析

Fig.2Input force analysis of the underwater robot

记螺旋桨等效力作用点为O_i，作用力和反扭矩分别为F_i和τ_i（i=1，2，3，4），推进器之间距离为a、b。螺旋桨的推力与反扭力为：

\{\begin{matrix} F_{i} = s i g n (w_{i}) c_{T} w_{i}^{2} \\ τ_{i} = λ_{i} s i g n (w_{i}) c_{M} w_{i}^{2} \end{matrix} i = 1,2, 3,4

(6)

其中：w_i为螺旋桨转速；c_T、c_M分别为推力系数与反扭力系数，可通过实验测量获取；λ_i为方向系数，由螺旋桨安装方向决定。本文中相邻螺旋桨旋向相反，即λ₁=λ₃=1，λ₂=λ₄=-1。

波动鳍在波动运动中会产生两个方向的力，即平行于波动鳍基线的推力和垂直于基线的侧向力^[18]，分别记为F_P和F_N。记波动鳍偏转角度为β，并规定向右偏转为正。仿真研究表明，波动鳍的推力与波动频率近似为二阶关系：

\{\begin{matrix} F_{P} = s i g n (w_{f}) c_{P} w_{f}^{2} \\ F_{N} = c_{N} w_{f}^{2} \end{matrix}

(7)

其中：w_f为波动鳍的波频；c_P和c_N分别为波动鳍推力系数与侧向力系数，可通过仿真与实验获取。当波动方向相反时，推力F_P方向也相反，从而产生前进或后退运动；侧向力F_N的方向则始终从波动鳍外沿指向其基线。当β=0时指向-z方向，力的效果表现为增加浮力。

等效作用点P_f取在鳍面的中心，机器人重心为G，令GP_f=l，则等效力作用点坐标为P_f（0，ls_β，lc_β）。当β≠0时，侧向力F_N将会产生y轴和z轴的分力。另u=（F₁，F₂，F₃，F₄，F_P，F_N）^T，设k=c_M/c_T为螺旋桨推力与反扭力的比例系数，六自由度控制力求解如下：

(\begin{matrix} τ_{X} \\ τ_{Y} \\ τ_{Z} \\ τ_{K} \\ τ_{M} \\ τ_{N} \end{matrix}) = (\begin{matrix} 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & s_{β} \\ 1 & 1 & 1 & 1 & 0 & c_{β} \\ a & a & - a & - a & 0 & 0 \\ - b & b & b & - b & c_{β} & 0 \\ k & - k & k & - k & - s_{β} & 0 \end{matrix}) \cdot (\begin{matrix} F_{1} \\ F_{2} \\ F_{3} \\ F_{4} \\ F_{P} \\ F_{N} \end{matrix})

(8)

由式（8）可知，水下航行器的六自由度控制力与输入存在耦合关系。机器人可以通过控制输入力的大小和方向产生六个自由度的运动，从而实现前进/后退、上升/下潜、原地旋转、前进转向、空间平移等多模式运动，通过桨鳍协同调节实现姿态稳定控制。表1为水下机器人多模式运动控制输入策略。

表1水下机器人多模式运动控制输入策略

Tab.1 Input strategy of multi-mode motion control for the underwater robot

注：+/-表示力或角度的方向，↑/↓表示增加或减小力。

1.4 机器人水动力模型

水下机器人运动过程中受到的阻力非常复杂，目前常用的方法是将阻尼表示为一次阻尼与二次阻尼的无耦合叠加形式。阻尼力矩阵如式（9）所示，其中（

X_{u} ， Y_{v} ， Z_{w} ， K_{p} ， M_{q} ， N_{r}

）依次表示六自由度力和力矩的一次阻尼系数，（

X_{u | u |} ，

Y_{v | v |} ， Z_{w | w |} ， K_{p | p |} ， M_{q | q |} ， N_{r | r |}

）依次表示六自由度力和力矩的二次阻尼系数。

\begin{matrix} D (v) = d i a g (X_{u} + X_{u | u |} | u |, Y_{v} + Y_{v | v |} | v |, \\ Z_{w} + Z_{w | w |} | w |, K_{p} + K_{p | p |} | p | \\ M_{q} + M_{{q|}_{q} ∣} | q |, N_{r} + N_{r | r |} | r |) \end{matrix}

(9)

计算流体力学（computer fluid dynamics，CFD）是水动力分析的有效手段，通过CFD仿真模拟机器人水下运动的受力情况。在SolidWorks中建立水下机器人运动的流体域简化模型，通过ICEM软件划分计算网格，导入Fluent软件进行数值仿真。流体域截面图如图3（a）所示，对内流域进行局部网格加密，外流域总长与总高设置为内流域的10倍，以模拟机器人在前进方向与垂直方向的运动。机器人网格划分如图3（b）所示。

图3流体域设计与计算网格划分

Fig.3Fluid domain design and computational grid meshing

设置机器人不同的直线运动速度与角运动速率，仿真得到对应的阻尼力/力矩，通过最小二乘法辨识模型参数，得到升沉运动、前后运动、侧移运动、滚转运动、俯仰运动、偏航运动下的阻尼力系数。其中侧移运动仅附加在前进转向中，无法单独实现，故仿真计算中忽略不计。图4为机器人阻尼力、阻尼力矩数值仿真结果与拟合曲线，辨识结果见表2。

图4机器人阻尼力与力矩数值仿真结果

Fig.4Numerical simulation results of damping force and damping torque of the robot

表2计算流体力学数值仿真的阻尼系数结果

Tab.2 Result of damping coefficient in CFD numerical simulation

机器人的设计浮心略高于重心，以提供一定的恢复力保证机器人在紧急情况下浮到水面，恢复力矩同时能够辅助机器人保持机体平衡。记浮心位置P_B（0，0，z_B），重力和浮力分别为W和B，则恢复力矩阵为：

g (η) = (\begin{matrix} (W - B) s_{θ} \\ (B - W) c_{θ} s_{ϕ} \\ (B - W) c_{θ} c_{ϕ} \\ - z_{B} B c_{θ} s_{ϕ} \\ - z_{B} B s_{θ} \\ 0 \end{matrix})

(10)

机器人的惯性矩阵由两部分组成，一部分是机器人本身的质量m和惯性矩（J_xx，J_yy，J_zz），另一部分是由加速度引起的附加质量力，记为（

X_{\dot{u}}

，

Y_{\dot{v}}

，

Z_{\dot{w}}

，

K_{\dot{p}}

，

M_{\dot{q}}

，

N_{\dot{r}}

）^T，则惯性矩阵求解为：

M (v) = (m + X_{\dot{u}}, m + Y_{\dot{v}}, m + Z_{\dot{w}}, J_{x x} + K_{\dot{p}},

{J_{y y} + M_{\dot{q}}, J_{z z} + N_{\dot{r}})}^{T}

(11)

为简化建模与计算，做以下简化：①由于机器人在低速下航行，可以忽略科氏力和向心力的影响，即C（v）=0；②不考虑机器人的侧移运动。将控制力矩阵、质量矩阵、阻尼矩阵与恢复力矩阵代入式（1）中并展开，得到机器人六自由度动力学方程为：

\{\begin{matrix} m \dot{u} = τ_{X} - X_{\dot{u}} \dot{u} - (X_{u} + X_{u | u |} | u |) u - (W - B) s_{θ} \\ m \dot{v} = 0 \\ m \dot{w} = τ_{Z} - Z_{\dot{w}} \dot{w} - (Z_{w} + Z_{{w|}_{w}} | w |) w - (B - W) c_{θ} c_{ϕ} \\ J_{x x} \dot{p} = τ_{K} - K_{\dot{p}} \dot{p} - (K_{p} + K_{{p|}_{p} ∣} | p |) p + z_{B} B c_{θ} s_{ϕ} \\ J_{y y} \dot{q} = τ_{M} - M_{\dot{q}} \dot{q} - (M_{q} + M_{{q|}_{q} ∣} | q |) q + z_{B} B s_{θ} \\ J_{z z} \dot{r} = τ_{N} - N_{r} \dot{r} - (N_{r} + N_{r | r |} | r |) r \end{matrix}

(12)

2 控制系统设计与仿真

2.1 控制器设计

水下航行器是典型的多输入多输出系统，存在欠驱动、非线性、强耦合特性。机器人在运动过程中，由于外部水流扰动与实际系统的非线性、时变特性，系统易出现振荡甚至失稳。设计基于桨鳍协同推进的多自由度串级比例-积分-微分（proportional-integral-derivative，PID）控制器，实现对深度与三轴角度的闭环控制。

前进通道采用速度开环控制，对三轴角度与z轴位置设计独立的串级PID闭环控制器，引入速度反馈提高系统的响应速度。系统控制器设计如图5所示。

图5深度与姿态角串级PID控制器

Fig.5Cascade PID controller of depth and attitude angle

由于四个螺旋桨与波动鳍的驱动力和力矩存在耦合，四个独立通道控制器输出的控制力和力矩需要通过控制分配器求取最终的系统控制量，即螺旋桨的转速、波动鳍的波动频率与偏转角度。对螺旋桨的控制分配算法为：

(\begin{matrix} w_{1} \\ w_{2} \\ w_{3} \\ w_{4} \end{matrix}) = \frac{1}{4} (\begin{matrix} 1 & 1 & - 1 & 1 \\ 1 & 1 & 1 & - 1 \\ 1 & - 1 & 1 & 1 \\ 1 & - 1 & - 1 & - 1 \end{matrix}) \cdot (\begin{matrix} τ_{Z d} \\ τ_{K d} \\ τ_{M d} \\ τ_{N d} \end{matrix})

(13)

式中，

τ_{Z d} ， τ_{K d} ， τ_{M d} ， τ_{N d}

分别为PID控制器计算得到的期望升沉推力、期望滚转力矩、期望俯仰力矩与期望偏航力矩。

偏航控制器输出τ_N_d同时映射到波动鳍偏转角度。波动鳍通过整体角度左右偏置，产生顺时针或逆时针偏航力矩，与四旋翼协同控制机器人方向。在运动过程中偏置角度实时调整，对波动鳍偏置角度的控制分配采用线性输出：

β = k_{N} τ_{N d} - 30^{\circ} < β < 30^{\circ}

(14)

2.2 仿真模型

表3给出了机器人的质量参数与推进器、波动鳍的动力参数。将系统参数代入式（12），并将附加质量力归为外部干扰项，得到机器人的数学模型。

1）前进方向模型

\dot{u} = 0.067 τ_{X} - (0.048 + 0.832 | u |) u + 0.653 s_{θ}

2）垂直方向模型：

\dot{w} = 0.067 τ_{Z} - (0.733 + 5.256 | w |) w - 0.653 c_{θ} c_{ϕ}

3）滚转角模型：

\dot{p} = 8.4 τ_{K} - (0.639 + {2.823|}_{p} ∣) p - 52.7 c_{θ} s_{ϕ}

4）俯仰角模型：

\dot{q} = 1.689 τ_{M} - (0.045 + 1.533 | q |) q - 52.7 s_{θ}

5）偏航角模型：

\dot{r} = 1.776 τ_{N} - (0.024 + 5.291 | r |) r

表3机器人系统参数

Tab.3 System parameters of the underwater robot

2.3 仿真结果

为验证控制器效果与模拟PID参数对控制性能的影响，使用Simulink搭建系统仿真模型。设参考输入为

R = {(Z_{d} ， ϕ_{d} ， θ_{d} ， ψ_{d} ， w_{d})}^{T}

，分别表示四个通道期望位置与波动鳍波动角频率，初始状态相应为

s = {(Z_{0} ， ϕ_{0} ， θ_{0} ， ψ_{0} ， w_{0})}^{T}

。在桨鳍协同推进的各种运动模式下，期望滚转角与俯仰角均被设置为0，即φ_d=0，θ_d=0，机器人运动过程中将始终保持机体平衡。通过设置不同的参考输入与初始条件，观察系统仿真结果，探讨机器人在升沉运动、原地转向运动、定深前进运动等典型模式下的机动性与姿态控制性能。

2.3.1 升沉运动

传统的水下机器人使用舵面或采用改变重心的方式控制俯仰角，依靠改变力的方向实现沉浮，这两种方式机体都需要改变姿态并保持一定的推进速度，无法在狭小空间展开。四旋翼的布局则可以实现单自由度沉浮运动。由于升沉运动中无前后运动，波动鳍波动频率设置为0。

设定期望状态为R=（1，0，0，0，0）^T（t＜20 s）和R=（0，0，0，0，0）^T（t≥20 s）。初始状态设为s=（0，0，0，0，0）^T，给深度和三轴角度分别施加幅值为2 cm和2°的随机噪声干扰，施加频率为10 Hz，模拟机器人在随机扰动下的升沉运动。仿真结果如图6所示，机器人能够快速到达期望深度，并且超调和稳态误差较小，同时在有噪声干扰的情况下能调节姿态角稳定在0°附近。

图6机器人升沉运动仿真结果

Fig.6Simulation result of robot′s heaving motion

2.3.2 原地转向运动

四旋翼的布局同样使机器人具备原地转向的机动能力，使得机器人在狭小空间具备优势。原地转向时控制四个电机同向转动，则同时产生单方向偏航力矩，即可实现绕中心转动。

设定参考输入R=（0，0，0，90°，0）^T，初始状态设为s=（0，10°，-10°，0，0）^T，施加同样的噪声干扰，模拟机器人原地转向运动。仿真结果如图7所示，机器人快速跟踪到给定航向角，运动过程中滚转角和俯仰角稳定在水平位置，深度保持在0。

图7机器人原地转向运动仿真位姿-时间曲线

Fig.7Simulation pose-time result of robot′s in-place rotation motion

2.3.3 定深前进运动

水下机器人定深潜航是最为重要的运动模式。由于前进时阻力复杂，机器人容易在水作用力下偏航或倾覆。通过四旋翼与波动鳍摆角的实时调节，控制机器人前进时始终保持定深、定航向与机体水平。

波动鳍的设计频率为2 Hz，设定参考输入R=（1 m，0，0，0，2 Hz）^T，初始状态s=（0，0，0，0，0）^T，即从水面静止状态运动到1 m深度，同时使波动鳍按照2 Hz的频率前进，模拟波动鳍高速波动时机器人定深前进运动。给定前述噪声干扰，仿真结果如图8所示，机器人在跟踪到期望深度的同时，加速到最大速度后向前运动，运动过程中没有侧向位移，姿态角和深度稳定为参考输入值0附近。

2.3.4 抗干扰运动仿真

水下机器人需要在受到外部扰动之后迅速调节机体回到平衡位置，单纯依靠机器人自身恢复力矩难以实现快速、稳定调节，通过螺旋桨的补偿可以提高机器人抗干扰能力。

设定参考输入R=（0，0，0，0，0）^T，初始状态设为s=（0，10°，10°，10°，0）^T，模拟机器人在悬停状态下受到x轴、y轴和z轴干扰下的系统响应。仿真结果如图9所示，机器人迅速调节姿态角到平衡状态，说明机器人具有良好的抗干扰能力，能够在复杂水环境下快速调节自身平衡。

图8机器人定深前进运动仿真结果

Fig.8Simulation result of robot′s depth-keeping motion

图9给定干扰下的机器人运动仿真结果

Fig.9Simulation result of robot motion under disturbance

3 实验验证

3.1 实验平台

为验证控制算法与仿真分析的正确性，在4 m×2 m×2 m（长×宽×高）的室外水池进行机器人水下实验，如图10所示为机器人样机。

机器人的控制方式为手持遥控器控制与地面站控制同时进行。使用基于Qt自主开发的上位机软件，界面如图11所示。通过上位机发送期望指令到机器人，机载STM32控制器接收指令后控制机器人运动。机器人通过机载惯性传感器、水压传感器、电压计等感知位姿、速度、电量等信息，通过离散PID控制器调节电机输出，实现机器人位姿控制。同时将机器人的实时状态上传到上位机进行数据显示与数据保存。

图10机器人样机

Fig.10Prototype of the robot

图11水下机器人地面站界面

Fig.11Interface of the underwater robot′s ground station software

3.2.1 升沉运动

机器人升沉运动实验结果如图12所示。从深度曲线可以看出，机器人快速响应设置的期望深度并最终稳定悬停，最大跟踪速度约为0.23 m/s，超调量约为12%，稳态误差为±2 cm。从姿态角曲线看出，机器人下降过程中伴随着角度波动，但能够迅速调节，稳态时角度波动范围小于±2°。

3.2.2 原地转向运动

机器人原地转向运动实验结果如图13所示，机器人能够迅速跟踪期望偏航角，最大跟踪速度约为18（°）/s，超调量约为11%，稳态误差为±3°。航向调节初始阶段伴随深度增加，随后在定深调节下回到稳定，深度值波动范围小于±4 cm。滚转角与俯仰角在初始波动后恢复稳定，波动范围小于±3°。

图12机器人升沉运动实验结果

Fig.12Experiment result of robot′s heaving motion

图13机器人原地转向运动实验结果

Fig.13Experimental result of robot′s in-place rotation motion

3.2.3 定深前进运动

由于水池长度限制，定深实验设置机器人往返运动，前进后退时波动频率分别为120 r/min与-120 r/min，对应频率为2 Hz与-2 Hz。实验结果如图14所示，机器人平均前进速度约为0.27 m/s，运动中三轴角度在PID控制器的作用下保持在给定的0附近，同时深度在1 m附近调节，可认为机体水平，航向一致，深度保持。实验结果表明，相比于升沉运动与定深悬停，定深前进中深度和角度波动范围更大，但机器人能够有效调节姿态与深度在稳定位置，深度波动小于±5 cm，角度小于± 4°。

从实验结果来看，机器人在多种运动模式下实现了对深度与姿态角的良好控制，验证了理论分析与控制器设计的有效性。与仿真结果对比，实验条件下机器人的位姿曲线整体趋势较吻合，但超调量与稳态误差相比仿真结果更大。分析有以下原因：①理论仿真模型中进行了适当简化，实际模型中的未建模与非线性分量对系统有一定的影响。如仿真模型中电机为理想输出，实际系统中电机受电压、负载等因素限制，其输出为非线性，且受响应时间约束。②由于PID控制器本质为线性控制器，而水下机器人为复杂的非线性系统。实验中PID参数较多，整定较为困难，没有达到仿真模型的理想效果。③仿真模型中的反馈数据来源于计算结果，而实际系统中传感器存在误差与噪声，且采样频率有限。④实际系统的外部干扰难以预测，对系统的响应也有一定影响。

图14机器人定深前进运动实验结果

Fig.14Experimental result of robot′s depth-keeping motion

4 结论

结合螺旋桨驱动与仿生波动鳍驱动的优势，设计了一种四旋翼与单波动鳍协同推进的水下机器人，并对其姿态控制展开了研究。①建立了桨鳍协同推进的运动学与动力学模型，通过计算流体力学仿真模拟了机器人的水动力系数，构建了机器人多自由度动力方程；②针对三轴姿态角与深度的位姿控制问题，设计了基于四自由度串级PID的空间运动控制器；③在MATLAB/Simulink中建立了系统仿真模型，仿真分析了机器人多模式运动的机动性与稳定性。④开展了样机实验，实验结果证明机器人能够稳定执行前进后退、定深悬停、垂直升沉、原地转向等多模式运动，机器人在低速与狭小空间下具有较好的机动性与稳定性。试验环境下机器人定深前进速度为0.27 m/s，垂直运动速度为0.23 m/s，原地转向速度18 （°）/s，运动时三轴姿态角误差小于± 4°，深度控制误差小于±5 cm。下一步将针对仿生机器人样机进行改进及优化，包括：①仿生流体外形优化，减小水阻力；②安装红外、水声等传感器，实现机器人水下导航与避障。

图1水下机器人原理样机

Fig.1Principle prototype of the underwater robot

下载: 全尺寸图片

图2水下机器人控制力分析

Fig.2Input force analysis of the underwater robot

下载: 全尺寸图片

图3流体域设计与计算网格划分

Fig.3Fluid domain design and computational grid meshing

下载: 全尺寸图片

图4机器人阻尼力与力矩数值仿真结果

Fig.4Numerical simulation results of damping force and damping torque of the robot

下载: 全尺寸图片

图5深度与姿态角串级PID控制器

Fig.5Cascade PID controller of depth and attitude angle

下载: 全尺寸图片

图6机器人升沉运动仿真结果

Fig.6Simulation result of robot′s heaving motion

下载: 全尺寸图片

图7机器人原地转向运动仿真位姿-时间曲线

Fig.7Simulation pose-time result of robot′s in-place rotation motion

下载: 全尺寸图片

图8机器人定深前进运动仿真结果

Fig.8Simulation result of robot′s depth-keeping motion

下载: 全尺寸图片

图9给定干扰下的机器人运动仿真结果

Fig.9Simulation result of robot motion under disturbance

下载: 全尺寸图片

图10机器人样机

Fig.10Prototype of the robot

下载: 全尺寸图片

图11水下机器人地面站界面

Fig.11Interface of the underwater robot′s ground station software

下载: 全尺寸图片

图12机器人升沉运动实验结果

Fig.12Experiment result of robot′s heaving motion

下载: 全尺寸图片

图13机器人原地转向运动实验结果

Fig.13Experimental result of robot′s in-place rotation motion

下载: 全尺寸图片

图14机器人定深前进运动实验结果

Fig.14Experimental result of robot′s depth-keeping motion

下载: 全尺寸图片

表1水下机器人多模式运动控制输入策略

下载: 全尺寸图片

表2计算流体力学数值仿真的阻尼系数结果

下载: 全尺寸图片

表3机器人系统参数

下载: 全尺寸图片

图1水下机器人原理样机

Fig.1Principle prototype of the underwater robot

图2水下机器人控制力分析

Fig.2Input force analysis of the underwater robot

图3流体域设计与计算网格划分

Fig.3Fluid domain design and computational grid meshing

图4机器人阻尼力与力矩数值仿真结果

Fig.4Numerical simulation results of damping force and damping torque of the robot

图5深度与姿态角串级PID控制器

Fig.5Cascade PID controller of depth and attitude angle

图6机器人升沉运动仿真结果

Fig.6Simulation result of robot′s heaving motion

图7机器人原地转向运动仿真位姿-时间曲线

Fig.7Simulation pose-time result of robot′s in-place rotation motion

图8机器人定深前进运动仿真结果

Fig.8Simulation result of robot′s depth-keeping motion

图9给定干扰下的机器人运动仿真结果

Fig.9Simulation result of robot motion under disturbance

图10机器人样机

Fig.10Prototype of the robot

图11水下机器人地面站界面

Fig.11Interface of the underwater robot′s ground station software

图12机器人升沉运动实验结果

Fig.12Experiment result of robot′s heaving motion

图13机器人原地转向运动实验结果

Fig.13Experimental result of robot′s in-place rotation motion

图14机器人定深前进运动实验结果

Fig.14Experimental result of robot′s depth-keeping motion

表1水下机器人多模式运动控制输入策略

表2计算流体力学数值仿真的阻尼系数结果

表3机器人系统参数

图1水下机器人原理样机

Fig.1Principle prototype of the underwater robot

图2水下机器人控制力分析

Fig.2Input force analysis of the underwater robot

图3流体域设计与计算网格划分

Fig.3Fluid domain design and computational grid meshing

图4机器人阻尼力与力矩数值仿真结果

Fig.4Numerical simulation results of damping force and damping torque of the robot

图5深度与姿态角串级PID控制器

Fig.5Cascade PID controller of depth and attitude angle

图6机器人升沉运动仿真结果

Fig.6Simulation result of robot′s heaving motion

图7机器人原地转向运动仿真位姿-时间曲线

Fig.7Simulation pose-time result of robot′s in-place rotation motion

图8机器人定深前进运动仿真结果

Fig.8Simulation result of robot′s depth-keeping motion

图9给定干扰下的机器人运动仿真结果

Fig.9Simulation result of robot motion under disturbance

图10机器人样机

Fig.10Prototype of the robot

图11水下机器人地面站界面

Fig.11Interface of the underwater robot′s ground station software

图12机器人升沉运动实验结果

Fig.12Experiment result of robot′s heaving motion

图13机器人原地转向运动实验结果

Fig.13Experimental result of robot′s in-place rotation motion

图14机器人定深前进运动实验结果

Fig.14Experimental result of robot′s depth-keeping motion

表1水下机器人多模式运动控制输入策略

表2计算流体力学数值仿真的阻尼系数结果

表3机器人系统参数

TU G R L, TOBITA S, WATANABE K,et al. The design and production of an X4-AUV[C]//Proceedings of the 54th Annual Conference of the Society of Instrument and Control Engineers of Japan(SICE),2015.

WU Z W, PENG H S, HU B,et al. Trajectory tracking of a novel underactuated AUV via nonsingular integral terminal sliding mode control[J]. IEEE Access,2021,9:103407-103418.

宋保维, 张宝收, 姜军, 等. 四旋翼碟形自主水下航行器运动方程建立与流体特性仿真研究[J]. 兵工学报,2016,37(2):299-306. SONG B W, ZHANG B S, JIANG J,et al. Estimation of equation of motion of four-rotor dish-shaped AUV and simulation research on its hydrodynamic characteristics[J]. Acta Armamentarii,2016,37(2):299-306.(in Chinese)

BIAN J W, XIANG J. QUUV:a quadrotor-like unmanned underwater vehicle with thrusts configured as X shape[J]. Applied Ocean Research,2018,78:201-211.

KOU L W, CHEN Z Y, XIANG J. Cooperative fencing control of multiple vehicles for a moving target with an unknown velocity[J]. IEEE Transactions on Automatic Control,2022,67(2):1008-1015.

BAK J, MOON Y, KIM J,et al. Hovering control of an underwater robot with tilting thrusters using the decomposition and compensation method based on a redundant actuation model[J]. Robotics and Autonomous Systems,2022,150:103995.

TAN Y H, CHEN B M. Underwater stability of a morphable aerial-aquatic quadrotor with variable thruster angles[C]//Proceedings of the IEEE International Conference on Robotics and Automation(ICRA),2021:314-320.

RAHMAN M M, TODA Y, MIKI H. Computational study on a squid-like underwater robot with two undulating side fins[J]. Journal of Bionic Engineering,2011,8(1):25-32.

ZHOU C L, LOW K H. Better endurance and load capacity:an improved design of manta ray robot(RoMan-Ⅱ)[J]. Journal of Bionic Engineering,2010,7: S137-S144.

CURET O M, PATANKAR N A, LAUDER G V,et al. Mechanical properties of a bio-inspired robotic knifefish with an undulatory propulsor[J]. Bioinspiration & Biomimetics,2011,6(2):026004.

LIU H L, GONG F X, CURET O. Unleashing the potential of undulating fin propulsion using a biomimetic robotic vessel[J]. Marine Technology Society Journal,2017,51(5):79-93.

徐海军, 潘存云, 谢海斌, 等. 水下仿生推进器阀控液压摆动关节建模与动态特性分析[J]. 国防科技大学学报,2010,32(6):116-121. XU H J, PAN C Y, XIE H B,et al. Modeling and dynamic characteristics analysis of the valve-controlled hydraulic swaying cylinder in bionic underwater thruster[J]. Journal of National University of Defense Technology,2010,32(6):116-121.(in Chinese)

高飞, 王玉魁, 王振龙, 等. 形状记忆合金丝驱动的仿生墨鱼水下机器人的原型设计[J]. 机器人,2013,35(3):346-351. GAO F, WANG Y K, WANG Z L,et al. Prototype design of a kind of biomimetic cuttlefish underwater robot actuated by SMA wires[J]. Robot,2013,35(3):346-351.(in Chinese)

WANG Y, WANG R, WANG S,et al. Underwater bio-inspired propulsion:from inspection to manipulation[J]. IEEE Transactions on Industrial Electronics,2020,67(9):7629-7638.

王扬威, 闫勇程, 刘凯, 等. 基于CPG的仿生环形长鳍波动推进器运动控制[J]. 机器人,2016,38(6):746-753. WANG Y W, YAN Y C, LIU K,et al. Motion control of a bionic circular long-fin undulating propeller based on CPG[J]. Robot,2016,38(6):746-753.(in Chinese)

PANG S X, QIN F H, SHANG W W,et al. Optimized design and investigation about propulsion of bionic tandem undulating fins Ⅰ:effect of phase difference[J]. Ocean Engineering,2021,239:109842.

沈林成, 王光明. 仿鱼长鳍波动推进器研究的进展与分析[J]. 国防科技大学学报,2005,27(4):96-100. SHEN L C, WANG G M. The progress and analysis of the research on the underwater biomimetic propulsor employing the long-fin undulations[J]. Journal of National University of Defense Technology,2005,27(4):96-100.(in Chinese)

MAR C, WANG Y, WANG R,et al. Development of a propeller with undulating fins and its characteristics[C]//Proceedings of the IEEE International Conference on Real-time Computing and Robotics(RCAR),2019:737-742.

首页

期刊介绍

投稿指南

编委会

出版声明

开放获取声明

联系我们

期刊订阅

Rss

English

1 控制模型

2 控制系统设计与仿真

3 实验验证

4 结论