A 3D path tracking control method and device for a bionic robotic fish

Abstract

The present invention provides a three-dimensional path tracking control method and device for a bionic robotic fish. The method includes: determining the desired speed, desired heading angle, and desired depth of the bionic robotic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic robotic fish, the desired path, and the adaptive line-of-sight navigation strategy; estimating and compensating for the sideslip angle in the three-dimensional path tracking control process by using a nonlinear disturbance observer to correct the control law of the inner-loop controller; adjusting the speed, heading angle, and depth of the bionic robotic fish based on the desired speed, desired heading angle, and desired depth and by using the inner-loop controller to achieve the three-dimensional path tracking control of the bionic robotic fish; the inner-loop controller includes a speed controller, a heading controller, and a depth controller. The method provided by the present invention can adapt to more complex path tracking tasks by compensating for the control process, effectively improving the accuracy and robustness of the path tracking control of the bionic robotic fish.

Description

Technical Field

[0001] The present invention relates to the technical field of artificial intelligence, and particularly relates to a three-dimensional path tracking control method and device for a bionic robotic fish. Additionally, it also relates to an electronic device and a processor-readable storage medium. Background Art

[0002] In recent years, with the continuous development of the underwater robot industry, autonomous underwater vehicles (AUVs) have gradually replaced humans to complete operation tasks in dangerous marine environments and have broad application prospects. At the same time, the complex and changeable control tasks and operation environments also pose great challenges to existing underwater vehicles. Therefore, a large number of research tasks have emerged to make up for the shortcomings of intelligent autonomous operation of underwater vehicles, including autonomous cruising, underwater obstacle avoidance, path planning, and path tracking. For underwater vehicles, path tracking control is a typical motion control task, and high-precision three-dimensional path tracking can lay a good foundation for intelligent operation tasks such as underwater exploration, submarine pipeline and cable detection, and environmental monitoring.

[0003] According to different control objectives, path tracking control is mainly divided into two categories: geometric structure-based path tracking control and control theory-based path tracking control. The geometric structure-based method mainly calculates the tracking target point at each moment based on the relative pose of the vehicle and the desired path using geometric relationships. This method is generally intuitive and easy to implement. The control theory-based method, on the other hand, more describes the path tracking control task starting from the tracking error and controls the vehicle to approach the desired path by reducing the tracking error, making it easier to verify the stability of the control algorithm. In the real marine environment, due to external environmental disturbances, there is often a certain angular deviation between the heading and the resultant velocity direction of the vehicle, which can be measured by the sideslip angle. The existence of the sideslip angle may cause the control law to fail to meet the original control effect, or even lead to control instability. Therefore, in order to improve the control accuracy, it is necessary to consider the influence of the sideslip angle. For multi-joint bionic robotic fish, as a special type of underwater vehicle, it has better performance in terms of motion efficiency and maneuverability. However, the motion mode of the bionic robotic fish requires the multi-joint robotic fish to use the undulating motion of the body to provide forward thrust, making its path tracking task more complex. The resulting large range and frequently changing sideslip angle also make it difficult for traditional path tracking control and sideslip angle compensation schemes to achieve high-precision control. Therefore, how to design a bionic robotic fish three-dimensional path tracking control scheme that can achieve high precision has very important practical significance. Summary of the Invention

[0004] To this end, the present invention provides a three-dimensional path tracking control method and device for a bionic fish, so as to solve the deficiencies in the prior art that the limitations of path tracking control and sideslip angle compensation schemes are relatively high, resulting in poor accuracy and robustness of the three-dimensional path tracking control of the bionic fish.

[0005] In a first aspect, the present invention provides a three-dimensional path tracking control method for a bionic fish, including: determining the desired speed, desired heading angle, and desired depth of the bionic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic fish, the desired path, and a preset adaptive line-of-sight navigation strategy;

[0006] estimating and compensating the sideslip angle during the three-dimensional path tracking control process by using a nonlinear disturbance observer to correct the control law of the inner-loop controller based on sliding mode;

[0007] adjusting the speed, heading angle, and depth of the bionic fish based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller to achieve the three-dimensional path tracking control of the bionic fish; wherein, the inner-loop controller includes a speed controller, a heading controller, and a depth controller.

[0008] Further, the step of estimating and compensating the sideslip angle during the three-dimensional path tracking control process by using a nonlinear disturbance observer to correct the control law of the inner-loop controller based on sliding mode specifically includes: determining the frequently changing sideslip angle of the bionic fish during the movement process as an external disturbance term, and using a nonlinear disturbance observer to estimate and compensate the sideslip angle, so as to dynamically correct the control law of the inner-loop controller during the three-dimensional path tracking control process of the bionic fish.

[0009] Further, the three-dimensional path tracking control method for the bionic fish further includes: determining a corresponding dynamic model in advance according to the current motion mode of the bionic fish; specifically, determining a corresponding dynamic model in advance according to the mechanical structure and motion characteristics of the bionic fish.

