A camera stabilization and target tracking system and method based on a robotic fish

Abstract

The application relates to a camera stabilizing and target tracking system and method based on a robotic fish, which comprises a holder control mechanism, a camera, a fish body control mechanism, a fish head posture sensor, a fish body posture sensor, a first controller and a second controller; the second controller comprises an image processing unit, a heading target angle controller and a feedforward controller; the application calculates the rotating angle of the holder by the horizontal distance between the target and the image point collected by the camera and the shaking angle of the robotic fish, so that the camera is more stable; and the swinging angle of the robotic fish is calculated according to the horizontal distance obtained above and the actual heading angle of the fish body measured by the fish body posture sensor, so that the robotic fish swims towards the target, the field of view of the robotic fish can be adjusted, and the target is always in the central field of view of the robotic fish.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of camera stabilization and target tracking, and particularly relates to a camera stabilization and target tracking system and method based on a robotic fish. BACKGROUND

[0002] The robotic fish has the advantages of low noise, low energy consumption and good camouflage in underwater operation due to its bionics. Compared with traditional methods, visual recognition can achieve higher control accuracy when operating in a near horizontal distance. However, most of the existing researches on robotic fish involve the imitation of fish swimming mechanism, corresponding motion control and cooperation mechanism, and less optimization of visual perception of the robotic fish.

[0003] Because of the special propulsion method of the robotic fish, the robotic fish will inevitably swing left and right when moving forward. This swing is also reflected on the front visual system, which reduces the quality of the picture and increases the difficulty of recognition. In addition, after discovering the target object, the robotic fish still needs to swing, and at this time the target object is easy to move out of the field of view and be lost. SUMMARY

[0004] The purpose of the present application is to provide a camera stabilization and target tracking system and method based on a robotic fish, which makes the camera more stable and can adjust the field of view of the robotic fish so that the target is always in the center of the field of view of the robotic fish.

[0005] To achieve the above purpose, the present application provides the following scheme:

[0006] A camera stabilization system based on a robotic fish, comprising:

[0007] a gimbal control mechanism, a camera, a fish head attitude sensor, a fish body attitude sensor, a first controller, a second controller; the second controller comprises an image processing unit, a heading target angle controller and a feedforward controller;

[0008] The camera is installed on the robotic fish and is used to collect images of the area in front of the robotic fish.

[0009] The image processing unit is in communication connection with the camera, is used to identify the target in the image, obtain the target position of the target in the image, and calculate the horizontal distance between the target position and the center point of the image.

[0010] The heading target angle controller is in communication connection with the image processing unit and the fish head attitude sensor, and is used to calculate the fish body target heading angle of the robotic fish according to the horizontal distance and the actual heading angle of the gimbal measured by the fish head attitude sensor.

[0011] The feedforward controller is in communication connection with the heading target angle controller and the fish body posture sensor, and is configured to calculate the shaking angle of the camera according to the fish body target heading angle and the actual fish body heading angle measured by the fish body posture sensor.

[0012] The first controller is in communication connection with the second controller and the fish head posture sensor, and is configured to calculate the rotating angle of the camera according to the horizontal distance, the actual heading angle of the holder and the shaking angle.

[0013] The holder control mechanism is drivingly connected with the camera, and is configured to control the camera to rotate according to the rotating angle of the camera, so as to keep the camera stable.

[0014] Optionally, the holder control mechanism comprises a holder and a driving rudder.

[0015] The camera is fixedly connected with the rotating shaft of the holder.

[0016] The driving rudder is drivingly connected with the rotating shaft, and is configured to drive the camera to rotate.

[0017] Optionally, the winding disc is further provided, a driven wheel of the winding disc is in transmission connection with the rotating shaft, and a driving wheel of the winding disc is in transmission connection with the driving rudder.

[0018] Optionally, one end of the elastic line of the winding disc is connected with the driven wheel, and the other end is connected with the driving wheel; and the elastic line is distributed in the shape of “8”.

[0019] Optionally, a connecting frame is further provided, the connecting frame is installed on the robotic fish, and is configured to fix the holder control mechanism.

[0020] Optionally, an upper support and a lower support are further provided.

[0021] The upper support is connected with the upper end of the rotating shaft through a bearing, and the lower support is connected with the lower end of the rotating shaft through a bearing.

[0022] Optionally, the image processing unit is configured to identify the target in the image by using KCF-DSST algorithm and YOLO-X algorithm.

[0023] A target tracking system based on a robotic fish, comprising:

[0024] a fish body control mechanism, a holder control mechanism, a camera, a fish head posture sensor, a fish body posture sensor, a first controller and a second controller; the second controller comprises an image processing unit, a heading target angle controller and a feedforward controller.

[0025] The camera is installed on the robotic fish and used to collect images of the area in front of the robotic fish.

[0026] The image processing unit is in communication connection with the camera and used to identify a target in the image, obtain a target position of the target in the image, and calculate a horizontal distance between the target position and a center point of the image.

[0027] The heading target angle controller is in communication connection with the image processing unit and the fish head attitude sensor and used to calculate a fish body target heading angle of the robotic fish according to the horizontal distance and a pan-tilt actual heading angle measured by the fish head attitude sensor.

[0028] The feedforward controller is in communication connection with the heading target angle controller and the fish body attitude sensor and used to calculate a shaking angle of the camera according to the fish body target heading angle and a fish body actual heading angle measured by the fish body attitude sensor.

[0029] The first controller is in communication connection with the second controller and the fish head attitude sensor and used to calculate a rotation angle of the camera according to the horizontal distance, the pan-tilt actual heading angle and the shaking angle.

[0030] The pan-tilt control mechanism is in driving connection with the camera and used to control the camera to rotate according to the rotation angle of the camera so as to turn the camera to the target.

[0031] The fish body control mechanism is used to control the robotic fish to move according to the fish body target heading angle so as to turn the robotic fish to the target.

[0032] A camera stabilizing method based on a robotic fish, comprising:

[0033] Collecting images of the area in front of the robotic fish;

[0034] Identifying a target in the image by using an image processing unit, obtaining a target position of the target in the image, and calculating a horizontal distance between the target position and a center point of the image;

[0035] Calculating a fish body target heading angle of the robotic fish by a heading target angle controller according to the horizontal distance and a pan-tilt actual heading angle measured by a fish head attitude sensor;

[0036] Calculating a shaking angle of the camera by a feedforward controller according to the fish body target heading angle and a fish body actual heading angle measured by a fish body attitude sensor;

[0037] calculating, by a first controller, a rotation angle of the camera according to the horizontal distance, the actual yaw angle of the holder and the shaking angle;

[0038] controlling, by a holder control mechanism, the camera to rotate according to the rotation angle of the camera, so that the camera keeps stable.

[0039] A control method of a target tracking system based on a robotic fish, comprising:

[0040] collecting an image of a region in front of the robotic fish;

[0041] identifying a target in the image by using an image processing unit, obtaining a target position of the target in the image, and calculating a horizontal distance between the target position and a center point of the image;

[0042] calculating, by a heading target angle controller, a body target yaw angle of the robotic fish according to the horizontal distance and an actual yaw angle of the holder measured by a fish head posture sensor;

[0043] calculating, by a feedforward controller, a shaking angle of the camera according to the body target yaw angle and an actual yaw angle of the body measured by a body posture sensor;

[0044] calculating, by a first controller, a rotation angle of the camera according to the horizontal distance, the actual yaw angle of the holder and the shaking angle;

[0045] controlling, by a holder control mechanism, the camera to rotate according to the rotation angle of the camera, so that the camera keeps stable.

[0046] controlling, by a body control mechanism, the robotic fish to move according to the body target yaw angle, so that the robotic fish turns to the target.

[0047] According to the specific embodiments provided by the present application, the present application discloses the following technical effects: the camera stabilizing and target tracking system and method based on a robotic fish provided by the present application, comprising: a holder control mechanism, a fish body control mechanism, a camera, a fish head attitude sensor, a fish body attitude sensor, a first controller, and a second controller; the second controller comprises an image processing unit, a heading target angle controller, and a feedforward controller; the camera is installed on the robotic fish and used for collecting images of the area in front of the robotic fish; the image processing unit is in communication connection with the camera, used for identifying the target in the images, obtaining the target position of the target in the images, and calculating the horizontal distance between the target position and the center point of the images; the heading target angle controller is in communication connection with the image processing unit and the fish head attitude sensor, used for calculating the fish body target heading angle of the robotic fish according to the horizontal distance and the actual heading angle of the holder measured by the fish head attitude sensor; the feedforward controller is in communication connection with the heading target angle controller and the fish body attitude sensor, used for calculating the shaking angle of the camera according to the fish body target heading angle and the actual heading angle of the fish body measured by the fish body attitude sensor; the first controller is in communication connection with the second controller and the fish head attitude sensor, used for calculating the rotation angle of the camera according to the horizontal distance, the actual heading angle of the holder, and the shaking angle; the holder control mechanism is in driving connection with the camera; the holder control mechanism is used for controlling the rotation of the camera according to the rotation angle of the camera, so as to keep the camera stable. The present application calculates the rotation angle of the holder by the horizontal distance between the target and the center point of the images collected by the camera and the shaking angle of the robotic fish, so that the camera is more stable; and the swinging angle of the robotic fish is calculated according to the horizontal distance obtained above and the actual heading angle of the fish body measured by the fish body attitude sensor, so that the robotic fish swims towards the target, the field of view of the robotic fish can be adjusted, and the target is always in the center of the field of view of the robotic fish. BRIEF DESCRIPTION OF DRAWINGS

[0048] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments will be briefly introduced below. Obviously, the drawings in the following description only constitute some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0049] Figure 1 A robotic fish structure diagram is provided for the embodiment 1 of the present application.

[0050] Figure 2 A camera stabilizing system structure diagram based on a robotic fish is provided for the embodiment 1 of the present application.

[0051] Figure 3 A control block diagram of a holder camera stabilizing system of a robotic fish is provided for the embodiment 1 of the present application.

[0052] Figure 4 A test effect diagram of the camera stabilizing system based on the robotic fish provided for the embodiment 1 of the present application is shown in the figure;

[0053] Figure 5 A fish tail motion curve diagram provided for the embodiment 1 of the present application is shown in the figure;

[0054] Figure 6 A test effect diagram of the target tracking system based on the robotic fish provided for the embodiment 2 of the present application is shown in the figure;

[0055] Figure 7 An underwater target tracking system experiment diagram provided for the embodiment 2 of the present application is shown in the figure.

[0056] The figure shows the specific embodiments of the present application, wherein the figure is a schematic diagram of the camera stabilizing system based on the robotic fish. DETAILED DESCRIPTION

[0057] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to 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 of the present application. Based on the embodiments in the present application, all the other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

[0058] The purpose of the present application is to provide a camera stabilizing and target tracking system and method based on the robotic fish, so that the camera is more stable, and the field of view of the robotic fish can be adjusted to keep the target in the center of the field of view of the robotic fish.

[0059] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0060] Embodiment 1

[0061] The embodiment provides a camera stabilizing system based on the robotic fish, as shown in the figures, Figure 1 and Figure 2 ,

[0062] The camera stabilizing system comprises:

[0063] The gimbal control mechanism, the camera 8, the fish head posture sensor 3, the fish body posture sensor 5, the first controller 4, and the second controller 6; the second controller 6 includes an image processing unit, a heading target angle controller, and a feedforward controller. The fish head posture sensor 3 is installed on the head of the robotic fish 7 and is used to measure the actual heading angle of the gimbal relative to the world coordinate system. The fish body posture sensor 5 is arranged in the middle of the fish body of the robotic fish 7 and is used to measure the actual heading angle of the fish body relative to the world coordinate system.

[0064] The camera 8 is installed on the robotic fish 7 and is used to collect images of the area in front of the robotic fish 7.

[0065] The image processing unit is in communication with the camera 8 and is used to identify the target in the image, obtain the target position of the target in the image, and calculate the horizontal distance between the target position and the center point of the image.

[0066] The heading target angle controller is in communication with the image processing unit and the fish head posture sensor 3 and is used to calculate the target heading angle of the fish body of the robotic fish 7 according to the horizontal distance and the actual heading angle of the gimbal measured by the fish head posture sensor 3.

[0067] The feedforward controller is in communication with the heading target angle controller and the fish body posture sensor 5 and is used to calculate the shaking angle of the camera 8 according to the target heading angle of the fish body and the actual heading angle of the fish body measured by the fish body posture sensor 5.

[0068] The first controller 4 is in communication with the second controller 6 and the fish head posture sensor 3 and is used to calculate the rotation angle of the camera 8 according to the horizontal distance, the actual heading angle of the gimbal, and the shaking angle.

[0069] The gimbal control mechanism is drivingly connected to the camera 8; the gimbal control mechanism is used to control the rotation of the camera 8 according to the rotation angle of the camera 8 to keep the camera 8 stable.

[0070] In this embodiment, the head part of the robotic fish 7 is composed of a transparent water-tight cover and is fixed to the fish body part by bolts and nuts; the fish body part of the robotic fish 7 is composed of a plurality of joints that can swing and can swing by being connected to the fish body drive steering engine 1; the tail part of the robotic fish 7 is composed of a silicone material and is internally supplemented with a skeleton material and is connected to the last joint of the fish body part by bolts and nuts and freely moves with the body swing.

[0071] The gimbal control mechanism comprises a gimbal and a driving rudder 1; the camera 8 is fixedly connected with a rotating shaft of the gimbal; the driving rudder 1 is drivingly connected with the rotating shaft, for driving the camera 8 to rotate.

[0072] The rear end of the gimbal control mechanism in the embodiment is fixed, and is provided with the driving rudder 1 and necessary fixing structures; the front end is rotatable, and comprises a fish head posture sensor 3 and a camera 8, which can rotate together. The fish head posture sensor 3 is installed at the top end of the gimbal control mechanism. Next, the specific designs of the front end and the rear end will be explained respectively.

[0073] In the embodiment, the camera stabilizing system based on the robotic fish 7 further comprises a winding reel 2; a driven wheel of the winding reel 2 is drivingly connected with the rotating shaft, and a driving wheel of the winding reel 2 is drivingly connected with the driving rudder 1. The front end and the rear end are connected through the winding reel 2, so that the rudder can control the angle of the structure of the front end. One end of a spring wire of the winding reel 2 is connected with the driven wheel, and the other end is connected with the driving wheel; the spring wire is distributed in the shape of "8". The rotation generated by the rudder is transmitted to the gimbal part, and the transmission ratio thereof can accelerate the rotation generated by the rudder, so that the gimbal is more sensitive. The torsion increases the range of action of the spring wire, reduces the dead zone that may be generated when the driving rudder 1 changes the direction of rotation, and enhances the sensitivity of the whole device.

[0074] In the embodiment, the camera stabilizing system based on the robotic fish 7 further comprises a connecting frame 9, an upper support 10 and a lower support 11; the connecting frame 9 is installed on the robotic fish 7, for fixing the gimbal control mechanism.

[0075] Specifically, the connecting frame 9 is made by cutting a carbon fiber plate, and plays a role of supporting the whole camera stabilizing system and connecting it to the watertight front cover. A set of connecting holes on the connecting frame 9 fix the fixing frame of the driving rudder 1, which can be connected to the fixed part of the driving rudder 1 through 3D printing processing. The rotating part of the rudder is connected with the driving wheel in the winding reel 2, can be transmitted to the driven wheel, and then makes the whole front end structure rotate.

