CN110758698B - 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 - Google Patents
仿生滑翔机器海豚的滑翔深度控制方法、***、装置 Download PDFInfo
- Publication number
- CN110758698B CN110758698B CN201911189693.2A CN201911189693A CN110758698B CN 110758698 B CN110758698 B CN 110758698B CN 201911189693 A CN201911189693 A CN 201911189693A CN 110758698 B CN110758698 B CN 110758698B
- Authority
- CN
- China
- Prior art keywords
- gliding
- depth
- dolphin
- control
- bionic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 241001481833 Coryphaena hippurus Species 0.000 title claims abstract description 125
- 239000011664 nicotinic acid Substances 0.000 title claims abstract description 93
- 238000000034 method Methods 0.000 title claims abstract description 64
- 230000009189 diving Effects 0.000 claims abstract description 29
- 230000007246 mechanism Effects 0.000 claims description 20
- 239000011159 matrix material Substances 0.000 claims description 19
- 210000000006 pectoral fin Anatomy 0.000 claims description 19
- 230000033001 locomotion Effects 0.000 claims description 15
- 230000003592 biomimetic effect Effects 0.000 claims description 14
- 230000005484 gravity Effects 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 11
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 8
- 238000013016 damping Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 4
- 230000002146 bilateral effect Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 230000001276 controlling effect Effects 0.000 description 14
- 230000008569 process Effects 0.000 description 12
- 238000013461 design Methods 0.000 description 10
- 230000006870 function Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 241000251468 Actinopterygii Species 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000007789 sealing Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000001808 coupling effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 229910052744 lithium Inorganic materials 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000013575 regulation of buoyancy Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
- B63G—OFFENSIVE OR DEFENSIVE ARRANGEMENTS ON VESSELS; MINE-LAYING; MINE-SWEEPING; SUBMARINES; AIRCRAFT CARRIERS
- B63G8/00—Underwater vessels, e.g. submarines; Equipment specially adapted therefor
- B63G8/14—Control of attitude or depth
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
- B63C—LAUNCHING, HAULING-OUT, OR DRY-DOCKING OF VESSELS; LIFE-SAVING IN WATER; EQUIPMENT FOR DWELLING OR WORKING UNDER WATER; MEANS FOR SALVAGING OR SEARCHING FOR UNDERWATER OBJECTS
- B63C11/00—Equipment for dwelling or working underwater; Means for searching for underwater objects
- B63C11/52—Tools specially adapted for working underwater, not otherwise provided for
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Aviation & Aerospace Engineering (AREA)
- Ocean & Marine Engineering (AREA)
- Toys (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
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 smallrobotic 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,andH.Fang,“Depth control of a bioinspired robotic dolphin based on sliding modefuzzy 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{m1,m2,m3}表示包含附加质量的惯性质量矩阵,m1,m2,m3为质量参数,D=diag{d1,d2,d3}表示被简化为常数项的阻尼矩阵,d1,d2,d3为阻尼参数,表示输入矩阵,uc为实时控制量,为机器人重心与浮心的位置向量,Gm为重力加速度,θ为俯仰角,C(v)为科氏力和向心力矩阵,为速度导数。
在一些优选的实施方式中,步骤S300中“通过偏航控制器获取两侧胸鳍的控制量”其方法为:
在一些优选的实施方式中,步骤S400中“构建贝塞尔曲线并进行分段,获取分段式的下潜速度参考轨迹”,其方法为:
基于所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息,构建二阶贝塞尔曲线轨迹,将其作为所述仿生滑翔机器海豚的深度参考轨迹;
对所述深度参考轨迹求导,得到下潜速度参考轨迹;
根据所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息的差的绝对值与预设的深度阈值的大小,对所述下潜速度参考轨迹进行分段,得到分段式的下潜速度参考轨迹。
在一些优选的实施方式中,所述分段式的下潜速度参考轨迹,其计算方法为:
其中,Yref为分段式的下潜速度参考轨迹,c1、c2表示权重系数,dr为目标深度,d为深度传感器的测量值,dtheshold为预设的深度阈值,Vref为下潜速度参考轨迹。
在一些优选的实施方式中,步骤S500中“通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置”,其方法为:
其中,rsr为活塞的目标位置,ρ表示水的密度,g表示重力加速度,S表示浮力调节机构的底面积,uc(t)为下潜控制量。
本发明的第二方面,提出了一种仿生滑翔机器海豚的滑翔深度控制的***,该***包括获取预设值模块、获取胸鳍控制模块、获取下潜控制模块、获取活塞控制模块、控制滑翔模块;
所述获取预设值模块,配置为获取预设滑翔深度、预设偏航角;
所述获取胸鳍控制模块,配置为基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息,通过滑模观测器获取估计速度,并结合所述预设偏航角,通过偏航控制器获取两侧胸鳍的控制量;
所述获取下潜控制模块,配置为基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息,构建贝塞尔曲线并进行分段,获取分段式的下潜速度参考轨迹,并结合当前时刻的估计速度,通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量;
所述获取活塞控制模块,配置为根据所述下潜控制量,通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置,并依据当前活塞位置获取活塞的控制量;
所述控制滑翔模块,配置为基于所述活塞的控制量和所述两侧胸鳍的控制量,控制所述仿生滑翔机器海豚进行滑翔。
本发明的第三方面,提出了一种存储装置,其中存储有多条程序,所述程序应用由处理器加载并执行以实现上述的仿生滑翔机器海豚的滑翔深度控制方法。
本发明的第四方面,提出了一种处理装置,包括处理器、存储装置;处理器,适用于执行各条程序;存储装置,适用于存储多条程序;所述程序适用于由处理器加载并执行以实现上述的仿生滑翔机器海豚的滑翔深度控制方法。
本发明的有益效果:
本发明提高了仿生滑翔机器海豚滑翔定深控制的精确度。本发明通过构建仿生滑翔机器海豚的全状态动力学模型,并忽略侧向运动对全动态动力学模型进行简化,得到简化的动力学模型。利用深度及姿态等信息,通过滑模观测器获取仿生滑翔机器海豚的估计速度。考虑到仿生滑翔机器海豚***延迟大的特点,基于估计速度,通过设计的分段式贝塞尔参考轨迹以及基于模型预测的深度控制器,调节仿生滑翔机器海豚的浮力调节机构的活塞运动,从而控制浮力,实现了高精度的滑翔深度控制。
同时,基于改进PID的偏航控制器,获取仿生滑翔机器海豚的两侧胸鳍的控制量,利用可动胸鳍的差动运动实现滑翔姿态调整。避免滑翔定深过程中的偏航扰动,保持仿生滑翔机器海豚滑翔定深阶段航向不变。
附图说明
通过阅读参照以下附图所做的对非限制性实施例所做的详细描述,本申请的其他特征、目的和优点将会变得更明显。
图1是本发明一种实施例的仿生滑翔机器海豚的滑翔深度控制方法的流程示意图;
图2是本发明一种实施例的仿生滑翔机器海豚的滑翔深度控制***的框架示意图;
图3是本发明一种实施例的仿生滑翔机器海豚的***坐标系的示意图;
图4是本发明一种实施例的仿生滑翔机器海豚的机电结构的示意图;
图5是本发明一种实施例的仿生滑翔机器海豚的滑翔深度控制结构的示意图;
图6是本发明一种实施例的仿生滑翔机器海豚的滑翔定深实验的示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是,此处所描述的具体实施例仅用于解释相关发明,而非对该发明的限定。另外还需要说明的是,为了便于描述,附图中仅示出了与有关发明相关的部分。
需要说明的是,在不冲突的情况下,本申请中的实施例及实施例中的特征可以相互组合。
本发明的仿生滑翔机器海豚的滑翔深度控制方法,如图1所示,包括以下步骤:
步骤S100,获取预设滑翔深度、预设偏航角;
步骤S200,基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息,通过滑模观测器获取估计速度,并结合所述预设偏航角,通过偏航控制器获取两侧胸鳍的控制量;
步骤S300,基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息,构建贝塞尔曲线并进行分段,获取分段式的下潜速度参考轨迹,并结合当前时刻的估计速度,通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量;
步骤S400,根据所述下潜控制量,通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置,并依据当前活塞位置获取活塞的控制量;
步骤S500,基于所述活塞的控制量和所述两侧胸鳍的控制量,控制所述仿生滑翔机器海豚进行滑翔。
为了更清晰地对本发明仿生滑翔机器海豚的滑翔深度控制方法进行说明,下面结合附图对本发明方法一种实施例中各步骤进行展开详述。
步骤S100,获取预设滑翔深度、预设偏航角。
在本实施例中,首先获取控制仿生滑翔机器海豚进行滑翔的预设深度及偏航角。其中,预设滑翔深度为手动设定,预设偏航角设置为初始方向,即保持航向不变。
步骤S200,基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息,通过滑模观测器获取估计速度,并结合所述预设偏航角,通过偏航控制器获取两侧胸鳍的控制量。
滑翔机器海豚的***坐标系如图3所示,其中,cg=ogxgygzg和cb=obxbybzb分别表示全局惯性坐标系和海豚坐标系,其中海豚坐标系的x轴方向为海豚头部正方向,z轴正方向沿着重力方向。
其次,本发明在各个可动关节处均建立各自的联体坐标系,包括cw=owxwywzw,ct=otxtytzt,cl=olxlylzl和cr=orxryrzr,分别表示腰部,尾部,左胸鳍和右胸鳍坐标系。
图4为仿生滑翔机器海豚的机电结构示意图,主要包括三个舱,分别是胸鳍舱、电池舱和腰尾舱,图4中展示了胸鳍舱和电池舱的结构图,胸鳍舱由浮力调节结构、活塞电机、胸鳍舵机构成,电池舱由移动滑块、滑块电机、锂电池组构成。其中,腰部关节、尾部关节、浮力调节机构以及重心调节机构均由电机驱动,两侧胸鳍由舵机驱动。仿生滑翔机器海豚还包括各传感器、电源开关,图中展示了惯导传感器、深度传感器。
根据图3中的坐标***,构建仿生滑翔机器海豚的全状态三维动力学模型,兼具滑翔模态和海豚模态。其中,滑翔模态利用动能动量关系计算如公式(1)(2)所示:
p=gRbP (1)
π=gRbΠ+l×p (2)
其中,p和π分别表示惯性坐标系下的***动量和角动量,P和Π分别表示海豚坐标系下的***动量和角动量,gRb表示旋转矩阵,l表示惯性坐标系原点到海豚坐标系原点的向量。
通过求取***动能,建立滑翔模态的牛顿欧拉方程。
对于海豚模态,根据多连杆动力学理论,分别建立腰部关节、尾部关节以及左右胸鳍关节的牛顿欧拉方程,通过正向运动学的速度传递,分别求取各个关节的力和力矩,从而获得加速度和速度。
通过忽略侧向运动,对上述的动力学模型进行简化,使其更加适用于真实嵌入式平台,如公式(3)所示:
其中,v=[u,w,q]T分别表示海豚坐标系下的前向速度、纵向速度以及俯仰角速度,M=diag{m1,m2,m3}表示包含附加质量的惯性质量矩阵,m1,m2,m3为质量参数,D=diag{d1,d2,d3}表示被简化为常数项的阻尼矩阵,d1,d2,d3为阻尼参数,表示输入矩阵,C(v)表示科氏力和向心力矩阵,为速度导数,uc为实时控制量,为机器人重心与浮心的位置向量,Gm为重力加速度,θ为俯仰角。
因此,根据科氏力和向心力矩阵,通过简化动力学模型提取竖直方向分量,可得到公式(4):
在设计控制器时,需要计算简化动力学模型的速度向量,而目前用于精确水下定位的传感器大多笨重且昂贵,因此,本发明提出基于滑模观测器的速度估计算法。其中,仿生滑翔机器海豚的滑翔深度控制方法的具体过程如图5所示,dr表示预设滑翔深度,ψr表示预设偏航角,在下述步骤中,对图5中各模块进行说明。
首先获取传感器的实测值,其包括深度传感器、惯导传感器以及浮力调节机构的活塞位置传感器的测量值,即深度信息、惯导信息、活塞信息,惯导传感器通过九轴算法,获取机器人头部三轴欧拉角、角速度以及加速度,作为导航信息。
然后根据惯性坐标系和海豚坐标系的转换关系,计算出全局坐标系的估计速度,如公式(7)所示:
其中,为全局坐标系的估计加速度,(cx cy cz)表示滑模观测器的权重向量,sat(s)表示饱和函数,此项的设计目的是减小滑模观测器的抖振效应,gRb为海豚坐标系到惯性坐标系的旋转矩阵,为角速度的斜对称矩阵,表示海豚坐标系下的估计加速度。根据估计的加速度,通过迭代可以得到估计速度。
由于机械结构间隙,仿生滑翔机器海豚在滑翔过程中易发生偏航方向的偏转,因此,需要设计偏航控制器,以满足简化动力学模型中忽略侧向运动的假设。由于偏航运动的基本原理是依靠两侧胸鳍偏转产生的差动水动力矩,而水动力跟速度关系密切,在速度较大的时候,偏航控制量不宜过大,以免产生超调。因此,本实施例中提出一种偏航控制器,基于上述获取的速度向量(估计速度),设计基于速度的偏航控制器,获取控制量,控制两侧胸鳍偏置角。
首先,根据估计的速度,求取偏航控制器的权重系数kf,求解过程如公式(8)所示:
其中,vmax表示最大滑翔速度,该值是通过仿真获取的定值,vex为估计的x轴速度,vez为估计的z轴速度。然后,将此系数应用至经典PID控制器,获取最终控制量uf,如公式(9)所示:
通过调节PID参数,将控制量直接映射为胸鳍左、右角度κl、κr,本实施例优选只用单侧胸鳍进行偏航调整,即当其中一侧胸鳍发生偏转时,另一侧胸鳍角度为零,其调整方法如公式(10)所示:
步骤S300,基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息,构建贝塞尔曲线并进行分段,获取分段式的下潜速度参考轨迹,并结合当前时刻的估计速度,通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量。
由于滑翔运动响应速度较慢,易造成***较大的延迟,而模型预测控制比较适合于此类***,且对模型准确性要求不高。因此,根据上述得到的简化动力学模型和估计速度,本发明提出一种基于模型预测控制的深度控制器,通过改进其参考轨迹,实现高精度的深度控制。
具体如下:
将上述的简化动力学模型转换为离散域表示,表示如公式(11)所示:
w(k+1)=Aw(k)+Buc(k)+L(k) (11)
为了方便表示,将***变量进行统一,定义如公式(12)所示:
其中,(k|t)表示在时间t的基础上预测k步,可将简化动力学模型表示如公式(13)(14):
然后,通过设定控制步数Nc和预测步数Np,迭代预测未来的状态变量,如公式(15)所示:
Y(t)=Υξ(k|t)+ΗΔUc(t)+Δ (16)
然后,通过优化模型预测控制方法的目标函数,计算出控制信号的最优解,该函数的选择应通过两个因素来考虑。首先,应将深度稳态误差(目标深度与实际深度之间的差)控制到最小。本发明将控制目标从深度转换为下潜速度,通过设计合适的下潜速度参考轨迹,可达到目标深度。其次,控制增量不宜过大,否则易导致机器人的机械和电气损坏。因此,基于以上考虑,本发明提出一种基于稳态误差和控制增量的目标函数,如公式(17)所示:
其中,J(ξ(t),ΔUc(t))表示基于稳态误差和控制增量的目标函数,ηref(.)表示参考轨迹,R为控制增量参数,Q为误差参数。
将目标函数整合为矩阵形式,得到公式(18)(19):
通过优化上述函数的可行解,得到控制步数内的控制增量序列,如公式(20)所示:
ΔUc *=(Δuc(t)*…Δuc(t+Nc-1)*)T (20)
其中,ΔUc *表示控制增量序列,Δuc(t)*表示最优控制量。
然后选取序列中第一个值作为最终控制增量值,得到控制量,如公式(21)所示:
uc(t)=uc(t-1)+Δuc(t)* (21)
对于大时延***,设计一个良好的参考轨迹可以避免超调。因此,本发明提出一种基于贝塞尔曲线的参考轨迹设计方法。贝塞尔曲线Bézier curve通常用于平滑路径,通过设置不同的控制点,更改曲线形状。在深度控制***中,我们希望尽可能减小超调。因此,基于预测步数,本发明首先将二阶贝塞尔曲线应用于深度参考轨迹。之后,通过计算深度参考点,以提供实时的下潜速度参考轨迹。深度参考轨迹Pref设计如公式(22)所示:
Pref(i)=(1-t(i))2d+2t(i)(1-t(i))2dr+t(i)2dr (22)
通过对深度参考轨迹求导得到下潜速度的参考轨迹。在实际控制过程中,由于目标深度的不同,对于参考轨迹的设计可以有所改变,因此,本发明提出了一种分段式的参考轨迹设计方法,通过设置深度阈值dtheshold,实现不同阶段的参考轨迹,如公式(23)所示:
其中,Yref为分段式的下潜速度参考轨迹,c1和c2表示权重系数。但是在实际应用中,不应限制于两段式参考轨迹,根据目标深度的不同,可以设置多段轨迹,甚至设计连续函数去实现深度控制。
步骤S400,根据所述下潜控制量,通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置,并依据当前活塞位置获取活塞的控制量。
在本实施例中,由于滑翔模态下是通过调节浮力来实现深度控制,而浮力调节是通过机构中活塞移动以实现吸排水,因此需要将得到的控制量映射为浮力调节机构中活塞的位置。鉴于得到控制量的物理意义为力,根据浮力原理,可得到活塞的目标位置,计算过程如公式(24)所示:
其中,rsr为活塞的目标位置,ρ表示水的密度,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中“通过滑模观测器获取估计速度”,其方法为:
基于所述仿生滑翔机器海豚的坐标系,构建其对应的全状态三维动力学模型;
通过忽略侧向运动对所述全状态三维动力学模型进行简化,得到简化动力学模型;
根据当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息,通过滑模观测器获取所述简化动力学模型的速度向量,将其作为所述仿生滑翔机器海豚的估计速度。
5.根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法,其特征在于,步骤S400中“构建贝塞尔曲线并进行分段,获取分段式的下潜速度参考轨迹”,其方法为:
基于所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息,构建二阶贝塞尔曲线轨迹,将其作为所述仿生滑翔机器海豚的深度参考轨迹;
对所述深度参考轨迹求导,得到下潜速度参考轨迹;
根据所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息的差的绝对值与预设的深度阈值的大小,对所述下潜速度参考轨迹进行分段,得到分段式的下潜速度参考轨迹。
8.一种仿生滑翔机器海豚的滑翔深度控制***,其特征在于,该***包括获取预设值模块、获取胸鳍控制模块、获取下潜控制模块、获取活塞控制模块、控制滑翔模块;
所述获取预设值模块,配置为获取预设滑翔深度、预设偏航角;
所述获取胸鳍控制模块,配置为基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息,通过滑模观测器获取估计速度,并结合所述预设偏航角,通过偏航控制器获取两侧胸鳍的控制量;
所述获取下潜控制模块,配置为基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息,构建贝塞尔曲线并进行分段,获取分段式的下潜速度参考轨迹,并结合当前时刻的估计速度,通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量;
所述获取活塞控制模块,配置为根据所述下潜控制量,通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置,并依据当前活塞位置获取活塞的控制量;
所述控制滑翔模块,配置为基于所述活塞的控制量和所述两侧胸鳍的控制量,控制所述仿生滑翔机器海豚进行滑翔。
9.一种存储装置,其中存储有多条程序,其特征在于,所述程序应用由处理器加载并执行以实现权利要求1-7任一项所述的仿生滑翔机器海豚的滑翔深度控制方法。
10.一种处理装置,包括处理器、存储装置;处理器,适用于执行各条程序;存储装置,适用于存储多条程序;其特征在于,所述程序适用于由处理器加载并执行以实现权利要求1-7任一项所述的仿生滑翔机器海豚的滑翔深度控制方法。
Priority Applications (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911189693.2A CN110758698B (zh) | 2019-11-28 | 2019-11-28 | 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 |
PCT/CN2020/085043 WO2021103391A1 (zh) | 2019-11-28 | 2020-04-16 | 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 |
US17/069,868 US10935986B1 (en) | 2019-11-28 | 2020-10-14 | Gliding depth control method, system and device for biomimetic gliding robotic dolphin |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911189693.2A CN110758698B (zh) | 2019-11-28 | 2019-11-28 | 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110758698A CN110758698A (zh) | 2020-02-07 |
CN110758698B true CN110758698B (zh) | 2020-10-02 |
Family
ID=69339663
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911189693.2A Active CN110758698B (zh) | 2019-11-28 | 2019-11-28 | 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN110758698B (zh) |
WO (1) | WO2021103391A1 (zh) |
Families Citing this family (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10935986B1 (en) | 2019-11-28 | 2021-03-02 | Institute Of Automation, Chinese Academy Of Sciences | Gliding depth control method, system and device for biomimetic gliding robotic dolphin |
CN110758698B (zh) * | 2019-11-28 | 2020-10-02 | 中国科学院自动化研究所 | 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 |
CN111547212B (zh) * | 2020-06-02 | 2021-12-07 | 哈尔滨工程大学 | 一种无动力式快速潜浮auv的浮力控制方法 |
CN111830832B (zh) * | 2020-07-27 | 2021-08-31 | 中国科学院自动化研究所 | 仿生滑翔机器海豚平面路径跟踪方法及*** |
CN112363401B (zh) * | 2020-11-29 | 2022-06-07 | 西北工业大学 | 一种基于浮力调节的水下滑翔机自适应反演深度控制方法 |
CN113325857B (zh) * | 2021-06-08 | 2022-08-05 | 西北工业大学 | 基于质心与浮力***的仿蝠鲼水下航行器定深控制方法 |
CN113277046B (zh) * | 2021-06-08 | 2022-08-05 | 西北工业大学 | 一种基于质心与尾鳍的仿蝠鲼水下航行器定深控制方法 |
CN113515134B (zh) * | 2021-06-24 | 2021-12-14 | 天津海翼科技有限公司 | 应用于水下/水面探测的水下机器人 |
CN113419422B (zh) * | 2021-07-02 | 2022-01-28 | 哈尔滨理工大学 | 一种船用舵鳍联合减摇控制*** |
CN113734393B (zh) * | 2021-09-28 | 2022-12-02 | 深圳先进技术研究院 | 基于攻角法的仿生机器鱼定深控制方法和装置 |
CN114371698A (zh) * | 2021-12-09 | 2022-04-19 | 沈阳航天新光集团有限公司 | 一种自主避障游动仿生机器鱼 |
CN114355777B (zh) * | 2022-01-06 | 2023-10-10 | 浙江大学 | 一种基于分布式压力传感器与分段姿态控制的动态滑翔方法及*** |
CN115774967A (zh) | 2022-09-15 | 2023-03-10 | 厦门理工学院 | 多关节仿生海豚运动控制方法、***及水下损伤探测方法 |
CN116700015B (zh) * | 2023-07-28 | 2023-10-31 | 中国科学院自动化研究所 | 水下航行器主动增稳控制方法及装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE19712257A1 (de) * | 1997-03-24 | 1998-10-01 | Rudolph Stephan Dr Ing | Tauchgerät und Verfahren zu seiner Herstellung |
CN102962843A (zh) * | 2012-12-03 | 2013-03-13 | 中国科学院自动化研究所 | 一种跃水机器海豚 |
CN104627342A (zh) * | 2014-12-08 | 2015-05-20 | 中国科学院自动化研究所 | 一种滑翔机器海豚 |
CN104881045A (zh) * | 2015-06-17 | 2015-09-02 | 中国科学院自动化研究所 | 嵌入式视觉引导下仿生机器鱼三维追踪控制方法 |
CN109018271A (zh) * | 2018-06-27 | 2018-12-18 | 哈尔滨工程大学 | 一种新型大翼展混合驱动无人水下航行器 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6814634B2 (en) * | 2003-01-30 | 2004-11-09 | Seagoon Boat Building | Self-propelled aquatic toy |
CN100519331C (zh) * | 2005-04-12 | 2009-07-29 | 北京大学 | 智能机器海豚 |
CN101315563A (zh) * | 2007-05-31 | 2008-12-03 | 中国科学院自动化研究所 | 基于重心改变的仿生机器鱼深度控制方法 |
CN100589055C (zh) * | 2008-04-08 | 2010-02-10 | 哈尔滨工程大学 | 基于递归模糊神经网络的水下潜器空间变结构控制方法 |
CN101916071B (zh) * | 2010-08-04 | 2012-05-02 | 中国科学院自动化研究所 | 仿生机器鱼运动的cpg反馈控制方法 |
KR101278708B1 (ko) * | 2011-05-18 | 2013-06-25 | 부산대학교 산학협력단 | 수중로봇과 그 제어방법 |
CN104554681B (zh) * | 2014-12-30 | 2017-02-22 | 中国科学院自动化研究所 | 机器海豚的运动控制方法和装置 |
CN105676867B (zh) * | 2016-04-21 | 2019-02-22 | 南京工程学院 | 一种rov水下机器人悬浮姿态稳定控制方法 |
CN110758698B (zh) * | 2019-11-28 | 2020-10-02 | 中国科学院自动化研究所 | 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 |
-
2019
- 2019-11-28 CN CN201911189693.2A patent/CN110758698B/zh active Active
-
2020
- 2020-04-16 WO PCT/CN2020/085043 patent/WO2021103391A1/zh active Application Filing
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE19712257A1 (de) * | 1997-03-24 | 1998-10-01 | Rudolph Stephan Dr Ing | Tauchgerät und Verfahren zu seiner Herstellung |
CN102962843A (zh) * | 2012-12-03 | 2013-03-13 | 中国科学院自动化研究所 | 一种跃水机器海豚 |
CN104627342A (zh) * | 2014-12-08 | 2015-05-20 | 中国科学院自动化研究所 | 一种滑翔机器海豚 |
CN104881045A (zh) * | 2015-06-17 | 2015-09-02 | 中国科学院自动化研究所 | 嵌入式视觉引导下仿生机器鱼三维追踪控制方法 |
CN109018271A (zh) * | 2018-06-27 | 2018-12-18 | 哈尔滨工程大学 | 一种新型大翼展混合驱动无人水下航行器 |
Non-Patent Citations (1)
Title |
---|
胸鳍推进型机器鱼的CPG控制及实现;汪明等;《机器人》;20100330;第32卷(第二期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
WO2021103391A1 (zh) | 2021-06-03 |
CN110758698A (zh) | 2020-02-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110758698B (zh) | 仿生滑翔机器海豚的滑翔深度控制方法、***、装置 | |
US10935986B1 (en) | Gliding depth control method, system and device for biomimetic gliding robotic dolphin | |
CN107168312B (zh) | 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法 | |
CN104589349B (zh) | 一种混合悬浮微重力环境下带有单关节机械臂的组合体自主控制方法 | |
Suebsaiprom et al. | Maneuverability modeling and trajectory tracking for fish robot | |
Xue et al. | Coordinate control, motion optimization and sea experiment of a fleet of Petrel-II gliders | |
Zhang et al. | Reaction-wheel-based roll stabilization for a robotic fish using neural network sliding mode control | |
JP2017206154A (ja) | 水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラム | |
Zhang et al. | A flexible, reaction-wheel-driven fish robot: Flow sensing and flow-relative control | |
CN114967714A (zh) | 一种自主式水下机器人抗扰运动控制方法及*** | |
Jing et al. | Self-tuning adaptive active disturbance rejection pitch control of a manta-ray-like underwater glider | |
Binugroho et al. | EROV: depth and balance control for rov motion using fuzzy pid method | |
CN114545935A (zh) | 基于改进麻雀算法的模糊pid动力定位铺缆船循迹控制方法 | |
Yan et al. | Towards unusual rolled swimming motion of a bioinspired robotic hammerhead shark under negative buoyancy | |
Song et al. | Using an active disturbance rejection decoupling control algorithm to improve operational performance for underwater glider applications | |
CN113110512A (zh) | 一种减弱未知干扰与抖振影响的可底栖式auv自适应轨迹跟踪控制方法 | |
Antervedi et al. | Terrain-relative diver following with autonomous underwater vehicle for coral reef mapping | |
Dong et al. | Gliding motion optimization for a biomimetic gliding robotic fish | |
Guerreiro et al. | Trajectory tracking nonlinear model predictive control for autonomous surface craft | |
CN110376883A (zh) | 四旋翼无人机终端滑模控制方法、***、介质及设备 | |
Naser et al. | Labriform swimming robot with steering and diving capabilities | |
CN112904719B (zh) | 一种适用于水下机器人位置环形区域跟踪控制方法 | |
Roche et al. | LFT/H∞ varying sampling control for Autonomous Underwater Vehicles | |
KR20170071441A (ko) | 원격 무인 잠수정을 위한 비선형 적응제어 시스템 | |
Palmisano et al. | Design, development, and testing of flapping fins with actively controlled curvature for an unmanned underwater vehicle |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |