WO2021103391A1 - 仿生滑翔机器海豚的滑翔深度控制方法、系统、装置 - Google Patents

Abstract

本发明属于水下机器人控制领域，具体涉及一种仿生滑翔机器海豚的滑翔深度控制方法、系统、装置，旨在解决仿生滑翔机器海豚滑翔定深控制精确度低的问题。本系统方法包括获取预设滑翔深度、偏航角；基于深度、惯导信息，通过滑模观测器获取估计速度，并结合预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；基于预设滑翔深度、深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合估计速度，通过模型预测控制方法获取下潜控制量；根据下潜控制量，通过浮力原理得到活塞的目标位置，并依据当前活塞位置获取活塞的控制量；基于活塞和两侧胸鳍的控制量，控制滑翔。本发明提高了仿生滑翔机器海豚滑翔定深控制的精确度。

Description

仿生滑翔机器海豚的滑翔深度控制方法、系统、装置 技术领域

本发明属于水下机器人控制领域，具体涉及一种仿生滑翔机器海豚的滑翔深度控制方法、系统、装置。

背景技术

近年来，水下仿生机器人在水下探测、搜救、设施维护等作业方面起着越来越重要的作用，引起广泛关注。通过模仿海豚的背腹式波动运动，仿生机器海豚获得了高机动性能，成功实现了跃水、前后滚翻等高难度动作，但是高机动运动往往导致极大的能量消耗。因此，为了提高机器海豚的续航能力，科研学者将水下滑翔器的浮力驱动机制引入到仿生机器海豚的设计，研制了仿生滑翔机器海豚平台，通过结合滑翔运动和仿海豚波动运动，同时实现高机动与强续航，极大拓展了应用领域和范围。

深度控制问题一直以来都是水下机器人的研究热点。高精度的深度控制能够为水下机器人的自主导航和路径规划奠定坚实的基础，对水下机器人的自主作业意义重大。根据控制方式不同，水下机器人深度控制主要分为三类：重心调节、浮力调节及可动鳍面调节。其中，重心调节主要依靠体内可动滑块改变重心分布，获取俯仰力矩，实现深度控制；浮力调节是通过改变自身排水量，依靠浮力调节，实现深度控制。这两种方法由于只需静密封操作，安全性高，实现难度小。但是，由于滑块运动和浮力调节速度限制，导致系统延迟较大，容易超调。可动鳍面调节方式通过控制鳍面的转角实现深度控制，响应速度快，结构简单，应用广泛。但是，该方法对于密封要求较高，且对偏航运动易产生强耦 合效应。上述的方法广泛应用于水下仿生机器人研究中。Shen针对仿生机器海豚的定深问题，利用重心调节方式，设计了模糊比例–积分–微分(PID)控制器实现了其重心调节，获得了5厘米的深度误差。可参考文献：(1)沈飞，“仿生机器海豚的建模、控制及其在水质监测中的应用研究,”博士学位论文,北京:中国科学院研究生院,2012.

Makrodimitris针对仿生机器鱼的定深问题，利用浮力调节方式，实现了2厘米的深度误差。可参考文献：(2)M.Makrodimitris,I.Aliprantis,and E.Papadopoulos,“Design and implementation of a low cost,pump-based,depth control of a small robotic fish,”in Proc.IEEE/RSJ Int.Conf.Intell.Robots Syst.,Chicago,USA,Sep.2014,pp.1127–1132.

Yu针对仿生机器海豚的定深问题，利用可动鳍面调节方式，通过改进滑模模糊控制器控制胸鳍转角，实现了0.5厘米的深度误差。可参考文献：(3)J.Yu,J.Liu,Z.Wu,and H.Fang,“Depth control of a bioinspired robotic dolphin based on sliding mode fuzzy control method,”IEEE Trans.Ind.Electron.,vol.65,no.3,pp.2429–2438,2018.

