Under-actuated underwater robotic fish formation control method, system and equipment based on dynamic surface control

Abstract

The invention discloses an underactuated underwater robotic fish formation control method, system and device based on dynamic surface control, and relates to the technical field of underwater robot control, the method comprises the steps of constructing a virtual formation topology, calculating the linear distance between a virtual follower and a real follower, and calculating the linear distance between the virtual follower and the real follower; calculating a position error of the real follower and the target reference position in the fish body coordinate system based on the linear distance, and determining a yaw angle; determining a reference course angle based on the linear distance and the yaw angle; determining a course error based on the reference course angle and the course angle; obtaining an output signal of the dynamic surface filter based on the reference course angle; determining a course angular velocity control law and a velocity control law based on the position error, the course error and an output signal of the dynamic surface filter; and controlling the real follower robotic fish to move according to the virtual formation topology based on the course angular velocity control law and the velocity control law. According to the invention, the stability of the underwater under-actuated system formation and the real-time performance of the underwater weak computing power system are improved.

Description

Technical Field

[0001] The present application relates to the technical field of underwater robot control, and particularly to a formation control method, system and device for underactuated underwater robotic fish based on dynamic surface control. Background Art

[0002] In recent years, the application of underwater robots (such as robotic fish) in fields such as ocean exploration, environmental monitoring, and resource development has gradually increased, and their formation control technology has become a research hotspot. However, due to physical property limitations, underactuated underwater robotic fish only have degrees of freedom for forward speed and heading angle control, lacking the ability to control lateral movement, and still face many challenges in achieving stable formation control in complex dynamic environments. There are more existing methods for formation control of pectoral fin flapping robotic fish, mainly due to their mechanism stability and direct controllability advantages of lateral movement; while the underactuated characteristics of tail-swing driven robotic fish are more obvious, resulting in differences in their formation control from the former. Among them, considering the position of the leader robotic fish is available for the control design of the follower, but the dynamics and control inputs of the leader are not available for the follower. In traditional leader-follower based formation schemes, the follower usually directly tracks the heading, speed, or acceleration of the leader, but in an underactuated robotic fish system, the follower cannot fully replicate the motion parameters of the leader, resulting in problems such as large control errors and serious overshoot.

[0003] In addition, the existing robotic fish formation control methods also have the following problems: 1) Since underactuated bionic robotic fish only have degrees of freedom for forward speed and heading angle control and lack lateral control ability, but there is an unignorable side-slip speed, which is a significant difference between robotic fish and wheeled mobile robots in control law design, resulting in the inability to guarantee the stability of robotic fish formation under existing control methods. 2) Traditional path planning methods rely strongly on real-time performance, while the underwater robotic fish formation system usually has limited computing power and weak dynamic response ability to complex environments; 3) In existing formation control methods for underwater unmanned systems, the controller design requires multiple differentiations, resulting in high computational complexity, especially in underactuated systems, which will cause the so-called "differential complexity explosion" problem.

[0004] Therefore, there is an urgent need for a formation control method for underactuated underwater robotic fish based on dynamic surface control to solve the above problems. Summary of the Invention

[0005] The purpose of the present application is to provide a formation control method, system and device for underactuated underwater robotic fish based on dynamic surface control, which improves the stability of the formation of underwater underactuated systems and the real-time performance of underwater weak computing power systems.

[0006] To achieve the above purpose, the present application provides the following solutions.

[0007] In a first aspect, the present application provides an underactuated underwater robotic fish formation control method based on dynamic surface control. The underactuated underwater robotic fish formation control method based on dynamic surface control includes: Construct a virtual formation topology with the leader robotic fish as the vertex and multiple virtual follower robotic fish as the nodes; Based on the virtual formation topology, calculate the straight-line distance between the virtual follower robotic fish and the real follower robotic fish; the position of the virtual follower robotic fish is the target reference position of the real follower robotic fish; Based on the straight-line distance, calculate the position error between the real follower robotic fish and the target reference position in the body coordinate system of the fish; the body coordinate system of the fish is a coordinate system established with the real follower robotic fish as the reference point; Based on the position error, determine the yaw angle of the real follower robotic fish; Based on the straight-line distance and the yaw angle of the real follower robotic fish, determine the reference heading angle of the real follower robotic fish; Based on the reference heading angle of the real follower robotic fish and the heading angle of the real follower robotic fish, determine the heading error of the real follower robotic fish; Based on the reference heading angle of the real follower robotic fish, obtain the output signal of the dynamic surface filter; Based on the position error, the heading error of the real follower robotic fish, and the output signal of the dynamic surface filter, determine the heading angular velocity control law and the velocity control law of the real follower robotic fish; Based on the heading angular velocity control law and the velocity control law of the real follower robotic fish, control the real follower robotic fish to move according to the virtual formation topology.

[0008] Optionally, the straight-line distance between the virtual follower robotic fish and the real follower robotic fish The expression is: ; where represents the relative distance between the virtual follower robotic fish and the real follower robotic fish on the x-axis; represents the relative distance between the virtual follower robotic fish and the real follower robotic fish on the y-axis.

[0009] Optionally, the expression of the position error between the real follower robotic fish and the target reference position in the body coordinate system of the fish is: ; where is the x-axis position error between the real follower robotic fish and the target reference position in the body coordinate system of the fish; is the y-axis position error between the real follower robotic fish and the target reference position in the body coordinate system of the fish; represents the heading angle of the real follower robotic fish; ; wherein, is the yaw angle of the real follower robotic fish.

[0010] Optionally, the expression of the reference heading angle of the real follower robotic fish is: ; wherein, is the reference heading angle of the real follower robotic fish; is the heading angle of the real follower robotic fish; is a positive constant; is the heading angle of the leader robotic fish; The expression of the heading error of the real follower robotic fish is: ; wherein, is the heading error of the real follower robotic fish.

[0011] Optionally, the expression of the output signal of the dynamic surface filter is: ; wherein, is the output signal of the dynamic surface filter at the t-th moment; is the output signal of the dynamic surface filter at the initial moment; is the time constant of the dynamic surface filter; is the frequency domain value of the first derivative of the reference heading angle of the real follower robotic fish; s is the integration variable.

[0012] Optionally, based on the position error, the heading error of the real follower robotic fish, and the output signal of the dynamic surface filter, the heading angular velocity control law and the speed control law of the real follower robotic fish are determined, specifically including: Based on the position error and the heading error of the real follower robotic fish, a Lyapunov candidate function is determined; The derivative of the Lyapunov candidate function is obtained to get the derivative equation of the Lyapunov candidate function; the derivative equation of the Lyapunov candidate function contains the high-order derivative of the reference heading angle of the real follower robotic fish; The output signal of the dynamic surface filter is used to replace the high-order derivative in the derivative equation of the Lyapunov candidate function, thereby determining the heading angular velocity control law and the speed control law of the real follower robotic fish.

[0013] Optionally, the expression of the derivative equation of the Lyapunov candidate function is: ; wherein, is the derivative of the Lyapunov candidate function; is a constant; is an intermediate variable; is the speed of the virtual follower robotic fish; is the speed of the real follower robotic fish; is the heading angular velocity of the real follower robotic fish; is a positive constant.

[0014] Optionally, the expression of the heading angular velocity control law of the real follower robotic fish is: ; wherein, is the heading angular velocity control law; is a positive constant; The expression of the speed control law of the real follower robotic fish is: ; wherein, is the speed control law; is a positive constant.

[0015] In a second aspect, the present application provides an underactuated underwater robotic fish formation control system based on dynamic surface control, and the underactuated underwater robotic fish formation control system based on dynamic surface control is used to implement the underactuated underwater robotic fish formation control method described above. The underactuated underwater robotic fish formation control system based on dynamic surface control includes: A virtual formation topology construction unit, configured to construct a virtual formation topology with the leader robotic fish as the vertex and multiple virtual follower robotic fish as the nodes; A straight-line distance determination unit, configured to calculate the straight-line distance between the virtual follower robotic fish and the real follower robotic fish based on the virtual formation topology; the position where the virtual follower robotic fish is located is the target reference position of the real follower robotic fish; A position error determination unit, configured to calculate the position error between the real follower robotic fish and the target reference position in the body coordinate system based on the straight-line distance; the body coordinate system is a coordinate system established with the real follower robotic fish as the reference point; A yaw angle determination unit, configured to determine the yaw angle of the real follower robotic fish based on the position error; A reference heading angle determination unit, configured to determine the reference heading angle of the real follower robotic fish based on the straight-line distance and the yaw angle of the real follower robotic fish; A heading error determination unit, configured to determine the heading error of the real follower robotic fish based on the reference heading angle and the heading angle of the real follower robotic fish; An output signal determination unit of a dynamic surface filter, configured to obtain an output signal of the dynamic surface filter based on a reference course angle of a real follower robotic fish; A course angular velocity control law and speed control law determination unit, configured to determine a course angular velocity control law and a speed control law of a real follower robotic fish based on the position error, a course error of the real follower robotic fish, and the output signal of the dynamic surface filter; A control unit, configured to control the real follower robotic fish to move according to a virtual formation topology based on the course angular velocity control law and the speed control law of the real follower robotic fish.

[0016] In a third aspect, the present application provides a computer device, including: a memory, a processor, and a computer program stored on the memory and executable on the processor, where the processor executes the computer program to implement the formation control method for underactuated underwater robotic fish based on dynamic surface control according to any one of the above.

[0017] According to the specific embodiments provided by the present application, the present application has the following technical effects: The present application discloses a formation control method, system, and device for underactuated underwater robotic fish based on dynamic surface control. First, by constructing a virtual formation topology, the formation maintenance problem is transformed into a path tracking problem, reducing the dependence on a complete dynamic model in controller design, avoiding communication burdens and the computational burden of real-time path planning for followers, and improving the real-time performance of system response. Second, under the conditions of considering the nonholonomicity and significant sideslip effect of a tail-swinging robotic fish system, a control law is designed based on the Lyapunov stability analysis method, improving the stability of the system. Third, a dynamic reference course strategy is proposed to achieve a natural transition from fast convergence to steady tracking, alleviating overshoot or oscillation problems caused by dynamic limitations in the underactuated system of robotic fish and enhancing the formation transient process performance. Description of the Drawings

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

[0019] Figure 1 It is a schematic flowchart of a formation control method for underactuated underwater robotic fish based on dynamic surface control provided by an embodiment of the present application.

[0020] Figure 2 It is a schematic diagram of a virtual formation topology and a fish body coordinate system provided by an embodiment of the present application.

[0021] Figure 3 Schematic diagram of the tracking performance of the DSC filter provided by an embodiment of the present application.

[0022] Figure 4 Schematic diagram of three robotic fish achieving a triangular formation effect provided by an embodiment of the present application.

[0023] Figure 5 Schematic diagram of the speed change of the robotic fish formation provided by an embodiment of the present application.

[0024] Figure 6 Schematic diagram of the structure of a computer device provided by an embodiment of the present application. Detailed implementation manners

[0025] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present application.

[0026] To make the above objects, features, and advantages of the present application more obvious and understandable, the present application will be further described in detail below in conjunction with the drawings and specific implementation manners.

[0027] In an exemplary embodiment, as Figure 1 shown, a formation control method for underactuated underwater robotic fish based on dynamic surface control is provided, which specifically includes the following steps S1 to S9.

[0028] Step S1, construct a virtual formation topology with the leader robotic fish as the vertex and multiple virtual follower robotic fish as the nodes. The schematic diagram of the virtual formation topology is as Figure 2 shown. This topological structure is determined by the side lengths and apex angles set artificially. The shape of this topological structure can be triangular or other shapes, which is not limited here.

[0029] Step S2, based on the virtual formation topology, calculate the straight-line distance between the virtual follower robotic fish and the real follower robotic fish; the position where the virtual follower robotic fish is located is the target reference position of the real follower robotic fish. That is to say, the goal of the real follower robotic fish is to gradually approach the position where the virtual follower robotic fish is located , so the problem of maintaining the formation of the formation is transformed into the problem of path tracking of the real follower robotic fish to the virtual follower robotic fish.

[0030] As an alternative implementation, in step S2, the straight-line distance between the virtual follower robotic fish and the real follower robotic fish has the following expression: (1) where represents the relative distance between the virtual follower robotic fish and the real follower robotic fish on the x-axis ; represents the relative distance between the virtual follower robotic fish and the real follower robotic fish on the y-axis. Among them: (2) (3) Step S3: Based on the straight-line distance, calculate the position error between the real follower robotic fish and the target reference position in the fish body coordinate system; the fish body coordinate system is a coordinate system established with the real follower robotic fish as the reference point. Taking the centroid of the real follower robotic fish as the origin and aligning the head orientation with the positive direction of the Figure 2 axis, establish the fish body coordinate system as shown in . Then, transfer the position of the reference point in the world coordinate system to the fish body coordinate system of the real follower to avoid the difficulty of discussing the control of nonholonomic systems in the generalized coordinate system.

[0031] As an alternative implementation, in step S3, the expression for the position error between the real follower robotic fish and the target reference position in the fish body coordinate system is: (4) (5) where is the x-axis position error between the real follower robotic fish and the target reference position in the fish body coordinate system; is the y-axis position error between the real follower robotic fish and the target reference position in the fish body coordinate system; represents the heading angle of the real follower robotic fish.

[0032] Step S4: Based on the position error, determine the yaw angle of the real follower robotic fish.

[0033] Defining the counterclockwise rotation along the positive direction of the x-axis as the positive direction, the expression for the yaw angle of the real follower robotic fish is: (6) where is the yaw angle of the real follower robotic fish.

[0034] Step S5: Determine the reference heading angle of the real follower robotic fish based on the straight-line distance and the yaw angle of the real follower robotic fish.

[0035] As an alternative implementation, in step S5, the expression for the reference heading angle of the real follower robotic fish is: (7) where is the reference heading angle of the real follower robotic fish; is the heading angle of the real follower robotic fish; is a positive constant; is the heading angle of the leader robotic fish.

[0036] Specifically, the underlying servo control form of the tail-swinging robotic fish underactuated system is generally , where are the swing amplitude, swing frequency, and offset respectively. The forward speed of the robotic fish can be controlled by , and the heading is adjusted by . Obviously, there is no lateral displacement control amount, so there is an obvious underactuation problem in the formation task. Therefore, this application designs the real follower robotic fish to track the dynamic reference heading as shown in equation (7). Equation (7) aims to implement a dynamic reference heading adjustment mechanism. When the straight-line distance between the real follower robotic fish and the target reference position is large, the system preferentially drives the real follower robotic fish to quickly approach the target reference position; while when the straight-line distance gradually shrinks, the control strategy gradually switches to tracking the leader's heading to ensure that the overshoot is minimized during the transient-to-steady-state transition of the real follower robotic fish's heading.

[0037] Step S6: Determine the heading error of the real follower robotic fish based on the reference heading angle and the heading angle of the real follower robotic fish. The expression for the heading error of the real follower robotic fish is: (8) where is the heading error of the real follower robotic fish.

[0038] Step S7: Obtain the output signal of the dynamic surface filter based on the reference heading angle of the real follower robotic fish.

[0039] As an alternative implementation, in step S7, the expression for the output signal of the dynamic surface filter is: (9) where is the output signal of the dynamic surface filter at the t-th moment; is the output signal of the dynamic surface filter at the initial moment; is the time constant of the dynamic surface filter and is an adjustable variable; is the frequency domain value of the first derivative of the reference course angle of the real follower robotic fish; s is the integration variable.

[0040] Specifically, taking the derivative of the reference course angle in formula (7) obtained in step S5, we get The expression of is as follows: (10) Among them, the derivative of the relative distance , and the derivative of the yaw angle is . Obviously, the finally obtained has a high computational complexity, which is not conducive to building an underwater low-computing-power robot. Therefore, this application regards the complex expression of as an ideal virtual control signal and generates a smooth dynamic surface output through a first-order filter as an alternative signal for the course angle control quantity and the speed control quantity. Among them, the dynamic equation of the filter is: (11) Among them, is the output signal of the filter.

[0041] Solving the dynamic equation (11), the approximate expression of the output form of the filter is formula (10). From its integral term, it can be seen that the filter retains the main low-frequency characteristics of the input signal and weakens the high-frequency signal components. Define the tracking error of the filter as: (12) Substituting (11) into (12), we get: (13) The solution of equation (13) is , indicating that the tracking error decays exponentially with time. Therefore, when , the tracking error will remain at a small value within a certain period of time and finally converge to zero . Among them is the Euclidean norm. In the design of the course angle and speed control laws of this application, it is replaced by . The purpose of step S7 is to obtain an alternative signal for the higher-order derivative of the reference course, that is, the output signal of the dynamic surface filter.

[0042] Step S8, based on the position error, the course error of the real follower robotic fish, and the output signal of the dynamic surface filter, determine the course angular velocity control law and the speed control law of the real follower robotic fish.

[0043] As an alternative implementation, step S8 specifically includes: Step S81, based on the position error and the heading error of the real follower robotic fish, determine the Lyapunov candidate function. The expression of the Lyapunov candidate function is as follows: (14) where is a positive constant.

[0044] Step S82, take the derivative of the Lyapunov candidate function to obtain the derivative equation of the Lyapunov candidate function; the derivative equation of the Lyapunov candidate function contains the high-order derivative of the reference heading angle of the real follower robotic fish.

[0045] Specifically, taking the derivative of the Lyapunov candidate function shown in Equation (14), we get: (15) Then, calculate and in Equation (15). Specifically, take the derivative of the position error to obtain the position error dynamics model of the real follower robotic fish: (16) Define , , so as to simplify the position error dynamics model of the real follower robotic fish and obtain: (17) where represents the heading angular velocity of the real follower robotic fish. Combining the coordinate transformation equations of Formulas (4) and (5), the final result of the position error model is as follows: (18) where , respectively represent the forward and lateral velocity errors in the body coordinate system of the fish.

