Method and system for closed-loop heading control and target tracking of a tail-swimming robotic fish

Abstract

This invention relates to a heading closed-loop control and target tracking method and system for a tail-swivel robotic fish, belonging to the field of robotic fish motion control technology. It is applied to a tail-swivel robotic fish driven by a single servo motor. First, it calculates and determines whether the error value of the current control cycle exceeds a preset threshold. If so, the error value of the current control cycle is used as input, and the PD controller updates the servo control signal of the tail-swivel robotic fish. The movement of the tail-swivel robotic fish in the next control cycle is controlled according to the updated servo control signal. If not, the movement of the tail-swivel robotic fish in the next control cycle is controlled according to the servo control signal of the current control cycle. Therefore, it is possible to achieve autonomous control of the tail-swivel robotic fish without the need to build an accurate model, prepare a dataset in advance, or use any auxiliary equipment, thus ensuring the heading stability of the tail-swivel robotic fish.

Description

Technical Field

[0001] This invention relates to the field of robotic fish motion control technology, and in particular to a method and system for closed-loop heading control and target tracking of a tail-swishing robotic fish. Background Technology

[0002] Due to the complexity and uniqueness of the underwater environment, unmanned underwater systems are easily affected by external factors such as waves, currents and seabed topography during their movement, and the communication environment is also harsh, resulting in significant differences in their motion control compared to traditional land and air vehicles or robots.

[0003] Fish-shaped robots (i.e., robotic fish) based on biomimetic principles exhibit diverse shapes and propulsion modes, leading to significant differences in control methods between different propulsion modes. For robotic fish propelled by a single servo motor, the head swaying amplitude generated by the tail swing is relatively large, requiring a relatively high swaying frequency to generate a certain forward speed. Therefore, solving the directional swaying problem caused by the tail swing is crucial for the directional stability control of this type of robotic fish.

[0004] Existing research on autonomous control of robotic fish generally faces challenges in applying it to real-world aquatic environments, primarily due to difficulties in modeling, complex motion, and insufficient sensing capabilities. Firstly, model-dependent control methods often introduce complex dynamic models during model building, leading to a mismatch between the robotic fish's computational capabilities and the algorithm's performance. Secondly, control methods relying on pre-prepared motion datasets or motion mapping tables limit the robotic fish's ability to cope with unknown disturbances in real-world environments. Furthermore, over-reliance on auxiliary equipment such as laboratory-scale cameras directly hinders the application of robotic fish in actual aquatic environments. Therefore, reducing the model dependence of robotic fish, developing more robust control methods, and enhancing the application of airborne sensing systems are crucial for improving the autonomy of robotic fish systems. Summary of the Invention

[0005] The purpose of this invention is to provide a method and system for closed-loop heading control and target tracking of a tail-wagging robotic fish, which can realize autonomous control of the robotic fish and is easy to apply in actual aquatic environments.

[0006] To achieve the above objectives, the present invention provides the following solution:

[0007] A heading closed-loop control method for a tail-swivel robotic fish, applied to a tail-swivel robotic fish driven by a single servo motor, the heading closed-loop control method comprising:

[0008] Obtain the set of fish head headings collected in the current control cycle; the set of fish head headings includes multiple fish head heading angles collected within the current control cycle;

[0009] Using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, the average heading deviation of the current control cycle is calculated using the average value calculation formula.

[0010] Calculate the difference between the average heading deviation of the current control cycle and the initial specified heading to obtain the error value of the current control cycle; determine whether the error value of the current control cycle exceeds a preset threshold.

[0011] If so, the error value of the current control cycle is used as input, and the PD controller is used to update the servo control signal of the tail-wagging robotic fish. The movement of the tail-wagging robotic fish in the next control cycle is controlled according to the updated servo control signal.

[0012] If not, then the movement of the tail-wagging robotic fish in the next control cycle is controlled according to the servo control signal of the current control cycle.

[0013] A heading closed-loop control system for a tail-swivel robotic fish, applied to a tail-swivel robotic fish driven by a single servo motor, the heading closed-loop control system comprising:

[0014] The data acquisition module is used to acquire the set of fish head headings collected in the current control cycle; the set of fish head headings includes multiple fish head heading angles collected in the current control cycle;

[0015] The deviation calculation module is used to calculate the average heading deviation of the current control period using the fish head heading set of the current control period and the fish head heading set of the previous control period as inputs and the average value calculation formula.

[0016] The judgment module is used to calculate the difference between the average heading deviation of the current control cycle and the initial specified heading to obtain the error value of the current control cycle; and to determine whether the error value of the current control cycle exceeds a preset threshold.

[0017] The control module is configured to, if yes, use the error value of the current control cycle as input, update the servo control signal of the tail-wagging robotic fish using the PD controller, and control the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal; if no, control the movement of the tail-wagging robotic fish in the next control cycle according to the servo control signal of the current control cycle.

[0018] A target tracking method for a tail-swivel robotic fish, applied to a tail-swivel robotic fish driven by a single servo motor, the target tracking method comprising:

[0019] Acquire images of the target object obtained by taking pictures of the target object within the current control cycle, and determine the desired course of the tail-wagging robotic fish based on the target object images;

[0020] Obtain the fish head heading set collected in the current control cycle; using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, calculate the average heading deviation of the current control cycle using the average value calculation formula; the fish head heading set includes multiple fish head heading angles;

[0021] Calculate the difference between the desired heading in the current control cycle and the average heading deviation in the current control cycle to obtain the error value of the current control cycle;

[0022] Using the error value of the current control cycle as input, the PD controller updates the servo control signal of the tail-wagging robotic fish, and controls the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal.

[0023] A target tracking system for a tail-swivel robotic fish, applied to a tail-swivel robotic fish driven by a single servo motor, the target tracking system comprising:

[0024] The expected course calculation module is used to acquire images of the target object obtained by taking pictures of the target object within the current control cycle, and to determine the expected course of the tail-wagging robotic fish based on the target object images.

[0025] The heading deviation calculation module is used to obtain the fish head heading set collected in the current control cycle; using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, the average heading deviation of the current control cycle is calculated using the average value calculation formula; the fish head heading set includes multiple fish head heading angles;

[0026] The error value calculation module is used to calculate the difference between the expected heading of the current control cycle and the average heading deviation of the current control cycle, so as to obtain the error value of the current control cycle.

[0027] The tracking module is used to update the servo control signal of the tail-wagging robotic fish using the PD controller with the error value of the current control cycle as input, and to control the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal.

[0028] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0029] This invention provides a method and system for closed-loop heading control and target tracking of a tail-swivel robotic fish. Applied to a single servo-driven tail-swivel robotic fish, the method uses the fish's head heading set of the current control cycle and the fish's head heading set of the previous control cycle as inputs. The average heading deviation of the current control cycle is calculated using an average value calculation formula. The difference between the average heading deviation of the current control cycle and the initially specified heading is calculated to obtain the error value of the current control cycle. It then determines whether the error value of the current control cycle exceeds a preset threshold. If it does, the servo control signal of the tail-swivel robotic fish is updated using the PD controller, and the movement of the tail-swivel robotic fish in the next control cycle is controlled according to the updated servo control signal. If not, the movement of the tail-swivel robotic fish in the next control cycle is controlled according to the servo control signal of the current control cycle. Therefore, autonomous control of the tail-swivel robotic fish can be achieved without establishing an accurate model, preparing a dataset in advance, or using any auxiliary equipment, ensuring the heading stability of the tail-swivel robotic fish. Attached Figure Description

[0030] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0031] Figure 1 This is a flowchart of the heading closed-loop control method provided in Embodiment 1 of the present invention;

[0032] Figure 2 This is a simulation diagram showing the stable heading of the robotic fish under closed-loop control provided in Embodiment 1 of the present invention.

[0033] Figure 3 This is a block diagram of the closed-loop control process of the robotic fish provided in Embodiment 1 of the present invention;

[0034] Figure 4 This is a graph showing the closed-loop test data of the robotic fish provided in Embodiment 1 of the present invention;

[0035] Figure 5 This is a system block diagram of the heading closed-loop control system provided in Embodiment 2 of the present invention;

[0036] Figure 6 This is a flowchart of the active tracking method provided in Embodiment 3 of the present invention;

[0037] Figure 7 This is a block diagram illustrating the principle of the active tracking method provided in Embodiment 3 of the present invention;

[0038] Figure 8This is a schematic diagram illustrating the effect of active tracking provided in Embodiment 3 of the present invention;

[0039] Figure 9 This is a system block diagram of the active tracking system provided in Embodiment 4 of the present invention. Detailed Implementation

[0040] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0041] The purpose of this invention is to provide a method and system for closed-loop heading control and target tracking of a tail-wagging robotic fish, which can realize autonomous control of the robotic fish and is easy to apply in actual aquatic environments.

[0042] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0043] Example 1:

[0044] This embodiment provides a closed-loop heading control method for a tail-swinging robotic fish, applied to a single servo-driven tail-swinging robotic fish. It enables autonomous control of the robotic fish relying solely on a single servo for tail-swinging. In this single-servo-driven tail-swinging robotic fish, the mechanism primarily uses a single servo within the robotic fish's body to drive a cable reel, thereby driving the tail to swing and providing power. The cable reel moves the tail via a wound steel wire. Because the elastic deformation of the steel wire along its length is extremely small, the amplitude of the tail swing can be controlled by adjusting the rotation angle of the servo during control. This type of tail-swinging robotic fish is an existing structure and will not be described in detail here. Figure 1 As shown, the heading closed-loop control method includes:

[0045] S1: Obtain the set of fish head headings collected in the current control cycle; the set of fish head headings includes multiple fish head heading angles collected in the current control cycle;