仿生滑翔机器海豚的深度控制分为滑翔定深和海豚定深，其中由于系统延迟大和精确度低的特点，滑翔定深控制的精确性问题一直没有得到很好地解决。本发明针对滑翔机器海豚的滑翔深度控制问题，通过设计深度控制器控制浮力，实现定深控制任务。

发明内容

为了解决现有技术中的上述问题，即为了解决现有仿生滑翔机器海豚的滑翔定深控制精确度低的问题，本发明第一方面，提出了一种仿生滑翔机器海豚的滑翔深度控制的方法，该方法包括：

步骤S100，获取预设滑翔深度、预设偏航角；

步骤S200，基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；

步骤S300，基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量；

步骤S400，根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量；

步骤S500，基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。

在一些优选的实施方式中，步骤S200中“通过滑模观测器获取估计速度”，其方法为：

基于所述仿生滑翔机器海豚的坐标系，构建其对应的全状态三维动力学模型；

通过忽略侧向运动对所述全状态三维动力学模型进行简化，得到简化动力学模型；

根据当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取所述简化动力学模型的速度向量，将其作为所述仿生滑翔机器海豚的估计速度。

在一些优选的实施方式中，所述简化动力学模型，其表示为：

其中，v＝[u,w,q] ^T分别表示海豚坐标系下的前向速度、纵向速度以及俯仰角速度，M＝diag{m ₁,m ₂,m ₃}表示包含附加质量的惯性质量矩阵,m ₁,m ₂,m ₃为质量参数，D＝diag{d ₁,d ₂,d ₃}表示被简化为常数项的阻尼矩阵， d ₁,d ₂,d ₃为阻尼参数，

表示输入矩阵，u _c为实时控制量，

为机器人重心与浮心的位置向量，G _m为重力加速度，θ为俯仰角，C(v)为科氏力和向心力矩阵，

为速度导数。

在一些优选的实施方式中，步骤S300中“通过偏航控制器获取两侧胸鳍的控制量”其方法为：

其中，u _f为两侧胸鳍的控制量，k _f为偏航控制器的权重系数，k _p为比例因子，k _i为积分因子，k _d为微分因子，e _ψ为偏航角误差，

为偏航角误差导数。

在一些优选的实施方式中，步骤S400中“构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹”，其方法为：

基于所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息，构建二阶贝塞尔曲线轨迹，将其作为所述仿生滑翔机器海豚的深度参考轨迹；

对所述深度参考轨迹求导，得到下潜速度参考轨迹；

根据所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息的差的绝对值与预设的深度阈值的大小，对所述下潜速度参考轨迹进行分段，得到分段式的下潜速度参考轨迹。

在一些优选的实施方式中，所述分段式的下潜速度参考轨迹，其计算方法为：

其中，Y _ref为分段式的下潜速度参考轨迹，c ₁、c ₂表示权重系数，d _r为目标深度，d为深度传感器的测量值，d _theshold为预设的深度阈值，V _ref为下潜速度参考轨迹。

在一些优选的实施方式中，步骤S500中“通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置”，其方法为：

其中，r _sr为活塞的目标位置，ρ表示水的密度，g表示重力加速度，S表示浮力调节机构的底面积，u _c(t)为下潜控制量。

本发明的第二方面，提出了一种仿生滑翔机器海豚的滑翔深度控制的系统，该系统包括获取预设值模块、获取胸鳍控制模块、获取下潜控制模块、获取活塞控制模块、控制滑翔模块；

所述获取预设值模块，配置为获取预设滑翔深度、预设偏航角；

所述获取胸鳍控制模块，配置为基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；

所述获取下潜控制模块，配置为基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量；

所述获取活塞控制模块，配置为根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量；

所述控制滑翔模块，配置为基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。

本发明的第三方面，提出了一种存储装置，其中存储有多条程序，所述程序应用由处理器加载并执行以实现上述的仿生滑翔机器海豚的滑翔深度控制方法。

本发明的第四方面，提出了一种处理装置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；所述程序适用于由处理器加载并执行以实现上述的仿生滑翔机器海豚的滑翔深度控制方法。