[0046] Finally, taking the derivative of the heading error, the heading error dynamics model is as follows: (19) Step S83, use the output signal of the dynamic surface filter to replace the high-order derivative in the derivative equation of the Lyapunov candidate function, so as to determine the heading angular velocity control law and the velocity control law of the real follower robotic fish.

[0047] Specifically, substituting Equation (18) and Equation (19) into Equation (15), the expression of the derivative equation of the Lyapunov candidate function is: (20) Wherein, is the derivative of the Lyapunov candidate function; is a constant; is an intermediate variable; is the speed of the virtual follower robotic fish; is the speed of the real follower robotic fish; is the heading angular velocity of the real follower robotic fish; is a positive constant.

[0048] Finally, according to Equation (20), design the speed control law and the heading angular velocity control law of the real follower robotic fish. Note that the differential calculation is significantly involved in the heading angular velocity control law, and the output of the DSC filter proposed in step S is substituted for the signal. Finally, the forward speed control law and the heading angular velocity control law of the robotic fish are obtained. Specifically, the expression of the heading angular velocity control law of the real follower robotic fish is: (21) Wherein, is the heading angular velocity control law; is a positive constant.

[0049] The expression of the speed control law of the real follower robotic fish is: (22) Wherein, is the speed control law; is a positive constant.

[0050] Further, substitute the heading angular velocity control law (21) as into Equation (20), and substitute the speed control law (22) as into Equation (20), and the simplified form of Equation (20) is obtained as follows: (23) Wherein, is a relatively small bounded constant, then there is: (24) Wherein, . Therefore, it can be seen that the finally obtained speed control law and the heading angular velocity control law of the real follower robotic fish can make the motion error of the robotic fish formation system be uniformly ultimately bounded, so that the overall control system achieves asymptotic stability under the control of Equation (21) and Equation (22).

[0051] Step S9: Based on the heading angular velocity control law and speed control law of the real follower robotic fish, control the real follower robotic fish to move according to the virtual formation topology.

[0052] Advantages of this application: 1. For the tail-swing drive robotic fish system, this application transforms the formation keeping problem into a path tracking problem by generating a virtual formation, reducing the dependence on the complete dynamic model in controller design, avoiding communication burdens and the computational burden of real-time path planning for followers.

[0053] 2. This application introduces the dynamic surface control method in the heading control of the robotic fish system, avoiding the "complexity explosion" problem in high-order derivative calculations, significantly reducing implementation complexity, and being easy to deploy in actual underwater systems. At the same time, it plays a role in suppressing external disturbances caused by nonlinear motion in a dynamic environment and improving the robustness of the control system.

[0054] 3. This application proposes a dynamic reference heading strategy to achieve a natural transition from fast convergence to steady tracking, alleviating the overshoot or oscillation problems caused by dynamic limitations in the underactuated robotic fish system and improving the performance of the formation transient process.

[0055] 4. Under the conditions of considering the nonholonomicity and significant sideslip effect of the tail-swing robotic fish system, this application designs a controller based on the Lyapunov stability analysis method and strictly proves the stability of the system, fully considering the specificity of the tail-swing robotic fish system.

[0056] The following further elaborates on the underactuated underwater robotic fish formation control method based on dynamic surface control in this application in combination with the technical solutions and drawings.

[0057] Taking the formation system of 3 robotic fish as an implementation case below, all robotic fish are equipped with IMU sensors for measuring heading and UWB sensors for positioning and communication.

[0058] Step 1: Generate a virtual formation.

[0059] Taking the leader robotic fish as the vertex, generate a triangular virtual formation topology, as Figure 2 shown, the side length of the virtual formation and apex angle are set, and specific details can be seen in Table 1. The position of the leader robotic fish is determined, and the position of the virtual follower robotic fish is determined. The global position information is collected in real time by the positioning sensor UWB carried by the robotic fish, that is, the generalized position coordinates are dynamically updated by the sensor since the system runs. In the generalized coordinate system, calculate the relative distances and As shown in Equation (2) and Equation (3).

[0060] Since are all known, calculate the straight-line distance of the real follower robotic fish to its target reference position As shown in Equation (1).

[0061] The heading angle of the real follower robotic fish is measured in real time by the IMU sensor, and the range is , combining Equation (2) and Equation (3) to calculate the position error of the real follower robotic fish and the target reference position in the body coordinate system of the fish and As shown in Equation (4) and Equation (5), and calculate the yaw angle according to Equation (6) .

[0062] Step 2: Calculate the dynamic reference heading.

[0063] This embodiment is applied to the implementation verification of a single servo-driven tail-swinging robotic fish system. Based on the calculation results of formulas (1) and (6) and the heading data collected by the IMU in real time , calculate the dynamic reference heading angle to be tracked by the real follower robotic fish (i.e., the reference heading angle of the real follower robotic fish) As shown in Equation (7). Among them, is a positive constant, and its setting is shown in Table 1. Then calculate the error between the real follower robotic fish and the reference heading (i.e., the heading error) according to Equation (8).

[0064] Step 3: Dynamic surface control.

[0065] Calculate the speed error , where is the axial speed of the leader robotic fish in the world coordinate system, which can be measured in real time by the UWB sensor. The derivative of the position error of the real follower robotic fish in the body coordinate system is calculated as: .

[0066] Derive Equation (8) to obtain the heading error dynamics model as shown in Equation (19). Among them, .

[0067] Therefore, a smooth dynamic surface output is generated through a first-order filter as an alternative signal for the design of the heading angular velocity control quantity, and its dynamic equation expression is as shown in Equation (11).

[0068] Furthermore, to prove the tracking performance of the above first-order filter, a signal simulating a near characteristic is simulated in the simulation. This signal contains a high-frequency part and a low-frequency part, and it is set , observe the output characteristics of the filter and design the time constant See Table 1. In addition, Figure 3 As shown, it is proved The filter can retain the main low-frequency characteristics of the input signal and weaken the high-frequency signal components. The discrete time expression of is: .

[0069] in, represents the current time step, represents the previous time step, represents the discrete time step. In practical engineering, it can be approximated as The smoothing effect of DSC can avoid the direct use of the differential signal due to Oscillation or instability caused by improper selection.

[0070] Step 4: Calculate Lyapunov stability control law.

[0071] According to equations (14) to (22), the forward velocity control law and the heading angular velocity control law are designed, and the output of the DSC filter (11) proposed in step 3 is cited. Alternative We obtain equations (21) and (22).

[0072] Then, the formation motion error is verified using formula (23) and formula (24) is uniformly bounded, indicating that the designed control system is asymptotically stable. On this basis, the calculation method of the underlying servo drive control quantity of the follower robot fish is: .

[0073] The experimental results of the three robot fish completing the above formation are shown in Figure 4 , Figure 4 A, B, and C represent three robot fish, of which A is the leader robot fish, and B and C are the real follower robot fish. Figure 4 It illustrates that three robot fish start from an initial side-by-side position. After a period of time, the formation topology of the three fish gradually converges to the trajectory change of the target triangle topology. The formation change and convergence illustrate the effectiveness of the heading controller proposed in this application.

[0074] Figure 5It is a schematic diagram of the speed changes of three robotic fish during the formation process. Fish1, as the leader, shows a state of gradually accelerating and cruising at a constant speed in the free swimming state; Fish2 and Fish3, as followers, always have a speed lower than that of the leader in the initial stage under the adjustment of the proposed speed controller and maintain the same speed as the leader after the formation is formed, demonstrating the effectiveness of the speed controller proposed in this application.

[0075] Table 1 Setting of formation structure parameters and controller parameters

[0076] Based on the same inventive concept, the embodiment of this application also provides a dynamic surface control-based underactuated underwater robotic fish formation control system for implementing the above-mentioned dynamic surface control-based underactuated underwater robotic fish formation control method. The implementation solutions provided by this system to solve problems are similar to those recorded in the above method. Therefore, the specific limitations in one or more of the following embodiments of the dynamic surface control-based underactuated underwater robotic fish formation control system can refer to the limitations on the dynamic surface control-based underactuated underwater robotic fish formation control method in the above text and will not be repeated here.

[0077] In an exemplary embodiment, a dynamic surface control-based underactuated underwater robotic fish formation control system is provided, including: A virtual formation topology construction unit for constructing a virtual formation topology with the leader robotic fish as the vertex and multiple virtual follower robotic fish as the nodes.

[0078] A straight-line distance determination unit for calculating the straight-line distance between the virtual follower robotic fish and the real follower robotic fish based on the virtual formation topology; the position where the virtual follower robotic fish is located is the target reference position of the real follower robotic fish.

[0079] A position error determination unit for calculating the position error between the real follower robotic fish and the target reference position in the body coordinate system based on the straight-line distance; the body coordinate system is a coordinate system established with the real follower robotic fish as the reference point.

[0080] A yaw angle determination unit for determining the yaw angle of the real follower robotic fish based on the position error.

[0081] A reference heading angle determination unit for determining the reference heading angle of the real follower robotic fish based on the straight-line distance and the yaw angle of the real follower robotic fish.