[0076] The upper support 10 is connected with the upper end of the rotating shaft through a bearing, and the lower support 11 is connected with the lower end of the rotating shaft through a bearing. In the embodiment, the bearing is a type MF115ZZ flange bearing 12, for reducing the friction in the rotating process of the front end. The camera 8 and the connecting piece pass through two shafts respectively, are connected with the fish head posture sensor 3 above, and are connected with the driven wheel of the winding reel 2. At this time, the fish head posture sensor 3, the camera 8 part and the driven wheel of the winding reel 2 below are fixed on the same shaft and rotate together. Such a design guarantees the rotating range of the front end, and can control the rotation of the front end under the condition that the rear end is fixed.

[0077] The camera 8 collects the area information in front of the robotic fish 7 to obtain a plurality of images; the image processing unit adopts an image recognition method combining the KCF-DSST algorithm and the YOLO-X algorithm to recognize the target in the image, and the target is found again after being lost, so as to improve the stability of target recognition.

[0078] In the embodiment, when the target does not exist in the image captured by the camera 8 all the time, the target tracking function of the robotic fish 7 is not enabled, and the initial heading straight flight recorded by the fish body posture sensor 5 after the water is pressed and initialized.

[0079] When the target exists in the original camera 8 screen, and the target does not exist in the current screen due to movement of the target or external disturbance, the robotic fish 7 moves in the direction of the last recorded target.

[0080] In the embodiment, the target position is obtained by the camera 8, and then the image processing unit in the second controller 6 recognizes the target. The above-mentioned target position is the position of the target object in the camera 8 screen. The horizontal coordinate of the position is subtracted from the horizontal coordinate of the central field of view to obtain the horizontal distance d1 of the target object from the image center point of the central field of view. The horizontal distance of the target position from the central field of view obtained after calculation will enter a heading target angle controller in the second controller 6, and the heading target angle controller can be a PID controller, the output of which will be transmitted to the first controller 4 through a serial port, and added to the initial angle recorded by the fish head posture sensor 3 and the output angle of the feedforward controller. The sum of the two becomes a new angle to be tracked by the camera.

[0081] In the embodiment, the control system of the robotic fish 7 includes a gimbal target angle controller, a feedforward controller and a heading target angle controller. The above-mentioned three controllers are control models. The first controller 4 and the second controller 6 described above are single-chip microcomputers actually mounted on the robotic fish 7. The gimbal target angle controller is calculated in the first controller 4; the image recognition, the feedforward controller and the heading target angle controller are calculated in the second controller 6. In the embodiment, the first controller 4 is an stm32 control board, and the second controller 6 is a Raspberry Pi control board.

[0082] I. Heading target angle controller

[0083] The distance d1 between the target object and the center of the field of view in the input signal of the heading target angle controller is obtained by using a KCF-DSST algorithm combined with a YOLO-X algorithm image recognition method after the second controller 6 reads the camera 8 picture, and the difference d2 between the actual heading angle of the gimbal measured by the fish head posture sensor and the initial value of the fish head posture sensor is obtained by communicating with the first controller 4. The sum d1+d2 of the two is used as the input of the heading target angle controller, and the output of the heading target angle controller is the angle that the robotic fish 7 needs to adjust, which acts on the fish body driving rudder to adjust the heading of the robotic fish 7. Specifically:

[0084] 1. Obtain the error amount e of the previous two iterations 1pre_2 and the error amount e of the previous iteration pre_1 ;

[0085] 2. Calculate the new iteration error amount e1 of the current iteration

[0086]

[0087] Where e1 is the error amount of the current iteration;

[0088] is the average value of the readings of the current fish body posture sensor 5; the average value is the average value of all fish body posture sensor 5 readings in one second; θ body_init is the initial reading of the fish body posture sensor 5; d1 is the horizontal distance between the target object and the center of the field of view obtained after image recognition; α is an empirical parameter for converting the horizontal distance in the picture to a rotation angle; d2=θ head -θ head_init (i.e., the measured value of the fish head posture sensor 3 minus the initial reading of the fish head posture sensor 3).

[0089] Then, the error amount e of the previous two iterations 1pre_2 and the error amount e of the previous iteration pre_1 and the error amount e1 of the current iteration are used to calculate the offset angle (angle of swing) of the swing of the fish tail driving rudder 1 (i.e., the fish body driving rudder 1) in the current iteration:

[0090] tail_out=tail_out+k p *e1+k i *e 1pre_1 +k d *(e1+e 1pre_2 -2*e 1pre_1 )

[0091] Where tail_out is the offset angle of the fish tail swing, changing the value can achieve the adjustment of the heading of the robotic fish 7; k p is the proportional coefficient; k i is the integral coefficient; kd is the differential coefficient.

[0092] II. Feedforward controller

[0093] The robot fish 7 has a fixed swimming frequency, and this embodiment takes this swimming mode into account, that is, the second controller 6 obtains the difference between the actual target heading of the fish body and the target heading of the fish body, that is, the shaking angle of the fish body, and transmits it to the camera stabilizer, which generates an opposite angle to offset it, that is, a feedforward control link is added to the system. Through this link, the system can better obtain the shaking angle that will be generated, and actively rotate in the opposite direction to offset this angle. At the same time, the camera stabilizer can also offset the small shaking from other sources in this process.

