一种仿生机器鱼的水平面路径跟踪控制研究

刘锦豪，宋晓茹

（西安工业大学电子信息工程学院，西安 720021）

0 引言

随着各国对海洋探索程度的提高和海洋防御重视程度的加深，高机动、高隐蔽、高效率、低噪音的水下仿生机器人越来越受到各国学者的关注。仿生机器鱼是水下仿生机器人的代表之一，由于其所处环境的不确定：强干扰、非线性以及自身复杂的运动机理，使得其运动控制尤为困难。Ren等［1］以数据驱动为手段研究机器鱼的运动控制方式，避免了复杂的动力学研究问题和机器鱼的复杂数学模型的构建问题，但并未考虑运动过程的非线性和扰动影响。Xie等［2］提出一种基于中枢模式发生器（CPG）的机器鱼控制方法，只使用一个电机进行波幅震荡，可以很容易地实现CPG控制，使得机器鱼的转动更加自然有效，但该方法较为简单，没有考虑到鱼体运动时胸鳍与尾鳍之间的作用。Suebsaiprom等［3］提出基于滑膜控制的方法来满足运动模型不稳定、非线性的状况，虽然模拟实验取得了较好的控制效果，但滑模控制易产生抖震问题。Moe等［4］给出了一种基于视线法的航线路径追踪控制器，并充分考虑海流环境的影响，通过设计视线制导律和自适应反馈线性控制器进行无人船自主航线路径跟踪控制实验，取得了不错的跟踪效果，但仍无法克服抖震问题。

综上所述，为使仿生机器鱼的路径跟踪控制达到良好效果，本文给出了基于自抗扰控制器的机器鱼路径追踪控制方法。首先建立多关节机器鱼数学模型；其次通过机器鱼路径跟踪误差模型推导跟踪误差方程和期望角函数，将机器鱼的路径误差收敛问题转化为航速差和航向差的收敛问题；最后设计机器鱼路径跟踪导引律并基于ADRC控制器实现参数整定。仿真结果证明了基于ADRC的机器鱼路径跟踪控制器有良好的控制效果。

1 多关节机器鱼数学模型

机器鱼以身体-尾鳍模式游动且由于水环境具有强扰动的特点，因此对其进行受力分析极具困难。本文首先分析机器鱼在鱼体坐标系下的平移和旋转。根据图1所示的基于直游和转弯的两种基本数学模型，研究机器鱼的二维运动。

图1 多关节机器鱼的游动与转动模型

针对鱼尾的游动和转弯行为，采用拉格朗日方程建立了鱼尾的动力学模型。因此，多关节机器鱼的动态模型可以写成如下二阶矩阵形式［5-6］：

M(γ)是具有水动力学效应的惯性矩阵，C(γ,̇)是科里奥利矩阵，B是阻尼系数矩阵，K是弹簧系数矩阵，F是包含水动力的力矢量，γ=[γ0γ1γ2γ3]T是机器鱼的链接角度。

图2 机器鱼鱼尾等效模型

2 机器鱼路径跟踪误差模型

机器鱼运行于复杂多变的水下环境，为了得到易于观测的控制效果，本文只研究其水平面的二维运动。如图3所示，机器鱼的路径跟踪示意图可以使用以下三个坐标系来表示，地球坐标系U，鱼体坐标系B以及Serret-Frenet坐标系S F。其中，在SF坐标系上的点P为参考路线上任意一个点，表示虚拟机器人。假设参照点P（虚拟机器人）以速度VP按参考路线运动，P轴表示运动的切线方向，Pη轴表示运动的法线方向。Q为机器鱼的重心，其速度向量在坐标系B上可表示为[u,v,r]T，(u,v,r)分别代表机器鱼的前向速度、横向速度以及转向速度。ψ为Bu与zx的夹角，β=arctan(v/u)是机器鱼的侧滑角，ψw=ψ+β。τe,ye分别表示P点与Q点间距离在Pη轴和Pξ轴上的投影长度，且偏航角ψe为合速度Vt与Pη轴的夹角。因此，机器鱼运动学方程可表示为

图3 机器鱼路径跟踪示意图

将鱼体坐标系{B}绕z轴旋转角度β得到新的坐标系{W}，则在{W}坐标系下，机器鱼的水平运动学方程可以表示为式（3）：