[0046] In this embodiment, an onboard inertial unit (i.e., IMU sensor) is installed at the head of the tail-swishing robotic fish. Since the head and tail of the tail-swishing robotic fish are rigidly connected, the onboard inertial unit collects the heading angle of the tail-swishing robotic fish in real time. Based on the sampling frequency of the onboard inertial unit, the number of heading angles that can be collected in one control cycle is determined, and the heading angle set of each control cycle is obtained.

[0047] S2: Using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, the average heading deviation of the current control cycle is calculated using the average value calculation formula;

[0048] The formula for calculating the average value used in this embodiment is:

[0049]

[0050] Where, Δx k This represents the average heading deviation; The heading angle of the i-th fish head in the current control cycle's fish head heading set; Let be the i-th head heading angle of the head heading set in the previous control cycle; N is the total number of head heading angles in the head heading set; i = 1, 2, ..., N.

[0051] It should be noted that the above average calculation formula is essentially an averaging method. This averaging method needs to be performed between two control cycles, rather than within a single control cycle. This is because during the movement of the robotic fish, the fluctuating heading pattern makes sensor sampling highly susceptible to noise interference. The average heading obtained by averaging within a single control cycle is inaccurate in the presence of random noise interference, leading to inaccurate error values ​​obtained by subtracting the expected heading from the average heading. This embodiment, however, uses the above average calculation formula to obtain a more accurate heading offset between two control cycles with less noise, effectively avoiding the influence of glitch points.

[0052] S3: Calculate the difference between the average heading deviation of the current control cycle and the initial specified heading to obtain the error value of the current control cycle; determine whether the error value of the current control cycle exceeds a preset threshold.

[0053] The essence of closed-loop control is to compare the deviation between the current heading and the desired heading, and to correct the heading by reducing this deviation. When the tail-wagging robotic fish moves, its measured head heading angle fluctuates periodically. To ensure it moves according to the initially specified heading, the error value of the current control cycle is checked at the end of each control cycle to see if it is within an acceptable range. If the error value of the current control cycle exceeds a preset threshold, meaning the tail-wagging robotic fish has deviated from the initially specified heading in the current control cycle, deviation correction is required. If the error value of the current control cycle does not exceed the preset threshold, deviation correction is not required. In this embodiment, the average heading of the initial control cycle is used as the initial specified heading, which is also the desired heading; therefore, the current control cycle is greater than or equal to 2.

[0054] S4: If so, the error value of the current control cycle is used as input, the PD controller is used to update the servo control signal of the tail-wagging robotic fish, and the movement of the tail-wagging robotic fish in the next control cycle is controlled according to the updated servo control signal.

[0055] In this embodiment, the expression for the PD controller is:

[0056] U PD (k)=K p e(k)+K d Δe(k);

[0057] Among them, U PD (k) represents the output value of the PD controller; K p For the proportional element parameters of the PD controller; K d Here are the differential parameters of the PD controller; e(k) is the error; and Δe(k) is the error rate of change.

[0058] In this embodiment, the expression for the servo control signal is:

[0059] α(t) = Asin(ωt) + B(t);

[0060] Where α(t) is the servo control signal; A is the amplitude of the fish tail swing; ω is the angular frequency; t is the time; and B(t) is the bias adjustment amount.

[0061] At this point, updating the servo control signal of the tail-wagging robotic fish using the PD controller, with the error value of the current control cycle as input, can include: using the error value of the current control cycle as the error, and the difference between the error value of the current control cycle and the error value of the previous control cycle as the error change rate, inputting these values ​​to the PD controller to obtain the output value U of the PD controller. PD (k); The output value U of the PD controller PD (k) is added to the discrete form of the bias adjustment amount B(t) of the servo control signal of the tail-wagging robotic fish, such that B(k+1)=B(k)+U PD (k) adjusts the bias adjustment amount B(t) in the servo control signal to update the servo control signal of the tail-wagging robotic fish.

[0062] S5: If not, then control the movement of the tail-wagging robotic fish in the next control cycle according to the servo control signal of the current control cycle.

[0063] If not, the error and error rate of the PD controller are both 0, and the output value of the PD controller is also 0. Therefore, the servo control signal will not be updated. At this time, B(k+1) = B(k) and B(0) = 0.

[0064] The tail-wagging propulsion motion of the robotic fish in this embodiment causes periodic fluctuations in the measured heading, which is particularly characterized by a higher oscillation frequency and larger amplitude compared to robotic fish with other propulsion methods. This embodiment uses the initial designated heading of the tail-wagging robotic fish and the real-time heading collected by the onboard inertial unit to perform deviation correction control, thereby achieving closed-loop stable control of the robotic fish's heading.

[0065] In this embodiment, a relationship between the tail swing and the head heading is established based on the robotic fish's swing model, forming a PD controller and control parameters. The method for establishing the PD controller includes:

[0066] (1) Using the servo control signal as the input signal and the fish head heading motion signal as the output signal, construct the transfer function between the input signal and the output signal;

