CN114397899A - 一种仿生机器鱼三维路径跟踪控制方法及装置 - Google Patents
一种仿生机器鱼三维路径跟踪控制方法及装置 Download PDFInfo
- Publication number
- CN114397899A CN114397899A CN202111501861.4A CN202111501861A CN114397899A CN 114397899 A CN114397899 A CN 114397899A CN 202111501861 A CN202111501861 A CN 202111501861A CN 114397899 A CN114397899 A CN 114397899A
- Authority
- CN
- China
- Prior art keywords
- bionic robot
- robot fish
- expected
- fish
- path tracking
- 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.)
- Pending
Links
- 241000251468 Actinopterygii Species 0.000 title claims abstract description 184
- 239000011664 nicotinic acid Substances 0.000 title claims abstract description 142
- 238000000034 method Methods 0.000 title claims abstract description 98
- 230000008569 process Effects 0.000 claims abstract description 40
- 230000003044 adaptive effect Effects 0.000 claims abstract description 17
- 230000003592 biomimetic effect Effects 0.000 claims description 35
- 238000004590 computer program Methods 0.000 claims description 14
- 210000000006 pectoral fin Anatomy 0.000 claims description 13
- 238000013507 mapping Methods 0.000 claims description 11
- 230000004888 barrier function Effects 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 10
- 230000006870 function Effects 0.000 description 7
- 239000011159 matrix material Substances 0.000 description 7
- 238000013461 design Methods 0.000 description 6
- 230000005484 gravity Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 3
- 210000005036 nerve Anatomy 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000007935 neutral effect Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 241001465754 Metazoa Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000002146 bilateral effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 239000012530 fluid Substances 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 239000004816 latex Substances 0.000 description 1
- 229920000126 latex Polymers 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001020 rhythmical effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Toys (AREA)
Abstract
本发明提供一种仿生机器鱼三维路径跟踪控制方法及装置。该方法包括：基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及自适应视线导航策略，确定仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对内环控制器的控制律进行修正；基于期望速度、期望航向角及期望深度，并利用内环控制器对仿生机器鱼的速度、航向角和深度进行调整，以实现仿生机器鱼的三维路径跟踪控制；内环控制器包括速度控制器、航向控制器及深度控制器。本发明提供的方法，通过对控制过程进行补偿，能够适应更为复杂的路径跟踪任务，有效提高了仿生机器鱼路径跟踪控制的精确度和鲁棒性。
Description
技术领域
本发明涉及人工智能技术领域，具体涉及一种仿生机器鱼三维路径跟踪控制方法及装置。另外，还涉及一种电子设备及处理器可读存储介质。
背景技术
近年来，随着水下机器人产业的不断发展，自主水下航行器(AutonomousUnderwater Vehicle,AUV)逐步代替人类实现在危险的海洋环境中完成作业任务，并且有着广阔的应用前景。与此同时，复杂多变的控制任务和作业环境也给现有的水下航行器带来较大的挑战，因而出现大量的研究任务来弥补水下航行器智能自主作业的短板，其中包括自主巡航、水下避障、路径规划及路径跟踪等。对于水下航行器而言，路径跟踪控制是一种典型的运动控制任务，而高精度的三维路径跟踪，可以为水下勘探、海底管道线缆检测和环境监测等智能作业任务奠定良好的基础。
根据控制目标的不同，路径跟踪控制主要分为两类：基于几何结构的路径跟踪控制和基于控制理论的路径跟踪控制。基于几何结构的方法主要根据航行器和期望路径的相对位姿，利用几何关系推算每个时刻的跟踪目标点，这种方法一般比较直观且易于实现；基于控制理论的方法则更多地由跟踪误差出发描述路径跟踪控制任务，通过减小跟踪误差的方法控制航行器贴近期望路径，更容易验证控制算法的稳定性。真实海洋环境中，由于存在外界环境的干扰，航行器的航向和合速度方向往往存在一定角度偏差，可以用侧滑角来衡量，侧滑角的存在可能导致控制律无法满足原有的控制效果，甚至出现控制不稳定的情况。所以为了提高控制精度，有必要考虑侧滑角的影响。针对多关节仿生机器鱼，其作为一种特殊的水下航行器在运动高效性和机动性等方面都有更好的表现，但是，仿生机器鱼的运动模态使多关节机器鱼需要利用身体的波动运动来提供前向推力，其路径跟踪任务更为复杂，随之产生的较大范围且频繁变化的侧滑角也导致传统的路径跟踪控制及侧滑角补偿方案很难实现高精度的控制。因此，如何设计一种能够实现高精度的仿生机器鱼三维路径跟踪控制方案具有非常重要的现实意义。
发明内容
为此，本发明提供一种仿生机器鱼三维路径跟踪控制方法及装置，以解决现有技术中存在的路径跟踪控制及侧滑角补偿方案局限性较高，导致对仿生机器鱼三维路径跟踪控制精度和鲁棒性较差的缺陷。
第一方面，本发明提供一种仿生机器鱼三维路径跟踪控制方法，包括：基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；
利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；
基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
进一步的，所述利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正，具体包括：将所述仿生机器鱼在运动过程中频繁变化的侧滑角确定为外界扰动项，利用非线性扰动观测器对所述侧滑角进行估计及补偿，以实现在所述仿生机器鱼的三维路径跟踪控制过程中对所述内环控制器的控制律进行动态修正。
进一步的，所述的仿生机器鱼三维路径跟踪控制方法，还包括：预先根据所述仿生机器鱼的当前运动模态确定相应的动力学模型；所述预先根据所述仿生机器鱼的当前运动模态确定相应的动力学模型，具体为：预先根据所述仿生机器鱼的机械结构及运动特征，确定相应的动力学模型。
进一步的，基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，具体包括：采用模糊映射将所述内环控制器输出的前向推力、俯仰力矩和偏航力矩分别映射成预设的中枢模式发生器的参数频率和偏置，以及相应的胸鳍偏置角；基于所述参数频率、偏置以及所述胸鳍偏置角，并利用所述中枢模式发生器对所述仿生机器鱼进行节律性控制，以实现对所述仿生机器鱼的速度、航向角和深度进行调整。
进一步的，所述中枢模式发生器为基于Hopf振荡器的中枢模式发生器。
进一步的，所述自适应视线导航策略是基于预设障碍函数来确定的自适应视线导航策略。
第二方面，本发明还提供一种仿生机器鱼三维路径跟踪控制装置，包括：三维路径导航单元，用于基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；
侧滑角补偿单元，用于利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；
三维路径跟踪处理单元，用于基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
进一步的，所述侧滑角补偿单元，具体用于：将所述仿生机器鱼在运动过程中频繁变化的侧滑角确定为外界扰动项，利用非线性扰动观测器对所述侧滑角进行估计及补偿，以实现在所述仿生机器鱼的三维路径跟踪控制过程中对所述内环控制器的控制律进行动态修正。
进一步的，所述的仿生机器鱼三维路径跟踪控制装置，还包括：动力学模型确定单元，用于预先根据所述仿生机器鱼的当前运动模态确定相应的动力学模型；
所述动力学模型确定单元，具体用于：预先根据所述仿生机器鱼的机械结构及运动特征确定相应的动力学模型。
进一步的，所述三维路径跟踪处理单元，具体用于：采用模糊映射将所述内环控制器输出的前向推力、俯仰力矩和偏航力矩分别映射成预设的中枢模式发生器的参数频率和偏置，以及相应的胸鳍偏置角；基于所述参数频率、偏置以及所述胸鳍偏置角，并利用所述中枢模式发生器对所述仿生机器鱼进行节律性控制，以实现对所述仿生机器鱼的速度、航向角和深度进行调整。
进一步的，所述中枢模式发生器为基于Hopf振荡器的中枢模式发生器。
进一步的，所述自适应视线导航策略是基于预设障碍函数来确定的自适应视线导航策略。
第三方面，本发明还提供一种电子设备，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行所述计算机程序时实现如上述任意一项所述仿生机器鱼三维路径跟踪控制方法的步骤。
第四方面，本发明还提供一种处理器可读存储介质，所述处理器可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如上述任意一项所述仿生机器鱼三维路径跟踪控制方法的步骤。
本发明提供的仿生机器鱼三维路径跟踪控制方法，通过动力学模型、期望路径、自适应视线导航策略获取相应的期望值，并利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以实现对控制过程进行补偿，进而实现提高仿生机器鱼的三维路径跟踪控制精度，能够适应更为复杂的路径跟踪任务，有效提高了仿生机器鱼路径跟踪控制的稳定度和鲁棒性，从而提升了控制效果。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获取其他的附图。
图1是本发明实施例提供的仿生机器鱼三维路径跟踪控制方法的流程示意图；
图2是本发明实施例提供的仿生机器鱼的三维路径跟踪系统框架示意图；
图3是本发明实施例提供的仿生机器鱼的系统坐标系示意图；
图4是本发明实施例提供的仿生机器鱼的三维路径跟踪坐标系示意图；
图5是本发明实施例提供的仿生机器鱼的机电结构示意图；
图6是本发明实施例提供的仿生机器鱼三维路径跟踪控制装置的结构示意图；
图7是本发明实施例提供的电子设备的实体结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获取的所有其他实施例，都属于本发明保护的范围。
下面基于本发明所述仿生机器鱼三维路径跟踪控制方法，对其实施例进行详细描述。如图1所示，其为本发明实施例提供的仿生机器鱼三维路径跟踪控制方法的流程示意图，具体实现过程包括以下步骤：
步骤101：基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度。
在本发明实施例中，执行本步骤之前，需要预先根据所述仿生机器鱼的当前运动模态确定相应的动力学模型，具体的：可根据所述仿生机器鱼的机械结构及运动特征来确定相应的动力学模型。然后，基于所述动力学模型、期望路径以及自适应视线导航策略确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度。所述自适应视线导航策略是基于预设障碍函数来确定的自适应视线导航策略。
其中，所述仿生机器鱼为多关节仿生机器鱼，具体可如图5所示，其为该多关节仿生机器鱼的外型及机电结构示意图。硬质的仿生机器鱼外壳通过乳胶蒙皮的包裹实现防水；四个尾部舵机相互串联，为仿生机器鱼提供主要的推进力；胸鳍舵机则通过改变鳍面偏置角实现仿生机器鱼的上浮下潜。根据多关节仿生机器鱼的运动模态搭建模型坐标系进而确定相应的动力学模型。该模型坐标系具体如图3所示，{Cw}和{C0}分别表示全局惯性坐标系和鱼体坐标系；其中鱼体坐标系的x轴方向表示鱼头正方向；{CG}和{CB}分别表示以重心G为原点的重力坐标系和以浮心B为原点的浮力坐标系。多关节仿生机器鱼的每个可动关节可以看作串联连杆，并分别以连杆Bi的起点为原点建立连杆坐标系{Ci}，其中i∈[1,n]表示第i个连杆，而n＝4表示连杆的总数。连杆的长度和对应关节的关节角可以分别用li和θi表示。左右两侧对称的胸鳍坐标系可以分别用{Cl}和{Cr}描述。本发明所述的仿生机器鱼三维路径跟踪控制方法，具体为对仿生机器鱼运动目标进行控制的方法。如图2所示为本发明中仿生机器鱼的三维路径跟踪系统框架示意图。
本发明实施例中，所述控制方法具体包括以下步骤：预先通过分析多关节仿生机器鱼的运动模态搭建完备的动力学模型。
具体的，根据如图3所示的坐标系系统，在鱼体坐标系{C0}下建立多关节仿生机器鱼的动力学模型，用牛顿-欧拉方程加以描述：
0TG,B＝-0HGG+0HBB
式中，M∑包括多连杆和胸鳍的惯性矩阵和附加惯性矩阵；V0表示六维广义速度矢量；
表示对速度矢量的导数，也就是加速度矢量；左右胸鳍的鳍面升阻力0Tl和0Tr为仿生机器鱼提供主动的俯仰力矩，而在仿生机器鱼离开水平姿态时0TG,B用于提供被动的恢复力矩；fdr,i是周围流体对仿生机器鱼产生的拖拽力；而0E∑表示每部分连杆的科里奥利力；0Tb和0Tp表示与加速度相关的连杆间相互作用力，同时ηj，ηl和ηr分别代表
和
的计算结果；0Td表示外界扰动，而Z＝[01×5,1]T表示六维单位矢量。其中，
和
等是动力学模型构建过程中的中间项，本申请基于预先构建完成的动力学模型的结果即可实现仿生机器鱼三维路径跟踪控制。
需要说明的是，根据如图3所示建立的坐标系，通过坐标系的力或者变换关系用上下脚标就可以简化表示，比如基于坐标系A的力可以简化表示为AT，而从坐标系C到坐标系D的变换矩阵可以简化表示为DHC，以此类推。其中，坐标系A、C、D可以分别是指坐标系{Cw}、{C0}、{Ci}、{CG}、{CB}、{Cl}、{Cr}等，在此不做具体描述。
通过建立完备的动力学模型，可以更好地在仿真环境中模拟仿生机器鱼仿照鱼类的运动模态。而在本发明具体控制任务中，为了实现基于动力学模型的控制方法的鲁棒性更优，针对上述的完备动力学模型进行进一步简化。考虑到仿生机器鱼运动时的非完整约束，通过引入控制力τ＝[τu,τq,τr]T和对应的速度V＝[u,q,r]T，得到的仿射的简化动力学模型：
式中，从动力学模型中选取三个主要的维度控制仿生机器鱼的三维运动，即τu、τq和τr分别表示前向推力、俯仰力矩和偏航力矩，并且分别对应前向速度u、俯仰角速度q和偏航角速度r。
M＝diag{m11,m55,m66}表示惯性矩阵和附加惯性矩阵，D(V)＝diag{d11,d55,d66}表示水动力阻尼项，g(η)＝[0,(zgG-zbB)sinθ,0]T则表示重力和浮力项，θ表示俯仰角，zg和zb分别对应重心和浮心在{C0}坐标系下的z坐标，G和B分别表示重力和浮力，科里奥利力C(V)可以表示如下：
式中，v、w和p分别表示横荡速度、垂荡速度和横滚角速度，对应的惯性矩阵和附加惯性矩阵系数表示为m22、m33和m44，计算并简化得到最终的动力学模型为：
需要说明的是，用牛顿-欧拉方程描述的六维详细动力学模型经过模型简化得到基于六维广义控制力的仿射模型，并选取三个主要控制维度，利用τ＝[τu,τq,τr]T实现仿生机器鱼的三维运动控制，上述速度V＝[u,q,r]T变量上加点分别表示速度对时间的导数也就是三维加速度矢量。
在本步骤具体实施过程中可通过动力学模型、期望路径以及基于障碍函数的自适应视线导航策略，选取仿生机器鱼当前时刻的期望速度、期望航向角和期望深度。
具体的，根据图4中所示的坐标系系统，仿生机器鱼的当前位置和速度分别用P＝[x,y,z]T和
表示，期望路径则用参数化形式描述为
通过标量
遍历整个期望路径，并以此建立基于期望路径的SF(Serret-Frenet)坐标系{CSF}，路径点的合速度表示为Up，对应的速度矢量表示为：
具体用Rp表示从{CSF}到{Cw}的旋转矩阵，根据仿生机器鱼和路径点的相对位置，在SF坐标系下定义误差向量为：
式中，xe、ye和ze分别表示沿迹误差、横向误差和垂向误差。选取其中决定跟踪误差的向量γ＝[ye,ze]T，构建控制李雅普诺夫函数(Control Lyapunov Function，CLF)：
根据路径跟踪坐标系描述的几何关系，计算得到对应的导数：
式中，U表示仿生机器鱼的合速度，而χr＝χ-χp和υr＝υ-υp分别表示仿生机器鱼和路径点的角度偏差，χ和υ表示机器鱼的方位角和仰角，χp和υp则表示路径点的方位角和仰角。根据几何关系基于视线导航法选取视线点Plos作为仿生机器鱼当前的目标点，从而设计控制律为：
若选择视线导航法的前视距离为dl，此时导航系数Δy和Δz可以表示为：
本发明引入指数收敛率κ，通过李雅普诺夫函数的导数证明跟踪误差一致指数收敛：
需要说明的是，尽管沿迹误差xe通常并不是作为衡量跟踪误差的关键因素，其收敛性是保证跟踪一致且稳定的前提。而对于多关节仿生机器鱼而言，内部空间的受限使其无法搭载高精度的传感器，同时由于不可避免的频繁往复摆动使其在运动过程中往往无法获得准确的速度估计，这种估计误差或是有时滞的误差补偿，极大地影响传统导航策略中沿迹误差的收敛性，从而导致路径跟踪控制的不确定性。除此之外，本发明针对多关节仿生机器鱼的运动模态和仿生机器鱼运动过程中的速度估计误差，设计了基于log(对数)型障碍李雅普诺夫函数(即所述的障碍函数)的自适应视线导航策略，在保证跟踪过程中沿迹误差收敛性的同时，自适应地调整跟踪的前视距离，使仿生机器鱼可以平稳地应对更加复杂的三维路径。
本发明所述的自适应视线导航策略，可以在保证收敛性的同时约束沿迹误差xe到一个理想的范围。首先引入障碍李雅普诺夫函数(Barrier Lyapunov Function，BLF)：
式中，kb表示边界系数，当kb收敛到无穷时，BLF为传统导航策略，通过设定kb的范围，当沿迹误差将要违反限制时，BLF可以根据理想的约束范围动态地调整控制变量，此时计算BLF的导数：
确定路径点速度Up为：
此时BLF的导数可以化简为：
所以本发明的基于障碍函数的自适应视线导航策略在约束沿迹误差的同时可以保证误差的收敛性，计算对应的路径点的迭代速率为：
为了适应更加复杂多变的期望路径，本发明将跟踪过程分为两部分，分别设置自适应的前视距离。当仿生机器鱼处于初始位置时，距离期望路径的位置相对较远，仿生机器鱼处于“接近状态”，此时主要的控制任务是保证仿生机器鱼较快接近期望路径，同时保证姿态的平滑过渡。通过引入sigmoid函数描述此时前视距离dl和沿迹误差xe的关系：
式中，dlmin是前视距离的下限，同时dl0是xe＝0时的预设距离。
当仿生机器鱼到达期望路径时，沿迹误差小于一定范围||xe||＜δ，仿生机器鱼进入“跟踪状态”，此时主要的控制任务是让跟踪误差收敛到0，根据此时的路径曲率K∈[0,Kmax]设置dl为：
最终本发明确定的自适应视线导航策略为：
步骤102：利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正。
将所述仿生机器鱼在运动过程中频繁变化的侧滑角确定为外界扰动项，利用非线性扰动观测器(Nonlinear Disturbance Observer，NDOB)对所述侧滑角进行估计及补偿，以实现在所述仿生机器鱼的三维路径跟踪控制过程中对所述内环控制器的控制律进行动态修正。
本发明实施例中，所述控制方法具体还包括：利用非线性扰动观测器估计并补偿控制过程中的侧滑角，进而减小频繁往复变化的侧滑角对路径跟踪控制的影响。
针对横向误差，对自适应视线导航策略中的李雅普诺夫函数的导数进行简化：
式中，ψ＝χ-β表示偏航角，β表示侧滑角。假设侧滑角的范围满足|β|＜10°，进行进一步简化得：
式中，f(ye)＝Usin(ψ-χp)cos(υr)，而g(ye)＝Ucos(ψ-χp)cos(υr)。需要说明的是，本发明不利用基于速度估计的方式通过β＝arcsin(v,U)来获得侧滑角β，而是将侧滑角β视为外界扰动项，基于NDOB进行补偿。设定
为β的估计，定义估计误差为
通过确定非线性扰动观测器的增益l(ye)，以保证非线性扰动观测器一致指数收敛：
最终NDOB的形式可以表示为：
式中，ι表示中间标量，通过对β的估计抵消非零的侧滑角。
通过将仿生机器鱼运动过程中频繁变化的侧滑角确定为外界扰动项，并利用非线性扰动观测器进行估计，实现了侧滑角的动态补偿，同时有效避免了传统侧滑角跟踪及补偿机制中由于速度估计精度不足导致错误补偿的问题，从而进一步提升控制精度
步骤103：基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
本发明实施例中，采用模糊映射将所述内环控制器输出的前向推力、俯仰力矩和偏航力矩分别映射成预设的中枢模式发生器的参数频率和偏置，以及相应的胸鳍偏置角；基于所述参数频率、偏置以及所述胸鳍偏置角，并利用所述中枢模式发生器对所述仿生机器鱼进行节律性控制，以实现对所述仿生机器鱼的速度、航向角和深度进行调整。所述中枢模式发生器为基于Hopf振荡器的中枢模式发生器。
所述控制方法具体还包括：基于所述期望速度、所述期望航向角以及所述期望深度，利用基于滑模的内环控制器，实现速度、航向角和深度控制，并最终实现多关节仿生机器鱼的三维路径跟踪控制。
针对多关节仿生机器鱼对应的动力学模型的非线性和强耦合性，本发明采用基于动力学模型的全局快速终端滑模控制(Global Fast Terminal Sliding Mode Control，GFTSMC)设计航向控制器和深度控制器。
选取航向控制滑模面sψ1为：
式中，sψ0＝ψd-ψ，αψ和βψ为正系数，qψ0和pψ0为正整数，结合上述简化的动力学模型，推导滑模面的导数为：
通过引入终端吸引子，可以很大程度上减小滑模控制产生的抖振，设计趋近律为：
式中，φψ和γψ为正系数，qψ1和pψ1为正整数，得到最终的航向控制律为：
针对仿生机器鱼欠驱动的运动方式，本发明将期望深度zd转化为期望俯仰角θd，通过控制俯仰角实现最终的深度控制，利用基于视线圆的视线导航法(Line Of Sight，LOS)设计转化策略为：
θd＝arcsin(-(zd-z)/R)
式中，R为视线导航法中视线圆的半径。选取sθ0＝θd-θ，同理可以得到最终的俯仰控制律为：
式中，αθ、βθ、φθ和γθ为正系数；qθ0、pθ0、qθ1和pθ1为正整数，针对速度控制的相对阶与航向和深度控制不同，本发明采用超螺旋滑模控制(Super-Twisting Sliding ModeControl，STSMC)设计速度控制器，高阶的积分项可以极大减小抖振的影响，引入速度滑模面su＝ud-u控制前向速度u，此时的速度控制律可以表示为：
τu＝λ|su|σsign(su)+τI
式中，σ表示指数调节系数，sign(·)表示符号函数，τI表示积分项，同时λ和W为正增益系数。
为了提供稳定且连续的控制律，在本发明实施例中，通过引入基于Hopf振荡器的中枢模式发生器(Central Pattern Generator，CPG)实现多关节仿生机器鱼的节律性控制，同时利用模糊映射将滑模控制律输出的τu、τr和τq分别映射成CPG参数频率ω、偏置b以及胸鳍偏置角θp。因此，本发明设计的内环控制器可以稳定且平稳地跟踪导航策略输出的期望速度、期望航向以及期望深度，最终实现多关节仿生机器鱼的三维路径跟踪控制。中枢模式发生器是产生动物节律运动行为的生物神经环路，由一系列神经振荡器组成，是神经振荡器与多重反射回路系统集成在一起组成的一个复杂的分布式神经网络。需要说明的是本发明上述出现的相同参数表示的含义也相同，不再重复赘述。
本发明实施例所述的仿生机器鱼三维路径跟踪控制方法，通过动力学模型、期望路径、自适应视线导航策略获取相应的期望值，并利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以实现对控制过程进行补偿，进而实现提高仿生机器鱼的三维路径跟踪控制精度，能够适应更为复杂的路径跟踪任务，有效提高了仿生机器鱼路径跟踪控制的稳定度和鲁棒性，从而提升了控制效果。
与上述提供的一种仿生机器鱼三维路径跟踪控制方法相对应，本发明还提供一种仿生机器鱼三维路径跟踪控制装置。由于该装置的实施例相似于上述方法实施例，所以描述得比较简单，相关之处请参见上述方法实施例部分的说明即可，下面描述的仿生机器鱼三维路径跟踪控制装置的实施例仅是示意性的。请参考图6所示，其为本发明实施例提供的一种仿生机器鱼三维路径跟踪控制装置的结构示意图。
本发明所述的仿生机器鱼三维路径跟踪控制装置，具体包括：
三维路径导航单元601，用于基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；
侧滑角补偿单元602，用于利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；
三维路径跟踪处理单元603，用于基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
进一步的，所述侧滑角补偿单元，具体用于：将所述仿生机器鱼在运动过程中频繁变化的侧滑角确定为外界扰动项，利用非线性扰动观测器对所述侧滑角进行估计及补偿，以实现在所述仿生机器鱼的三维路径跟踪控制过程中对所述内环控制器的控制律进行动态修正。
进一步的，所述的仿生机器鱼三维路径跟踪控制装置，还包括：动力学模型确定单元，用于预先根据所述仿生机器鱼的当前运动模态确定相应的动力学模型；
所述动力学模型确定单元，具体用于：预先根据所述仿生机器鱼的机械结构及运动特征确定相应的动力学模型。
进一步的，所述三维路径跟踪处理单元，具体用于：采用模糊映射将所述内环控制器输出的前向推力、俯仰力矩和偏航力矩分别映射成预设的中枢模式发生器的参数频率和偏置，以及相应的胸鳍偏置角；基于所述参数频率、偏置以及所述胸鳍偏置角，并利用所述中枢模式发生器对所述仿生机器鱼进行节律性控制，以实现对所述仿生机器鱼的速度、航向角和深度进行调整。
进一步的，所述中枢模式发生器为基于Hopf振荡器的中枢模式发生器。
进一步的，所述自适应视线导航策略是基于预设障碍函数来确定的自适应视线导航策略。
本发明实施例所述的仿生机器鱼三维路径跟踪控制装置，通过动力学模型、期望路径、自适应视线导航策略获取相应的期望值，并利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以实现对控制过程进行补偿，进而实现提高仿生机器鱼的三维路径跟踪控制精度，能够适应更为复杂的路径跟踪任务，有效提高了仿生机器鱼路径跟踪控制的稳定度和鲁棒性，从而提升了控制效果。
与上述提供的仿生机器鱼三维路径跟踪控制方法相对应，本发明还提供一种电子设备。由于该电子设备的实施例相似于上述方法实施例，所以描述得比较简单，相关之处请参见上述方法实施例部分的说明即可，下面描述的电子设备仅是示意性的。如图7所示，其为本发明实施例公开的一种电子设备的实体结构示意图。该电子设备可以包括：处理器(processor)701、存储器(memory)702和通信总线703，其中，处理器701，存储器702通过通信总线703完成相互间的通信，通过通信接口704与外部进行通信。处理器701可以调用存储器702中的逻辑指令，以执行仿生机器鱼三维路径跟踪控制方法，该方法包括：基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
此外，上述的存储器702中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：存储芯片、U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
另一方面，本发明实施例还提供一种计算机程序产品，所述计算机程序产品包括存储在处理器可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法实施例所提供的仿生机器鱼三维路径跟踪控制方法。该方法包括：基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
又一方面，本发明实施例还提供一种处理器可读存储介质，所述处理器可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各实施例提供的仿生机器鱼三维路径跟踪控制方法。该方法包括：基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
所述处理器可读存储介质可以是处理器能够存取的任何可用介质或数据存储设备，包括但不限于磁性存储器(例如软盘、硬盘、磁带、磁光盘(MO)等)、光学存储器(例如CD、DVD、BD、HVD等)、以及半导体存储器(例如ROM、EPROM、EEPROM、非易失性存储器(NANDFLASH)、固态硬盘(SSD))等。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.一种仿生机器鱼三维路径跟踪控制方法，其特征在于，包括：
基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；
利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；
基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
2.根据权利要求1所述的仿生机器鱼三维路径跟踪控制方法，其特征在于，所述利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正，具体包括：将所述仿生机器鱼在运动过程中频繁变化的侧滑角确定为外界扰动项，利用非线性扰动观测器对所述侧滑角进行估计及补偿，以实现在所述仿生机器鱼的三维路径跟踪控制过程中对所述内环控制器的控制律进行动态修正。
3.根据权利要求1所述的仿生机器鱼三维路径跟踪控制方法，其特征在于，还包括：预先根据所述仿生机器鱼的当前运动模态确定相应的动力学模型；所述预先根据所述仿生机器鱼的当前运动模态确定相应的动力学模型，具体为：预先根据所述仿生机器鱼的机械结构及运动特征，确定相应的动力学模型。
4.根据权利要求1所述的仿生机器鱼三维路径跟踪控制方法，其特征在于，基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，具体包括：
采用模糊映射将所述内环控制器输出的前向推力、俯仰力矩和偏航力矩分别映射成预设的中枢模式发生器的参数频率和偏置，以及相应的胸鳍偏置角；基于所述参数频率、偏置以及所述胸鳍偏置角，并利用所述中枢模式发生器对所述仿生机器鱼进行节律性控制，以实现对所述仿生机器鱼的速度、航向角和深度进行调整。
5.根据权利要求4所述的仿生机器鱼三维路径跟踪控制方法，其特征在于，所述中枢模式发生器为基于Hopf振荡器的中枢模式发生器。
6.根据权利要求1所述的仿生机器鱼三维路径跟踪控制方法，其特征在于，所述自适应视线导航策略是基于预设障碍函数来确定的自适应视线导航策略。
7.一种仿生机器鱼三维路径跟踪控制装置，其特征在于，包括：
三维路径导航单元，用于基于仿生机器鱼当前运动模态对应的动力学模型、期望路径以及预设的自适应视线导航策略，确定所述仿生机器鱼在当前时刻的期望速度、期望航向角和期望深度；
侧滑角补偿单元，用于利用非线性扰动观测器估计并补偿三维路径跟踪控制过程中的侧滑角，以对基于滑模的内环控制器的控制律进行修正；
三维路径跟踪处理单元，用于基于所述期望速度、所述期望航向角以及所述期望深度，并利用所述内环控制器对所述仿生机器鱼的速度、航向角和深度进行调整，以实现所述仿生机器鱼的三维路径跟踪控制；其中，所述内环控制器包括速度控制器、航向控制器以及深度控制器。
8.根据权利要求7所述的仿生机器鱼三维路径跟踪控制装置，其特征在于，所述侧滑角补偿单元，具体用于：将所述仿生机器鱼在运动过程中频繁变化的侧滑角确定为外界扰动项，利用非线性扰动观测器对所述侧滑角进行估计及补偿，以实现在所述仿生机器鱼的三维路径跟踪控制过程中对所述内环控制器的控制律进行动态修正。
9.一种电子设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任意一项所述仿生机器鱼三维路径跟踪控制方法的步骤。
10.一种处理器可读存储介质，所述处理器可读存储介质上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求1至6任意一项所述仿生机器鱼三维路径跟踪控制方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111501861.4A CN114397899A (zh) | 2021-12-09 | 2021-12-09 | 一种仿生机器鱼三维路径跟踪控制方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111501861.4A CN114397899A (zh) | 2021-12-09 | 2021-12-09 | 一种仿生机器鱼三维路径跟踪控制方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114397899A true CN114397899A (zh) | 2022-04-26 |
Family
ID=81226613
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111501861.4A Pending CN114397899A (zh) | 2021-12-09 | 2021-12-09 | 一种仿生机器鱼三维路径跟踪控制方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114397899A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115268475A (zh) * | 2022-08-09 | 2022-11-01 | 哈尔滨工程大学 | 基于有限时间扰动观测器的机器鱼精确地形跟踪控制方法 |
| CN115303455A (zh) * | 2022-09-16 | 2022-11-08 | 北京大学 | 水下仿生机器人运动控制方法、装置、设备及存储介质 |
| JP7501946B1 (ja) | 2023-02-22 | 2024-06-18 | 大連海事大学 | 水中グライダのベクトル場における経路追跡の制御方法 |
-
2021
- 2021-12-09 CN CN202111501861.4A patent/CN114397899A/zh active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115268475A (zh) * | 2022-08-09 | 2022-11-01 | 哈尔滨工程大学 | 基于有限时间扰动观测器的机器鱼精确地形跟踪控制方法 |
| CN115303455A (zh) * | 2022-09-16 | 2022-11-08 | 北京大学 | 水下仿生机器人运动控制方法、装置、设备及存储介质 |
| JP7501946B1 (ja) | 2023-02-22 | 2024-06-18 | 大連海事大学 | 水中グライダのベクトル場における経路追跡の制御方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Miao et al. | Spatial curvilinear path following control of underactuated AUV with multiple uncertainties | |
| CN110333739B (zh) | 一种基于强化学习的auv行为规划及动作控制方法 | |
| CN107168312B (zh) | 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法 | |
| Li et al. | Robust time-varying formation control for underactuated autonomous underwater vehicles with disturbances under input saturation | |
| CN108427414B (zh) | 一种自主水下航行器水平面自适应轨迹跟踪控制方法 | |
| CN106444806B (zh) | 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法 | |
| CN114397899A (zh) | 一种仿生机器鱼三维路径跟踪控制方法及装置 | |
| CN107024863B (zh) | 一种避免微分爆炸的uuv轨迹跟踪控制方法 | |
| Su et al. | Attitude control of underwater glider combined reinforcement learning with active disturbance rejection control | |
| CN109240091B (zh) | 一种基于强化学习的水下机器人控制方法及其进行跟踪的控制方法 | |
| Yan et al. | Consensus formation tracking for multiple AUV systems using distributed bioinspired sliding mode control | |
| CN112558612B (zh) | 一种基于云模型量子遗传算法的异构智能体编队控制方法 | |
| CN111123923B (zh) | 一种无人船舶局部路径动态优化方法 | |
| CN114115262B (zh) | 基于方位角信息的多auv执行器饱和协同编队控制系统和方法 | |
| Yan et al. | Robust nonlinear model predictive control of a bionic underwater robot with external disturbances | |
| Wang et al. | Path-following optimal control of autonomous underwater vehicle based on deep reinforcement learning | |
| Peng et al. | Research on hover control of AUV uncertain stochastic nonlinear system based on constructive backstepping control strategy | |
| Lv et al. | A collision-free planning and control framework for a biomimetic underwater vehicle in dynamic environments | |
| Hu et al. | Disturbance Observer-Based Model Predictive Control for an Unmanned Underwater Vehicle | |
| Yang et al. | Trajectory tracking with external disturbance of bionic underwater robot based on CPG and robust model predictive control | |
| CN114943168B (zh) | 一种水上浮桥组合方法及系统 | |
| Dang et al. | Identification of hydrodynamic coefficients of a robotic fish using improved extended kalman filter | |
| CN115793639A (zh) | 基于强化学习算法的无人艇复杂路径规划方法及装置 | |
| CN115480580A (zh) | 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 | |
| CN113110512B (zh) | 一种减弱未知干扰与抖振影响的可底栖式auv自适应轨迹跟踪控制方法 |
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 |