本发明的有益效果：

本发明提高了仿生滑翔机器海豚滑翔定深控制的精确度。本发明通过构建仿生滑翔机器海豚的全状态动力学模型，并忽略侧向运动对全动态动力学模型进行简化，得到简化的动力学模型。利用深度及姿态等信息，通过滑模观测器获取仿生滑翔机器海豚的估计速度。考虑到仿生滑翔机器海豚系统延迟大的特点，基于估计速度，通过设计的分段式贝塞尔参考轨迹以及基于模型预测的深度控制器，调节仿生滑翔机器海豚的浮力调节机构的活塞运动，从而控制浮力，实现了高精度的滑翔深度控制。

同时，基于改进PID的偏航控制器，获取仿生滑翔机器海豚的两侧胸鳍的控制量，利用可动胸鳍的差动运动实现滑翔姿态调整。避免滑翔定深过程中的偏航扰动，保持仿生滑翔机器海豚滑翔定深阶段航向不变。

附图说明

通过阅读参照以下附图所做的对非限制性实施例所做的详细描述，本申请的其他特征、目的和优点将会变得更明显。

图1是本发明一种实施例的仿生滑翔机器海豚的滑翔深度控制方法的流程示意图；

图2是本发明一种实施例的仿生滑翔机器海豚的滑翔深度控制系统的框架示意图；

图3是本发明一种实施例的仿生滑翔机器海豚的系统坐标系的示意图；

图4是本发明一种实施例的仿生滑翔机器海豚的机电结构的示意图；

图5是本发明一种实施例的仿生滑翔机器海豚的滑翔深度控制结构的示意图；

图6是本发明一种实施例的仿生滑翔机器海豚的滑翔定深实验的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

本发明的仿生滑翔机器海豚的滑翔深度控制方法，如图1所示，包括以下步骤：

步骤S100，获取预设滑翔深度、预设偏航角；

步骤S200，基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；

步骤S300，基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量；

步骤S400，根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量；

步骤S500，基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。

为了更清晰地对本发明仿生滑翔机器海豚的滑翔深度控制方法进行说明，下面结合附图对本发明方法一种实施例中各步骤进行展开详述。

步骤S100，获取预设滑翔深度、预设偏航角。

在本实施例中，首先获取控制仿生滑翔机器海豚进行滑翔的预设深度及偏航角。其中，预设滑翔深度为手动设定，预设偏航角设置为初始方向，即保持航向不变。

步骤S200，基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量。

滑翔机器海豚的系统坐标系如图3所示，其中，c _g＝o _gx _gy _gz _g和c _b＝o _bx _by _bz _b分别表示全局惯性坐标系和海豚坐标系，其中海豚坐标系的x轴方向为海豚头部正方向，z轴正方向沿着重力方向。

其次，本发明在各个可动关节处均建立各自的联体坐标系，包括c _w＝o _wx _wy _wz _w，c _t＝o _tx _ty _tz _t，c _l＝o _lx _ly _lz _l和c _r＝o _rx _ry _rz _r，分别表示腰部，尾部，左胸鳍和右胸鳍坐标系。

图4为仿生滑翔机器海豚的机电结构示意图，主要包括三个舱，分别是胸鳍舱、电池舱和腰尾舱，图4中展示了胸鳍舱和电池舱的结构图，胸鳍舱由浮力调节结构、活塞电机、胸鳍舵机构成，电池舱由移动滑块、滑块电机、锂电池组构成。其中，腰部关节、尾部关节、浮力调节机构以及重心调节机构均由电机驱动，两侧胸鳍由舵机驱动。仿生滑翔机器海豚还包括各传感器、电源开关，图中展示了惯导传感器、深度传感器。

根据图3中的坐标系统，构建仿生滑翔机器海豚的全状态三维动力学模型，兼具滑翔模态和海豚模态。其中，滑翔模态利用动能动量关系计算如公式(1)(2)所示：

p＝ ^gR _bP    (1)