[0067] This robotic fish, propelled by a single servo motor, is powered solely by a tail servo motor that mechanically couples to drive the tail's swaying motion. Its key feature is the simplification of energy consumption units while retaining the biomimetic nature of a fish's tail-swaying movement. By controlling the amplitude, frequency, and offset of the tail's swaying motion, the robotic fish can perform actions such as starting, accelerating, and turning. Based on the unique motion mode of this single-servo-driven tail-swaying robotic fish—where the servo motor outputs a control signal to drive the tail swaying, which in turn causes the head to sway, changing the head's heading—the tail swaying control signal (i.e., the servo motor control signal) α(t) is in the form α(t) = Asin(ωt) + B(t), where A is the tail swaying amplitude, B(t) is the offset adjustment, and the head heading, experimentally measured to be the same as the tail swaying frequency, exhibits a nonlinear, periodic oscillating motion signal. The form is Where D is the amplitude of the fish head sway. It is easy to see that the servo control signal and the fish head heading motion signal have the same fluctuation pattern, only the sway amplitude is different, and there is some lag between the two signals. By analyzing the open-loop experimental data, the transfer function G(s) of the servo control signal input and the fish head heading output can be expressed in the form of:

[0068]

[0069] Where s is a complex independent variable, and a and b are system model parameters. This model mainly reflects the time lag between the control signal and the output signal, which can be adjusted primarily by parameter a.

[0070] The open-loop data was acquired by allowing the robotic fish to swim in an open-loop state while simultaneously recording the servo rotation data and the fish's head heading data. By comparing the two sets of data, the input-output relationship was determined. It was found that the two signals were sinusoidal signals with equal frequencies but a certain phase difference, and their relationship characteristics could be described using the transfer function described above. The adjustment of parameters a and b was also based on fitting the open-loop experimental data, and the adjustment method was to compensate for the time lag between the input and output.

[0071] (2) Obtain the differential equation based on the transfer function;

[0072] The differential equation can be derived from the transfer function as follows:

[0073]

[0074] Where Y(s) is the complex expression of the output signal; U(s) is the complex expression of the input signal; Y(t) and y(t) are the time-domain expressions of the output signal; and U(t) and u(t) are the time-domain expressions of the input signal.

[0075] (3) Select the fish head heading angle as the state variable, and determine the state space according to the differential equation;

[0076] Select the heading angle of the robotic fish Let x be the first state variable of the system, representing the angular velocity of the robotic fish's head. This is the second state variable of the system, namely... The state-space description of the robotic fish's head heading and control signals is obtained as follows:

[0077]

[0078] (4) Discretize the state space to obtain discrete state expressions;

[0079] set up Its discrete state transition quantity dx=f(x)·Δt, then its discrete state expression is:

[0080] x i+1 =x i +f(x i )·Δt.

[0081] (5) Perform simulation analysis based on the discrete state expression to determine the parameters of the PD controller in order to establish the PD controller.

[0082] A robotic fish simulation platform based on the above transfer function was built in Matlab. This platform can simulate the movement of a robotic fish. By inputting servo control signals, it outputs two state variables: the robotic fish's heading angle and angular velocity. At this point, the controller structure and parameter design can begin. The simulation analysis process is as follows:

[0083] The heading data of adjacent control cycles during the movement of the robotic fish can be collected. To obtain the accurate average offset between adjacent control cycles in the nonlinear motion of the robotic fish, state variables are introduced when two or more control cycles are run. and Let represent the heading data for the k-th control cycle and the heading data for the (k-1)-th control cycle, respectively. The number of data points in each control cycle is N. The average heading deviation for the k-th control cycle is obtained using the average value calculation formula. Based on the error between the average heading deviation and the initial specified heading, the bias adjustment B(t) in the control input signal α(t) = Asin(ωt) + B(t) is modified, such that B... k+1 =B k +qΔx k B k+1 This is the updated bias adjustment, which is also the bias adjustment in the (k+1)th control cycle; B k Δx represents the original bias adjustment, i.e., the bias adjustment in the k-th control cycle; q represents the controller parameter, Δx. k The error value of the k-th control cycle forms a basic closed-loop controller. This allows for the construction of a heading control simulation model for the robotic fish. The controller parameter q is then adjusted to stabilize the heading within a certain timeframe. The order of magnitude range of the control parameters can be determined. Under the controller's regulation, the robotic fish reduces the deviation from the initial specified heading caused by disturbances starting from the second control cycle, thus achieving stable heading control under the average deviation analysis of the control cycle. Specifically, in the simulation, the input signal is α(t) = 15sin(2πt), and the output simulates a heading deviation disturbance signal r(t), causing the robotic fish to deviate by 0.01 rad / s in open-loop uncontrolled bias. Here, the initial specified heading is taken as the desired heading. By adjusting the controller parameter q, the heading angle is stabilized to the initial specified heading. Figure 2 To closely approximate real-world conditions, white noise was superimposed on the base signal to simulate inherent underwater noise interference. This simulation demonstrates the model's controllability and allows for initial adjustment of the controller parameters.

[0084] Accurate dynamic modeling of a robotic fish is extremely difficult. Simplified kinematic modeling methods, such as those based on transfer functions, often lack information on internal mechanism features and higher-order motion components, making it impossible to guide controller selection or parameter adjustment through simulation. Regarding controller selection, the robotic fish system exhibits high-frequency nonlinear motion characteristics during operation, thus requiring a high response speed. Considering the potential for control lag or system integral saturation due to integral components, this embodiment uses a PD controller to achieve closed-loop heading control of the robotic fish. The robotic fish's heading is acquired in real-time by an onboard inertial unit, and the average deviation over control cycles is calculated. The error between the average heading deviation of adjacent control cycles and the initial specified heading is used as the input error e(k) of the PD controller. The parameter K is first adjusted near q. p This achieves basic heading stability, and then the appropriate parameter K is adjusted based on the experimental results. d The stable speed can be adjusted to the optimal value. The block diagram of this closed-loop control process is as follows: Figure 3 .

[0085] This method was tested in a laboratory setting using a water tank measuring 13 meters long and 2 meters wide. Figure 4 The experimental data collected are shown on the horizontal axis as time and the vertical axis as the swing angle. The curve with the triangle represents the predetermined heading, the curve with the multiplication sign represents the real-time heading angle, and the other curve represents the real-time swing angle of the servo motor. It can be seen that the robotic fish maintained the predetermined heading for the first 8 seconds. To see the control effect more intuitively, the robotic fish was pushed away from the predetermined heading in the 8th second of its movement. To simulate the uncertain disturbances in actual water, it can be seen that the robotic fish was able to return to the predetermined heading autonomously. This shows that the control method can enable the robotic fish to achieve heading stability under external disturbances.

[0086] This embodiment provides a closed-loop heading control method for a robotic fish that achieves tail-wagging drive using only a single servo motor. A stable control model for this variable frequency and variable amplitude tail-wagging motion mode is proposed. The system collects the robotic fish's state information in real time through an onboard inertial unit. After data calculation, processing, and decision-making, a control signal is generated to adjust the robotic fish's heading, thereby achieving heading stability. This provides an effective reference for the control of biomimetic robotic fish under this driving mode and realizes unmanned, autonomous heading control of the robotic fish underwater without external equipment assistance.

[0087] Compared to existing technologies, the heading closed-loop control method disclosed in this embodiment has the following advantages:

[0088] (1) It can realize autonomous heading control of the nonlinear motion of the robotic fish. This method does not require the establishment of an accurate model of the robotic fish, but simplifies the model of its motion mechanism so that its motion control does not depend on the complex dynamic model of the mechanism. It is easy to implement and computationally economical.

[0089] (2) No need to prepare datasets or a large number of motion relationship mapping tables in advance, thus achieving data lightweighting during the operation of the robotic fish.

[0090] (3) Robotic fish with the same motion mechanism, especially those with similar single servo drive but different machine size or fish body wave curve, have similar control performance and parameter adjustment rules.

[0091] (4) It has a certain degree of robustness, especially against uncertain interference from internal mechanisms, noise interference from signal acquisition, and unknown interference from the external environment.

[0092] (5) No external global camera or other equipment is required.

[0093] Example 2:

[0094] This embodiment provides a heading closed-loop control system for a tail-swivel robotic fish, applied to a tail-swivel robotic fish driven by a single servo motor, such as... Figure 5 As shown, the heading closed-loop control system includes:

[0095] The data acquisition module M1 is used to acquire the fish head heading set collected in the current control cycle; the fish head heading set includes multiple fish head heading angles collected in the current control cycle.

[0096] The deviation calculation module M2 is used to calculate the average heading deviation of the current control period using the fish head heading set of the current control period and the fish head heading set of the previous control period as inputs and the average value calculation formula.

[0097] The judgment module M3 is used to calculate the difference between the average heading deviation of the current control cycle and the initial specified heading to obtain the error value of the current control cycle; and to determine whether the error value of the current control cycle exceeds a preset threshold.

[0098] The control module M4 is used to, if yes, update the servo control signal of the tail-wagging robotic fish using the PD controller with the error value of the current control cycle as input, and control the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal; if no, control the movement of the tail-wagging robotic fish in the next control cycle according to the servo control signal of the current control cycle.

[0099] Example 3:

[0100] This embodiment provides a target tracking method for a tail-swivel robotic fish, applied to a single servo-driven tail-swivel robotic fish. It achieves active target tracking based on the robotic fish's onboard vision assistance system and the heading closed-loop control method of Embodiment 1. Figure 6 and Figure 7 As shown, the target tracking method includes:

[0101] T1: Acquire the target object image obtained by taking pictures of the target object within the current control cycle, and determine the desired course of the tail-wagging robotic fish based on the target object image;