[0010] Further, based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller to adjust the speed, heading angle, and depth of the bionic fish, specifically including: using fuzzy mapping to map the forward thrust, pitch moment, and yaw moment output by the inner-loop controller into the parameter frequencies and biases of a preset central pattern generator, and the corresponding pectoral fin bias angles; based on the parameter frequencies, biases, and the pectoral fin bias angles, and using the central pattern generator to perform rhythmic control on the bionic fish, so as to adjust the speed, heading angle, and depth of the bionic fish.

[0011] Further, the central pattern generator is a central pattern generator based on a Hopf oscillator.

[0012] Further, the adaptive line-of-sight navigation strategy is an adaptive line-of-sight navigation strategy determined based on a preset obstacle function.

[0013] In a second aspect, the present invention further provides a three-dimensional path tracking control device for a bionic fish, including: a three-dimensional path navigation unit, configured to determine the desired speed, desired heading angle, and desired depth of the bionic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic fish, the desired path, and a preset adaptive line-of-sight navigation strategy;

[0014] A sideslip angle compensation unit, configured to estimate and compensate the sideslip angle during the three-dimensional path tracking control process by using a nonlinear disturbance observer, so as to correct the control law of the inner-loop controller based on sliding mode;

[0015] A three-dimensional path tracking processing unit, configured to adjust the speed, heading angle, and depth of the bionic fish based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller, so as to achieve the three-dimensional path tracking control of the bionic fish; wherein, the inner-loop controller includes a speed controller, a heading controller, and a depth controller.

[0016] Further, the sideslip angle compensation unit is specifically configured to: determine the frequently changing sideslip angle of the bionic fish during movement as an external disturbance term, and estimate and compensate the sideslip angle by using a nonlinear disturbance observer, so as to dynamically correct the control law of the inner-loop controller during the three-dimensional path tracking control process of the bionic fish.

[0017] Further, the three-dimensional path tracking control device for the bionic fish further includes: a dynamic model determination unit, configured to pre-determine the corresponding dynamic model according to the current motion mode of the bionic fish;

[0018] The dynamic model determination unit is specifically configured to: determine a corresponding dynamic model in advance according to the mechanical structure and motion characteristics of the bionic fish robot.

[0019] Further, the three-dimensional path tracking processing unit is specifically configured to: use fuzzy mapping to map the forward thrust, pitch moment, and yaw moment output by the inner loop controller into the parameter frequencies and biases of a preset central pattern generator, and the corresponding pectoral fin bias angles respectively; based on the parameter frequencies, biases, and the pectoral fin bias angles, and use the central pattern generator to perform rhythmic control on the bionic fish robot to adjust the speed, heading angle, and depth of the bionic fish robot.

[0020] Further, the central pattern generator is a central pattern generator based on a Hopf oscillator.

[0021] Further, the adaptive line-of-sight navigation strategy is an adaptive line-of-sight navigation strategy determined based on a preset obstacle function.

[0022] In a third aspect, the present invention further provides an electronic device, including: a memory, a processor, and a computer program stored on the memory and executable on the processor, where when the processor executes the computer program, the steps of the bionic fish robot three-dimensional path tracking control method as described in any one of the above are implemented.

[0023] In a fourth aspect, the present invention further provides a processor-readable storage medium, on which a computer program is stored, and when the computer program is executed by the processor, the steps of the bionic fish robot three-dimensional path tracking control method as described in any one of the above are implemented.

[0024] The bionic fish robot three-dimensional path tracking control method provided by the present invention obtains corresponding expected values through a dynamic model, an expected path, and an adaptive line-of-sight navigation strategy, and uses a nonlinear disturbance observer to estimate and compensate the sideslip angle in the three-dimensional path tracking control process to compensate the control process, thereby improving the three-dimensional path tracking control accuracy of the bionic fish robot, being able to adapt to more complex path tracking tasks, effectively improving the stability and robustness of the bionic fish robot path tracking control, and thus enhancing the control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0026] Figure 1 It is a schematic flow chart of the three-dimensional path tracking control method for the bionic robotic fish provided by the embodiment of the present invention;

[0027] Figure 2 It is a schematic diagram of the framework of the three-dimensional path tracking system for the bionic robotic fish provided by the embodiment of the present invention;

[0028] Figure 3 It is a schematic diagram of the system coordinate system of the bionic robotic fish provided by the embodiment of the present invention;

[0029] Figure 4 It is a schematic diagram of the three-dimensional path tracking coordinate system of the bionic robotic fish provided by the embodiment of the present invention;

[0030] Figure 5 It is a schematic diagram of the electromechanical structure of the bionic robotic fish provided by the embodiment of the present invention;

[0031] Figure 6 It is a schematic diagram of the structure of the three-dimensional path tracking control device for the bionic robotic fish provided by the embodiment of the present invention;

[0032] Figure 7 It is a schematic diagram of the physical structure of the electronic device provided by the embodiment of the present invention. Detailed implementation manners

[0033] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0034] Based on the three-dimensional path tracking control method for the bionic robotic fish described in the present invention, its embodiments will be described in detail below. As Figure 1 shown, it is a schematic flow chart of the three-dimensional path tracking control method for the bionic robotic fish provided by the embodiment of the present invention, and the specific implementation process includes the following steps:

[0035] Step 101: Based on the dynamic model corresponding to the current motion mode of the bionic robotic fish, the desired path, and the preset adaptive line-of-sight navigation strategy, determine the desired speed, desired heading angle, and desired depth of the bionic robotic fish at the current moment.

[0036] In an embodiment of the present invention, before performing this step, it is necessary to pre-determine a corresponding dynamic model according to the current motion mode of the bionic fish. Specifically: the corresponding dynamic model can be determined according to the mechanical structure and motion characteristics of the bionic fish. Then, based on the dynamic model, the desired path, and the adaptive line-of-sight navigation strategy, the desired speed, desired heading angle, and desired depth of the bionic fish at the current moment are determined. The adaptive line-of-sight navigation strategy is an adaptive line-of-sight navigation strategy determined based on a preset obstacle function.

[0037] Among them, the bionic fish is a multi-joint bionic fish, specifically as Figure 5 shown, which is a schematic diagram of the appearance and electromechanical structure of the multi-joint bionic fish. The hard bionic fish shell is waterproofed by wrapping with latex skin; four tail servo motors are connected in series to provide the main propulsion force for the bionic fish; the pectoral fin servo motor realizes the up and down diving of the bionic fish by changing the fin surface offset angle. According to the motion mode of the multi-joint bionic fish, a model coordinate system is built to determine the corresponding dynamic model. The model coordinate system is specifically as Figure 3 shown, {C w} and {C0} respectively represent the global inertial coordinate system and the fish body coordinate system; where the x-axis direction of the fish body coordinate system represents the positive direction of the fish head; {C G} and {C B} respectively represent the gravity coordinate system with the center of gravity G as the origin and the buoyancy coordinate system with the center of buoyancy B as the origin. Each movable joint of the multi-joint bionic fish can be regarded as a series of connecting rods, and a link coordinate system {C i} is established with the starting point of the link B i as the origin, where i ∈ [1, n] represents the i-th link, and n = 4 represents the total number of links. The length of the link and the joint angle of the corresponding joint can be represented by l i and θ i respectively. The pectoral fin coordinate systems symmetric on both sides can be described by {C l} and {C r} respectively. The three-dimensional path tracking control method of the bionic fish described in the present invention is specifically a method for controlling the motion target of the bionic fish. As Figure 2 shown is a schematic diagram of the three-dimensional path tracking system framework of the bionic fish in the present invention.

[0038] In an embodiment of the present invention, the control method specifically includes the following steps: pre-build a complete dynamic model by analyzing the motion mode of the multi-joint bionic fish.

[0039] Specifically, according to the coordinate system shown in Figure 3 , a dynamic model of the multi-joint bionic fish is established in the fish body coordinate system {C0} and described by the Newton-Euler equation:

[0040]

[0041]

[0042]

[0043] 0 T G,B = - 0 H G G + 0 H B B

[0044]

[0045]

[0046]

[0047] In the formula, M ∑ includes the inertia matrix and additional inertia matrix of the multi-link and pectoral fins; V0 represents the six-dimensional generalized velocity vector; represents the derivative of the velocity vector, that is, the acceleration vector; the lift and drag forces on the fin surfaces of the left and right pectoral fins 0 T l and 0 T r provide an active pitching moment for the bionic robotic fish, and when the bionic robotic fish leaves the horizontal attitude 0 T G,B is used to provide a passive restoring moment; f dr,i is the drag force generated by the surrounding fluid on the bionic robotic fish; and 0 E ∑ represents the Coriolis force of each part of the link; 0 T b and 0 T p represent the interaction forces between links related to acceleration. At the same time, η j , η l and η r respectively represent and the calculation results; 0 T d represents the external disturbance, and Z = [0 1×5 , 1] T represents the six-dimensional unit vector. Among them, and etc. are intermediate terms in the process of constructing the dynamic model. Based on the results of the pre-constructed dynamic model, the three-dimensional path tracking control of the bionic robotic fish can be realized in this application.

[0048] It should be noted that, according to the coordinate system established as shown in Figure 3 , the forces or transformation relationships in the coordinate system can be simply represented by upper and lower subscripts. For example, the force based on coordinate system A can be simply represented as A T, and the transformation matrix from coordinate system C to coordinate system D can be simply represented as D H C , and so on. Among them, coordinate systems A, C, and D can respectively refer to coordinate systems {C w}, {C0}, {C i}, {C G}, {C B}, {C l}, {C r}, etc., and specific descriptions are not given here.