π＝ ^gR _bΠ+l×p    (2)

其中，p和π分别表示惯性坐标系下的系统动量和角动量，P和Π分别表示海豚坐标系下的系统动量和角动量， ^gR _b表示旋转矩阵，l表示惯性坐标系原点到海豚坐标系原点的向量。

通过求取系统动能，建立滑翔模态的牛顿欧拉方程。

对于海豚模态，根据多连杆动力学理论，分别建立腰部关节、尾部关节以及左右胸鳍关节的牛顿欧拉方程，通过正向运动学的速度传递，分别求取各个关节的力和力矩，从而获得加速度和速度。

通过忽略侧向运动，对上述的动力学模型进行简化，使其更加适用于真实嵌入式平台，如公式(3)所示：

其中，v＝[u,w,q] ^T分别表示海豚坐标系下的前向速度、纵向速度以及俯仰角速度，M＝diag{m ₁,m ₂,m ₃}表示包含附加质量的惯性质量矩阵，m ₁,m ₂,m ₃为质量参数，D＝diag{d ₁,d ₂,d ₃}表示被简化为常数项的阻尼矩阵，d ₁,d ₂,d ₃为阻尼参数，

表示输入矩阵，C(v)表示科氏力和向心力矩阵，

为速度导数，u _c为实时控制量，

为机器人重心与浮心的位置向量，G _m为重力加速度，θ为俯仰角。

因此，根据科氏力和向心力矩阵，通过简化动力学模型提取竖直方向分量，可得到公式(4)：

其中，

为下潜速度导数。

在设计控制器时，需要计算简化动力学模型的速度向量，而目前用于精确水下定位的传感器大多笨重且昂贵，因此，本发明提出基于滑模观测器的速度估计算法。其中，仿生滑翔机器海豚的滑翔深度控制方法的具体过程如图5所示，d _r表示预设滑翔深度，ψ _r表示预设偏航角，在下述步骤中，对图5中各模块进行说明。

首先获取传感器的实测值，其包括深度传感器、惯导传感器以及浮力调节机构的活塞位置传感器的测量值，即深度信息、惯导信息、活塞信息，惯导传感器通过九轴算法，获取机器人头部三轴欧拉角、角速度以及加速度，作为导航信息。

根据深度信息d，获取真实的下潜速度，通过定义惯性坐标系的估计下潜速度

定义估计误差s，分别如公式(5)(6)所示：

其中，

为深度信息的导数， ^gU _bz为真实下潜速度。

然后根据惯性坐标系和海豚坐标系的转换关系，计算出全局坐标系的估计速度，如公式(7)所示：

其中，

为全局坐标系的估计加速度，(c _x c _y c _z)表示滑模观测器的权重向量，sat(s)表示饱和函数，此项的设计目的是减小滑模观测器的抖振效应， ^gR _b为海豚坐标系到惯性坐标系的旋转矩阵，

为角速度的斜对称矩阵，

表示海豚坐标系下的估计加速度。根据估计的加速度，通过迭代可以得到估计速度。

由于机械结构间隙，仿生滑翔机器海豚在滑翔过程中易发生偏航方向的偏转，因此，需要设计偏航控制器，以满足简化动力学模型中忽略侧向运动的假设。由于偏航运动的基本原理是依靠两侧胸鳍偏转产生的差动水动力矩，而水动力跟速度关系密切，在速度较大的时候，偏航控制量不宜过大，以免产生超调。因此，本实施例中提出一种偏航控制器，基于上述获取的速度向量(估计速度)，设计基于速度的偏航控制器，获取控制量，控制两侧胸鳍偏置角。

首先，根据估计的速度，求取偏航控制器的权重系数k _f，求解过程如公式(8)所示：

其中，v _max表示最大滑翔速度，该值是通过仿真获取的定值，v _ex为估计的x轴速度，v _ez为估计的z轴速度。然后，将此系数应用至经典PID控制器，获取最终控制量u _f，如公式(9)所示：

其中，k _p为比例因子，k _i为积分因子，k _d为微分因子，e _ψ为偏航角误差，

为偏航角误差导数，i表示下标。

通过调节PID参数，将控制量直接映射为胸鳍左、右角度κ _l、κ _r，本实施例优选只用单侧胸鳍进行偏航调整，即当其中一侧胸鳍发生偏转时，另一侧胸鳍角度为零，其调整方法如公式(10)所示：

步骤S300，基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量。

由于滑翔运动响应速度较慢，易造成系统较大的延迟，而模型预测控制比较适合于此类系统，且对模型准确性要求不高。因此，根据上述得到的简化动力学模型和估计速度，本发明提出一种基于模型预测控制的深度控制器，通过改进其参考轨迹，实现高精度的深度控制。具体如下：

将上述的简化动力学模型转换为离散域表示，表示如公式(11)所示：

w(k+1)＝Aw(k)+Bu _c(k)+L(k)   (11)

其中，

k表示自然数。

为了方便表示，将系统变量进行统一，定义如公式(12)所示：

其中，(k|t)表示在时间t的基础上预测k步，可将简化动力

学模型表示如公式(13)(14)：

其中，

Δu _c为控制

增量，η(k|t)为扩展输出量。

然后，通过设定控制步数N _c和预测步数N _p，迭代预测未来

的状态变量，如公式(15)所示：

本实施例中

项在一次预测过程中保持常数项，以解除下潜速度与前进速度以及俯仰角速度的耦合。其次，将预测的状态变量进行统一整合，得到公式(16)：

Y(t)＝Υξ(k|t)+ΗΔU _c(t)+Δ    (16)

其中，Y(t)＝(η(k+1|t),…,η(k+N _c|t),…,η(k+N _p|t)) ^T，上标T为表示转置，

I为单位矩阵，

然后，通过优化模型预测控制方法的目标函数，计算出控制信号的最优解，该函数的选择应通过两个因素来考虑。首先，应将深度稳态误差(目标深度与实际深度之间的差)控制到最小。本发明将控制目标从深度转换为下潜速度，通过设计合适的下潜速度参考轨迹，可达到目标深度。其次，控制增量不宜过大，否则易导致机器人的机械和电气损坏。因此，基于以上考虑，本发明提出一种基于稳态误差和控制增量的目标函数，如公式(17)所示：

其中，J(ξ(t),ΔU _c(t))表示基于稳态误差和控制增量的目标函数，η _ref(.)表示参考轨迹，R为控制增量参数，Q为误差参数。

将目标函数整合为矩阵形式，得到公式(18)(19)：

其中，

E(t)＝Υξ(k|t)+Δ-Y _ref(t)，

Y _ref表示分段式的下潜速度参考轨迹，在下文中进行获取，N _x为状态量个数。

通过优化上述函数的可行解，得到控制步数内的控制增量序列，如公式(20)所示：

其中，ΔU _c ^*表示控制增量序列，Δu _c(t) ^*表示最优控制量。

然后选取序列中第一个值作为最终控制增量值，得到控制量，如公式(21)所示：

u _c(t)＝u _c(t-1)+Δu _c(t) ^*    (21)

对于大时延系统，设计一个良好的参考轨迹可以避免超调。因此，本发明提出一种基于贝塞尔曲线的参考轨迹设计方法。贝塞尔曲线Bézier curve通常用于平滑路径，通过设置不同的控制点，更改曲线形状。在深度控制系统中，我们希望尽可能减小超调。因此，基于预测步数，本发明首先将二阶贝塞尔曲线应用于深度参考轨迹。之后，通过计算深度参考点，以提供实时的下潜速度参考轨迹。深度参考轨迹P _ref设计如公式(22)所示：

P _ref(i)＝(1-t(i)) ²d+2t(i)(1-t(i)) ²d _r+t(i) ²d _r   (22)

其中，

d _r表示预设滑翔深度，d表示实时深度，即深度信息。

通过对深度参考轨迹求导