[0102] The tail-wagging robotic fish in this embodiment is equipped with a vision assistance system, such as a camera. The controller of the robotic fish is equipped with the YOLOX visual recognition algorithm. Before use, the robotic fish is pre-trained to learn images of one or more target objects to be recognized. During application, the robotic fish can recognize all the learned target objects and, according to the task requirements, can choose to track a specific target object. Therefore, with the assistance of the vision assistance system, the robotic fish can actively identify and capture target objects in its field of vision and acquire images of the target objects. During the movement of the robotic fish, the swaying of its head causes periodic camera shake, which is detrimental to image recognition and target localization. Therefore, during the robotic fish's movement, the onboard inertial unit sends control signals to the vision assistance system, capturing the target only when the fish's head returns to the preset intermediate position. The target image is then obtained when the fish's head moves to the preset intermediate position. Since the fish's head trajectory follows a sine function, it will return to the preset intermediate position twice within one control cycle. Thus, two target images are obtained in one control cycle. The position is calculated using these two target images from one control cycle, thereby reducing noise in target localization and obtaining the target's location, which is the desired trajectory.

[0103]

[0104] Where Φ represents the desired heading; (u, v) are the pixel coordinate pairs of the target object; (u1, v1) are the pixel coordinate pairs of the target object in the first image of the target object; and (u2, v2) are the pixel coordinate pairs of the target object in the second image of the target object. Due to this shooting method, it can be seen that the obtained Φ value is the deviation angle between the actual median phase of the robotic fish and the target position during the current control cycle.

[0105] It should be noted that during the robotic fish's movement, if the target object is captured in the field of vision, it is used to calculate the target's position and update the desired course. If it is not captured, it travels along the desired course obtained in the previous control cycle. YOLOX's inference speed can meet the above-mentioned image capture frequency requirements and ensure recognition accuracy.

[0106] T2: Obtain the fish head heading set collected in the current control cycle; using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, calculate the average heading deviation of the current control cycle using the average value calculation formula; the fish head heading set includes multiple fish head heading angles;

[0107] In the target tracking task, the average heading deviation between adjacent control cycles will be continuously calculated to reduce the noise caused by the heading error extracted solely by image recognition, thereby enhancing the control effect.

[0108] T3: Calculate the difference between the desired heading in the current control cycle and the average heading deviation in the current control cycle to obtain the error value of the current control cycle;

[0109] The formula for calculating the error value is:

[0110] Where e(k) is the error value of the robotic fish during the k-th control cycle, which is also the error of the PD controller; Φ(k) is the expected heading during the k-th control cycle. Let be the average heading deviation during the k-th control cycle.

[0111] When Φ(k) is less than a certain threshold (in this example, the threshold is ±5°), it means that in reality the robotic fish is considered to be very close to the target. To avoid... The resulting static error will be ignored during the subsequent approach motion, and the error value will be calculated using the formula e(k)=Φ(k). Otherwise, the above error calculation formula will be used.

[0112] T4: Using the error value of the current control cycle as input, update the servo control signal of the tail-wagging robotic fish using the PD controller, and control the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal.

[0113] Updating the servo control signal of the tail-swivel robotic fish using the current control cycle error value as input can include: using the current control cycle error value as the error, and the difference between the current control cycle error value and the previous control cycle error value as the error change rate, inputting these values ​​to the PD controller to obtain the PD controller's output value U. PD (k); The output value U of the PD controller PD (k) is added to the bias adjustment amount B(t) of the servo control signal of the tail-wagging robotic fish, so that B(k+1)=B(k)+U PD (k) adjusts the bias adjustment amount B(t) in the servo control signal to update the servo control signal of the tail-wagging robotic fish.

[0114] This embodiment reduces the error between the actual and desired course of the robotic fish using a PD controller. Based on an airborne vision assistance system and a closed-loop course control method, it achieves autonomous target tracking control in the presence of a target, enabling the robotic fish to actively track targets. The active target tracking experiment was tested in a laboratory tank measuring 2.5 meters long and 1.9 meters wide. Figure 8 As shown, this is a schematic diagram of the target tracking effect of the robotic fish in this experiment. The black sphere is the target object tracked by the robotic fish, and the dashed trajectory is the movement trajectory of the robotic fish. It can be seen that the target tracking method of this embodiment can effectively realize the tracking process of the robotic fish on the target object.

[0115] It should be noted that the robotic fish cannot perform autonomous underwater operations without the assistance of various sensors in order to cope with the complex working conditions of real-world work scenarios and the different needs of actual underwater tasks. Apart from the target tracking control based on the visual perception method mentioned in this embodiment, the same tracking control achieved by using other optical sensors and acoustic sensors for target positioning should not be considered to be beyond the scope of this invention.

[0116] The method disclosed in this invention primarily aims to enhance the autonomy of robotic fish, providing an easy-to-implement and effective closed-loop control method for robotic fish propelled by a single servo motor. This method differs from heading control based on multi-servo motor propelled robotic fish, mainly because the swing amplitude and frequency of multi-servo motor propelled robotic fish and single-servo motor propelled robotic fish are not in the same range, resulting in significant differences in their biomimetic swimming waveforms. Stable control algorithms achievable on multi-servo motor robotic fish often cannot achieve the same stability on single-servo motor robotic fish. Furthermore, this embodiment should be distinguished from existing research that attempts to achieve similar effects by controlling robotic fish through action groups. Based on this embodiment, the functionality, safety, autonomy, and scale of tasks performed by the robotic fish can be expanded and improved.

