技术领域technical field
本发明涉及水下机器人控制技术领域，尤其涉及一种仿生机器海豚的多模态轨迹跟踪控制方法、装置及设备。The invention relates to the technical field of underwater robot control, in particular to a multi-modal trajectory tracking control method, device and equipment for a bionic robot dolphin.
背景技术Background technique
轨迹跟踪控制问题一直以来都是水下机器人的研究热点，其目标是使机器人能够从任一点出发，设计跟踪控制器，在时间约束下逐渐收敛于目标轨迹。轨迹跟踪技术是水下作业的重要组成部分，对于海洋作业的顺利完成有着重要的意义。Trajectory tracking control has always been a research hotspot for underwater robots. Its goal is to enable the robot to start from any point, design a tracking controller, and gradually converge to the target trajectory under time constraints. Trajectory tracking technology is an important part of underwater operations, and it is of great significance to the smooth completion of marine operations.
然而，现有技术中的轨迹跟踪控制大多仅通过仿真开展方法验证，对于轨迹跟踪过程中的避障因素欠缺考虑。However, most of the trajectory tracking control in the prior art is only verified by simulation, and the obstacle avoidance factors in the trajectory tracking process are not considered.
发明内容Contents of the invention
本发明提供一种仿生机器海豚的多模态轨迹跟踪控制方法、装置及设备，用以解决现有技术中轨迹跟踪控制大多仅通过仿真开展了方法验证，对于轨迹跟踪过程中的避障因素欠缺考虑的缺陷。The invention provides a multi-modal trajectory tracking control method, device and equipment for a bionic robot dolphin, which is used to solve the problem that most of the trajectory tracking control in the prior art is only verified through simulation, and the obstacle avoidance factors in the trajectory tracking process are lacking. Consider the flaws.
本发明提供一种仿生机器海豚的多模态轨迹跟踪控制方法，包括：The invention provides a multimodal trajectory tracking control method for a bionic robot dolphin, comprising:
获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹；Obtain the current position, current yaw attitude and target trajectory of the bionic robot dolphin;
基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度；determining a target forward linear velocity and a target yaw angular velocity based on the current position, the current yaw attitude, and the target trajectory;
基于所述仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及所述目标前向线速度和所述目标偏航角速度，得到所述跟踪控制器输出的前向推力和偏航力矩；Based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and the target yaw angular velocity, the forward thrust and yaw output by the tracking controller are obtained torque;
基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，并基于所述控制执行参数，进行仿生机器海豚的轨迹跟踪控制。Based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the yaw moment, determine the control execution parameters of the tracking controller, and based on the control execution parameters, perform a bionic machine Trajectory tracking control for dolphins.
根据本发明提供的一种仿生机器海豚的多模态轨迹跟踪控制方法，所述基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，包括：According to a multi-modal trajectory tracking control method of a bionic robot dolphin provided by the present invention, the determination is based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the yaw moment The control execution parameters of the tracking controller include:
基于所述跟踪控制器的前向推力确定前向推力变化率，以及基于所述偏航力矩确定偏航力矩变化率；determining a rate of change of forward thrust based on the forward thrust of the tracking controller, and determining a rate of change of yaw moment based on the yaw moment;
基于所述偏航力矩，确定所述仿生机器海豚的转向模态；Based on the yaw moment, determining the steering mode of the bionic robot dolphin;
基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力、所述偏航力矩、所述前向推力变化率和所述偏航力矩变化率，确定所述跟踪控制器的控制执行参数。Determine the tracking controller based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller, the yaw moment, the rate of change of the forward thrust, and the rate of change of the yaw moment The control execution parameters.
根据本发明提供的一种仿生机器海豚的多模态轨迹跟踪控制方法，所述基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力、所述偏航力矩、所述前向推力变化率和所述偏航力矩变化率，确定所述跟踪控制器的控制执行参数，包括：According to a multi-modal trajectory tracking control method of a bionic robot dolphin provided by the present invention, the steering mode based on the bionic robot dolphin, the forward thrust output by the tracking controller, the yaw moment, the The rate of change of the forward thrust and the rate of change of the yaw moment determine the control execution parameters of the tracking controller, including:
基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述前向推力变化率，确定所述控制执行参数中的尾鳍摆动频率；Based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the rate of change of the forward thrust, determine the tail fin swing frequency in the control execution parameters;
基于所述仿生机器海豚的转向模态、所述偏航力矩和所述偏航力矩变化率，确定所述控制执行参数中的胸鳍拍动频率。Based on the steering mode of the bionic robot dolphin, the yaw moment and the rate of change of the yaw moment, the pectoral fin flapping frequency among the control execution parameters is determined.
根据本发明提供的一种仿生机器海豚的多模态轨迹跟踪控制方法，所述基于所述偏航力矩，确定所述仿生机器海豚的转向模态，包括：According to a multi-modal trajectory tracking control method of a bionic robot dolphin provided by the present invention, the determination of the steering mode of the bionic robot dolphin based on the yaw moment includes:
基于如下公式，确定所述仿生机器海豚的转向模态M：Based on the following formula, the steering mode M of the bionic robot dolphin is determined:
其中，M1表示第一转向模态，M2表示第二转向模态，M3表示第三转向模态，所述第一转向模态、所述第二转向模态和所述第三转向模态的胸鳍拍动状态不同，g1和g2表示调节权重参数，|τr|表示偏航力矩的绝对值，τrmax表示偏航力矩的最大值。Wherein, M1 represents the first steering mode, M2 represents the second steering mode, M3 represents the third steering mode, the first steering mode, the second steering mode and the third steering mode The flapping states of the pectoral fins are different, g1 and g2 represent the adjustment weight parameters, |τr | represents the absolute value of the yaw moment, and τrmax represents the maximum value of the yaw moment.
根据本发明提供的一种仿生机器海豚的多模态轨迹跟踪控制方法，所述基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度，包括：According to the multi-modal trajectory tracking control method of a bionic robot dolphin provided by the present invention, the target forward linear velocity and target yaw angular velocity are determined based on the current position, the current yaw attitude and the target trajectory ,include:
基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定跟踪误差；determining a tracking error based on the current position, the current yaw attitude, and the target trajectory;
基于所述跟踪误差，确定所述目标前向线速度和所述目标偏航角速度。Based on the tracking error, the target forward linear velocity and the target yaw angular velocity are determined.
根据本发明提供的一种仿生机器海豚的多模态轨迹跟踪控制方法，所述速度控制律τu基于如下公式确定：According to the multimodal trajectory tracking control method of a bionic robot dolphin provided by the present invention, the speed control lawτ is determined based on the following formula:
所述偏航控制律τr基于如下公式确定：The yaw control law τr is determined based on the following formula:
其中，表示gu(u)的倒数，/>表示gr(r)的倒数，/>表示目标前向加速度，/>表示目标偏航角加速度，ue＝u-ud,re＝r-rd，ue表示前向线速度的误差变量，re表示偏航角速度的误差变量，k1和k2均为正系数，/>表示目标前向线速度的估计量，/>表示目标偏航角速度的估计量，M＝diag(m11,m22,m33)表示质量参数矩阵，D＝diag(d11,d22,d33)表示阻尼参数矩阵。in, Indicates the reciprocal of gu (u), /> Indicates the reciprocal of gr (r), /> Indicates the target forward acceleration, /> Indicates the target yaw angular acceleration, ue = uud , re = rrd , ue indicates the error variable of the forward linear velocity, re indicates the error variable of the yaw angular velocity, k1 and k2 are both positive coefficients, /> represents an estimate of the target's forward linear velocity, /> represents the estimated amount of target yaw angular velocity, M=diag(m11 ,m22 ,m33 ) represents the mass parameter matrix, and D=diag(d11 ,d22 ,d33 ) represents the damping parameter matrix.
本发明还提供一种仿生机器海豚的多模态轨迹跟踪控制装置，包括：The present invention also provides a multimodal trajectory tracking control device for a bionic robot dolphin, including:
获取单元，用于获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹；An acquisition unit is used to acquire the current position, current yaw attitude and target trajectory of the bionic robot dolphin;
确定单元，用于基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度；a determining unit, configured to determine a target forward linear velocity and a target yaw angular velocity based on the current position, the current yaw attitude, and the target trajectory;
跟踪控制单元，用于基于所述仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及所述目标前向线速度和所述目标偏航角速度，得到所述跟踪控制器输出的前向推力和偏航力矩；The tracking control unit is used to obtain the output of the tracking controller based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and the target yaw angular velocity forward thrust and yaw moment;
轨迹跟踪控制单元，用于基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，并基于所述控制执行参数，进行仿生机器海豚的轨迹跟踪控制。The trajectory tracking control unit is used to determine the control execution parameters of the tracking controller based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the yaw moment, and based on the Control the execution parameters, and carry out the trajectory tracking control of the bionic robot dolphin.
本发明还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述仿生机器海豚的多模态轨迹跟踪控制方法。The present invention also provides an electronic device, including a memory, a processor, and a computer program stored on the memory and operable on the processor. When the processor executes the program, it realizes the bionic robot dolphin as described above. Multimodal trajectory tracking control method.
本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述仿生机器海豚的多模态轨迹跟踪控制方法。The present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the multi-modal trajectory tracking control method for the bionic robot dolphin described in any one of the above-mentioned methods is realized.
本发明还提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现如上述任一种所述仿生机器海豚的多模态轨迹跟踪控制方法。The present invention also provides a computer program product, including a computer program. When the computer program is executed by a processor, the multi-modal trajectory tracking control method for the bionic robot dolphin described in any one of the above-mentioned methods is realized.
本发明提供的仿生机器海豚的多模态轨迹跟踪控制方法、装置及设备，基于仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及目标前向线速度和目标偏航角速度，得到跟踪控制器输出的前向推力和偏航力矩，再基于仿生机器海豚的转向模态、跟踪控制器输出的前向推力和所述偏航力矩，确定跟踪控制器的控制执行参数，并基于控制执行参数，进行仿生机器海豚的轨迹跟踪控制，此过程充分考虑了避障距离和航向角信息，提高了避障路径的平滑性和安全性，无需通过仿真开展方法验证，并且确定目标前向线速度和目标偏航角速度是基于非线性预测模型的规划器，提高了抗干扰能力，进一步提高了仿生机器海豚的轨迹跟踪控制的精度和准确性。The multi-modal trajectory tracking control method, device and equipment of the bionic robot dolphin provided by the present invention are based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and target yaw angular velocity, Obtain the forward thrust and yaw moment output by the tracking controller, and then determine the control execution parameters of the tracking controller based on the steering mode of the bionic dolphin, the forward thrust output by the tracking controller and the yaw moment, and based on Control the execution parameters, and carry out the trajectory tracking control of the bionic robot dolphin. This process fully considers the obstacle avoidance distance and heading angle information, improves the smoothness and safety of the obstacle avoidance path, does not need to be verified by simulation, and determines the target forward direction. The linear velocity and the target yaw angular velocity are planners based on the nonlinear prediction model, which improves the anti-interference ability and further improves the precision and accuracy of the trajectory tracking control of the bionic robot dolphin.
附图说明Description of drawings
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。In order to more clearly illustrate the present invention or the technical solutions in the prior art, the accompanying drawings that need to be used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the accompanying drawings in the following description are the present invention. For some embodiments of the invention, those skilled in the art can also obtain other drawings based on these drawings without creative effort.
图1是本发明提供的仿生机器海豚的多模态轨迹跟踪控制方法的流程示意图；Fig. 1 is the schematic flow chart of the multimodal track tracking control method of the bionic robot dolphin provided by the present invention;
图2是本发明提供的复杂环境下轨迹跟踪控制的示意图；Fig. 2 is a schematic diagram of trajectory tracking control in a complex environment provided by the present invention;
图3是本发明提供的仿生机器海豚的结构示意图；Fig. 3 is the structural representation of the bionic robot dolphin provided by the present invention;
图4是本发明提供的仿生机器海豚的多模态轨迹跟踪控制装置的结构示意图；Fig. 4 is the structural schematic diagram of the multi-modal trajectory tracking control device of the bionic robot dolphin provided by the present invention;
图5是本发明提供的电子设备的结构示意图。Fig. 5 is a schematic structural diagram of an electronic device provided by the present invention.
具体实施方式Detailed ways
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。In order to make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the present invention. Obviously, the described embodiments are part of the embodiments of the present invention , but not all examples. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without creative efforts fall within the protection scope of the present invention.
本发明的说明书和权利要求书中的术语“第一”、“第二”等是用于区别类似的对象，而不用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施，且“第一”、“第二”等所区分的对象通常为一类。The terms "first", "second" and the like in the description and claims of the present invention are used to distinguish similar objects, and are not used to describe a specific sequence or sequence. It should be understood that the terms so used are interchangeable under appropriate circumstances such that the embodiments of the application can be practiced in sequences other than those illustrated or described herein, and that references to "first," "second," etc. distinguish Objects are usually a class.
相关技术中，随着经济和水下技术的发展，人类愈加渴望探索未知的海洋。近年来，海底勘探、海洋观测、海洋搜索、救援等水下自主作业越来越受到众多科学家和工程师的关注。为了成功高效地完成上述任务，在复杂狭窄水下环境下实现稳定有效的跟踪控制是关键环节。基于螺旋桨驱动的传统自主水下航行器(Autonomous Underwater Vehicle,AUV)具有结构紧凑、控制简单的优点，在水下自主作业中发挥了重要作用。然而，传统AUV存在噪声大、环境友好性差、机动性差的固有缺陷。近年来，在新兴仿生和机器人技术的帮助下，水下仿生机器人应运而生。通过模仿自然生物的外形和运动机制，水下仿生机器人具有高效、高机动和强隐蔽等优点。因此，与传统AUV相比，水下仿生机器人更适合在复杂狭窄的环境中进行自主作业。In related technologies, with the development of economy and underwater technology, human beings are increasingly eager to explore the unknown ocean. In recent years, underwater autonomous operations such as seabed exploration, ocean observation, ocean search, and rescue have attracted more and more attention from many scientists and engineers. In order to successfully and efficiently complete the above tasks, it is the key link to achieve stable and effective tracking control in complex and narrow underwater environments. The traditional autonomous underwater vehicle (AUV) driven by propellers has the advantages of compact structure and simple control, and has played an important role in autonomous underwater operations. However, traditional AUVs have inherent defects such as loud noise, poor environmental friendliness, and poor maneuverability. In recent years, with the help of emerging bionic and robotic technologies, underwater bionic robots have emerged as the times require. By imitating the shape and movement mechanism of natural organisms, underwater bionic robots have the advantages of high efficiency, high maneuverability and strong concealment. Therefore, compared with traditional AUVs, underwater bionic robots are more suitable for autonomous operations in complex and narrow environments.
轨迹跟踪控制问题一直以来都是水下机器人的研究热点，其目标是使机器人能够从任一点出发，设计跟踪控制器，在时间约束下逐渐收敛于目标轨迹。轨迹跟踪技术是水下作业的重要组成部分，对于海洋作业的顺利完成有着重要的意义。Trajectory tracking control has always been a research hotspot for underwater robots. Its goal is to enable the robot to start from any point, design a tracking controller, and gradually converge to the target trajectory under time constraints. Trajectory tracking technology is an important part of underwater operations, and it is of great significance to the smooth completion of marine operations.
现有技术中，有的将轨迹跟踪问题表述为一个凸优化问题，并应用了带有扰动观测器的动态控制器，实现目标跟踪。有的提出了一种基于鲁棒自适应的轨迹跟踪控制方法，该方法结合了鲁棒滑模控制器和自适应律。有的采用无持续激励的偏航动力学来解决轨迹跟踪问题，并采用有限时间不确定性观测器来估计不确定性，实现了轨迹跟踪。In the prior art, some formulate the trajectory tracking problem as a convex optimization problem, and apply a dynamic controller with a disturbance observer to realize target tracking. Some proposed a trajectory tracking control method based on robust adaptation, which combines robust sliding mode controller and adaptive law. Some use yaw dynamics without continuous excitation to solve the problem of trajectory tracking, and use a finite time uncertainty observer to estimate the uncertainty and realize trajectory tracking.
然而，现有技术中的轨迹跟踪控制大多仅通过仿真开展了方法验证，对于轨迹跟踪过程中的避障因素欠缺考虑。However, most of the trajectory tracking control in the prior art is only verified by simulation, and the obstacle avoidance factors in the trajectory tracking process are not considered.
基于上述问题，本发明提供一种仿生机器海豚的多模态轨迹跟踪控制方法，图1是本发明提供的仿生机器海豚的多模态轨迹跟踪控制方法的流程示意图，如图1所示，该方法包括：Based on the above problems, the present invention provides a multimodal trajectory tracking control method for a bionic robot dolphin. Fig. 1 is a schematic flow chart of the multimodal trajectory tracking control method for a bionic robot dolphin provided by the present invention. As shown in Fig. 1, the Methods include:
步骤110，获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹。Step 110, acquiring the current position, current yaw attitude and target trajectory of the bionic robot dolphin.
具体地，考虑到在进行水下作业时，经常遇到错综复杂的狭窄环境，如暗礁丛生、地势凹凸不平或者具有障碍物的水域。仿生机器海豚具有较高的机动性能，凭借其多模态运动形式，能够完成复杂环境下的跟踪作业。Specifically, it is considered that when performing underwater operations, complex and narrow environments are often encountered, such as waters with dense reefs, uneven terrain, or waters with obstacles. The bionic robot dolphin has high maneuverability, and with its multi-modal motion form, it can complete tracking operations in complex environments.
仿生机器海豚是高速游动海洋生物海豚的简化模型，仿生机器海豚内设置有规划器、跟踪控制器和模态分配器，仿生机器海豚主要由腰尾装置和胸鳍装置构成，其中腰尾关节均由电机驱动，两侧胸鳍由舵机驱动。在本发明实施例中，腰尾装置的身体/尾鳍模式(Bodyand/or Caudal Fin，BCF)主要用于提供推力，胸鳍装置的中央鳍/对鳍模式(Medianand/or Paired Fin，MPF)则用来产生偏航力矩。The bionic robot dolphin is a simplified model of a high-speed swimming marine creature dolphin. The bionic robot dolphin is equipped with a planner, a tracking controller and a mode distributor. The bionic robot dolphin is mainly composed of a waist and tail device and a pectoral fin device. Driven by a motor, the pectoral fins on both sides are driven by a steering gear. In the embodiment of the present invention, the body/caudal fin pattern (Bodyand/or Caudal Fin, BCF) of the waist and tail device is mainly used to provide thrust, and the central fin/paired fin model (Medianand/or Paired Fin, MPF) of the pectoral fin device is used to generate the yaw moment.
可以获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹，此处的仿生机器海豚的当前位置是指仿生机器海豚当前所处的位置坐标，仿生机器海豚的当前位置可以用(x,y)表示。当前偏航姿态反映了仿生机器海豚的当前位置偏离目标轨迹的程度，当前偏航姿态可以包括偏航角、平面线速度和偏航角速度，偏航角可以用ψ表示，平面线速度和偏航角速度可以表示为(u,v,r)，其中，u和v表示平面线速度，r表示偏航角速度。The current position, current yaw attitude and target trajectory of the bionic robot dolphin can be obtained. The current position of the bionic robot dolphin here refers to the current position coordinates of the bionic robot dolphin. The current position of the bionic robot dolphin can be used (x,y )express. The current yaw attitude reflects the degree to which the current position of the bionic robot dolphin deviates from the target trajectory. The current yaw attitude can include yaw angle, plane linear velocity and yaw angular velocity. The yaw angle can be expressed by ψ, plane linear velocity and yaw The angular velocity can be expressed as (u,v,r), where u and v represent the plane linear velocity, and r represents the yaw angular velocity.
由此，可以推导出仿生机器海豚的运动学方程如下：From this, the kinematic equation of the bionic robot dolphin can be deduced as follows:
此处的目标轨迹是指仿生机器海豚需要跟踪的目标的轨迹，目标轨迹可以用pd(t)＝(xd(t),yd(t))表示。The target trajectory here refers to the trajectory of the target that the bionic robot dolphin needs to track, and the target trajectory can be represented by pd (t)=(xd (t), yd (t)).
步骤120，基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度。Step 120, based on the current position, the current yaw attitude and the target trajectory, determine the target forward linear velocity and target yaw angular velocity.
具体地，考虑到轨迹跟踪精度和安全避障双层因素，本发明实施例在仿生机器海豚中设置了规划器。在获取到当前位置、当前偏航姿态和目标轨迹之后，可以基于当前位置、当前偏航姿态和目标轨迹，确定目标前向线速度和目标偏航角速度。Specifically, considering the two-layer factors of trajectory tracking accuracy and safe obstacle avoidance, the embodiment of the present invention sets a planner in the bionic robot dolphin. After the current position, current yaw attitude and target trajectory are obtained, the target forward linear velocity and target yaw angular velocity can be determined based on the current position, current yaw attitude and target trajectory.
此处的目标前向线速度是指仿生机器海豚的期望达到的前向线速度，目标前向线速度可以用ud表示，目标偏航角速度是指仿生机器海豚的期望达到的偏航角速度，目标偏航角速度可以用rd表示。The target forward linear velocity here refers to the expected forward linear velocity of the bionic robot dolphin, and the target forward linear velocity can be represented by ud , and the target yaw angular velocity refers to the expected yaw angular velocity of the bionic robot dolphin, The target yaw rate can be represented byrd .
即，规划器的输入可以为当前位置、当前偏航姿态和目标轨迹，规划器的输出可以为目标前向线速度和目标偏航角速度，即规划器的输出为uf＝(ud,rd)。That is, the input of the planner can be the current position, the current yaw attitude and the target trajectory, and the output of the planner can be the target forward linear velocity and the target yaw angular velocity, that is, the output of the planner is uf =(ud ,rd ).
因此，跟踪误差可以表示为pe(t)＝(xe(t),ye(t))＝(xd-x,yd-y)。进一步，考虑到速度规划是一个具有多目标和运动约束的优化问题，本发明实施例设计了一个基于非线性模型预测方法的规划器来完成实时任务，该优化问题可表述为以下最小化代价函数：Therefore, the tracking error can be expressed as pe( t) = (xe (t), ye (t)) = (xd -x, yd -y). Further, considering that velocity planning is an optimization problem with multiple objectives and motion constraints, the embodiment of the present invention designs a planner based on a nonlinear model prediction method to complete real-time tasks. The optimization problem can be expressed as the following minimization of the cost function :
J(pe,Dob,uf,tk)＝J(pe(tk),Dob(tk),uf(tk))J(pe ,Dob ,uf ,tk )＝J(pe (tk ),Dob (tk ),uf (tk ))
L＝pe(τ|tk)TQpe(τ|tk)+Dob(τ|tk)THDob(τ|tk)+uf(τ|tk)TRuf(τ|tk)L＝pe (τ|tk )T Qpe (τ|tk )+Dob (τ|tk )T HDob (τ|tk )+uf (τ|tk )T Ruf ( τ|tk )
g＝ΞTKΞg=ΞT KΞ
Ξ＝(pe(tk+T|tk),Dob(tk+T|tk))Ξ＝(pe (tk +T|tk ),Dob (tk +T|tk ))
uf∈[ufmin,ufmax]uf ∈[ufmin ,ufmax ]
其中，T表示预测周期，控制周期设置为预测周期的一半，L表示轨迹跟踪代价量，g表示终端代价，Q，R，H和K表示正系数矩阵，Dob表示设计的非线性避障项，J(pe,Dob,uf,tk)表示代价函数，是整个待求解优化问题的目标，在不断调整uf值的情况下期望能够使该代价函数值J(pe,Dob,uf,tk)达到最小，pe(tk)表示当前轨迹和期望轨迹间的跟踪误差，uf表示控制输出，是一个二维向量，uf包括了目标前向线速度和目标偏航角速度，tk表示第k个控制时刻，即仿生机器海豚当前准备对控制输出量进行优化计算的时刻。Among them, T represents the prediction period, the control period is set to half of the prediction period, L represents the trajectory tracking cost amount, g represents the terminal cost, Q, R, H and K represent the positive coefficient matrix, and Dob represents the designed nonlinear obstacle avoidance item , J(pe ,Dob ,uf ,tk ) represents the cost function, whichis the goal of the entire optimization problem to be solved. It is expected that the value of the cost function J(pe ,Dob ,uf ,tk ) reaches the minimum,pe (tk ) represents the tracking error between the current trajectory and the desired trajectory, uf represents the control output, which is a two-dimensional vector, uf includes the target forward linear velocity and The target yaw rate, tk represents the kth control moment, that is, the moment when the bionic machine dolphin is currently preparing to optimize the control output.
该项充分考虑了避障距离和航向角信息，提高了避障路径的平滑性和安全性，具体求解过程如下：This item fully considers the obstacle avoidance distance and heading angle information, and improves the smoothness and safety of the obstacle avoidance path. The specific solution process is as follows:
第一步，输入n个障碍坐标In the first step, input n obstacle coordinates
第二步，轮询找到距离最近的障碍The second step, polling to find the nearest obstacle
第三步，得到实时位置与最近障碍的位置矢量和方向矢量/>The third step is to get the real-time position and the position vector of the nearest obstacle and the direction vector />
第四步，计算实时位置与最近障碍的相对方位角dob(t)＝||ξob(t)||。The fourth step is to calculate the relative azimuth between the real-time position and the nearest obstacle dob (t)=||ξob (t)||.
第五步，the fifth step,
d(t)＝c1·dmaxd(t)=c1 ·dmax
else if|ξob(t)||＞dthreshelse if|ξob (t)||＞dthresh
d(t)＝c1·dmaxd(t)=c1 ·dmax
elseelse
第六步，计算最终障碍惩罚项：The sixth step is to calculate the final obstacle penalty:
其中，dthresh和dmax分别表示距离阈值和最大安全距离(均为正常数)，c1和c2分别表示两个权重系数(均为正常数)。Among them,dthresh anddmax respectively represent the distance threshold and the maximum safety distance (both are normal numbers), andc1 andc2 represent two weight coefficients (both are normal numbers).
然后，通过求解上述的优化问题，最优的控制序列可以获得。进一步，通过将控制序列的第一个值应用于仿生机器海豚的规划器，并重复该过程，得到目标前向线速度和目标偏航角速度。Then, by solving the above optimization problem, the optimal control sequence can be obtained. Further, by applying the first value of the control sequence to the planner of the bionic robot dolphin, and repeating the process, the target forward linear velocity and target yaw angular velocity are obtained.
步骤130，基于所述仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及所述目标前向线速度和所述目标偏航角速度，得到所述跟踪控制器输出的前向推力和偏航力矩。Step 130, based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and the target yaw angular velocity, obtain the forward thrust output by the tracking controller and yaw moment.
具体地，在得到目标前向线速度和目标偏航角速度之后，可以基于仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及目标前向线速度和目标偏航角速度，得到跟踪控制器输出的前向推力和偏航力矩。此处的速度控制律用于对仿生机器海豚的速度进行控制，此处的偏航控制律用于对仿生机器海豚的偏航姿态进行控制。此处的跟踪控制器的前向推力是指仿生机器海豚进行跟踪控制所需的前向推力，此处的偏航力矩是指使仿生机器海豚偏航角发生改变的一类力矩的总称。Specifically, after obtaining the target forward linear velocity and target yaw angular velocity, the tracking can be obtained based on the velocity control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and target yaw angular velocity The forward thrust and yaw moment output by the controller. The speed control law here is used to control the speed of the bionic robot dolphin, and the yaw control law here is used to control the yaw attitude of the bionic robot dolphin. The forward thrust of the tracking controller here refers to the forward thrust required by the bionic robot dolphin for tracking control, and the yaw moment here refers to a general term for a class of torque that changes the yaw angle of the bionic robot dolphin.
首先，忽略仿生机器海豚的俯仰和横滚运动，可以得到动力学方程如下：First, ignoring the pitch and roll motion of the bionic robot dolphin, the dynamic equation can be obtained as follows:
其中，M＝diag(m11,m22,m33)表示质量参数矩阵；D＝diag(d11,d22,d33)表示阻尼参数矩阵；diag(·)表示对角矩阵；τu和τr分别表示跟踪控制器的前进推力和偏航力矩；(δu,δv,δr)表示外界干扰量。Among them, M=diag(m11 ,m22 ,m33 ) represents the mass parameter matrix; D=diag(d11 ,d22 ,d33 ) represents the damping parameter matrix; diag(·) represents the diagonal matrix; τu and τr represent the forward thrust and yaw moment of the tracking controller respectively; (δu , δv , δr ) represent the external disturbance.
为了简洁表达，上述动力学在前向速度和偏航角速度维度上的子项可重新整理为：For concise expression, the subitems of the above dynamics in the dimensions of forward velocity and yaw angular velocity can be rearranged as:
图2是本发明提供的复杂环境下轨迹跟踪控制的示意图，如图2所示，进一步，为了降低外界干扰对跟踪控制的影响，本发明实施例施加了一种非线性观测器，通过估计外界干扰，以补偿跟踪控制，观测器形式如下：Figure 2 is a schematic diagram of trajectory tracking control in a complex environment provided by the present invention. Disturbance, to compensate for tracking control, the observer form is as follows:
其中，j＝u,r分别代表前向和偏航子项；表示估计量，本发明假设δv＝0；lj(j)表示观测器增益值，观测器增益值可通过/>所计算得到；/>表示估计误差，估计误差/>在一定条件下能够被证明可以实现渐进收敛，例如条件是：如果lj(j)的选择满足从而可以获得/>其中Bj均为正常数。Among them, j=u, r represent forward and yaw sub-items respectively; Represents the estimator, the present invention assumes that δv =0; lj (j) represents the gain value of the observer, and the gain value of the observer can be obtained by /> Calculated; /> Indicates the estimation error, estimation error /> Under certain conditions, it can be proved that asymptotic convergence can be achieved. For example, the condition is: if the choice of lj (j) satisfies so that you can get /> where Bj are all normal numbers.
最后，跟踪控制器主要以设计李雅普诺夫函数为准则，通过推导反步控制律，同时保证系统的收敛性。首先，定义误差变量ue＝u-ud,re＝r-rd，然后定义李雅普诺夫函数，如下：Finally, the tracking controller mainly takes the design of Lyapunov function as the criterion, and by deriving the backstepping control law, the convergence of the system is guaranteed at the same time. First, define the error variable ue = uud , re = rrd , and then define the Lyapunov function as follows:
通过带入上述速度和偏航动力学，可以得到导数形式：By substituting the above velocity and yaw dynamics, the derivative form can be obtained:
进一步，本发明实施例设计了速度控制律τu和偏航控制律τr，如下：Further, the embodiment of the present invention designs the speed control law τu and the yaw control law τr , as follows:
其中，表示gu(u)的倒数，/>表示gr(r)的倒数，/>表示目标前向加速度，/>表示目标偏航角加速度，ue＝u-ud,re＝r-rd，ue表示前向线速度的误差变量，re表示偏航角速度的误差变量，k1和k2均为正系数，/>表示目标前向线速度的估计量，/>表示目标偏航角速度的估计量，M＝diag(m11,m22,m33)表示质量参数矩阵，D＝diag(d11,d22,d33)表示阻尼参数矩阵。in, Indicates the reciprocal of gu (u), /> Indicates the reciprocal of gr (r), /> Indicates the target forward acceleration, /> Indicates the target yaw angular acceleration, ue = uud , re = rrd , ue indicates the error variable of the forward linear velocity, re indicates the error variable of the yaw angular velocity, k1 and k2 are both positive coefficients, /> represents an estimate of the target's forward linear velocity, /> represents the estimated amount of target yaw angular velocity, M=diag(m11 ,m22 ,m33 ) represents the mass parameter matrix, and D=diag(d11 ,d22 ,d33 ) represents the damping parameter matrix.
因此，基于Young不等式，Therefore, based on Young's inequality,
其中，κ＝min{2k1-1,2k2-1},Among them, κ=min{2k1 -1,2k2 -1},
本发明实施例通过调节参数k1和k2，使得κ＞0，便可以使得跟踪误差实现一致最终有界。In the embodiment of the present invention, by adjusting the parameters k1 and k2 so that κ>0, the tracking error can be uniformly ultimately bounded.
步骤140，基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，并基于所述控制执行参数，进行仿生机器海豚的轨迹跟踪控制。Step 140, based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the yaw moment, determine the control execution parameters of the tracking controller, and based on the control execution parameters, Carry out the trajectory tracking control of the bionic robot dolphin.
具体地，在得到跟踪控制器的前向推力和偏航力矩之后，可以基于仿生机器海豚的转向模态、跟踪控制器输出的前向推力和偏航力矩，确定跟踪控制器的控制执行参数。Specifically, after obtaining the forward thrust and yaw moment of the tracking controller, the control execution parameters of the tracking controller can be determined based on the steering mode of the bionic robot dolphin and the output forward thrust and yaw moment of the tracking controller.
此处的仿生机器海豚的转向模态是指仿生机器海豚的胸鳍拍动状态，仿生机器海豚的转向模态可以基于仿生机器海豚的模态分配器得到，仿生机器海豚的转向模态可以包括三种转向模态M1，M2，M3。Here, the turning mode of the bionic robot dolphin refers to the flapping state of the pectoral fins of the bionic robot dolphin. The turning mode of the bionic robot dolphin can be obtained based on the mode distributor of the bionic robot dolphin. The turning mode of the bionic robot dolphin can include three A steering mode M1, M2, M3.
其中，转向模态M1：仿生机器海豚的单侧胸鳍零位保持，另侧胸鳍拍动。拍动单侧胸鳍可产生前进推力，从而为机体提供偏航力矩。Among them, turning mode M1: the pectoral fin on one side of the bionic robot dolphin maintains zero position, and the pectoral fin on the other side flaps. Flapping a single pectoral fin produces forward thrust, which provides a yaw moment to the airframe.
转向模态M2：仿生机器海豚的单侧胸鳍90°偏置，另侧胸鳍拍动。偏置的胸鳍可在一侧产生阻力，从而增大两侧差动力矩。Steering mode M2: one side of the bionic robot dolphin is biased at 90°, and the other side is flapping. Offset pectoral fins create drag on one side, increasing the differential moment on both sides.
转向模态M3：仿生机器海豚的单侧胸鳍拍动，另侧胸鳍反向拍动，通过两侧拍动产生的差动力矩进行转向。其中，另侧胸鳍反向拍动是指先将胸鳍偏转180°，随后进行拍动。Steering mode M3: The bionic robot dolphin flaps one side of its pectoral fin, and the other side of its pectoral fin flips in the opposite direction, and turns by the differential torque generated by the flapping of both sides. Among them, the reverse flapping of the pectoral fin on the other side refers to first deflecting the pectoral fin by 180°, and then flapping.
显然，上述三种转向模态的机动性排序为：M3>M2>M1，尤其是差动拍动模态基本可实现原地转向。Obviously, the mobility order of the above three steering modes is: M3>M2>M1, especially the differential flapping mode can basically realize the in-situ steering.
此处的跟踪控制器的控制执行参数是指仿生机器海豚用于进行轨迹跟踪控制的参数，控制执行参数可以包括尾鳍摆动频率和胸鳍拍动频率。The control execution parameters of the tracking controller here refer to the parameters used by the bionic robot dolphin for trajectory tracking control, and the control execution parameters may include tail fin swing frequency and pectoral fin beat frequency.
此外，在确定跟踪控制器的控制执行参数时，还可以结合基于跟踪控制器的前向推力确定的前向推力变化率，以及基于偏航力矩确定的偏航力矩变化率。In addition, when determining the control execution parameters of the tracking controller, the rate of change of the forward thrust determined based on the forward thrust of the tracking controller and the rate of change of the yaw moment determined based on the yaw moment can also be combined.
在确定跟踪控制器的控制执行参数之后，可以基于控制执行参数，进行仿生机器海豚的轨迹跟踪控制。即，可以直接将控制执行参数作用于仿生机器海豚，从而进行仿生机器海豚的轨迹跟踪控制。After the control execution parameters of the tracking controller are determined, the trajectory tracking control of the bionic robot dolphin can be performed based on the control execution parameters. That is, the control execution parameters can be directly applied to the bionic robot dolphin, so as to perform trajectory tracking control of the bionic robot dolphin.
本发明实施例提供的方法，基于仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及目标前向线速度和目标偏航角速度，得到跟踪控制器输出的前向推力和偏航力矩，再基于仿生机器海豚的转向模态、跟踪控制器输出的前向推力和所述偏航力矩，确定跟踪控制器的控制执行参数，并基于控制执行参数，进行仿生机器海豚的轨迹跟踪控制，此过程充分考虑了避障距离和航向角信息，提高了避障路径的平滑性和安全性，无需通过仿真开展方法验证，并且确定目标前向线速度和目标偏航角速度是基于非线性预测模型的规划器，提高了抗干扰能力，进一步提高了仿生机器海豚的轨迹跟踪控制的精度和准确性。The method provided by the embodiment of the present invention is based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and target yaw angular velocity, to obtain the forward thrust and yaw output of the tracking controller Then, based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the yaw moment, the control execution parameters of the tracking controller are determined, and the trajectory tracking control of the bionic robot dolphin is performed based on the control execution parameters , this process fully considers the obstacle avoidance distance and heading angle information, improves the smoothness and safety of the obstacle avoidance path, does not need to be verified by simulation, and determines the target forward linear velocity and target yaw angular velocity based on nonlinear prediction The planner of the model improves the anti-interference ability, and further improves the precision and accuracy of the trajectory tracking control of the bionic robot dolphin.
基于上述实施例，步骤140包括：Based on the foregoing embodiments, step 140 includes:
步骤141，基于所述跟踪控制器的前向推力确定前向推力变化率，以及基于所述偏航力矩确定偏航力矩变化率；Step 141, determining the rate of change of forward thrust based on the forward thrust of the tracking controller, and determining the rate of change of yaw moment based on the yaw moment;
步骤142，基于所述偏航力矩，确定所述仿生机器海豚的转向模态；Step 142, based on the yaw moment, determine the steering mode of the bionic robot dolphin;
步骤143，基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力、所述偏航力矩、所述前向推力变化率和所述偏航力矩变化率，确定所述跟踪控制器的控制执行参数。Step 143, based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller, the yaw moment, the rate of change of the forward thrust, and the rate of change of the yaw moment, determine the Tracks the control execution parameters of the controller.
具体地，一方面，通过两侧胸鳍的配合运动，仿生机器海豚能够实现多种偏航模态，需根据不同模态的机动性能进行转向设计。另一方面，关节运动来源于中枢模式发生器产生的节律信号(包括尾鳍背腹式推进和胸鳍推进)，需要将前向推力和偏航力矩映射为控制执行参数。Specifically, on the one hand, through the coordinated movement of the pectoral fins on both sides, the bionic robot dolphin can realize various yaw modes, and the steering design needs to be carried out according to the maneuverability of different modes. On the other hand, articulation originates from the rhythmic signals generated by the central pattern generator (including dorsal fin propulsion and pectoral fin propulsion), which need to map forward thrust and yaw moment as control execution parameters.
在获取到前向推力和偏航力矩之后，考虑到实际应用时，跟踪控制器是利用嵌入式芯片进行周期性计算的，所以根据用户定义的控制周期，在记录上一控制周期的前向推力的情况下，利用当前的前向推力减去上一控制周期的前向推力得到前向推力差值，然后，以前向推力差值除以控制周期得到前向推力变化率。同理，偏航力矩也可以这样获得偏航力矩变化率。即，可以基于跟踪控制器的前向推力确定前向推力变化率，以及基于偏航力矩确定偏航力矩变化率。After obtaining the forward thrust and yaw moment, considering the actual application, the tracking controller uses the embedded chip to perform periodic calculations, so according to the user-defined control period, the forward thrust of the previous control period is recorded In the case of , subtract the forward thrust of the previous control period from the current forward thrust to obtain the forward thrust difference, and then divide the forward thrust difference by the control period to obtain the forward thrust change rate. Similarly, the rate of change of the yaw moment can also be obtained in this way for the yaw moment. That is, the rate of change of forward thrust may be determined based on the forward thrust of the tracking controller, and the rate of change of yaw moment may be determined based on the yaw moment.
然后，可以基于偏航力矩，确定仿生机器海豚的转向模态。Then, based on the yaw moment, the steering mode of the bionic robot dolphin can be determined.
即，可以基于如下公式，确定仿生机器海豚的转向模态M：That is, the steering mode M of the bionic robot dolphin can be determined based on the following formula:
其中，M1表示第一转向模态，M2表示第二转向模态，M3表示第三转向模态，所述第一转向模态、所述第二转向模态和所述第三转向模态的胸鳍拍动状态不同，g1和g2表示调节权重参数，|τr|表示偏航力矩的绝对值，τrmax表示偏航力矩的最大值。Wherein, M1 represents the first steering mode, M2 represents the second steering mode, M3 represents the third steering mode, the first steering mode, the second steering mode and the third steering mode The flapping states of the pectoral fins are different, g1 and g2 represent the adjustment weight parameters, |τr | represents the absolute value of the yaw moment, and τrmax represents the maximum value of the yaw moment.
此处的转向模态M1：仿生机器海豚的单侧胸鳍零位保持，另侧胸鳍拍动。拍动单侧胸鳍可产生前进推力，从而为机体提供偏航力矩。Turning mode M1 here: the pectoral fin on one side of the bionic robot dolphin maintains zero position, and the pectoral fin on the other side flaps. Flapping a single pectoral fin produces forward thrust, which provides a yaw moment to the airframe.
转向模态M2：仿生机器海豚的单侧胸鳍90°偏置，另侧胸鳍拍动。偏置的胸鳍可在一侧产生阻力，从而增大两侧差动力矩。Steering mode M2: one side of the bionic robot dolphin is biased at 90°, and the other side is flapping. Offset pectoral fins create drag on one side, increasing the differential moment on both sides.
转向模态M3：仿生机器海豚的单侧胸鳍拍动，另侧胸鳍反向拍动，通过两侧拍动产生的差动力矩进行转向。其中，另侧胸鳍反向拍动是指先将胸鳍偏转180°，随后进行拍动。本发明实施例中将转向模态M3作为主要模态。Steering mode M3: The bionic robot dolphin flaps one side of its pectoral fin, and the other side of its pectoral fin flips in the opposite direction, and turns by the differential torque generated by the flapping of both sides. Among them, the reverse flapping of the pectoral fin on the other side refers to first deflecting the pectoral fin by 180°, and then flapping. In the embodiment of the present invention, the steering mode M3 is taken as the main mode.
最后，可以先确定仿生机器海豚的转向模态，在确定仿生机器海豚的转向模态后，可以基于跟踪控制器输出的前向推力、偏航力矩、前向推力变化率和偏航力矩变化率，确定跟踪控制器的控制执行参数。Finally, the steering mode of the bionic robot dolphin can be determined first. After determining the steering mode of the bionic robot dolphin, the forward thrust, yaw moment, forward thrust change rate and yaw moment change rate can be based on the tracking controller output. , to determine the control execution parameters of the tracking controller.
此处，可以基于跟踪控制器输出的前向推力、偏航力矩、前向推力变化率和偏航力矩变化率，以及模糊规则表，确定跟踪控制器的控制执行参数。Here, the control execution parameters of the tracking controller can be determined based on the forward thrust, yaw moment, rate of change of forward thrust and yaw moment output by the tracking controller, and the fuzzy rule table.
本发明实施例提供的方法，基于偏航力矩，确定仿生机器海豚的转向模态，再基于仿生机器海豚的转向模态、跟踪控制器输出的前向推力、偏航力矩、前向推力变化率和偏航力矩变化率，确定跟踪控制器的控制执行参数，一方面，可以避免模态频繁切换损坏机械结构；另一方面，通过调节权重参数g1和g2，可以选择主要模态实现偏航运动。The method provided by the embodiment of the present invention determines the steering mode of the bionic robot dolphin based on the yaw moment, and then based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller, the yaw moment, and the rate of change of the forward thrust and the rate of change of yaw moment to determine the control execution parameters of the tracking controller. On the one hand, it can avoid frequent mode switching from damaging the mechanical structure; on the other hand, by adjusting the weight parameters g1 and g2 , the main mode can be selected to achieve sailing movement.
基于上述实施例，步骤143，包括：Based on the above embodiment, step 143 includes:
步骤1431，基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述前向推力变化率，确定所述控制执行参数中的尾鳍摆动频率；Step 1431, based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the rate of change of the forward thrust, determine the tail fin swing frequency in the control execution parameters;
步骤1432，基于所述仿生机器海豚的转向模态、所述偏航力矩和所述偏航力矩变化率，确定所述控制执行参数中的胸鳍拍动频率。Step 1432, based on the steering mode of the bionic robot dolphin, the yaw moment and the rate of change of the yaw moment, determine the flapping frequency of the pectoral fin in the control execution parameters.
具体地，可以先确定仿生机器海豚的转向模态，在确定仿生机器的转向模态后，本发明实施例采用模糊推理的方法进行控制执行参数映射，其主要步骤分为模糊化、模糊规则设计和解模糊化。模糊化旨在确定输入输出变量的基本论域，本发明实施例采用双输入单输出的模糊推理结构，输入包括前进推力(单位：N)、偏航力矩(单位：Nm)及其变化率，输出为两种运动频率(单位：Hz)，模糊规则库的设计是模糊推理算法的关键步骤，主要由数据和模糊语言规则组成。本发明实施例采用“IF-THEN”的模糊语言，模糊规则表如下：Specifically, the steering mode of the bionic machine dolphin can be determined first. After the steering mode of the bionic machine is determined, the embodiment of the present invention uses the method of fuzzy reasoning to map the control execution parameters. The main steps are fuzzification and fuzzy rule design. Reconcile blurring. The purpose of fuzzification is to determine the basic discourse domain of input and output variables. The embodiment of the present invention adopts a fuzzy reasoning structure with double input and single output. The input includes forward thrust (unit: N), yaw moment (unit: Nm) and its rate of change. The output is two kinds of motion frequencies (unit: Hz). The design of the fuzzy rule base is the key step of the fuzzy inference algorithm, which is mainly composed of data and fuzzy language rules. The embodiment of the present invention adopts the fuzzy language of "IF-THEN", and the fuzzy rule table is as follows:
表1.确定胸鳍拍动频率的模糊规则表Table 1. Table of fuzzy rules for determining pectoral fin beating frequency
由表1可知，1)if偏航力矩＝NB and偏航力矩变化率＝NB，then胸鳍拍动频率＝NB；It can be known from Table 1 that 1) if yaw moment = NB and yaw moment change rate = NB, then pectoral fin flapping frequency = NB;
2)if偏航力矩＝NB and偏航力矩变化率＝NM，then胸鳍拍动频率＝NB；2) if yaw moment = NB and yaw moment change rate = NM, then pectoral fin flapping frequency = NB;
3)if偏航力矩＝NB and偏航力矩变化率＝NS，then胸鳍拍动频率＝NM；3) if yaw moment = NB and yaw moment change rate = NS, then pectoral fin flapping frequency = NM;
4)if偏航力矩＝NB and偏航力矩变化率＝ZO，then胸鳍拍动频率＝NM；4) if yaw moment = NB and yaw moment change rate = ZO, then pectoral fin flapping frequency = NM;
5)if偏航力矩＝NB and偏航力矩变化率＝PS，then胸鳍拍动频率＝NS；5) if yaw moment = NB and yaw moment change rate = PS, then pectoral fin flapping frequency = NS;
6)if偏航力矩＝NB and偏航力矩变化率＝PM，then胸鳍拍动频率＝NS；6) if yaw moment = NB and yaw moment change rate = PM, then pectoral fin flapping frequency = NS;
7)if偏航力矩＝NB and偏航力矩变化率＝PB，then胸鳍拍动频率＝ZE，此处不再赘述。7) if yaw moment = NB and rate of change of yaw moment = PB, then pectoral fin beating frequency = ZE, which will not be repeated here.
表1中，偏航力矩的取值有7种情况，分别是NB、NM、NS、ZE、PS、PM和PB，偏航力矩的变化率的取值也有7种情况，分别是NB、NM、NS、ZO、PS、PM和PB，因此，共有7*7＝49条规则。In Table 1, there are 7 situations for the value of the yaw moment, namely NB, NM, NS, ZE, PS, PM and PB, and there are also 7 situations for the value of the change rate of the yaw moment, namely NB, NM , NS, ZO, PS, PM and PB, therefore, there are 7*7=49 rules in total.
此处的NB(negative big)指代“负得较大”，NM(negative middle)指代是“负得中等”，NS(negative small)指代“负得较小”，ZO(Zero)指代“不负也不正”，PS(positivesmall)指代“正得较小”，PM(positive middle)指代“正得中等”，PB(positive big)指代“正得较大”。这里得所谓正和负，以及较大和较小是人们对于数值得不精确或者是模糊描述，通俗意义上可以这样类比理解，譬如，他的身高较高，身高高得一般，身高高得不是很多，身高一般，身高较矮之类。迁移到控制上就是，水龙头开关拧过较多(PB)，拧的不是很过(PS)之类的，此处不再赘述。Here, NB (negative big) refers to "large negative", NM (negative middle) refers to "moderate negative", NS (negative small) refers to "small negative", ZO (Zero) refers to PS (positive small) means "positive small", PM (positive middle) means "positive middle", PB (positive big) means "positive big". The so-called positive and negative here, as well as larger and smaller are people's inaccurate or vague descriptions of numerical values. In a popular sense, it can be understood by analogy. For example, his height is tall, his height is average, and his height is not very high. Average height, short height and so on. Migrating to the control is that the faucet switch has been turned too much (PB), not too much (PS), etc., so I won’t repeat it here.
在由表1输出胸鳍拍动频率的隶属度函数之后，可以对胸鳍拍动频率的隶属度函数进行加权平均，最终解模糊化出最终的控制执行参数中的胸鳍拍动频率。After the membership function of pectoral fin beating frequency is output from Table 1, the membership function of pectoral fin beating frequency can be weighted and averaged, and finally the pectoral fin beating frequency in the final control execution parameters can be defuzzified.
此处的胸鳍拍动频率的隶属度函数可以是三角隶属度函数，也可以是梯形隶属度函数等，本发明实施例对此不作具体限定。The membership degree function of the pectoral fin flapping frequency here may be a triangular membership degree function, or a trapezoidal membership degree function, etc., which is not specifically limited in this embodiment of the present invention.
表2.确定尾鳍摆动频率的模糊规则表Table 2. Table of fuzzy rules for determining the beating frequency of the caudal fin
由表2可知，1)if前向推力＝ZE and前向推力变化率＝NB，then尾鳍摆动频率＝ZE；It can be seen from Table 2 that 1) if forward thrust = ZE and forward thrust change rate = NB, then tail fin swing frequency = ZE;
2)if前向推力＝ZE and前向推力变化率＝NM，then尾鳍摆动频率＝ZE；2) if forward thrust = ZE and forward thrust change rate = NM, then tail fin swing frequency = ZE;
3)if前向推力＝ZE and前向推力变化率＝NS，then尾鳍摆动频率＝ZE；3) if forward thrust = ZE and forward thrust change rate = NS, then tail fin swing frequency = ZE;
4)if前向推力＝ZE and前向推力变化率＝ZE，then尾鳍摆动频率＝ZE；4) if forward thrust = ZE and forward thrust change rate = ZE, then tail fin swing frequency = ZE;
5)if前向推力＝ZE and前向推力变化率＝PS，then尾鳍摆动频率＝PS；5) if forward thrust = ZE and forward thrust change rate = PS, then tail fin swing frequency = PS;
6)if前向推力＝ZE and前向推力变化率＝PM，then尾鳍摆动频率＝PS；6) if forward thrust = ZE and forward thrust change rate = PM, then tail fin swing frequency = PS;
7)if前向推力＝ZE and前向推力变化率＝PB，then尾鳍摆动频率＝PM，此处不再赘述。7) if forward thrust=ZE and forward thrust change rate=PB, then caudal fin swing frequency=PM, which will not be repeated here.
表2中，前向推力的取值有4种情况，分别是ZE、PS、PM和PB，前向推力变化率的取值有7种情况，分别是NB、NM、NS、ZE、PS、PM和PB，因此，共有4*7＝28种情况。In Table 2, there are 4 situations for the value of forward thrust, which are ZE, PS, PM and PB, and there are 7 situations for the value of forward thrust change rate, which are NB, NM, NS, ZE, PS, PM and PB, therefore, have a total of 4*7=28 cases.
在由表2输出尾鳍摆动频率的隶属度函数之后，可以对尾鳍摆动频率的隶属度函数进行加权平均，最终解模糊化出最终的控制执行参数中的尾鳍摆动频率。After the membership function of the tail fin swing frequency is output from Table 2, the membership function of the tail fin swing frequency can be weighted and averaged, and finally the tail fin swing frequency in the final control execution parameters can be defuzzified.
此处的尾鳍摆动频率的隶属度函数可以是三角隶属度函数，也可以是梯形隶属度函数等，本发明实施例对此不作具体限定。Here, the membership function of the tail fin swing frequency may be a triangular membership function, or a trapezoidal membership function, etc., which is not specifically limited in this embodiment of the present invention.
本发明实施例提供的方法，基于仿生机器海豚的转向模态、跟踪控制器输出的前向推力和前向推力变化率，确定控制执行参数中的尾鳍摆动频率，基于仿生机器海豚的转向模态、偏航力矩和偏航力矩变化率，确定控制执行参数中的胸鳍拍动频率，提高了尾鳍摆动频率和胸鳍拍动频率确定的准确性，将尾鳍摆动频率和胸鳍拍动频率用于轨迹跟踪控制，进一步提高了后续轨迹跟踪控制的准确性，可以用于复杂狭窄环境下的轨迹跟踪控制。The method provided by the embodiment of the present invention determines the tail fin swing frequency in the control execution parameters based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the rate of change of the forward thrust, and determines the tail fin swing frequency based on the steering mode of the bionic robot dolphin. , yaw moment and yaw moment change rate, determine the pectoral fin flapping frequency in the control execution parameters, improve the accuracy of determining the tail fin flapping frequency and pectoral fin flapping frequency, and use the tail fin flapping frequency and pectoral fin flapping frequency for trajectory tracking control, which further improves the accuracy of subsequent trajectory tracking control, and can be used for trajectory tracking control in complex and narrow environments.
基于上述实施例，步骤120包括：Based on the foregoing embodiments, step 120 includes:
步骤121，基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定跟踪误差；Step 121, determining a tracking error based on the current position, the current yaw attitude and the target trajectory;
步骤122，基于所述跟踪误差，确定所述目标前向线速度和所述目标偏航角速度。Step 122, based on the tracking error, determine the target forward linear velocity and the target yaw angular velocity.
具体地，仿生机器海豚的当前位置可以用(x,y)表示，当前偏航姿态可以包括偏航角、平面线速度和偏航角速度，偏航角可以用ψ表示，平面线速度和偏航角速度可以表示为(u,v,r)，其中，u和v表示平面线速度，r表示偏航角速度。目标轨迹可以用pd(t)＝(xd(t),yd(t))表示。Specifically, the current position of the bionic robot dolphin can be expressed by (x, y), the current yaw attitude can include yaw angle, plane linear velocity and yaw angular velocity, the yaw angle can be expressed by ψ, the plane linear velocity and yaw The angular velocity can be expressed as (u,v,r), where u and v represent the plane linear velocity, and r represents the yaw angular velocity. The target trajectory can be represented by pd (t)=(xd (t), yd (t)).
则跟踪误差可以表示为pe(t)＝(xe(t),ye(t))＝(xd-x,yd-y)。Then the tracking error can be expressed as pe( t)=(xe (t), ye (t))=(xd -x, yd -y).
将跟踪误差代入最小化代价函数：Substitute the tracking error into the minimized cost function:
J(pe,Dob,uf,tk)＝J(pe(tk),Dob(tk),uf(tk))J(pe ,Dob ,uf ,tk )＝J(pe (tk ),Dob (tk ),uf (tk ))
L＝pe(τ|tk)TQpe(τ|tk)+Dob(τ|tk)THDob(τ|tk)+uf(τ|tk)TRuf(τ|tk)L＝pe (τ|tk )T Qpe (τ|tk )+Dob (τ|tk )T HDob (τ|tk )+uf (τ|tk )T Ruf ( τ|tk )
g＝ΞTKΞg=ΞT KΞ
Ξ＝(pe(tk+T|tk),Dob(tk6T|tk))Ξ＝(pe (tk +T|tk ),Dob (tk 6T|tk ))
uf∈[ufmin,ufmax]uf ∈[ufmin ,ufmax ]
其中，uf包括了目标前向线速度和目标偏航角速度。Among them, uf includes the target forward linear velocity and target yaw angular velocity.
基于上述实施例，所述速度控制律τu基于如下公式确定：Based on the above embodiments, the speed control law τu is determined based on the following formula:
所述偏航控制律τr基于如下公式确定：The yaw control law τr is determined based on the following formula:
其中，表示gu(u)的倒数，/>表示gr(r)的倒数，/>表示目标前向加速度，/>表示目标偏航角加速度，ue＝u-ud,re＝r-rd，ue表示前向线速度的误差变量，re表示偏航角速度的误差变量，k1和k2均为正系数，/>表示目标前向线速度的估计量，/>表示目标偏航角速度的估计量，M＝diag(m11,m22,m33)表示质量参数矩阵，D＝diag(d11,d22,d33)表示阻尼参数矩阵。in, Indicates the reciprocal of gu (u), /> Indicates the reciprocal of gr (r), /> Indicates the target forward acceleration, /> Indicates the target yaw angular acceleration, ue = uud , re = rrd , ue indicates the error variable of the forward linear velocity, re indicates the error variable of the yaw angular velocity, k1 and k2 are both positive coefficients, /> represents an estimate of the target's forward linear velocity, /> represents the estimated amount of target yaw angular velocity, M=diag(m11 ,m22 ,m33 ) represents the mass parameter matrix, and D=diag(d11 ,d22 ,d33 ) represents the damping parameter matrix.
基于上述实施例，本发明提供一种仿生机器海豚的示意图，图3是本发明提供的仿生机器海豚的结构示意图，如图3所示，仿生机器海豚包括规划器、跟踪控制器和模态分配器，考虑轨迹跟踪精度和安全避障双层因素，设计了规划器，规划器中设置了目标函数、优化器和运动学模型，同时以线速度和角速度进行约束。Based on the above-mentioned embodiments, the present invention provides a schematic diagram of a bionic robot dolphin. Fig. 3 is a schematic structural diagram of the bionic robot dolphin provided by the present invention. As shown in Fig. 3, the bionic robot dolphin includes a planner, a tracking controller and a mode allocation Considering the two-layer factors of trajectory tracking accuracy and safe obstacle avoidance, a planner is designed. The objective function, optimizer and kinematics model are set in the planner, and the linear velocity and angular velocity are constrained at the same time.
跟踪控制器中包括了补偿器、控制器和动力学模型，其中，补偿器用于非线性扰动观测，从而使得仿生机器海豚可以用于复杂环境下的跟踪控制。The tracking controller includes a compensator, a controller and a dynamic model. The compensator is used for nonlinear disturbance observation, so that the bionic robot dolphin can be used for tracking control in complex environments.
模态分配器包括模态分配器，在确定转向模态之后，可以确定控制执行参数中的胸鳍拍动频率和尾鳍摆动频率。The mode allocator includes a mode allocator, after the steering mode is determined, the pectoral fin beating frequency and the tail fin beating frequency among the control execution parameters can be determined.
此外，模态分配器还会将状态反馈至跟踪控制器和规划器中，使得跟踪控制器和规划器进行参数更新。In addition, the modal allocator will also feed back the state to the tracking controller and planner, so that the tracking controller and planner can update their parameters.
基于上述任一实施例，一种仿生机器海豚的多模态轨迹跟踪控制方法，步骤如下：Based on any of the above-mentioned embodiments, a multi-modal trajectory tracking control method for a bionic robot dolphin, the steps are as follows:
第一步，获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹。The first step is to obtain the current position, current yaw attitude and target trajectory of the bionic robot dolphin.
第二步，基于当前位置、当前偏航姿态和目标轨迹，确定跟踪误差。基于跟踪误差，确定目标前向线速度和目标偏航角速度。In the second step, the tracking error is determined based on the current position, current yaw attitude, and target trajectory. Based on the tracking error, the target forward linear velocity and target yaw angular velocity are determined.
第三步，基于仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及目标前向线速度和目标偏航角速度，得到跟踪控制器输出的前向推力和偏航力矩。In the third step, based on the speed control law and yaw control law of the tracking controller in the bionic dolphin, as well as the target forward linear velocity and target yaw angular velocity, the forward thrust and yaw moment output by the tracking controller are obtained.
第四步，基于跟踪控制器的前向推力确定前向推力变化率，以及基于偏航力矩确定偏航力矩变化率。In the fourth step, the rate of change of forward thrust is determined based on the forward thrust of the tracking controller, and the rate of change of yaw moment is determined based on the yaw moment.
第五步，基于偏航力矩，确定仿生机器海豚的转向模态。The fifth step is to determine the steering mode of the bionic robot dolphin based on the yaw moment.
第六步，基于仿生机器海豚的转向模态、跟踪控制器输出的前向推力和前向推力变化率，确定控制执行参数中的尾鳍摆动频率；The sixth step is to determine the tail fin swing frequency in the control execution parameters based on the steering mode of the bionic dolphin, the forward thrust output by the tracking controller, and the forward thrust change rate;
第七步，基于仿生机器海豚的转向模态、偏航力矩和偏航力矩变化率，确定控制执行参数中的胸鳍拍动频率。In the seventh step, based on the steering mode, yaw moment and yaw moment change rate of the bionic robot dolphin, the pectoral fin flapping frequency in the control execution parameters is determined.
第八步，基于控制执行参数，进行仿生机器海豚的轨迹跟踪控制。The eighth step is to perform trajectory tracking control of the bionic robot dolphin based on the control execution parameters.
下面对本发明提供的仿生机器海豚的多模态轨迹跟踪控制装置进行描述，下文描述的仿生机器海豚的多模态轨迹跟踪控制装置与上文描述的仿生机器海豚的多模态轨迹跟踪控制方法可相互对应参照。The following describes the multimodal trajectory tracking control device of the bionic robot dolphin provided by the present invention. The multimodal trajectory tracking control device of the bionic robot dolphin described below can be compared with the multimodal trajectory tracking control method of the bionic robot dolphin described above. refer to each other.
基于上述任一实施例，本发明提供一种仿生机器海豚的多模态轨迹跟踪控制装置，图4是本发明提供的仿生机器海豚的多模态轨迹跟踪控制装置的结构示意图，如图4所示，该装置包括：Based on any of the above-mentioned embodiments, the present invention provides a multi-modal trajectory tracking control device for a bionic robot dolphin. FIG. The device includes:
获取单元410，用于获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹；An acquisition unit 410, configured to acquire the current position, current yaw attitude and target trajectory of the bionic robot dolphin;
确定单元420，用于基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度；A determining unit 420, configured to determine a target forward linear velocity and a target yaw angular velocity based on the current position, the current yaw attitude, and the target trajectory;
跟踪控制单元430，用于基于所述仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及所述目标前向线速度和所述目标偏航角速度，得到所述跟踪控制器输出的前向推力和偏航力矩；The tracking control unit 430 is used to obtain the output of the tracking controller based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and the target yaw angular velocity forward thrust and yaw moment;
轨迹跟踪控制单元440，用于基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，并基于所述控制执行参数，进行仿生机器海豚的轨迹跟踪控制。The trajectory tracking control unit 440 is configured to determine the control execution parameters of the tracking controller based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller, and the yaw moment, and based on the According to the above control execution parameters, the trajectory tracking control of the bionic robot dolphin is carried out.
本发明实施例提供的装置，基于仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及目标前向线速度和目标偏航角速度，得到跟踪控制器输出的前向推力和偏航力矩，再基于仿生机器海豚的转向模态、跟踪控制器输出的前向推力和所述偏航力矩，确定跟踪控制器的控制执行参数，并基于控制执行参数，进行仿生机器海豚的轨迹跟踪控制，此过程充分考虑了避障距离和航向角信息，提高了避障路径的平滑性和安全性，无需通过仿真开展方法验证，并且确定目标前向线速度和目标偏航角速度是基于非线性预测模型的规划器，提高了抗干扰能力，进一步提高了仿生机器海豚的轨迹跟踪控制的精度和准确性。The device provided by the embodiment of the present invention is based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, as well as the target forward linear velocity and target yaw angular velocity, to obtain the forward thrust and yaw output of the tracking controller Then, based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the yaw moment, the control execution parameters of the tracking controller are determined, and the trajectory tracking control of the bionic robot dolphin is performed based on the control execution parameters , this process fully considers the obstacle avoidance distance and heading angle information, improves the smoothness and safety of the obstacle avoidance path, does not need to be verified by simulation, and determines the target forward linear velocity and target yaw angular velocity based on nonlinear prediction The planner of the model improves the anti-interference ability, and further improves the precision and accuracy of the trajectory tracking control of the bionic robot dolphin.
基于上述任一实施例，轨迹跟踪控制单元具体用于：Based on any of the above-mentioned embodiments, the trajectory tracking control unit is specifically used for:
确定变化率单元，用于基于所述跟踪控制器的前向推力确定前向推力变化率，以及基于所述偏航力矩确定偏航力矩变化率；determining a rate of change unit configured to determine a rate of change of forward thrust based on the forward thrust of the tracking controller, and determine a rate of change of yaw moment based on the yaw moment;
确定转向模态单元，用于基于所述偏航力矩，确定所述仿生机器海豚的转向模态；Determining a steering mode unit for determining the steering mode of the bionic robot dolphin based on the yaw moment;
确定控制执行参数单元，用于基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力、所述偏航力矩、所述前向推力变化率和所述偏航力矩变化率，确定所述跟踪控制器的控制执行参数。determining the control execution parameter unit, used to base on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller, the yaw moment, the rate of change of the forward thrust and the change of the yaw moment rate, determine the control execution parameters of the tracking controller.
基于上述任一实施例，确定控制执行参数单元具体用于：Based on any of the above embodiments, the unit for determining the control execution parameters is specifically used for:
基于所述仿生机器海豚的转向模态、所述跟踪控制器输出的前向推力和所述前向推力变化率，确定所述控制执行参数中的尾鳍摆动频率；Based on the steering mode of the bionic robot dolphin, the forward thrust output by the tracking controller and the rate of change of the forward thrust, determine the tail fin swing frequency in the control execution parameters;
基于所述仿生机器海豚的转向模态、所述偏航力矩和所述偏航力矩变化率，确定所述控制执行参数中的胸鳍拍动频率。Based on the steering mode of the bionic robot dolphin, the yaw moment and the rate of change of the yaw moment, the pectoral fin flapping frequency among the control execution parameters is determined.
基于上述任一实施例，确定转向模态单元具体用于：Based on any of the above embodiments, it is determined that the steering mode unit is specifically used for:
基于如下公式，确定所述仿生机器海豚的转向模态M：Based on the following formula, the steering mode M of the bionic robot dolphin is determined:
其中，M1表示第一转向模态，M2表示第二转向模态，M3表示第三转向模态，所述第一转向模态、所述第二转向模态和所述第三转向模态的胸鳍拍动状态不同，g1和g2表示调节权重参数，|τr|表示偏航力矩的绝对值，τrmax表示偏航力矩的最大值。Wherein, M1 represents the first steering mode, M2 represents the second steering mode, M3 represents the third steering mode, the first steering mode, the second steering mode and the third steering mode The flapping states of the pectoral fins are different, g1 and g2 represent the adjustment weight parameters, |τr | represents the absolute value of the yaw moment, and τrmax represents the maximum value of the yaw moment.
基于上述任一实施例，确定单元具体用于：Based on any of the above embodiments, the determining unit is specifically configured to:
基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定跟踪误差；determining a tracking error based on the current position, the current yaw attitude, and the target trajectory;
基于所述跟踪误差，确定所述目标前向线速度和所述目标偏航角速度。Based on the tracking error, the target forward linear velocity and the target yaw angular velocity are determined.
基于上述任一实施例，所述速度控制律τu基于如下公式确定：Based on any of the above embodiments, the speed control law τu is determined based on the following formula:
所述偏航控制律τr基于如下公式确定：The yaw control law τr is determined based on the following formula:
其中，表示gu(u)的倒数，/>表示gr(r)的倒数，/>表示目标前向加速度，/>表示目标偏航角加速度，ue＝u-ud,re＝r-rd，ue表示前向线速度的误差变量，re表示偏航角速度的误差变量，k1和k2均为正系数，/>表示目标前向线速度的估计量，/>表示目标偏航角速度的估计量，M＝diag(m11,m22,m33)表示质量参数矩阵，D＝diag(d11,d22,d33)表示阻尼参数矩阵。in, Indicates the reciprocal of gu (u), /> Indicates the reciprocal of gr (r), /> Indicates the target forward acceleration, /> Indicates the target yaw angular acceleration, ue = uud , re = rrd , ue indicates the error variable of the forward linear velocity, re indicates the error variable of the yaw angular velocity, k1 and k2 are both positive coefficients, /> represents an estimate of the target's forward linear velocity, /> represents the estimated amount of target yaw angular velocity, M=diag(m11 ,m22 ,m33 ) represents the mass parameter matrix, and D=diag(d11 ,d22 ,d33 ) represents the damping parameter matrix.
图5示例了一种电子设备的实体结构示意图，如图5所示，该电子设备可以包括：处理器(processor)510、通信接口(Communications Interface)520、存储器(memory)530和通信总线540，其中，处理器510，通信接口520，存储器530通过通信总线540完成相互间的通信。处理器510可以调用存储器530中的逻辑指令，以执行仿生机器海豚的多模态轨迹跟踪控制方法，该方法包括：获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹；基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度；基于所述仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及所述目标前向线速度和所述目标偏航角速度，得到所述跟踪控制器的前向推力和偏航力矩；基于所述仿生机器海豚的转向模态、所述跟踪控制器的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，并基于所述控制执行参数，进行仿生机器海豚的轨迹跟踪控制。FIG. 5 illustrates a schematic diagram of the physical structure of an electronic device. As shown in FIG. 5, the electronic device may include: a processor (processor) 510, a communication interface (Communications Interface) 520, a memory (memory) 530 and a communication bus 540, Wherein, the processor 510 , the communication interface 520 , and the memory 530 communicate with each other through the communication bus 540 . The processor 510 can call the logic instructions in the memory 530 to execute the multi-modal trajectory tracking control method of the bionic robot dolphin. The method includes: obtaining the current position, current yaw attitude and target trajectory of the bionic robot dolphin; position, the current yaw attitude and the target trajectory, determine the target forward linear velocity and target yaw angular velocity; based on the speed control law and yaw control law of the tracking controller in the bionic robot dolphin, and the target Forward linear velocity and the target yaw angular velocity, obtain the forward thrust and yaw moment of the tracking controller; The yaw moment determines the control execution parameters of the tracking controller, and performs the trajectory tracking control of the bionic robot dolphin based on the control execution parameters.
此外，上述的存储器530中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。In addition, the above logic instructions in the memory 530 may be implemented in the form of software function units and be stored in a computer-readable storage medium when sold or used as an independent product. Based on this understanding, the essence of the technical solution of the present invention or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including Several instructions are used to make a computer device (which may be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes. .
另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，计算机程序可存储在非暂态计算机可读存储介质上，所述计算机程序被处理器执行时，计算机能够执行上述各方法所提供的仿生机器海豚的多模态轨迹跟踪控制方法，该方法包括：获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹；基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度；基于所述仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及所述目标前向线速度和所述目标偏航角速度，得到所述跟踪控制器的前向推力和偏航力矩；基于所述仿生机器海豚的转向模态、所述跟踪控制器的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，并基于所述控制执行参数，进行仿生机器海豚的轨迹跟踪控制。On the other hand, the present invention also provides a computer program product. The computer program product includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can Executing the multi-modal trajectory tracking control method of the bionic robot dolphin provided by the above methods, the method includes: obtaining the current position, the current yaw attitude and the target trajectory of the bionic robot dolphin; based on the current position, the current yaw attitude and the target trajectory, determine the target forward linear velocity and target yaw angular velocity; based on the speed control law and yaw control law of the tracking controller in the bionic machine dolphin, and the target forward linear velocity and the target yaw angular velocity, obtain the forward thrust and yaw moment of the tracking controller; The control execution parameters of the controller are tracked, and the trajectory tracking control of the bionic robot dolphin is performed based on the control execution parameters.
又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各方法提供的仿生机器海豚的多模态轨迹跟踪控制方法，该方法包括：获取仿生机器海豚的当前位置、当前偏航姿态和目标轨迹；基于所述当前位置、所述当前偏航姿态和所述目标轨迹，确定目标前向线速度和目标偏航角速度；基于所述仿生机器海豚中跟踪控制器的速度控制律和偏航控制律，以及所述目标前向线速度和所述目标偏航角速度，得到所述跟踪控制器的前向推力和偏航力矩；基于所述仿生机器海豚的转向模态、所述跟踪控制器的前向推力和所述偏航力矩，确定所述跟踪控制器的控制执行参数，并基于所述控制执行参数，进行仿生机器海豚的轨迹跟踪控制。In yet another aspect, the present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, it is implemented to perform the multimodal trajectory of the bionic robot dolphin provided by the above methods A tracking control method, the method comprising: obtaining the current position, the current yaw attitude and the target trajectory of the bionic robot dolphin; determining the forward linear velocity of the target and the target trajectory based on the current position, the current yaw attitude and the target trajectory Yaw angular velocity; based on the speed control law and yaw control law of the tracking controller in the bionic machine dolphin, as well as the target forward linear velocity and the target yaw angular velocity, obtain the forward thrust of the tracking controller and yaw moment; based on the steering mode of the bionic robot dolphin, the forward thrust of the tracking controller and the yaw moment, determine the control execution parameters of the tracking controller, and based on the control execution parameters , to carry out the trajectory tracking control of the bionic robot dolphin.
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。The device embodiments described above are only illustrative, and the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in One place, or it can be distributed to multiple network elements. Part or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. It can be understood and implemented by those skilled in the art without any creative effort.
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。Through the above description of the implementations, those skilled in the art can clearly understand that each implementation can be implemented by means of software plus a necessary general-purpose hardware platform, and of course also by hardware. Based on this understanding, the essence of the above technical solution or the part that contributes to the prior art can be embodied in the form of software products, and the computer software products can be stored in computer-readable storage media, such as ROM/RAM, magnetic discs, optical discs, etc., including several instructions to make a computer device (which may be a personal computer, server, or network device, etc.) execute the methods described in various embodiments or some parts of the embodiments.
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: it can still be Modifications are made to the technical solutions described in the foregoing embodiments, or equivalent replacements are made to some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the various embodiments of the present invention.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310449862.1ACN116520859A (en) | 2023-04-24 | 2023-04-24 | Multi-modal trajectory tracking control method, device and equipment for bionic robot dolphin |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310449862.1ACN116520859A (en) | 2023-04-24 | 2023-04-24 | Multi-modal trajectory tracking control method, device and equipment for bionic robot dolphin |
| Publication Number | Publication Date |
|---|---|
| CN116520859Atrue CN116520859A (en) | 2023-08-01 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310449862.1APendingCN116520859A (en) | 2023-04-24 | 2023-04-24 | Multi-modal trajectory tracking control method, device and equipment for bionic robot dolphin |
| Country | Link |
|---|---|
| CN (1) | CN116520859A (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR3030760A1 (en)* | 2014-12-18 | 2016-06-24 | Airbus Helicopters | METHOD AND SYSTEM FOR DETERMINING A TURNED ANGULAR SPEED FOR A ROTATING WING AIRCRAFT |
| CN111830832A (en)* | 2020-07-27 | 2020-10-27 | 中国科学院自动化研究所 | Plane path tracking method and system for bionic gliding robot dolphin |
| CN111829528A (en)* | 2020-07-27 | 2020-10-27 | 中国科学院自动化研究所 | Real-time path planning method and system for bionic gliding robot dolphin |
| CN115180102A (en)* | 2022-06-27 | 2022-10-14 | 徐州工业职业技术学院 | Bionic robot fish suitable for deepwater area |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR3030760A1 (en)* | 2014-12-18 | 2016-06-24 | Airbus Helicopters | METHOD AND SYSTEM FOR DETERMINING A TURNED ANGULAR SPEED FOR A ROTATING WING AIRCRAFT |
| CN111830832A (en)* | 2020-07-27 | 2020-10-27 | 中国科学院自动化研究所 | Plane path tracking method and system for bionic gliding robot dolphin |
| CN111829528A (en)* | 2020-07-27 | 2020-10-27 | 中国科学院自动化研究所 | Real-time path planning method and system for bionic gliding robot dolphin |
| CN115180102A (en)* | 2022-06-27 | 2022-10-14 | 徐州工业职业技术学院 | Bionic robot fish suitable for deepwater area |
| Publication | Publication Date | Title |
|---|---|---|
| CN112558612B (en) | Heterogeneous intelligent agent formation control method based on cloud model quantum genetic algorithm | |
| CN110333739B (en) | AUV (autonomous Underwater vehicle) behavior planning and action control method based on reinforcement learning | |
| Wu et al. | The autonomous navigation and obstacle avoidance for USVs with ANOA deep reinforcement learning method | |
| Wen et al. | A multi-robot path-planning algorithm for autonomous navigation using meta-reinforcement learning based on transfer learning | |
| CN108873687B (en) | Intelligent underwater robot behavior system planning method based on deep Q learning | |
| Peng et al. | Robust adaptive formation control of underactuated autonomous surface vehicles with uncertain dynamics | |
| Lan et al. | Path planning for underwater gliders in time-varying ocean current using deep reinforcement learning | |
| Gao et al. | Adaptive formation control of autonomous underwater vehicles with model uncertainties | |
| Xia et al. | Research on collision avoidance algorithm of unmanned surface vehicle based on deep reinforcement learning | |
| CN113010963B (en) | Variable-quality underwater vehicle obstacle avoidance method and system based on deep reinforcement learning | |
| CN114397899B (en) | Bionic robot fish three-dimensional path tracking control method and device | |
| Han et al. | Straight-path following and formation control of USVs using distributed deep reinforcement learning and adaptive neural network | |
| Cui et al. | Leader-follower formation control of underactuated auvs with leader position measurement | |
| Taheri et al. | Design boundary layer thickness and switching gain in SMC algorithm for AUV motion control | |
| Blekas et al. | RL-based path planning for an over-actuated floating vehicle under disturbances | |
| Chu et al. | Local 2-D path planning of unmanned underwater vehicles in continuous action space based on the twin-delayed deep deterministic policy gradient | |
| CN115903474A (en) | Unmanned ship automatic berthing control method based on reinforcement learning | |
| Lan et al. | Based on Deep Reinforcement Learning to path planning in uncertain ocean currents for Underwater Gliders | |
| CN116466730A (en) | A Control Method for AUV Docking Based on Event Trigger and Preset Performance | |
| Song et al. | Surface path tracking method of autonomous surface underwater vehicle based on deep reinforcement learning | |
| Li et al. | Obstacle avoidance path planning for AUVs in a three-dimensional unknown environment based on the C-APF-TD3 algorithm | |
| Bo et al. | 3D UAV path planning in unknown environment: A transfer reinforcement learning method based on low-rank adaption | |
| CN118363379A (en) | Unmanned ship dynamic positioning control method based on deep reinforcement learning | |
| CN118295441A (en) | A motion control method for an amphibious hexapod robot based on reinforcement learning | |
| CN116400700A (en) | A construction method and application of an unmanned boat swarm round-up control model |
| 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 |