文 | 中史华纳

编辑 | 中史华纳

随着水下机器人和仿生机器人技术的不断成熟，它们在科学研究和各种应用中的作用日益显著，一项研究成功设计和开发了一种仿生机器海豚，专门用于水质监测，并将其成功应用于实际水库环境中。

还有一项创新性的工作提出了基于视觉的自主仿生机器鱼，将其投入水球攻击任务。

仿生机器鱼在三维（3D）空间中的路径跟踪和运动控制是需要进一步研究的重要议题，具体而言需要控制机器鱼沿着向下的螺旋路径绕过障碍物移动，并采集巡检数据。

01

机械结构

这款仿生机器鱼以金枪鱼为外形设计，总长1.5米，重约15千克，为了模仿鱼类游动的灵活性，采用了双关节设计，并在尾部装备了推进器装置。

整个机体被分为头、腰、尾三个部分，由摆杆和电机连接并进行控制，头部舱主要用于处理机器鱼的控制和供电模块。

为了获得位置、速度等信息，机器鱼的全球定位系统背鳍和头部分别装备了GPS和惯性测量单元（IMU），并在IMU中嵌入了卡尔曼滤波器以校准相关数据，头部前舱还配备了深度传感器和摄像头，用于获取机器鱼的深度数据和水下视频图像。

为了实现仿生机器人的运动，设计了运动学模型，以模仿鱼类的运动行为，并通过螺旋桨提供推进力，为了保持在深海中向前移动的平衡，腰部和尾部被设计成可以在与头部相同的水平面内左右摆动。

胸鳍的存在赋予了机器鱼高度的运动性，可以在垂直平面内进行操纵，除了用于前进的动力学模型外，该设计还将腰部和尾部连接在一起以进行偏航操作，并添加了一对用于上升或下潜的机械胸鳍。

机器鱼的坐标系被定义为ob和参考坐标系og，质心位置被设置为鱼体框架的原点，横滚角(θb)、偏航角(ψb)和俯仰角(γb)也有了明确定义。

机器鱼的三维动态模型可以用方程来描述，[u, v, w]表示相对于机器鱼坐标系的前向、横向和纵向速度，[x, y, z]则表示相对于惯性坐标系的坐标，通过ẋ, ẏ, ż分别表示相对于惯性坐标系的前向、横向和纵向速度，机器鱼的运动状态得以准确描述。

采用牛顿-欧拉法机器鱼所受的净外力与机器鱼的质量和加速度相关，总外力与机器鱼的总惯性矩阵M相关，这包括机器鱼头部沿三个轴的速度，即Ftotal=M·V̇。

02

基于CPG模型的闭环运动控制

提出了一种闭环中央模式发生器（CPG）模型来控制深海机器鱼的运动，并使用基于Hopf的振荡器来生成周期电机。

上图方程中定义了Hopf振子的周期运动，其中j=1,2分别表示腰关节和尾关节，在这里，xj和yj代表了周期运动的状态变量，周期运动的变化通过这两个变量的一阶导数来描述。

具体参数中幅度μ决定了输出信号的收敛速度，aj表示控制信号的幅度，而ωj则决定了信号振荡的频率，调节元件cjcosphij用于调整相对于腰部和尾部关节之间的相位和幅度差。

为了实现螺旋路径跟随，需要建立路径跟随策略，以指导机器鱼的移动，控制系统根据这一策略执行控制，从而使机器鱼按照预期路径进行移动。

控制系统可以分为两个子系统，其中ψd表示所需的偏航角，而zd表示所需的深度，路径跟随子系统采用基于时变前瞻距离的LOS方法来计算所需的偏航角和深度，这一子系统将用于执行机器鱼的3D运动控制和路径跟踪。

在路径跟随中，引入了投影点的概念，投影点的位置与机器鱼的坐标和路径函数相关，通过使用机器鱼的参考路径和坐标信息，可以推断出投影点的三维坐标。

只要机器鱼以正速度Ub移动，就可以轻松地通过实时坐标和预定义的路径模型计算出参考点Pp(k*)，水平平面上的投影点位于机器鱼的参考路径和机器鱼与路径中心的连线之间。

在垂直平面上，投影点zp(k)的位置取决于参数k的选择，需要计算参数k以获得垂直平面上的投影点，通过求解穿过投影圆的水平平面上的投影点，可以导出参数k的值。

这些概念和方程式对于实现机器鱼的自主螺旋路径跟踪以及三维运动控制至关重要，有助于机器鱼在水下环境中完成各种任务。

03

螺旋路径跟踪中的3D坐标系

在3D空间中，机器鱼执行螺旋路径跟踪任务，其中og和ob分别代表了惯性坐标系和鱼体坐标系的原点，这些坐标系遵循右手螺旋定则，其轴的方向是根据这一规则确定的。

在路径跟踪过程中引入了Serret-Frenet（SF）坐标系，其中ψp表示SF坐标系的xp轴与惯性系的xg轴之间的夹角，γp则表示这两者之间的夹角，这一坐标系还包括zp轴和zg轴，用于描述投影点的位置和方向。

机器鱼的运动控制系统由三个主要部分组成：腰尾关节电机、左右胸鳍舵机和尾部推进器，腰部和尾部通过控制鱼躯干的摆动来实现偏航角的控制，而胸鳍舵机的偏角则负责控制上升或下潜，尾部推进器则用于调整机器鱼的速度。