[0117] Example 4:

[0118] This embodiment provides a target tracking system for a tail-swivel robotic fish, applied to a tail-swivel robotic fish driven by a single servo motor, such as... Figure 9 As shown, the target tracking system includes:

[0119] The expected heading calculation module M5 is used to acquire images of the target object obtained by taking pictures of the target object within the current control cycle, and to determine the expected heading of the tail-wagging robotic fish based on the target object images.

[0120] The heading deviation calculation module M6 is used to obtain the fish head heading set collected in the current control cycle; using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, the average heading deviation of the current control cycle is calculated using the average value calculation formula; the fish head heading set includes multiple fish head heading angles;

[0121] The error value calculation module M7 is used to calculate the difference between the expected heading of the current control cycle and the average heading deviation of the current control cycle, so as to obtain the error value of the current control cycle.

[0122] The tracking module M8 is used to update the servo control signal of the tail-wagging robotic fish using the PD controller with the error value of the current control cycle as input, and to control the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal.

[0123] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0124] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A closed-loop heading control method for a tail-swinging robotic fish, applied to a tail-swinging robotic fish driven by a single servo motor, wherein the tail is rigidly connected to the head, and the method is characterized in that... The heading closed-loop control method includes: Obtain the set of fish head headings collected in the current control cycle; the set of fish head headings includes multiple fish head heading angles collected within the current control cycle; Using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, the average heading deviation of the current control cycle is calculated using the average value calculation formula. Calculate the difference between the average heading deviation of the current control cycle and the initial specified heading to obtain the error value of the current control cycle; determine whether the error value of the current control cycle exceeds a preset threshold. If so, the error value of the current control cycle is used as input, and the PD controller is used to update the servo control signal of the tail-wagging robotic fish. The movement of the tail-wagging robotic fish in the next control cycle is controlled according to the updated servo control signal. If not, then the movement of the tail-wagging robotic fish in the next control cycle is controlled according to the servo control signal of the current control cycle. The formula for calculating the average value is: Where, Δx k This represents the average heading deviation; The heading angle of the i-th fish head in the current control cycle's fish head heading set; Let be the i-th head heading angle in the head heading set of the previous control cycle; N is the total number of head heading angles in the head heading set; i = 1, 2, ..., N; The expression for the servo control signal is: α(t) = Asin(ωt) + B(t); Where α(t) is the servo control signal; A is the amplitude of the fish tail swing; ω is the angular frequency; t is the time; and B(t) is the bias adjustment amount. The step of using the error value of the current control cycle as input to update the servo control signal of the tail-wagging robotic fish using the PD controller specifically includes: The error value of the current control cycle is used as the error, and the difference between the error value of the current control cycle and the error value of the previous control cycle is used as the error change rate. These are input to the PD controller to obtain the output value of the PD controller. The output value of the PD controller is added to the bias adjustment of the servo control signal of the tail-wagging robotic fish to update the servo control signal of the tail-wagging robotic fish.

2. The heading closed-loop control method according to claim 1, characterized in that, The expression for the PD controller is: U PD (k)=K p e(k)+K d Δe(k); Among them, U PD (k) represents the output value of the PD controller; K p For the proportional element parameters of the PD controller; K d Here are the differential parameters of the PD controller; e(k) is the error; and Δe(k) is the error rate of change.

3. The heading closed-loop control method according to claim 2, characterized in that, The method for establishing the PD controller includes: Using the servo control signal as the input signal and the fish head heading motion signal as the output signal, a transfer function is constructed between the input signal and the output signal. The differential equation is obtained based on the transfer function; The heading angle of the fish head is selected as the state variable, and the state space is determined according to the differential equation. Discretize the state space to obtain discrete state expressions; Simulation analysis is performed based on the discrete state expression to determine the parameters of the PD controller, thereby establishing the PD controller.