[0094] The input signal of the feedforward controller is the actual heading angle of the fish body measured by the fish body posture sensor 5 and the current target heading angle of the fish, and by subtracting the two, the shaking angle of the robot fish 7 can be obtained. After taking the opposite of the shaking angle, it is transmitted to the gimbal target angle controller to offset the shaking of the fish itself. Specifically:

[0095] out1 = -(θ body - θ body_init - θ body_adv )* β

[0096] Where out1 is the output result of the feedforward controller; θ body is the current reading of the fish body posture sensor 5; θ body_init is the initial reading of the fish body posture sensor 5; θ body_adv is the target heading angle of the fish body; and β is a parameter for adjusting the strength of the feedforward controller.

[0097] In this embodiment, the actual heading angle of the fish body measured and the current target heading angle of the fish can also be used as input to obtain the shaking angle of the robot fish 7 using the following transfer function.

[0098] III. Introduction to the iterative implementation process of the gimbal target angle controller:

[0099] Figure 3 The gimbal camera stabilizing control system of the robot fish 7, the error can be obtained by calculating the deviation between the initial angle and the heading angle collected by the current fish head posture sensor 3, and this error is used as the input of the PID controller. The corresponding rudder control signal can be obtained by calculating in the first controller 4, and the control signal is transmitted to the driving rudder 1 to rotate and reduce the angle error, so that the current heading angle continuously approaches the recorded initial heading angle.

[0100] The input signal of the gimbal target angle controller is the sum of the actual heading angle, initial angle, and target angle measured by the current fish head attitude sensor, where the initial angle is the reading of the fish head attitude sensor 3 after initialization. The target angle is given by the second controller 6, specifically the sum of the feedforward controller output out1 and the distance d1 between the target object and the center of the field of view obtained after image recognition, i.e., out1 + d1. The tracking of this target angle by the servo gimbal achieves: 1) counteracting the influence of the fish's own shaking on the camera 8 on the servo gimbal; 2) turning the camera 8 on the servo gimbal toward the target object. Specifically:

[0101] 1. Obtain the error e from the first two iterations. 2pre_2 Error e from the previous iteration pre_1 .

[0102] 2. Calculate the new iteration error e2.

[0103] e2=(θ head_init +d1*α+out1)-θ head

[0104] Where e2 is the iteration error of this round; θ head The current reading of the fish head attitude sensor 3; θ head_init Initialize the readings for the fish head posture sensor 3; d1 is the horizontal distance between the target object and the center of the field of view obtained after image recognition; α is an empirical parameter for converting the distance in the image into a rotation angle; out1 is the output result of the feedforward controller.

[0105] 3. Using the error e from the first two iterations 2pre_2 The error e from the previous iteration pre_1 And the control signal for the gimbal drive servo 1 in this iteration is calculated based on the error e2 of this iteration:

[0106] Servo_out = Servo_out + k p *e2+kd*(e2+e 2pre_2 -2*e 2pre_1 )

[0107] Servo_out is a PWM wave frequency; changing this value changes the output angle of servo motor 1. p k is the proportionality coefficient. d e is the differential coefficient; e is the error in this iteration; e pre_1 e represents the error from the previous iteration. pre_2 This represents the error amount from the first two iterations.

[0108] 4. The rotation angle of the camera holder 8 is obtained by multiplying the control signal of the gimbal drive steering engine 1 obtained in step 3 above by a parameter for converting the control signal into a rotation angle. The transfer function of the drive steering engine 1 is:

[0109]

[0110] wherein s1 is a complex independent variable, and t0 is a steering engine delay parameter.

[0111] As shown in Figure 4 , the camera stabilizing system provided in the embodiment is tested.

[0112] The feasibility of the target tracking system is detected by land-based experiments in the embodiment. Figure 4 (b) the picture effect after using the camera stabilizing system, compared with Figure 4 (a) the picture effect without using the camera stabilizing system, the angle change is obviously reduced. The result proves the effectiveness of the steering engine camera holder stabilizing system provided in the embodiment, and the camera 8 is more stable.

[0113] Embodiment 2

[0114] The embodiment is used to provide a target tracking system based on a robotic fish 7. Different from the embodiment 1, the target tracking system based on the robotic fish 7 provided in the embodiment further comprises a fish body control mechanism.

[0115] The holder control mechanism is drivingly connected with the camera 8. The holder control mechanism is used to control the rotation of the camera 8 according to the rotation angle of the camera 8, so that the camera 8 turns to the target.

[0116] The fish body control mechanism is used to control the movement of the robotic fish 7 according to the target heading angle of the fish body, so that the robotic fish 7 turns to the target.

[0117] In the embodiment, the robotic fish 7 adopts a tail fin propulsion method, and relies on a sine signal to swing the tail to generate forward thrust:

[0118] The steering engine pulls the steel wire to drive the tail movement by forward and reverse alternating rotation, so that the tail offset angle is controlled by the rotation angle of the steering engine, and the fish tail swings in a fixed amplitude range. The movement trajectory of the fish tail movement offset angle can be expressed by the following formula:

[0119]