[0049] By establishing a complete dynamic model, the motion mode of the bionic robotic fish imitating fish can be better simulated in the simulation environment. In the specific control task of the present invention, in order to achieve better robustness of the control method based on the dynamic model, the above-mentioned complete dynamic model is further simplified. Considering the non-holonomic constraints during the motion of the bionic robotic fish, by introducing the control force τ = [τ u , τ q , τ r T and the corresponding velocity V = [u, q, r] T , the following affine simplified dynamic model is obtained:

[0050]

[0051] In the formula, three main dimensions are selected from the dynamic model to control the three-dimensional motion of the bionic robotic fish, that is, τ u , τ q and τ r respectively represent the forward thrust, pitch moment, and yaw moment, and respectively correspond to the forward velocity u, pitch angular velocity q, and yaw angular velocity r.

[0052] M = diag{m 11 , m 55 , m 66} represents the inertia matrix and the additional inertia matrix, D(V) = diag{d 11 , d 55 , d 66} represents the hydrodynamic damping term, g(η) = [0, (z g G - z b B)sinθ, 0] T represents the gravity and buoyancy terms, θ represents the pitch angle, z g and z b ​Corresponding to the z - coordinates of the center of gravity and the center of buoyancy in the {C0} coordinate system, G and B represent gravity and buoyancy respectively, and the Coriolis force C(V) can be expressed as follows:

[0053]

[0054] In the formula, v, w, and p represent the surge velocity, heave velocity, and roll angular velocity respectively, and the corresponding inertia matrix and added - inertia matrix coefficients are expressed as m 22 , m 33 and m 44 , and after calculation and simplification, the final dynamic model is:

[0055]

[0056]

[0057] [[ID=2)3]]