4. A heading closed-loop control system for a tail-swinging robotic fish, applied to a tail-swinging robotic fish driven by a single servo motor, wherein the tail is driven to swing by a single servo motor, and the tail is rigidly connected to the head, characterized in that... The heading closed-loop control system includes: The data acquisition module is used to acquire the set of fish head headings collected in the current control cycle; the set of fish head headings includes multiple fish head heading angles collected in the current control cycle; The deviation calculation module is used to calculate the average heading deviation of the current control period using the fish head heading set of the current control period and the fish head heading set of the previous control period as inputs and the average value calculation formula. The judgment module is used to calculate the difference between the average heading deviation of the current control cycle and the initial specified heading to obtain the error value of the current control cycle; and to determine whether the error value of the current control cycle exceeds a preset threshold. The control module is configured to, if yes, use the error value of the current control cycle as input, update the servo control signal of the tail-wagging robotic fish using the PD controller, and control the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal; if no, control the movement of the tail-wagging robotic fish in the next control cycle according to the servo control signal of the current control cycle. The formula for calculating the average value is: Where, Δx k This represents the average heading deviation; The heading angle of the i-th fish head in the current control cycle's fish head heading set; Let be the i-th head heading angle in the head heading set of the previous control cycle; N is the total number of head heading angles in the head heading set; i = 1, 2, ..., N; The expression for the servo control signal is: α(t) = Asin(ωt) + B(t); Where α(t) is the servo control signal; A is the amplitude of the fish tail swing; ω is the angular frequency; t is the time; and B(t) is the bias adjustment amount. The step of using the error value of the current control cycle as input to update the servo control signal of the tail-wagging robotic fish using the PD controller specifically includes: The error value of the current control cycle is used as the error, and the difference between the error value of the current control cycle and the error value of the previous control cycle is used as the error change rate. These are input to the PD controller to obtain the output value of the PD controller. The output value of the PD controller is added to the bias adjustment of the servo control signal of the tail-wagging robotic fish to update the servo control signal of the tail-wagging robotic fish.

5. A target tracking method for a tail-swinging robotic fish, applied to a single servo-driven tail-swinging robotic fish, wherein the tail is rigidly connected to the head, and the method is characterized in that... The target tracking method includes: Acquire images of the target object obtained by taking pictures of the target object within the current control cycle, and determine the desired course of the tail-wagging robotic fish based on the target object images; Obtain the fish head heading set collected in the current control cycle; using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, calculate the average heading deviation of the current control cycle using the average value calculation formula; the fish head heading set includes multiple fish head heading angles; Calculate the difference between the desired heading in the current control cycle and the average heading deviation in the current control cycle to obtain the error value of the current control cycle; Using the error value of the current control cycle as input, the PD controller updates the servo control signal of the tail-wagging robotic fish, and controls the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal. The formula for calculating the average value is: Where, Δx k This represents the average heading deviation; The heading angle of the i-th fish head in the current control cycle's fish head heading set; Let be the i-th head heading angle in the head heading set of the previous control cycle; N is the total number of head heading angles in the head heading set; i = 1, 2, ..., N; The expression for the servo control signal is: α(t) = Asin(ωt) + B(t); Where α(t) is the servo control signal; A is the amplitude of the fish tail swing; ω is the angular frequency; t is the time; and B(t) is the bias adjustment amount. The step of using the error value of the current control cycle as input to update the servo control signal of the tail-wagging robotic fish using the PD controller specifically includes: The error value of the current control cycle is used as the error, and the difference between the error value of the current control cycle and the error value of the previous control cycle is used as the error change rate. These are input to the PD controller to obtain the output value of the PD controller. The output value of the PD controller is added to the bias adjustment of the servo control signal of the tail-wagging robotic fish to update the servo control signal of the tail-wagging robotic fish.

6. The target tracking method according to claim 5, characterized in that, The target image is an image taken when the head of the tail-wagging robotic fish moves to a preset middle position.

7. A target tracking system for a tail-swinging robotic fish, applied to a single servo-driven tail-swinging robotic fish, wherein the tail is rigidly connected to the head, and the system is characterized in that... The target tracking system includes: The expected course calculation module is used to acquire images of the target object obtained by taking pictures of the target object within the current control cycle, and to determine the expected course of the tail-wagging robotic fish based on the target object images. The heading deviation calculation module is used to obtain the fish head heading set collected in the current control cycle; using the fish head heading set of the current control cycle and the fish head heading set of the previous control cycle as input, the average heading deviation of the current control cycle is calculated using the average value calculation formula; the fish head heading set includes multiple fish head heading angles; The error value calculation module is used to calculate the difference between the expected heading of the current control cycle and the average heading deviation of the current control cycle, so as to obtain the error value of the current control cycle. The tracking module is used to update the servo control signal of the tail-wagging robotic fish using the PD controller with the error value of the current control cycle as input, and control the movement of the tail-wagging robotic fish in the next control cycle according to the updated servo control signal. The formula for calculating the average value is: Where, Δx k This represents the average heading deviation; The heading angle of the i-th fish head in the current control cycle's fish head heading set; Let be the i-th head heading angle in the head heading set of the previous control cycle; N is the total number of head heading angles in the head heading set; i = 1, 2, ..., N; The expression for the servo control signal is: α(t) = Asin(ωt) + B(t); Where α(t) is the servo control signal; A is the amplitude of the fish tail swing; ω is the angular frequency; t is the time; and B(t) is the bias adjustment amount. The step of using the error value of the current control cycle as input to update the servo control signal of the tail-wagging robotic fish using the PD controller specifically includes: The error value of the current control cycle is used as the error, and the difference between the error value of the current control cycle and the error value of the previous control cycle is used as the error change rate. These are input to the PD controller to obtain the output value of the PD controller. The output value of the PD controller is added to the bias adjustment of the servo control signal of the tail-wagging robotic fish to update the servo control signal of the tail-wagging robotic fish.

Images