[0120] In the formula, L is the non-dimensionalization of the body length, which can be considered as the actual length of the fish; λ and f are the oscillation speed of the fish, which are measured through experiments: λ = 0.95, f = 2 Hz; φ is a phase difference, which is used to describe from which position the fish tail starts to oscillate, and is taken as 0 here; x and y are the horizontal and vertical positions of the fish tail, and different curve equations will be obtained by changing the time t, i.e., the motion curve of the fish tail at different times.

[0121] By selecting different time points, different fish tail motion curves can be drawn, Figure 5 The fish tail motion curve drawn by selecting ten data points in one second is shown:

[0122] By simplifying the fish tail motion offset angle motion model, it can be described as a sine function, as shown in the following formula:

[0123] θ(t) = Asin(ωt) + B

[0124] Further, the average offset angle of the sine function can be taken as the average offset angle. That is:

[0125]

[0126] Only the average offset angle needs to be adjusted, and the adjustment of the heading of the fish body can be realized. Thus, the model is further simplified.

[0127] In theory, when the fish tail servo follows the sine rotation with the x-axis as the center axis, the oscillation of the fish tail of the robotic fish 7 is symmetrical on both sides, at this time the fish swims forward, and the heading angle does not change;

[0128] When the sine is translated along the y-axis, i.e., a constant is added, the fish tail of the robotic fish 7 no longer oscillates symmetrically, but oscillates around a left or right offset angle, at this time the fish turns, and the heading angle changes;

[0129] Adjusting the value of the constant, i.e., the average offset angle described above, can realize the adjustment of the heading of the robotic fish 7, and further realize the motion closed loop, i.e., target tracking.

[0130] As Figure 6 shown, the target tracking system provided in the embodiment is tested:

[0131] The embodiment detects the feasibility of the target tracking system through a land experiment. In the experiment, the camera stabilizer is placed on the desktop, and a water bottle is used as a target object in front of it to test its function. When the water bottle moves left and right in front of it, if the camera stabilizing system is stationary, the water bottle will be observed to move to the two sides of the picture. After the camera stabilizer dynamic tracking function is turned on, when the water bottle is detected, the camera stabilizing system will automatically rotate, so that the water bottle remains in the center of the field of view. By Figure 6From the (a), (b), (c) and (d) figures, it can be observed that the angle of the background is changed, but the water bottle is basically kept in the center of the field of view of the camera 8. It is illustrated that the steering gimbal camera stabilizing system is rotated autonomously during the test, and the target object is maintained in the center of the picture for a long time during the movement. The result confirms the effectiveness of the target tracking method of the robotic fish 7 proposed in the application.

[0132] The feasibility of the target tracking system is also detected by the underwater experiment in the embodiment. The target tracking system is applied to the robotic fish 7 to track the water bottle. Figure 7 From the (a), (b), (c), (d), (e), (f), (g) and (h) figures, it can be observed that the position of the target is changed, but the robotic fish is always swimming towards the target. It can be seen that the target tracking system of the embodiment can well realize the tracking process of the robotic fish to the target object.

[0133] Embodiment 3

[0134] The embodiment provides a camera stabilizing method based on a robotic fish, and the camera stabilizing method based on the robotic fish comprises the following steps:

[0135] An image of a front area of the robotic fish is collected.

[0136] An image processing unit is used to identify a target in the image, to obtain a target position of the target in the image, and to calculate a horizontal distance between the target position and a center point of the image.

[0137] A fish body target heading angle of the robotic fish is calculated by a heading target angle controller according to the horizontal distance and an actual heading angle of a gimbal measured by a fish head posture sensor.

[0138] A shaking angle of a camera is calculated by a feedforward controller according to the fish body target heading angle and an actual heading angle of a fish body measured by a fish body posture sensor.

[0139] A rotating angle of the camera is calculated by a first controller according to the horizontal distance, the actual heading angle of the gimbal and the shaking angle.

[0140] The camera is controlled to rotate by a gimbal control mechanism according to the rotating angle of the camera, so that the camera is kept stable.

[0141] Embodiment 4

[0142] The embodiment provides a control method of a target tracking system based on a robotic fish, and the control method of the target tracking system based on the robotic fish comprises the following steps:

[0143] An image of a front area of the robotic fish is collected.

[0144] An image processing unit is used to identify a target in the image, obtain a target position of the target in the image, and calculate a horizontal distance between the target position and a center point of the image.

[0145] A fish body target heading angle of the robotic fish is calculated by a heading target angle controller according to the horizontal distance and a gimbal actual heading angle measured by a fish head posture sensor.

[0146] A shaking angle of the camera is calculated by a feedforward controller according to the fish body target heading angle and a fish body actual heading angle measured by a fish body posture sensor.

[0147] A rotating angle of the camera is calculated by a first controller according to the horizontal distance, the gimbal actual heading angle and the shaking angle.

[0148] The camera is controlled to rotate by a gimbal control mechanism according to the rotating angle of the camera, so that the camera is turned to the target.

[0149] The robotic fish is controlled to move by a fish body control mechanism according to the fish body target heading angle, so that the robotic fish is turned to the target.

[0150] The embodiments in the specification are described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The same or similar parts of each embodiment can be referred to each other. For the method disclosed by the embodiments, the description is relatively simple because it corresponds to the system disclosed by the embodiments. The relevant part can be referred to the method part.