得到下潜速度的参考轨迹。在实际控制过程中，由于目标深度的不同，对于参考轨迹的设计可以有所改变，因此，本发明提出了一种分段式的参考轨迹设计方法，通过设置深度阈值d _theshold，实现不同阶段的参考轨迹，如公式(23)所示：

其中，Y _ref为分段式的下潜速度参考轨迹，c ₁和c ₂表示权重系数。但是在实际应用中，不应限制于两段式参考轨迹，根据目标深度的不同，可以设置多段轨迹，甚至设计连续函数去实现深度控制。

步骤S400，根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量。

在本实施例中，由于滑翔模态下是通过调节浮力来实现深度控制，而浮力调节是通过机构中活塞移动以实现吸排水，因此需要将得到的控制量映射为浮力调节机构中活塞的位置。鉴于得到控制量的物理 意义为力，根据浮力原理，可得到活塞的目标位置，计算过程如公式(24)所示：

其中，r _sr为活塞的目标位置，ρ表示水的密度，g表示重力加速度，S表示圆柱形浮力调节机构的底面积。但是，由于处于位置模式时的电机有较大的加减速，而深度控制是实时控制，控制周期较短，因此位置模式可能会给机械和电气结构带来损坏。为了保护浮力驱动机构，本发明将电机设置为速度模式，并根据此模式设计PID控制器实现位置环以平滑运动。

步骤S500，基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。

在本实施例中，通过活塞控制量控制仿生滑翔机器海豚的上浮、下潜，通过两侧胸鳍的控制量控制仿生滑翔机器海豚的偏航调整，从而实现对仿生滑翔机器海豚的滑翔控制。图6给出了仿生滑翔机器海豚在不同时刻的滑翔定深控制实验的示意图，黑色虚线表示目标深度，水面成像为海豚倒影。

本发明第二实施例的一种仿生滑翔机器海豚的滑翔深度控制系统，如图2所示，包括：获取预设值模块100、获取胸鳍控制模块200、获取下潜控制模块300、获取活塞控制模块400、控制滑翔模块500；

所述获取预设值模块100，配置为获取预设滑翔深度、预设偏航角；

所述获取胸鳍控制模块200，配置为基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；

所述获取下潜控制模块300，配置为基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量；

所述获取活塞控制模块400，配置为根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量；

所述控制滑翔模块500，配置为基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。

所述技术领域的技术人员可以清楚的了解到，为描述的方便和简洁，上述描述的系统的具体的工作过程及有关说明，可以参考前述方法实施例中的对应过程，在此不再赘述。

需要说明的是，上述实施例提供的仿生滑翔机器海豚的滑翔深度控制系统，仅以上述各功能模块的划分进行举例说明，在实际应用中，可以根据需要而将上述功能分配由不同的功能模块来完成，即将本发明实施例中的模块或者步骤再分解或者组合，例如，上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块，以完成以上描述的全部或者部分功能。对于本发明实施例中涉及的模块、步骤的名称，仅仅是为了区分各个模块或者步骤，不视为对本发明的不当限定。

本发明第三实施例的一种存储装置，其中存储有多条程序，所述程序适用于由处理器加载并实现上述的仿生滑翔机器海豚的滑翔深度控制方法。

本发明第四实施例的一种处理装置，包括处理器、存储装置；处理器，适于执行各条程序；存储装置，适于存储多条程序；所述程序适于由处理器加载并执行以实现上述的仿生滑翔机器海豚的滑翔深度控制方法。

所述技术领域的技术人员可以清楚的了解到，未描述的方便和简洁，上述描述的存储装置、处理装置的具体工作过程及有关说明，可以参考前述方法实例中的对应过程，在此不再赘述。

本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的模块、方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，软件模块、方法步骤对应的程序可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的设定应用和设计约束条件。本领域技术人员可以对每个设定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

术语“第一”、“第二”等是用于区别类似的对象，而不是用于描述或表示设定的顺序或先后次序。