[0082] A heading error determination unit for determining the heading error of the real follower robotic fish based on the reference heading angle and the heading angle of the real follower robotic fish.

[0083] An output signal determination unit of a dynamic surface filter, configured to obtain an output signal of the dynamic surface filter based on a reference course angle of a real follower robotic fish.

[0084] A course angular velocity control law and velocity control law determination unit, configured to determine a course angular velocity control law and a velocity control law of a real follower robotic fish based on the position error, the course error of the real follower robotic fish, and the output signal of the dynamic surface filter.

[0085] A control unit, configured to control a real follower robotic fish to move according to a virtual formation topology based on the course angular velocity control law and the velocity control law of the real follower robotic fish; wherein the virtual formation topology has a leader robotic fish as a vertex and a plurality of virtual follower robotic fish as nodes.

[0086] In an exemplary embodiment, a computer device is provided, including: a memory, a processor, and a computer program stored on the memory and executable on the processor, and the processor executes the computer program to implement an underactuated underwater robotic fish formation control method based on dynamic surface control.

[0087] In an exemplary embodiment, a computer device is provided. The computer device may be a server or a terminal, and its internal structure diagram may be as Figure 6 shown. The computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O), and a communication interface. Among them, the processor, the memory, and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The input / output interface of the computer device is used for exchanging information between the processor and external devices. The communication interface of the computer device is used for communicating with an external terminal through a network connection. The computer program, when executed by the processor, implements an underactuated underwater robotic fish formation control method based on dynamic surface control.

[0088] Those skilled in the art can understand that Figure 6 the structure shown in

[0089] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use, and processing of relevant data need to comply with relevant regulations.

[0090] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, database, or other medium used in the embodiments provided in this application can include at least one of non-volatile and volatile memories. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0091] The databases involved in the embodiments provided in this application can include at least one of relational databases and non-relational databases. Non-relational databases can include distributed databases based on blockchain, etc., without limitation. The processors involved in the embodiments provided in this application can be general-purpose processors, central processors, graphics processors, digital signal processors, programmable logic devices, data processing logics based on quantum computing, etc., without limitation.

[0092] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope recorded in this specification.

[0093] In this article, specific examples are used to elaborate on the principles and implementation manners of the present application. The description of the above embodiments is only used to help understand the method and its core idea of the present application; at the same time, for those of ordinary skill in the art, according to the idea of the present application, there will be changes in the specific implementation manners and application scopes. In summary, the content of this specification should not be construed as a limitation on the present application.

Claims

1. A formation control method for underactuated underwater robotic fish based on dynamic surface control, characterized in that The formation control method of underactuated underwater robotic fish based on dynamic surface control includes: Construct a virtual formation topology with the leader robotic fish as the vertex and multiple virtual follower robotic fish as the nodes; Based on the virtual formation topology, calculate the straight-line distance between the virtual follower robotic fish and the real follower robotic fish; the position of the virtual follower robotic fish is the target reference position of the real follower robotic fish; Based on the straight-line distance, calculate the position error between the real follower robotic fish and the target reference position in the body coordinate system of the fish; the body coordinate system of the fish is established with the real follower robotic fish as the reference point; Based on the position error, determine the yaw angle of the real follower robotic fish; Based on the straight-line distance and the yaw angle of the real follower robotic fish, determine the reference heading angle of the real follower robotic fish; Based on the reference heading angle and the heading angle of the real follower robotic fish, determine the heading error of the real follower robotic fish; Based on the reference heading angle of the real follower robotic fish, obtain the output signal of the dynamic surface filter; Based on the position error, the heading error of the real follower robotic fish, and the output signal of the dynamic surface filter, determine the heading angular velocity control law and the speed control law of the real follower robotic fish; Based on the heading angular velocity control law and the speed control law of the real follower robotic fish, control the real follower robotic fish to move according to the virtual formation topology.

2. The formation control method for underactuated underwater robotic fish based on dynamic surface control according to claim 1, wherein The straight-line distance between the virtual follower robotic fish and the real follower robotic fish The expression is as follows: ； Among them, represents the relative distance between the virtual follower robotic fish and the real follower robotic fish on the x-axis; represents the relative distance between the virtual follower robotic fish and the real follower robotic fish on the y-axis.

3. The formation control method for underactuated underwater robotic fish based on dynamic surface control according to claim 2, characterized in that The expression of the position error between the real follower robotic fish and the target reference position in the body coordinate system of the fish is: ； Among them, is the x-axis position error between the real follower robotic fish and the target reference position in the fish body coordinate system; is the y-axis position error between the real follower robotic fish and the target reference position in the fish body coordinate system; represents the heading angle of the real follower robotic fish; ； Among them, is the yaw angle of the real follower robotic fish.