[0151] The principles and implementation manners of the present application are described by using specific examples. The above description of the embodiments is only used to help understand the method of the present application and its core idea. For those skilled in the art, the specific implementation manners and application scope can be changed according to the idea of the present application. In conclusion, the content of the specification should not be understood as a limitation of the present application.

Claims

1. A camera stabilizing system based on a robotic fish, characterized in that, The cloud cover control mechanism, the camera, the fish head posture sensor, the fish body posture sensor, the first controller, the second controller; the second controller includes an image processing unit, a heading target angle controller and a feedforward controller; The camera is installed on the robotic fish and is used to collect images of the area in front of the robotic fish; The image processing unit is in communication connection with the camera and is used to identify the target in the image, obtain the target position of the target in the image, and calculate the horizontal distance between the target position and the center point of the image; The heading target angle controller is in communication connection with the image processing unit and the fish head posture sensor, and is used to calculate the fish body target heading angle of the robotic fish according to the horizontal distance and the actual heading angle of the cloud cover measured by the fish head posture sensor; specifically: Calculate the error amount e1 of this iteration: error quantity e of the previous two iterations 1pre_2 error quantity e of the previous iteration pre_1 ; Calculate the offset angle of the swing of the tail drive steering gear in this iteration: wherein is the average of the current body attitude sensor readings; θ body_init is the body attitude sensor initial reading; d1 is the horizontal distance between the target position and the center of the image; a is an empirical parameter to convert the horizontal distance into a rotation angle; d2 = θ head - θ head_init , θ head is the head attitude sensor measurement; θ head_init is the head attitude sensor initial reading; The feedforward controller is in communication connection with the heading target angle controller and the fish body posture sensor, and is used to calculate the shaking angle of the camera according to the fish body target heading angle and the actual heading angle of the fish body measured by the fish body posture sensor; specifically: tail_out = tail_out + k p * e1 + k i * e 1pre_1 + k d * (e1 + e 1pre_2 - 2 * e 1pre_1 ) Changes the tail_out implementation of the machine fish heading adjustment;k p is a proportional coefficient;k i is an integral coefficient;k d is a derivative coefficient; The first controller is in communication connection with the second controller and the fish head posture sensor, and is used to calculate the rotation angle of the camera according to the horizontal distance, the actual heading angle of the cloud cover and the shaking angle; out1 = - (θ body -θ body_init -θ body_adv )* β where θ body is the current fish body attitude sensor reading; θ body_adv is the fish body target heading angle; β is a parameter that adjusts the strength of the feedforward controller action; The cloud cover control mechanism is drivingly connected with the camera; The cloud cover control mechanism is used to control the rotation of the camera according to the rotation angle of the camera, so as to keep the camera stable. The cloud cover control mechanism includes a cloud cover and a drive steering gear; 2. The robotic fish based camera stabilizing system of claim 1, wherein, The camera is fixedly connected with the rotation shaft of the cloud cover; The drive steering gear is drivingly connected with the rotation shaft and is used to drive the rotation of the camera. It also includes a reel; the driven wheel of the reel is in transmission connection with the rotation shaft, and the driving wheel of the reel is in transmission connection with the drive steering gear.

3. The robotic fish based camera stabilizing system of claim 2, wherein, One end of the elastic line of the reel is connected with the driven wheel, and the other end is connected with the driving wheel; the elastic line is distributed in the shape of "8".

4. The robotic fish based camera stabilizing system of claim 3, wherein, It also includes a connecting frame; the connecting frame is installed on the robotic fish and is used to fix the cloud cover control mechanism.

5. The robotic fish based camera stabilizing system of claim 3, wherein, It also includes an upper support and a lower support; 6. The robotic fish based camera stabilizing system of claim 5, wherein, The upper support is connected with the upper end of the rotation shaft through a bearing, and the lower support is connected with the lower end of the rotation shaft through a bearing. The image processing unit is used to identify the target in the image by using KCF-DSST algorithm and YOLO-X algorithm.