术语“包括”或者任何其它类似用语旨在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备/装置不仅包括那些要素，而且还包括没有明确列出的其它要素，或者还包括这些过程、方法、物品或者设备/装置所固有的要素。

至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征作出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。

Claims (10)

1. 一种仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，该方法包括：

   步骤S100，获取预设滑翔深度、预设偏航角；

   步骤S200，基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；

   步骤S300，基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量；

   步骤S400，根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量；

   步骤S500，基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。
2. 根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，步骤S200中“通过滑模观测器获取估计速度”，其方法为：

   基于所述仿生滑翔机器海豚的坐标系，构建其对应的全状态三维动力学模型；

   通过忽略侧向运动对所述全状态三维动力学模型进行简化，得到简化动力学模型；

   根据当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取所述简化动力学模型的速度向量，将其作为所述仿生滑翔机器海豚的估计速度。
3. 根据权利要求2所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，所述简化动力学模型，其表示为：

   

   其中，v＝[u,w,q] ^T分别表示海豚坐标系下的前向速度、纵向速度以及俯仰角速度，M＝diag{m ₁,m ₂,m ₃}表示包含附加质量的惯性质量矩阵,m ₁,m ₂,m ₃为质量参数，D＝diag{d ₁,d ₂,d ₃}表示被简化为常数项的阻尼矩阵，d ₁,d ₂,d ₃为阻尼参数，

   

   表示输入矩阵，u _c为实时控制量，

   

   为机器人重心与浮心的位置向量，G _m为重力加速度，θ为俯仰角，C(v)为科氏力和向心力矩阵，

   

   为速度导数。
4. 根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，步骤S300中“通过偏航控制器获取两侧胸鳍的控制量”其方法为：

   

   其中，u _f为两侧胸鳍的控制量，k _f为偏航控制器的权重系数，k _p为比例因子，k _i为积分因子，k _d为微分因子，e _ψ为偏航角误差，

   

   为偏航角误差导数。
5. 根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，步骤S400中“构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹”，其方法为：

   基于所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息，构建二阶贝塞尔曲线轨迹，将其作为所述仿生滑翔机器海豚的深度参考轨迹；

   对所述深度参考轨迹求导，得到下潜速度参考轨迹；

   根据所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息的差的绝对值与预设的深度阈值的大小，对所述下潜速度参考轨迹进行分段，得到分段式的下潜速度参考轨迹。
6. 根据权利要求5所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，所述分段式的下潜速度参考轨迹，其计算方法为：

   

   其中，Y _ref为分段式的下潜速度参考轨迹，c ₁、c ₂表示权重系数，d _r为预设滑翔深度，d为深度信息，d _theshold为预设的深度阈值，V _ref为下潜速度参考轨迹。
7. 根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，步骤S500中“通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置”，其方法为：

   

   其中，r _sr为活塞的目标位置，ρ表示水的密度，g表示重力加速度，S表示浮力调节机构的底面积，u _c(t)为下潜控制量。
8. 一种仿生滑翔机器海豚的滑翔深度控制系统，其特征在于，该系统包括获取预设值模块、获取胸鳍控制模块、获取下潜控制模块、获取活塞控制模块、控制滑翔模块；

   所述获取预设值模块，配置为获取预设滑翔深度、预设偏航角；

   所述获取胸鳍控制模块，配置为基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；

   所述获取下潜控制模块，配置为基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量；

   所述获取活塞控制模块，配置为根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量；

   所述控制滑翔模块，配置为基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。
9. 一种存储装置，其中存储有多条程序，其特征在于，所述程序应用由处理器加载并执行以实现权利要求1-7任一项所述的仿生滑翔机器海豚的滑翔深度控制方法。
10. 一种处理装置，包括处理器、存储装置；处理器，适用于执行各条程序；存储装置，适用于存储多条程序；其特征在于，所述程序适用于由处理器加载并执行以实现权利要求1-7任一项所述的仿生滑翔机器海豚的滑翔深度控制方法。

Images