要实现对机器鱼的三维运动控制，需要考虑其实时偏航角、速度和深度，控制系统采用模糊逻辑比例积分微分（PID）控制器来控制偏航角，采用比例控制器来控制深度和速度，在运动控制中，深度的调整有时比深度的准确性和速度更加关键。

由于机器鱼躯干的摆动受到腰部和尾部的影响，控制系统还利用腰部和尾部关节的中枢模式生成器（CPG）来生成偏移参数boffset，并用于控制偏航角，以确保机器鱼的协调运动。

这一综合的运动控制系统使得机器鱼能够在水中执行复杂的螺旋路径跟踪任务，同时保持稳定的深度和速度，展现了其在水下环境中高度自主的运动能力。

为了计算式中的偏移值boffset，利用模糊PID算法根据期望偏航角ψd和当前偏航角ψb计算偏移值boffset，

PID控制的实现，err(t)、err(t-1)和err(t-2)表示期望偏航角与实时偏航角之间的误差连续迭代过程中的角度， Kp、Ki、Kd分别为比例系数，由模糊逻辑控制器优化的积分系数和微分系数， Δoffset为输出增量值，boffset为PID输出。

模糊化是将输入值量化为定义的隶属度的过程，在模糊化过程中，输入域设置为errε[-2π,2π]和Δerrε[4π,4π]，隶属度定义了7个语义变量来表示输入值的模糊子集，即Negative Large ( NL )、负中 (NM)、负小 (NS)、零 (ZO)、正小 (PS) 和正分别为中 (PM) 和正 (PL)。

模糊推理是一个推理大小Kp Ki 和Kd 控制系统响应速度的过程，系统使用参数Kp Ki和Kd分别调节收敛速度、稳态误差和动态特性。

当Kp值较大时，系统收敛速度较快，但容易出现较大的超调，导致系统在稳态附近出现明显的超调，降低Kp可以减少超调问题，但也会减慢系统的收敛速度。

实验在系统初始误差较大时设置较大的Kp，在接近稳态时设置较小的Kp，实验针对KiPID控制的不同阶段采用灵活的参数策略。

因为在PID控制初期，较大的Ki容易使积分过饱和，导致系统超调，实验中前期设置了较小的Ki，实验中后期Ki值随着系统稳定性的提高而增大。

实验设置不同的Kd来影响系统的动态特性，在偏差信号变化太大之前，可以在系统中引入校正信号，以提高系统的响应速度并减轻波动。

Kd取值过大，会导致调节周期增大，取值过小，会引起超调，因此，实验采用较大尺寸的Kd来减小前期的值，并在剩余阶段减小该值，以减少对系统性能的影响。

04

模拟实验与分析

结合运动控制器和路径跟随模型来实现机器鱼的3D螺旋路径跟随，提出的动力学和运动学模型在仿真实验中替换了机器鱼实体，以验证机器鱼的3D螺旋路径跟随。

为了验证提出的3D路径跟踪控制系统的性能，设计了仿真实验，在无干扰环境和不同强度干扰下测试所提出的系统。

除了仿真实验外，研究还进行了对比实验，验证时变方法和固定方法在前瞻距离路径跟踪中的性能。

在螺旋路径研究中，运用3D坐标函数来表达参考路，但该坐标函数很难适应网箱检查，因此，研究在参考路径定义中引入了极坐标。

其中r是路径在路径{Xp-yp}上投影的圆的半径，CX,Cy是圆的中心坐标，δ是路径zp(k)的下降系数，它决定了路径的下降路径速度。

极坐标表达式使系统在螺旋轨迹运动控制中更方便地控制参数，参数κ的范围从零到无穷大，并在机器鱼的螺旋运动过程中发生变化。

极坐标使系统能够通过机器鱼螺旋运动的圈数来控制k的范围，接下来是系数 d/2 pi，系统用它来控制机器鱼的下降速度。

从方程可以看出，pi参数κ每变化2弧度，平面zp(K)就减小d米，极坐标表达式使系统能够根据所需的下降深度和网格检查间隔确定参数κ和d的值。

两条机器鱼p1和p2处于同一水平但深度不同，黄色框为头部相机可以拍摄的范围，H为该范围的高度，d为路径方程中的纵坐标系数。

可见长度d是机器鱼下降过程中整个网射的下降间隔，d等于射高H，对于深度d，参考路径为DK=d/d，因此κ的取值范围确定为Kε[K0，K0+DK·2pi]。

由于参数 κ 是路径的唯一迭代参数，通过计算路径在水平投影平面上的投影点和机器鱼在路径上的投影点即可计算出k的值，然后zp即可得到参考点。

最后，研究对所提出的控制系统中的前瞻距离方法和基于时变的前瞻距离方法进行了比较分析，基于跟踪误差的方法不使用固定的距离值，而是采用灵活的距离来控制路径跟踪过程。

为了使结果观察更加简洁，实验中只允许机器鱼移动一次，并且指定参数κ的范围为2 pi，干扰值距离增加到1.5，其他参数与之前的实验设置一致，对比实验分别使用对应的 D = 1 和 D = 5。

当Δ=1时，结果表明机器鱼在逼近参考路径时出现超调现象，与时变值法相比稳定性较差。

当Δ=5时，机器鱼收敛到参考路径所需的时间明显较长（远大于3s），并且在处理干扰时，控制收敛的能力比不稳定时弱得多，更容易受到环境干扰。

实验结果表明，所设计的路径跟踪控制系统能够满足网箱检测的需求，3D路径跟踪控制系统能够操纵机器鱼沿着预定的参考路径移动，并且在干扰环境下也能获得令人满意的性能。