7. The robotic fish-based camera stabilizing system of claim 1, wherein, The fish body control mechanism, the cloud cover control mechanism, the camera, the fish head posture sensor, the fish body posture sensor, the first controller, the second controller; the second controller includes an image processing unit, a heading target angle controller and a feedforward controller; 8. A machine fish based target tracking system, characterized by, The camera is installed on the robotic fish and is used to collect images of the area in front of the robotic fish; ​ ​ The image processing unit is in communication connection with the camera, is used for identifying the target in the image, obtaining the target position of the target in the image, and calculating the horizontal distance between the target position and the center point of the image; The heading target angle controller is in communication connection with the image processing unit and the fish head attitude sensor, and is used for calculating the fish body target heading angle of the robotic fish according to the horizontal distance and the actual heading angle of the holder measured by the fish head attitude sensor; specifically: error quantity e of the previous two iterations 1pre_2 error quantity e of the previous iteration pre_1 ; Calculate the error amount e1 of this round of iteration wherein is the mean value of the current body posture sensor reading; θ body_init is the body posture sensor initial reading; d1 is the horizontal distance between the target position and the center point of the image; a is an empirical parameter for converting the horizontal distance into a rotation angle; d2 = θ head - θ head_init , θ head is the head posture sensor measurement, θ head_init is the head posture sensor initial reading; Calculate the offset angle of the swing of the fish tail drive steering gear in this round of iteration: tail_out = tail_out + k p * e1 + k i * e 1pre_1 + k d * (e1 + e 1pre_2 - 2 * e 1pre_1 ) Changes the tail_out implementation of the machine fish heading adjustment; k p is a proportional coefficient; k i is an integral coefficient; k d is a derivative coefficient; The feedforward controller is in communication connection with the heading target angle controller and the fish body attitude sensor, and is used for calculating the shaking angle of the camera according to the fish body target heading angle and the actual heading angle of the fish body measured by the fish body attitude sensor; specifically: out1 = - (θ body -θ body_init -θ body_adv )* β where θ body is the current fish body attitude sensor reading; θ body_adv is the fish body target heading angle; β is a parameter that adjusts the strength of the feedforward controller action; The first controller is in communication connection with the second controller and the fish head attitude sensor, and is used for calculating the rotation angle of the camera according to the horizontal distance, the actual heading angle of the holder and the shaking angle; The holder control mechanism is in driving connection with the camera, and is used for controlling the camera to turn to the target according to the rotation angle of the camera; The fish body control mechanism is used for controlling the robotic fish to turn to the target according to the fish body target heading angle. 9.A method for stabilizing a camera based on a robotic fish, characterized in that, It includes: Collecting the image of the front area of the robotic fish; Using the image processing unit to identify the target in the image, obtaining the target position of the target in the image, and calculating the horizontal distance between the target position and the center point of the image; The heading target angle controller is in communication connection with the image processing unit and the fish head attitude sensor, and is used for calculating the fish body target heading angle of the robotic fish according to the horizontal distance and the actual heading angle of the holder measured by the fish head attitude sensor; specifically: error quantity e of the previous two iterations 1pre_2 error quantity e of the previous iteration pre_1 ; Calculate the error amount e1 of this round of iteration wherein is the mean value of the current body attitude sensor reading; θ body_init is the body attitude sensor initial reading; di is the horizontal distance between the target position and the center point of the image; a is an empirical parameter for converting the horizontal distance into a rotation angle; d2 = θ head - θ head_init , θ head is the head attitude sensor measurement, θ head_init is the head attitude sensor initial reading; Calculate the offset angle of the swing of the fish tail drive steering gear in this round of iteration: tail_out = tail_out + k p * e1 + k i * e 1pre_1 + k d * (e1 + e 1pre_2 - 2 * e 1pre_1 ) Changes the tail_out implementation of the machine fish heading adjustment; k p is a proportional coefficient; k i is an integral coefficient; k d is a derivative coefficient; Through the feedforward controller, the shaking angle of the camera is calculated according to the fish body target heading angle and the actual heading angle of the fish body measured by the fish body attitude sensor; specifically: out1 = - (θ body -θ body_init -θ body_adv )* β where θ body is the current fish body attitude sensor reading; θ body_adv is the fish body target heading angle; β is a parameter that adjusts the strength of the feedforward controller action; Through the first controller, the rotation angle of the camera is calculated according to the horizontal distance, the actual heading angle of the holder and the shaking angle; Through the holder control mechanism, the camera is controlled to rotate according to the rotation angle of the camera, so that the camera remains stable.

10. A control method of a target tracking system based on a robotic fish, characterized by, It includes: Collecting the image of the front area of the robotic fish; Using the image processing unit to identify the target in the image, obtaining the target position of the target in the image, and calculating the horizontal distance between the target position and the center point of the image; The heading target angle controller is in communication connection with the image processing unit and the fish head attitude sensor, and is used for calculating the fish body target heading angle of the robotic fish according to the horizontal distance and the actual heading angle of the holder measured by the fish head attitude sensor; specifically: error quantity e of the previous two iterations 1pre_2 error quantity e of the previous iteration pre_1 ; Calculate the error amount e1 of this round of iteration wherein is the mean of the current body attitude sensor readings; θ body_init is the body attitude sensor initial reading; d1 is the horizontal distance between the target position and the center of the image; a is an empirical parameter to convert the horizontal distance into a rotation angle; d2 = θ head - θ head_init , θ head is the head attitude sensor measurement, θ head_init is the head attitude sensor initial reading; Calculate the offset angle of the swing of the fish tail drive steering gear in this round of iteration: tail_out = tail_out + k p * e1 + k i * e 1pre_1 + k d * (e1 + e 1pre_2 - 2 * e 1pre_1 ) Changes the tail_out implementation of the machine fish heading adjustment; k p is a proportional coefficient; k i is an integral coefficient; k d is a derivative coefficient; Through the feedforward controller, the shaking angle of the camera is calculated according to the fish body target heading angle and the actual heading angle of the fish body measured by the fish body attitude sensor; specifically: out1 = - (θ body -θ body_init -θ body_adv )* β where θ body is the current fish body attitude sensor reading; θ body_adv is the fish body target heading angle; β is a parameter that adjusts the strength of the feedforward controller action; calculating, by the first controller, a rotation angle of the camera according to the horizontal distance, the actual yaw angle of the holder, and the shaking angle; controlling, by the holder control mechanism, the camera to rotate to the target according to the rotation angle of the camera; controlling, by the fish body control mechanism, the robot fish to rotate to the target according to the fish body target yaw angle.

Images