4. The formation control method for underactuated underwater robotic fish based on dynamic surface control according to claim 3, characterized in that The expression of the reference heading angle of the real follower robotic fish is: ； wherein, is the reference heading angle of the real follower robotic fish; is the heading angle of the real follower robotic fish; is a positive constant; is the heading angle of the leader robotic fish; The expression of the heading error of the real follower robotic fish is: ； wherein, is the heading error of the real follower robotic fish.

5. The formation control method for underactuated underwater robotic fish based on dynamic surface control according to claim 4, characterized in that, The expression of the output signal of the dynamic surface filter is: ； Among them, is the output signal of the dynamic surface filter at the t-th moment; is the output signal of the dynamic surface filter at the initial moment; is the time constant of the dynamic surface filter; is the frequency domain value of the first derivative of the reference heading angle of the real follower robotic fish; s is the integration variable.

6. The formation control method for underactuated underwater robotic fish based on dynamic surface control according to claim 5, wherein Based on the position error, the heading error of the real follower robotic fish, and the output signal of the dynamic surface filter, determining the heading angular velocity control law and the speed control law of the real follower robotic fish specifically includes: Based on the position error and the heading error of the real follower robotic fish, determine the Lyapunov candidate function; Take the derivative of the Lyapunov candidate function to obtain the derivative equation of the Lyapunov candidate function; the derivative equation of the Lyapunov candidate function contains the high-order derivative of the reference heading angle of the real follower robotic fish; Use the output signal of the dynamic surface filter to replace the high-order derivative in the derivative equation of the Lyapunov candidate function, so as to determine the heading angular velocity control law and the speed control law of the real follower robotic fish.

7. The formation control method of underactuated underwater robotic fish based on dynamic surface control according to claim 6, characterized in that The expression of the derivative equation of the Lyapunov candidate function is: ； wherein, is the derivative of the Lyapunov candidate function; is a constant; is an intermediate variable; is the velocity of the virtual follower robotic fish; is the velocity of the real follower robotic fish; is the heading angular velocity of the real follower robotic fish; is a positive constant.

8. The formation control method of underactuated underwater robotic fish based on dynamic surface control according to claim 7, wherein The expression of the heading angular velocity control law of the real follower robotic fish is: ； Among them, is the heading angular velocity control law; is a positive constant; The expression of the speed control law of the real follower robotic fish is: ； Among them, is the speed control law; is a positive constant.

9. An underactuated underwater robotic fish formation control system based on dynamic surface control, characterized in that, The formation control system of underactuated underwater robotic fish based on dynamic surface control is used to implement the formation control method of underactuated underwater robotic fish based on dynamic surface control according to any one of claims 1-8. The formation control system of underactuated underwater robotic fish based on dynamic surface control includes: A virtual formation topology construction unit for constructing a virtual formation topology with the leader robotic fish as the vertex and multiple virtual follower robotic fish as the nodes; A straight-line distance determination unit for calculating the straight-line distance between the virtual follower robotic fish and the real follower robotic fish based on the virtual formation topology; the position where the virtual follower robotic fish is located is the target reference position of the real follower robotic fish; A position error determination unit for calculating the position error between the real follower robotic fish and the target reference position in the body coordinate system based on the straight-line distance; the body coordinate system is a coordinate system established with the real follower robotic fish as the reference point; A yaw angle determination unit for determining the yaw angle of the real follower robotic fish based on the position error; A reference heading angle determination unit for determining the reference heading angle of the real follower robotic fish based on the straight-line distance and the yaw angle of the real follower robotic fish; A heading error determination unit for determining the heading error of the real follower robotic fish based on the reference heading angle and the heading angle of the real follower robotic fish; An output signal determination unit of the dynamic surface filter for obtaining the output signal of the dynamic surface filter based on the reference heading angle of the real follower robotic fish; A heading angular velocity control law and velocity control law determination unit for determining the heading angular velocity control law and velocity control law of the real follower robotic fish based on the position error, the heading error of the real follower robotic fish, and the output signal of the dynamic surface filter; A control unit for controlling the real follower robotic fish to move according to the virtual formation topology based on the heading angular velocity control law and velocity control law of the real follower robotic fish.

10. A computer device, comprising: A memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that the processor executes the computer program to implement the underactuated underwater robotic fish formation control method based on dynamic surface control according to any one of claims 1-8.

Images