期望路径由曲线Γ(s)描述，曲线上参考点P由Γ(s)唯一确定，为了完成路径跟踪的目标，需要实现机器鱼质心Q与参考点P的距离为零(τe,ye=0)，合速度Vt与Pη轴的夹角为零(ψe=0)，将上述条件统一在SF坐标系下有：

上式中CC(s)̇是期望路径的曲率，s是期望路径的参数变量。

3 基于ADRC的路径追踪控制器

根据路径跟踪控制器的任务和控制目标的设计要求，该控制器必须使机器鱼到期望曲线的相对距离为零，机器鱼的速度矢量对准期望路径的切向方向，机器鱼的速度逐渐趋于分配的指定速度。

为了控制机器鱼的前向速度u在整个路径跟踪过程中都以稳定的期望速度Ud运动，采用Vt合速度对其进行调控。且为了保持整个系统的稳定性，本文通过设计虚拟目标的速度，使机器鱼动态特性曲线与期望路径的特性曲线相拟合。同时，为防止因位置误差而产生不连续点，对虚拟目标的前向位置依据切向偏差程度做出了适当的调整。其设计公式见式（5）：

其中，kτ为增益函数。基于运动学的理论设计满足光滑路径控制条件的非奇异路径参考式（6）：

法向误差需要利用ψ角进行控制。本文将引用期望角的计算方法调节机器鱼在跟踪路径过程中的瞬态行为，趋近角实际上是方位误差的期望角度。本文所选取期望角的表达式为

且满足

由趋近角函数可知，当期望路径上的参考目标点与机器鱼的实际位置误差存在较大的ye时，会产生一个较大的趋近角ψe，假若机器鱼在期望路径前向的右边，机器鱼就会向左偏转；相反，趋近角将导引机器鱼向右运动并趋向期望路径。从误差方程可以得到，如果姿态跟踪的误差角ψe收敛到ψd，并且τe收敛到零，即ye收敛到零，趋近角ψd也自然收敛到零。

综上所述，本文选取如下导引律:

本文主要通过ADRC控制技术，使得机器鱼可以更好地跟踪上式的控制率。基于上式中设计的导航算法，依此构建了二阶自抗扰路径的追踪控制器，其控制结构框图如图4所示。

图4 基于ADRC路径跟踪控制系统

4 仿真分析与实验验证

为了验证本文所设计控制器的有效性和鲁棒性，对机器鱼进行了路径跟踪控制实验，并分别进行了基于自抗扰控制器和常规PID路径跟踪控制器的曲线路径跟踪控制实验。利用MATLAB/SIMULINK建立机器鱼的路径跟踪控制器。

设定仿真参考直线路径，其参数方程为

上式中，θ=1/4 rad为x轴与直线路径的夹角。参考路径的起始坐标为（0,0），期望速度Ud=1.5 m/s，机器鱼起始坐标为（0,0），θ=0 rad，初始速度为0 m/s，其中加入的外界扰动为如下式：

仿真步长设置为自动，其中部分参数设置为Ke=1.2,Kr=1.2,K1=0.8，其仿真结果如下图所示。

图5—图7给出了机器鱼的直线路径跟踪运动曲线和速度变化曲线。直线路径跟踪图表明本研究所设计的控制器能稳定跟踪预定路径，快速收敛，并得到较光滑的跟踪曲线。通过与基于PID控制的路径追踪控制方法相比，本文的ADRC控制器的机器鱼具有更小震荡，收敛情况更平滑快速。其速度变化曲线同样证明了本文方法的超调更小，收敛速度更快，具有更好的控制效果。

图5 直线路径跟踪图

图6 前向速度

图7 转向速度

5 结语

本文提出了一种基于ADRC的机器鱼路径跟踪控制方法，首先建立机器鱼数学模型，在SF坐标系中引入跟踪误差方程和期望角度函数，设计了机器鱼的前向和转向制导函数，并采用自抗扰控制器对参数进行调整。仿真结果表明，基于自抗扰控制器的路径跟踪控制器在控制超调量、快速性和振荡时间方面优于传统PID控制，大大提高了机器鱼在路径跟踪过程中的精度和鲁棒性。