[0058] It should be noted that the six - dimensional detailed dynamic model described by the Newton - Euler equation is simplified to an affine model based on six - dimensional generalized control forces, and three main control dimensions are selected. Using τ = [τ u , τ q , τ r T to achieve the three - dimensional motion control of the bionic robot fish. The above - mentioned velocity V = [u, q, r] T with dots on the variables respectively representing the derivatives of the velocity with respect to time, that is, the three - dimensional acceleration vector.

[0059] In the specific implementation process of this step, the desired velocity, desired heading angle, and desired depth of the bionic robot fish at the current moment can be selected through the dynamic model, the desired path, and the adaptive line - of - sight navigation strategy based on the obstacle function.

[0060] Specifically, according to Figure 4 in the coordinate system shown, the current position and velocity of the bionic robot fish are represented by P = [x, y, z] T and respectively, and the desired path is described in parametric form as By traversing the entire desired path with the scalar and thus establishing the SF (Serret - Frenet) coordinate system {C SF}, the resultant velocity of the path points is represented as U p , and the corresponding velocity vector is expressed as:

[0061]

[0062] Specifically, R p is used to represent the transformation from {C​SF} to {C w} rotation matrix. According to the relative positions of the bionic robotic fish and the path points, the error vector is defined in the SF coordinate system as:

[0063]

[0064] In the formula, x e , y e and z e respectively represent the cross-track error, the lateral error, and the vertical error. Select the vector γ = [y e , z e T , which determines the tracking error, and construct the Control Lyapunov Function (CLF):

[0065]

[0066] According to the geometric relationship described by the path tracking coordinate system, the corresponding derivatives are calculated as:

[0067]

[0068] In the formula, U represents the combined velocity of the bionic robotic fish, and χ r = χ - χ p and υ r = υ - υ p respectively represent the angular deviation between the bionic robotic fish and the path points. χ and υ represent the azimuth angle and the elevation angle of the robotic fish, and χ p and υ p represent the azimuth angle and the elevation angle of the path points. According to the geometric relationship and based on the line-of-sight navigation method, select the line-of-sight point P los as the current target point of the bionic robotic fish, and thus design the control law as:

[0069]

[0070]

[0071] If the forward viewing distance of the line-of-sight navigation method is selected as d l , at this time, the navigation coefficients Δ y and Δ z can be expressed as:

[0072]

[0073]

[0074] The present invention introduces the exponential convergence rate κ, and proves the uniform exponential convergence of the tracking error through the derivative of the Lyapunov function: ​

[0075]

[0076] It should be noted that although the cross-track error x e is usually not a key factor in measuring the tracking error, its convergence is a prerequisite for ensuring consistent and stable tracking. For a multi-joint bionic robotic fish, the limited internal space makes it impossible to carry high-precision sensors. At the same time, due to the inevitable frequent reciprocating swing, it is often unable to obtain an accurate speed estimate during the movement. This estimation error or the error compensation with time delay greatly affects the convergence of the cross-track error in traditional navigation strategies, resulting in the uncertainty of path tracking control. In addition, the present invention designs an adaptive line-of-sight navigation strategy based on the log (logarithmic) type barrier Lyapunov function (i.e., the barrier function) for the motion mode of the multi-joint bionic robotic fish and the speed estimation error during the movement of the bionic robotic fish. While ensuring the convergence of the cross-track error during the tracking process, it adaptively adjusts the forward-looking distance of the tracking, enabling the bionic robotic fish to smoothly handle more complex three-dimensional paths.

[0077] The adaptive line-of-sight navigation strategy described in the present invention can constrain the cross-track error x e to an ideal range while ensuring convergence. First, introduce the Barrier Lyapunov Function (BLF):

[0078]

[0079] In the formula, k b represents the boundary coefficient. When k b converges to infinity, the BLF is the traditional navigation strategy. By setting the range of k b , when the cross-track error is about to violate the limit, the BLF can dynamically adjust the control variable according to the ideal constraint range. At this time, calculate the derivative of the BLF:

[0080]

[0081] Determine the path point speed U p as:

[0082]

[0083] At this time, the derivative of the BLF can be simplified to:

[0084]

[0085] Therefore, the adaptive line-of-sight navigation strategy based on the barrier function in the present invention can ensure the convergence of the error while constraining the cross-track error, and calculate the iteration rate of the corresponding path point as:

[0086]

[0087] In order to adapt to a more complex and changeable desired path, the present invention divides the tracking process into two parts and sets an adaptive look-ahead distance respectively. When the bionic fish is in the initial position, the position relative to the desired path is relatively far, and the bionic fish is in the "approaching state". At this time, the main control task is to ensure that the bionic fish approaches the desired path quickly while ensuring a smooth transition of the attitude. By introducing the sigmoid function to describe the look-ahead distance d l and the tracking error x e relationship:

[0088]

[0089] where d lmin is the lower limit of the look-ahead distance, and at the same time d l0 is the preset distance when x e = 0.

[0090] When the bionic fish reaches the desired path, the tracking error is less than a certain range ||x e || < δ, and the bionic fish enters the "tracking state". At this time, the main control task is to make the tracking error converge to 0. According to the path curvature K ∈ [0, K max at this time, set d l as:

[0091]

[0092] Finally, the adaptive line-of-sight navigation strategy determined by the present invention is:

[0093]

[0094] Step 102: Use a nonlinear disturbance observer to estimate and compensate the sideslip angle in the three-dimensional path tracking control process, so as to correct the control law of the inner-loop controller based on sliding mode.

[0095] Determine the sideslip angle that frequently changes during the movement of the bionic fish as an external disturbance term, and use a nonlinear disturbance observer (Nonlinear Disturbance Observer, NDOB) to estimate and compensate the sideslip angle, so as to dynamically correct the control law of the inner-loop controller during the three-dimensional path tracking control process of the bionic fish.

[0096] In the embodiment of the present invention, the control method specifically further includes: using a nonlinear disturbance observer to estimate and compensate the sideslip angle in the control process, thereby reducing the influence of the frequently reciprocating sideslip angle on the path tracking control.

[0097] For the lateral error, simplify the derivative of the Lyapunov function in the adaptive line-of-sight navigation strategy:

[0098]

[0099] In the formula, ψ = χ - β represents the yaw angle, and β represents the sideslip angle. Assuming that the range of the sideslip angle satisfies |β| < 10°, further simplification gives:

[0100]

[0101] In the formula, f(ye) = Usin(ψ - χ p )cos(υ r ), and g(ye) = Ucos(ψ - χ p )cos(υ r ). It should be noted that in the present invention, the sideslip angle β is not obtained by β = arcsin(v, U) using the speed estimation method, but the sideslip angle β is regarded as an external disturbance term and compensated based on NDOB. Set as the estimation of β, and define the estimation error as By determining the gain l(y e ) of the nonlinear disturbance observer, to ensure the uniform exponential convergence of the nonlinear disturbance observer:

[0102]

[0103] Finally, the form of NDOB can be expressed as:

[0104]

[0105] In the formula, ι represents an intermediate scalar, and the non-zero sideslip angle is canceled by the estimation of β.

[0106] By determining the frequently changing sideslip angle during the movement of the bionic robotic fish as an external disturbance term and estimating it using a nonlinear disturbance observer, the dynamic compensation of the sideslip angle is realized, and at the same time, the problem of incorrect compensation caused by insufficient speed estimation accuracy in the traditional sideslip angle tracking and compensation mechanism is effectively avoided, thereby further improving the control accuracy

[0107] Step 103: Based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller to adjust the speed, heading angle, and depth of the bionic robotic fish to achieve the three-dimensional path tracking control of the bionic robotic fish; wherein, the inner-loop controller includes a speed controller, a heading controller, and a depth controller.

[0108] In the embodiments of the present invention, fuzzy mapping is used to map the forward thrust, pitch moment, and yaw moment output by the inner-loop controller into the parameter frequency and bias of a preset central pattern generator, and the corresponding pectoral fin bias angle respectively; based on the parameter frequency, bias, and the pectoral fin bias angle, the central pattern generator is used to rhythmically control the bionic fish, so as to adjust the speed, heading angle, and depth of the bionic fish. The central pattern generator is a central pattern generator based on a Hopf oscillator.

[0109] Specifically, the control method further includes: based on the desired speed, the desired heading angle, and the desired depth, a sliding-mode-based inner-loop controller is used to achieve speed, heading angle, and depth control, and finally achieve three-dimensional path tracking control of the multi-joint bionic fish.

[0110] Aiming at the non-linearity and strong coupling of the dynamic model corresponding to the multi-joint bionic fish, the present invention adopts global fast terminal sliding mode control (GFTSMC) based on the dynamic model to design a heading controller and a depth controller.

[0111] Select the heading control sliding surface s ψ1 as:

[0112]

[0113] In the formula, s ψ0 = ψ d - ψ, α ψ and β ψ are positive coefficients, q ψ0 and p ψ0 are positive integers. Combining the above simplified dynamic model, the derivative of the sliding surface is deduced as:

[0114]

[0115] By introducing a terminal attractor, the chattering generated by the sliding mode control can be greatly reduced. The reaching law is designed as:

[0116]

[0117] In the formula, φ ψ and γ ψ are positive coefficients, q ψ1 and p ψ1 are positive integers. The final heading control law is obtained as:

[0118]

[0119] In view of the under-actuated motion mode of the bionic robotic fish, the present invention sets the desired depth z d Converted to the desired pitch angle θ d , by controlling the pitch angle to achieve the final depth control, the line of sight navigation method based on the line of sight circle (Line Of Sight, LOS) is used to design the conversion strategy as follows:

[0120] θ d =arcsin(-(z d -z) / R)

[0121] Where R is the radius of the sight circle in the sight navigation method. θ0 =θ d -θ, the final pitch control law can be obtained by the same logic:

[0122]

[0123] Where, α θ , β θ 、φ θ and γ θ is a positive coefficient; q θ0 、p θ0 ,q θ1 and p θ1 is a positive integer. Since the relative order of speed control is different from that of heading and depth control, the present invention adopts super-twisting sliding mode control (STSMC) to design the speed controller. The high-order integral term can greatly reduce the influence of chattering, and introduces the speed sliding mode surface s u =u d -u controls the forward speed u, and the speed control law at this time can be expressed as:

[0124] τ u =λ|s u | σ sign(s u )+τ I

[0125]

[0126] Where σ represents the exponential adjustment coefficient, sign(·) represents the sign function, and τ I represents the integral term, while λ and W are positive gain coefficients.

[0127] To provide a stable and continuous control law, in the embodiments of the present invention, a Central Pattern Generator (CPG) based on a Hopf oscillator is introduced to achieve the rhythmic control of a multi-joint bionic robotic fish. At the same time, a fuzzy mapping is used to map the τ u , τ r and τ q output by the sliding mode control law into the CPG parameters frequency ω, bias b, and the pectoral fin bias angle θ p respectively. Therefore, the inner-loop controller designed in the present invention can stably and smoothly track the desired speed, desired heading, and desired depth output by the navigation strategy, and finally achieve the three-dimensional path tracking control of the multi-joint bionic robotic fish. The central pattern generator is a biological neural circuit that generates the rhythmic movement behavior of animals. It consists of a series of neural oscillators and is a complex distributed neural network formed by integrating neural oscillators and multiple reflex loop systems. It should be noted that the same parameters appearing above in the present invention have the same meanings and will not be repeated.

[0128] The three-dimensional path tracking control method for the bionic robotic fish according to the embodiments of the present invention obtains corresponding expected values through a dynamic model, an expected path, and an adaptive line-of-sight navigation strategy, and uses a non-linear disturbance observer to estimate and compensate the sideslip angle in the three-dimensional path tracking control process to compensate for the control process, thereby improving the three-dimensional path tracking control accuracy of the bionic robotic fish, being able to adapt to more complex path tracking tasks, effectively improving the stability and robustness of the path tracking control of the bionic robotic fish, and thus enhancing the control effect.

[0129] Corresponding to the above-provided three-dimensional path tracking control method for a bionic robotic fish, the present invention also provides a three-dimensional path tracking control device for a bionic robotic fish. Since the embodiments of this device are similar to the above method embodiments, the description is relatively simple. For related parts, please refer to the description in the above method embodiment section. The following description of the embodiments of the three-dimensional path tracking control device for a bionic robotic fish is only illustrative. Please refer to Figure 6 as shown, which is a schematic structural diagram of a three-dimensional path tracking control device for a bionic robotic fish provided by the embodiments of the present invention.

[0130] The three-dimensional path tracking control device for the bionic robotic fish described in the present invention specifically includes:

[0131] A three-dimensional path navigation unit 601, configured to determine the desired speed, desired heading angle, and desired depth of the bionic robotic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic robotic fish, the desired path, and a preset adaptive line-of-sight navigation strategy;

[0132] The sideslip angle compensation unit 602 is used to estimate and compensate the sideslip angle during the three-dimensional path tracking control by using a non-linear disturbance observer, so as to correct the control law of the inner-loop controller based on sliding mode;

[0133] The three-dimensional path tracking processing unit 603 is used to adjust the speed, heading angle and depth of the bionic fish based on the desired speed, the desired heading angle and the desired depth, and by using the inner-loop controller, so as to achieve the three-dimensional path tracking control of the bionic fish; wherein, the inner-loop controller includes a speed controller, a heading controller and a depth controller.

[0134] Further, the sideslip angle compensation unit is specifically used for: determining the frequently changing sideslip angle of the bionic fish during movement as an external disturbance term, and estimating and compensating the sideslip angle by using a non-linear disturbance observer, so as to dynamically correct the control law of the inner-loop controller during the three-dimensional path tracking control of the bionic fish.

[0135] Further, the three-dimensional path tracking control device of the bionic fish further includes: a dynamic model determination unit, which is used to pre-determine a corresponding dynamic model according to the current motion mode of the bionic fish;

[0136] The dynamic model determination unit is specifically used for: pre-determining a corresponding dynamic model according to the mechanical structure and motion characteristics of the bionic fish.

[0137] Further, the three-dimensional path tracking processing unit is specifically used for: using fuzzy mapping to map the forward thrust, pitch moment and yaw moment output by the inner-loop controller into the parameter frequency and bias of a preset central pattern generator, and the corresponding pectoral fin bias angle respectively; and performing rhythmic control on the bionic fish based on the parameter frequency, bias and the pectoral fin bias angle, and by using the central pattern generator, so as to adjust the speed, heading angle and depth of the bionic fish.

[0138] Further, the central pattern generator is a central pattern generator based on a Hopf oscillator.

[0139] Further, the adaptive line-of-sight navigation strategy is an adaptive line-of-sight navigation strategy determined based on a preset obstacle function.

[0140] The three-dimensional path tracking control device of the bionic robotic fish according to the embodiments of the present invention obtains corresponding expected values through a dynamic model, an expected path, and an adaptive line-of-sight navigation strategy, and uses a non-linear disturbance observer to estimate and compensate for the sideslip angle in the three-dimensional path tracking control process, so as to compensate for the control process, and further improve the three-dimensional path tracking control accuracy of the bionic robotic fish, be able to adapt to more complex path tracking tasks, effectively improve the stability and robustness of the path tracking control of the bionic robotic fish, and thus improve the control effect.

[0141] Corresponding to the above-provided three-dimensional path tracking control method of the bionic robotic fish, the present invention also provides an electronic device. Since the embodiments of this electronic device are similar to the above method embodiments, the description is relatively simple. For the relevant parts, please refer to the description in the above method embodiment section. The electronic device described below is only illustrative. As Figure 7 shown, it is a schematic diagram of the physical structure of an electronic device disclosed in an embodiment of the present invention. The electronic device may include: a processor 701, a memory 702, and a communication bus 703. Among them, the processor 701 and the memory 702 communicate with each other through the communication bus 703 and communicate with the outside through a communication interface 704. The processor 701 can call the logical instructions in the memory 702 to execute the three-dimensional path tracking control method of the bionic robotic fish, and the method includes: determining the expected speed, expected heading angle, and expected depth of the bionic robotic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic robotic fish, the expected path, and a preset adaptive line-of-sight navigation strategy; using a non-linear disturbance observer to estimate and compensate for the sideslip angle in the three-dimensional path tracking control process to correct the control law of the inner-loop controller based on sliding mode; based on the expected speed, the expected heading angle, and the expected depth, and using the inner-loop controller to adjust the speed, heading angle, and depth of the bionic robotic fish to achieve the three-dimensional path tracking control of the bionic robotic fish; where the inner-loop controller includes a speed controller, a heading controller, and a depth controller.

[0142] In addition, when the logical instructions in the above-mentioned memory 702 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: storage chips, USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical disks, and other various media that can store program codes.

[0143] On the other hand, an embodiment of the present invention further provides a computer program product. The computer program product includes a computer program stored on a processor-readable storage medium. The computer program includes program instructions. When the program instructions are executed by a computer, the computer can execute the bionic robotic fish three-dimensional path tracking control method provided in each of the above method embodiments. The method includes: determining the desired speed, desired heading angle, and desired depth of the bionic robotic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic robotic fish, the desired path, and a preset adaptive line-of-sight navigation strategy; using a non-linear disturbance observer to estimate and compensate for the sideslip angle during the three-dimensional path tracking control process to correct the control law of the inner-loop controller based on sliding mode; based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller to adjust the speed, heading angle, and depth of the bionic robotic fish to achieve the three-dimensional path tracking control of the bionic robotic fish; where the inner-loop controller includes a speed controller, a heading controller, and a depth controller.

[0144] In another aspect, an embodiment of the present invention further provides a processor-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it is configured to execute the bionic robotic fish three-dimensional path tracking control method provided in the above embodiments. The method includes: determining the desired speed, desired heading angle, and desired depth of the bionic robotic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic robotic fish, the desired path, and a preset adaptive line-of-sight navigation strategy; estimating and compensating the sideslip angle in the three-dimensional path tracking control process by using a nonlinear disturbance observer to correct the control law of the inner-loop controller based on sliding mode; adjusting the speed, heading angle, and depth of the bionic robotic fish based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller to achieve the three-dimensional path tracking control of the bionic robotic fish; wherein the inner-loop controller includes a speed controller, a heading controller, and a depth controller.

[0145] The processor-readable storage medium may be any available medium or data storage device accessible by the processor, including but not limited to magnetic memories (such as floppy disks, hard disks, magnetic tapes, magneto-optical disks (MO), etc.), optical memories (such as CDs, DVDs, BDs, HVDs, etc.), and semiconductor memories (such as ROMs, EPROMs, EEPROMs, non-volatile memories (NANDFLASH), solid-state drives (SSD)).

[0146] The device embodiments described above are merely illustrative. 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 distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. A person of ordinary skill in the art can understand and implement it without creative effort.

[0147] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on such an understanding, the essence of the above technical solution, or the part that contributes to the prior art, can be embodied in the form of a software product. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disks, optical disks, etc., and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0148] 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 it; 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 they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on 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 embodiments of the present invention.

Claims

1. A three-dimensional path tracking control method for a bionic robotic fish, characterized in that, Comprising: Based on the dynamic model corresponding to the current motion mode of the bionic fish, the desired path, and a preset adaptive line-of-sight navigation strategy, determining the desired speed, desired heading angle, and desired depth of the bionic fish at the current moment; the adaptive line-of-sight navigation strategy is an adaptive line-of-sight navigation strategy determined based on a preset obstacle function, which adaptively adjusts the forward-looking distance during tracking while ensuring the convergence of the tracking along-track error; the obstacle function is a logarithmic obstacle Lyapunov function; Using a nonlinear disturbance observer to estimate and compensate for the sideslip angle during the three-dimensional path tracking control process to correct the control law of the inner-loop controller based on sliding mode; the using a nonlinear disturbance observer to estimate and compensate for the sideslip angle during the three-dimensional path tracking control process to correct the control law of the inner-loop controller based on sliding mode specifically includes: determining the frequently changing sideslip angle of the bionic fish during movement as an external disturbance term, and using a nonlinear disturbance observer to estimate and compensate for the sideslip angle to dynamically correct the control law of the inner-loop controller during the three-dimensional path tracking control process of the bionic fish; Based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller to adjust the speed, heading angle, and depth of the bionic fish to achieve the three-dimensional path tracking control of the bionic fish; wherein, the inner-loop controller includes a speed controller, a heading controller, and a depth controller; The based on the desired speed, the desired heading angle, and the desired depth, and using the inner-loop controller to adjust the speed, heading angle, and depth of the bionic fish specifically includes: using fuzzy mapping to map the forward thrust, pitch moment, and yaw moment output by the inner-loop controller into the parameter frequency and bias of a preset central pattern generator, and the corresponding pectoral fin bias angle respectively; based on the parameter frequency, bias, and the pectoral fin bias angle, and using the central pattern generator to perform rhythmic control on the bionic fish to achieve the adjustment of the speed, heading angle, and depth of the bionic fish; the central pattern generator is a central pattern generator based on a Hopf oscillator; Wherein, the logarithmic obstacle Lyapunov function is expressed as: where x e represents the tracking error, and k b represents the boundary coefficient; The adaptive line-of-sight navigation strategy is: where d l (x e , K) represents the look-ahead distance, K represents the path curvature, and K max represents the maximum value of the path curvature, d l0 represents the preset distance when x e = 0, and d lmin represents the lower limit of the look-ahead distance, and δ represents the threshold of the tracking error.

2. The three-dimensional path tracking control method of the bionic robotic fish according to claim 1, characterized in that Also comprising: Pre-determining a corresponding dynamic model according to the current motion mode of the bionic fish in advance; the pre-determining a corresponding dynamic model according to the current motion mode of the bionic fish in advance specifically is: pre-determining a corresponding dynamic model according to the mechanical structure and motion characteristics of the bionic fish.

3. A three-dimensional path tracking control device for a bionic robotic fish, characterized in that, Comprising: A three-dimensional path navigation unit is used to determine the desired velocity, desired heading angle, and desired depth of the bionic fish at the current moment based on the dynamic model corresponding to the current motion mode of the bionic fish, the desired path, and a preset adaptive line-of-sight navigation strategy; the adaptive line-of-sight navigation strategy is an adaptive line-of-sight navigation strategy determined based on a preset obstacle function, which adaptively adjusts the forward-looking distance during tracking while ensuring the convergence of the tracking along-track error; the obstacle function is a logarithmic obstacle Lyapunov function; A sideslip angle compensation unit is used to estimate and compensate the sideslip angle during the three-dimensional path tracking control process by using a nonlinear disturbance observer, so as to correct the control law of the inner-loop controller based on sliding mode; specifically, the sideslip angle compensation unit is configured to: determine the frequently changing sideslip angle of the bionic fish during movement as an external disturbance term, and use a nonlinear disturbance observer to estimate and compensate the sideslip angle, so as to dynamically correct the control law of the inner-loop controller during the three-dimensional path tracking control process of the bionic fish; A three-dimensional path tracking processing unit is used to adjust the velocity, heading angle, and depth of the bionic fish based on the desired velocity, the desired heading angle, and the desired depth, and use the inner-loop controller to achieve the three-dimensional path tracking control of the bionic fish; wherein, the inner-loop controller includes a velocity controller, a heading controller, and a depth controller; specifically, the three-dimensional path tracking processing unit is configured to: use fuzzy mapping to map the forward thrust, pitch moment, and yaw moment output by the inner-loop controller into the parameter frequency and bias of a preset central pattern generator, and the corresponding pectoral fin bias angle respectively; based on the parameter frequency, bias, and the pectoral fin bias angle, and use the central pattern generator to perform rhythmic control on the bionic fish, so as to adjust the velocity, heading angle, and depth of the bionic fish; the central pattern generator is a central pattern generator based on a Hopf oscillator; Wherein, the logarithmic obstacle Lyapunov function is expressed as: where x e represents the tracking error, and k b represents the boundary coefficient; The adaptive line-of-sight navigation strategy is: where d l (x e , K) represents the look-ahead distance, K represents the path curvature, and K max represents the maximum value of the path curvature, d l0 represents the preset distance when x e = 0, and d lmin represents the lower limit of the look-ahead distance, and δ represents the threshold of the tracking error.

4. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the bionic fish three-dimensional path tracking control method as described in claim 1 or 2.

5. A processor-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the bionic fish three-dimensional path tracking control method as described in claim 1 or 2.

Images