Underwater robot target tracking method and device, terminal equipment and storage medium

Abstract

The invention discloses an underwater robot target tracking method and device, terminal equipment and a storage medium, and the method comprises the steps: carrying out the calculation according to a current position, camera parameters and an identification code image, and obtaining a transverse movement position deviation and a yaw angle deviation with a target object; according to the transverse movement position deviation, the yaw angle deviation and the physical parameters of the robot, longitudinal movement control force and yaw control torque are obtained through calculation by taking transverse movement position deviation convergence and yaw angle deviation convergence as targets; according to the longitudinal movement control force, the yaw control moment, the transverse movement position deviation, the yaw angle deviation and the preset longitudinal movement speed, a propeller mapping model is constructed; solving the propeller mapping model according to the longitudinal movement control force, the yaw control moment, the transverse movement position deviation and the yaw angle deviation under the constraint of a preset longitudinal movement speed to obtain propeller control parameters; and performing tracking control on the target object according to the propeller control parameters. According to the invention, the tracking accuracy of the robot to the target object is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to an underwater robot target tracking method, device, terminal equipment and storage medium. Background Art

[0002] With the continuous development and utilization of marine resources by humans, underwater robots play an increasingly important role in various underwater operations, especially in complex tasks such as underwater target monitoring and pipeline maintenance. Their application value is becoming increasingly prominent. In these application scenarios, accurate path tracking capability is the key to whether underwater robots can complete tasks efficiently. For example, when performing underwater pipeline maintenance, the robot needs to track the pipeline along a preset path to ensure that the pipeline image can be fully collected. The nonlinear characteristics of the underwater environment, such as water flow disturbances and buoyancy changes, have a significant impact on the motion state of underwater robots. At present, the path tracking of underwater robots is mainly achieved through global vision to achieve robot tracking and positioning. The global vision method faces many limitations such as light attenuation and target occlusion, which leads to low or even failure of underwater robot tracking and positioning accuracy, and is difficult to achieve in actual environments. Summary of the invention

[0003] The embodiments of the present invention provide an underwater robot target tracking method, device, terminal equipment and storage medium, which can effectively solve the problem of low tracking and positioning accuracy of the robot based on global vision in the prior art.

[0004] An embodiment of the present invention provides a target tracking method of an underwater robot, comprising:

[0005] Get the current position, camera parameters, robot physical parameters and identification code images of several target objects;

[0006] Calculating a lateral position deviation and a yaw angle deviation from a target object according to the current position, the camera parameters and the identification code image;

[0007] According to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the yaw angle deviation as the goal, the longitudinal control force and the yaw control torque are calculated;

[0008] Constructing a thruster mapping model according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and a preset longitudinal speed;

[0009] Under the preset longitudinal speed constraint, the thruster mapping model is solved according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain the thruster control parameters;

[0010] The target object is tracked and controlled according to the thruster control parameters.

[0011] Furthermore, according to the current position, the camera parameters and the identification code image, the lateral position deviation and the yaw angle deviation from the target object are calculated, including:

[0012] Performing recognition according to the recognition code image to obtain recognition code corner point information;

[0013] Correct the position of the target object according to the identification code corner point information and the camera parameters to obtain the position of the target object;

[0014] Calculation is performed based on the target object position and the current position to obtain a lateral position deviation and a yaw angle deviation from the target object.

[0015] Further, according to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the convergence of the yaw angle deviation as the goal, the longitudinal control force and the yaw control torque are calculated, including:

[0016] Constructing a robot dynamics model according to the lateral position deviation, the yaw angle deviation and the robot physical parameters;

[0017] According to the lateral position deviation, the yaw angle deviation, the preset controller proportional coefficient, the preset controller integral coefficient and the preset differential coefficient, with the convergence of the lateral position deviation and the yaw angle deviation as the goal, the robot dynamics model is solved to obtain the longitudinal control force and the yaw control torque.

[0018] Further, under the preset longitudinal speed constraint, the thruster mapping model is solved according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain thruster control parameters, including:

[0019] Determining a corresponding lateral movement sub-controller output function according to the lateral movement position deviation, the yaw control torque and preset control parameters;

[0020] Determining a corresponding yaw sub-controller output function according to the yaw angle deviation, the yaw control torque and preset control parameters;

[0021] Calculating according to the lateral movement sub-controller output function, the yaw sub-controller output function and the preset longitudinal movement speed to obtain the first controller output result in the linear target tracking stage;

[0022] Calculating according to the yaw sub-controller output function and the preset longitudinal speed to obtain the output result of the second controller in the steering target tracking phase;

[0023] The thruster mapping model is solved according to the output result of the first controller and the output result of the second controller to obtain thruster control parameters.

[0024] Furthermore, the thruster control parameters include: a left side thruster frequency, a right side thruster frequency, a left tail thruster frequency, and a right tail thruster frequency.

[0025] Further, tracking and controlling the target object according to the thruster control parameters includes:

[0026] In the case of a linear target tracking stage, tracking and controlling the target object according to the left side propeller frequency and the right side propeller frequency;

[0027] In the case of the steering target tracking stage, comparing the yaw angle deviation with a preset deviation threshold;

[0028] If the yaw angle deviation is less than a preset deviation threshold, tracking and controlling the target object according to the left side thruster frequency and the right side thruster frequency;

[0029] If the yaw angle deviation is not less than a preset deviation threshold, the target object is tracked and controlled according to the left side thruster frequency, the right side thruster frequency, the left tail thruster frequency and the right tail thruster frequency.

[0030] As an improvement of the above solution, another embodiment of the present invention provides an underwater robot target tracking device, including:

[0031] A data acquisition module is used to obtain the current position, camera parameters, robot physical parameters and identification code images of several target objects;

[0032] A deviation calculation module, used to calculate the lateral position deviation and yaw angle deviation from the target object according to the current position, the camera parameters and the identification code image;

[0033] A control force and control torque calculation module, used for calculating the longitudinal control force and the yaw control torque according to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the yaw angle deviation as the goal;

[0034] A thruster mapping model building module, used to build a thruster mapping model according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and a preset longitudinal speed;

[0035] A control parameter solving module, used for solving the thruster mapping model under a preset longitudinal speed constraint according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain thruster control parameters;

[0036] The target tracking module is used to track and control the target object according to the thruster control parameters.

[0037] Furthermore, the deviation calculation module includes:

[0038] A corner point information recognition unit, used for performing recognition according to the recognition code image to obtain the recognition code corner point information;

[0039] A target position determination unit, configured to perform target object position correction according to the identification code corner point information and the camera parameters to obtain the target object position;

[0040] The position comparison unit is used to calculate according to the target object position and the current position to obtain the lateral position deviation and the yaw angle deviation from the target object.

[0041] Another embodiment of the present invention provides a terminal device, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, and when the processor executes the computer program, an underwater robot target tracking method as described in the above embodiment is implemented.

[0042] Another embodiment of the present invention provides a computer-readable storage medium, which includes a stored computer program, wherein when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute an underwater robot target tracking method described in the above embodiment.

[0043] By implementing the present invention, at least the following beneficial effects are achieved:

[0044] The present invention provides an underwater robot target tracking method, device, terminal equipment and storage medium. The method can obtain the current position, camera parameters, robot physical parameters and identification code images of several target objects; according to the current position, the camera parameters and the identification code image, the lateral position deviation and the yaw angle deviation with the target object are calculated; according to the lateral position deviation, the yaw angle deviation and the robot physical parameters, with the convergence of the lateral position deviation and the convergence of the yaw angle deviation as the goal, the longitudinal control force and the yaw control torque are calculated; according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and the preset longitudinal speed, a thruster mapping model is constructed; under the preset longitudinal speed constraint, according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation, the thruster mapping model is solved to obtain thruster control parameters; and the target object is tracked and controlled according to the thruster control parameters. By calculating the lateral position deviation and yaw angle deviation between the robot itself and the target object, and then obtaining the longitudinal control force and yaw control torque based on the lateral position deviation and yaw angle deviation, the thruster mapping model is used to map and solve the two control quantities of the longitudinal control force and yaw control torque to obtain the thruster control parameters. The robot can then be adaptively controlled based on the thruster control parameters. There is no need to achieve robot positioning through a global perspective, and there is no need to consider issues such as underwater light attenuation and target occlusion, thereby reducing external influence errors and improving the tracking accuracy of the underwater robot for the target object. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a flow chart of a target tracking method of an underwater robot provided by one embodiment of the present invention;

[0046] Figure 2 This is a schematic diagram of underwater target positioning based on robot vision provided by an embodiment of the present invention;

[0047] Figure 3 is a schematic diagram of underwater linear target tracking provided by an embodiment of the present invention;

[0048] Figure 4 This is a schematic diagram of underwater 90-degree turn target tracking provided by an embodiment of the present invention;

[0049] Figure 5 This is a schematic diagram of underwater "mouth"-shaped target tracking provided by an embodiment of the present invention;

[0050] Figure 6 The figure is a schematic diagram of the structure of an underwater robot target tracking device provided by one embodiment of the present invention. DETAILED DESCRIPTION

[0051] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0052] See also Figure 1 , is a flow chart of a method for underwater robot target tracking provided by an embodiment of the present invention, comprising:

[0053] S1, obtain the current position, camera parameters, robot physical parameters and identification code images of several target objects;

[0054] Specifically, the robot physical parameters include: robot mass, robot inertia and robot friction; the camera parameters include: focal length, optical center, rotation matrix and translation vector; the current position refers to the position of the working robot; the identification code image is the ArUco identification code. The robot is equipped with a visual system, including a camera, through which the ArUco identification code can be obtained. The ArUco identification code is arranged on several target objects, such as Figure 3 As shown, the ArUco identification code is arranged at several points of the underwater pipeline, and the ArUco identification code can be obtained through the robot camera.

[0055] S2. Calculate the lateral position deviation and yaw angle deviation from the target object according to the current position, the camera parameters and the identification code image;

[0056] Specifically, according to the current position, the camera parameters and the identification code image, the lateral position deviation and the yaw angle deviation from the target object are calculated, including:

[0057] Performing recognition according to the recognition code image to obtain recognition code corner point information;

[0058] Correct the position of the target object according to the identification code corner point information and the camera parameters to obtain the position of the target object;

[0059] Calculation is performed based on the target object position and the current position to obtain a lateral position deviation and a yaw angle deviation from the target object.

[0060] Preferably, the four corner points of a single ArUco identification code can provide posture information for the camera, and the identification code corner point information is the four corner point information for identifying the ArUco identification code. The lateral position deviation refers to the deviation between the target object and the actual position of the robot in the horizontal direction (i.e., lateral direction). This deviation may be caused by a variety of factors, including but not limited to camera installation errors, lens distortion, and the accuracy of the image processing algorithm. The size of the lateral position deviation directly affects the recognition accuracy and reliability of the machine vision system for the position of the target object. The yaw angle deviation refers to the deviation between the target object and the robot in the actual posture in the yaw direction (i.e., the rotation angle around the vertical axis). The yaw angle deviation is also affected by factors such as camera installation errors, lens distortion, and image processing algorithm accuracy. In the machine vision system, the size of the yaw angle deviation directly affects the accuracy and stability of the target object posture recognition.

[0061] In a preferred embodiment of the present invention, the identification code image is first identified to obtain the identification code corner point information; then the target object position is corrected according to the identification code corner point information and the camera parameters to obtain the target object position to ensure that the target object position is consistent with the actual position; finally, the lateral position deviation and yaw angle deviation with the target object are calculated according to the target object position and the current position. The identification code image recognition technology has strong adaptability and robustness, and can accurately identify in complex environments such as different lighting conditions and noise interference.

[0062] S3, according to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the yaw angle deviation as the goal, calculating the longitudinal control force and the yaw control torque;

[0063] Preferably, according to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the convergence of the yaw angle deviation as the goal, the longitudinal control force and the yaw control torque are calculated, including:

[0064] Constructing a robot dynamics model according to the lateral position deviation, the yaw angle deviation and the robot physical parameters;

[0065] According to the lateral position deviation, the yaw angle deviation, the preset controller proportional coefficient, the preset controller integral coefficient and the preset differential coefficient, with the convergence of the lateral position deviation and the yaw angle deviation as the goal, the robot dynamics model is solved to obtain the longitudinal control force and the yaw control torque.

[0066] Specifically, the convergence of the lateral position deviation and the yaw angle deviation is taken as the goal, which means that in order to make the robot locate and track the actual position of the target object, the lateral position deviation and the yaw angle deviation should be made to approach zero. R , the yaw angle deviation Δψ R and the physical parameters of the robot, a robot dynamics model is constructed, and then according to the lateral position deviation, the yaw angle deviation, the preset controller proportional coefficient, the preset controller integral coefficient and the preset differential coefficient, with the convergence of the lateral position deviation and the yaw angle deviation as the goal, the robot dynamics model is solved according to the control law to obtain the longitudinal control force τ u and the yaw control torque τ r .

[0067] In a preferred embodiment of the present invention, the yaw control torque τ r The output τ of the traverse subcontroller r1 and the output τ of the yaw sub-controller r2 To compose, for example, the output of the traverse subcontroller:

[0068]

[0069] Among them, k p_r1 is the preset proportional coefficient of the traverse subcontroller, k i_r1 is the preset integral coefficient, k d_r1 is the preset differential coefficient; i is the serial number of the target object; Δy R (i) is the lateral position deviation of the i-th target object.

[0070] S4, constructing a thruster mapping model according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and a preset longitudinal speed;

[0071] In a preferred embodiment of the present invention, the preset longitudinal speed u d Indicates the expected longitudinal speed of the underwater robot.

[0072] S5. Under a preset longitudinal speed constraint, according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation, the thruster mapping model is solved to obtain thruster control parameters;

[0073] Specifically, under the preset longitudinal speed constraint, the thruster mapping model is solved according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain thruster control parameters, including:

[0074] Determining a corresponding lateral movement sub-controller output function according to the lateral movement position deviation, the yaw control torque and preset control parameters;

[0075] Determining a corresponding yaw sub-controller output function according to the yaw angle deviation, the yaw control torque and preset control parameters;

[0076] Calculating according to the lateral movement sub-controller output function, the yaw sub-controller output function and the preset longitudinal movement speed to obtain the first controller output result in the linear target tracking stage;

[0077] Calculating according to the yaw sub-controller output function and the preset longitudinal speed to obtain the output result of the second controller in the steering target tracking phase;

[0078] The thruster mapping model is solved according to the output result of the first controller and the output result of the second controller to obtain thruster control parameters.

[0079] Specifically, the output function of the lateral movement sub-controller is the control output function f1(Δy R ), the yaw sub-controller output function is the control output function f2(Δψ R ). According to the lateral position deviation, the yaw control torque and the preset control parameters, the corresponding lateral sub-controller output function is determined:

[0080]

[0081] in, is a preset control parameter, which can be adjusted between [0,1]. R =0, f1(Δy R )=0; in Δy R When ≠0,

[0082] According to the yaw angle deviation, the yaw control torque and the preset control parameters, the corresponding yaw sub-controller output function is determined:

[0083]

[0084] When ΔψR=0, f2(ΔψR)=0; when ΔψR=0,

[0085] The first controller output result of the linear target tracking phase is obtained by calculating according to the lateral movement sub-controller output function, the yaw sub-controller output function and the preset longitudinal movement speed:

[0086] U P =[u d Δy R Δψ R ] T

[0087] The output result of the second controller in the steering target tracking phase is obtained by calculating according to the yaw sub-controller output function and the preset longitudinal speed:

[0088]

[0089] Finally, according to the output result of the first controller and the output result of the second controller, the thruster mapping model is solved to obtain the thruster control parameters.

[0090] Preferably, the thruster control parameters include: a left side thruster frequency, a right side thruster frequency, a left tail thruster frequency, and a right tail thruster frequency.

[0091] Specifically, the left side thruster frequency f Fin_L ∈[0,3]Hz, right side thruster frequency f Fin_R ∈[0,3]Hz, left tail thruster frequency f Flipper_L ∈[0,1.5]Hz and the right tail thruster frequency f Flipper_R ∈[0,1.5]Hz.

[0092] S6. Tracking and controlling the target object according to the thruster control parameters.

[0093] Schematically, tracking and controlling the target object according to the thruster control parameters includes:

[0094] In the case of a linear target tracking stage, tracking and controlling the target object according to the left side propeller frequency and the right side propeller frequency;

[0095] In the case of the steering target tracking stage, comparing the yaw angle deviation with a preset deviation threshold;

[0096] If the yaw angle deviation is less than a preset deviation threshold, tracking and controlling the target object according to the left side thruster frequency and the right side thruster frequency;

[0097] If the yaw angle deviation is not less than a preset deviation threshold, the target object is tracked and controlled according to the left side thruster frequency, the right side thruster frequency, the left tail thruster frequency and the right tail thruster frequency.

[0098] In a preferred embodiment of the present invention, the yaw control torque τ rThe longitudinal control force τ is generated by the difference in the frequency of the propeller fluctuations on both sides. u Generated by two thrusters. In the straight target tracking stage, the robot's tail thruster and the robot's side thrusters are both in working condition. Considering that the robot's yaw angle changes little at this stage, the robot's tail thruster does not participate in yaw control, so the target object is tracked and controlled according to the left side thruster frequency and the right side thruster frequency; in the turning target tracking stage, the yaw angle deviation is compared with the preset deviation threshold. If the yaw angle deviation is less than the preset deviation threshold, the longitudinal control force τ in the turning target tracking stage u = 0, when the robot deviates from the desired yaw angle by Δψ R When the yaw angle deviation is smaller than the preset deviation threshold, the required yaw control torque τ r The yaw moment is generated only by controlling the oscillation frequency of the thrusters on both sides to be the same and the oscillation direction to be opposite. The tail thruster of the robot stops working. Therefore, the target object is tracked and controlled according to the frequency of the left side thruster and the frequency of the right side thruster. When the robot deviates from the desired yaw angle by Δψ R When the yaw angle deviation is larger (the yaw angle deviation is not less than the preset deviation threshold), the required yaw control torque τ r The underwater robot is turning in situ. At this time, the robot's tail thruster and the thrusters on both sides of the robot are in working state, and the yaw control torque τ r It is generated by four thrusters together, so the target object is tracked and controlled according to the left side thruster frequency, the right side thruster frequency, the left tail thruster frequency and the right tail thruster frequency.

[0099] In another preferred embodiment of the present invention, Figure 3 It is a schematic diagram of underwater linear target tracking provided by an embodiment of the present invention, that is, it is always in the linear target tracking stage, and the target object is tracked and controlled according to the left side thruster frequency and the right side thruster frequency. Figure 4 is a schematic diagram of underwater 90-degree turning target tracking provided by an embodiment of the present invention, Figure 4 (a)-(c) are tracking of target 1, (d)-(e) are 90-degree turn tracking, and (f)-(h) are tracking of target 2. Figure 5: This is a schematic diagram of underwater "mouth"-shaped target tracking provided by an embodiment of the present invention, (a)-(c) are target 1 tracking, (d) are 90-degree turn tracking, (e)-(f) are target 2 tracking, (g) are 90-degree turn tracking, (h)-(i) are target 3 tracking, (j) are 90-degree turn tracking, (k)-(l) are target 4 tracking, and thus the underwater robot can track a variety of underwater targets, providing support for improving the inspection efficiency of the underwater cable laying operation robot.

[0100] By implementing this embodiment, the current position, camera parameters, robot physical parameters and identification code images of several target objects are obtained; according to the current position, the camera parameters and the identification code image, the lateral position deviation and the yaw angle deviation from the target object are calculated; according to the lateral position deviation, the yaw angle deviation and the robot physical parameters, with the convergence of the lateral position deviation and the convergence of the yaw angle deviation as the goal, the longitudinal control force and the yaw control torque are calculated; according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and the preset longitudinal speed, a thruster mapping model is constructed; under the preset longitudinal speed constraint, according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation, the thruster mapping model is solved to obtain thruster control parameters; according to the thruster control parameters, the target object is tracked and controlled. By calculating the lateral position deviation and yaw angle deviation between the robot itself and the target object, and then obtaining the longitudinal control force and yaw control torque based on the lateral position deviation and yaw angle deviation, the thruster mapping model is used to map and solve the two control quantities of the longitudinal control force and yaw control torque to obtain the thruster control parameters. The robot can then be adaptively controlled based on the thruster control parameters. There is no need to achieve robot positioning through a global perspective, and there is no need to consider issues such as underwater light attenuation and target occlusion, thereby reducing external influence errors and improving the tracking accuracy of the underwater robot for the target object.

[0101] See also Figure 6 , is a schematic diagram of the structure of an underwater robot target tracking device provided by an embodiment of the present invention, comprising:

[0102] A data acquisition module is used to obtain the current position, camera parameters, robot physical parameters and identification code images of several target objects;

[0103] A deviation calculation module, used to calculate the lateral position deviation and yaw angle deviation from the target object according to the current position, the camera parameters and the identification code image;

[0104] A control force and control torque calculation module, used for calculating the longitudinal control force and the yaw control torque according to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the yaw angle deviation as the goal;

[0105] A thruster mapping model building module, used to build a thruster mapping model according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and a preset longitudinal speed;

[0106] A control parameter solving module, used for solving the thruster mapping model under a preset longitudinal speed constraint according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain thruster control parameters;

[0107] The target tracking module is used to track and control the target object according to the thruster control parameters.

[0108] Specifically, the deviation calculation module includes:

[0109] A corner point information recognition unit, used for performing recognition according to the recognition code image to obtain the recognition code corner point information;

[0110] A target position determination unit, configured to perform target object position correction according to the identification code corner point information and the camera parameters to obtain the target object position;

[0111] The position comparison unit is used to calculate according to the target object position and the current position to obtain the lateral position deviation and the yaw angle deviation from the target object.

[0112] The present invention provides an underwater robot target tracking device. According to a data acquisition module, a current position, camera parameters, robot physical parameters and identification code images of several target objects are acquired; in a deviation calculation module, a lateral position deviation and a yaw angle deviation with the target object are calculated according to the current position, the camera parameters and the identification code images; in a control force and control torque calculation module, a longitudinal position deviation and a yaw angle deviation are calculated according to the lateral position deviation, the yaw angle deviation and the robot physical parameters with the convergence of the lateral position deviation and the convergence of the yaw angle deviation as the goal. The invention discloses a method for controlling the longitudinal movement control force and the yaw control torque; in the thruster mapping model construction module, a thruster mapping model is constructed according to the longitudinal movement control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and the preset longitudinal movement speed; through the control parameter solving module, under the preset longitudinal movement speed constraint, the thruster mapping model is solved according to the longitudinal movement control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain the thruster control parameters; finally, in the target tracking module, the target object is tracked and controlled according to the thruster control parameters. By calculating the lateral position deviation and yaw angle deviation between the robot itself and the target object, and then obtaining the longitudinal control force and yaw control torque based on the lateral position deviation and yaw angle deviation, the thruster mapping model is used to map and solve the two control quantities of the longitudinal control force and yaw control torque to obtain the thruster control parameters. The robot can then be adaptively controlled based on the thruster control parameters. There is no need to achieve robot positioning through a global perspective, and there is no need to consider issues such as underwater light attenuation and target occlusion, thereby reducing external influence errors and improving the tracking accuracy of the underwater robot for the target object.

[0113] It should be noted that the device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. In addition, in the accompanying drawings of the device embodiments provided by the present invention, the connection relationship between the modules indicates that there is a communication connection between them, which may be specifically implemented as one or more communication buses or signal lines. A person of ordinary skill in the art may understand and implement it without paying any creative effort.

[0114] Those skilled in the art can clearly understand that, for the sake of convenience and brevity, the specific working process of the device described above can refer to the corresponding process in the aforementioned method embodiment, and will not be repeated here.

[0115] Another embodiment of the present invention further provides a terminal device, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor implements an underwater robot target tracking method as described in the above embodiment when executing the computer program. The terminal device may be a computing device such as a desktop computer, a notebook, a PDA, and a cloud server. The terminal device may include, but is not limited to, a processor and a memory.

[0116] The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc. The processor is the control center of the terminal device, and uses various interfaces and lines to connect various parts of the entire terminal device.

[0117] The memory can be used to store the computer program, and the processor realizes various functions of the terminal device by running or executing the computer program stored in the memory and calling the data stored in the memory. The memory can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, at least one application required for a function, etc.; the data storage area can store data created according to the use of the mobile phone, etc. In addition, the memory can include a high-speed random access memory, and can also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device or other volatile solid-state storage device.

[0118] Another embodiment of the present invention provides a computer-readable storage medium, which includes a stored computer program, wherein when the computer program is running, the device where the computer-readable storage medium is located is controlled to execute an underwater robot target tracking method described in the above embodiment.

[0119] The storage medium is a computer-readable storage medium, and the computer program is stored in the computer-readable storage medium. When the computer program is executed by the processor, the steps of each of the above-mentioned method embodiments can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium.

[0120] The above is a preferred embodiment of the present invention. It should be pointed out that a person skilled in the art can make several improvements and modifications without departing from the principle of the present invention. These improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A target tracking method for an underwater robot, characterized in that: include: Get the current position, camera parameters, robot physical parameters and identification code images of several target objects; Calculating a lateral position deviation and a yaw angle deviation from a target object according to the current position, the camera parameters and the identification code image; According to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the yaw angle deviation as the goal, the longitudinal control force and the yaw control torque are calculated; Constructing a thruster mapping model according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and a preset longitudinal speed; Under the preset longitudinal speed constraint, the thruster mapping model is solved according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain the thruster control parameters; The target object is tracked and controlled according to the thruster control parameters.

2. The underwater robot target tracking method according to claim 1, characterized in that: According to the current position, the camera parameters and the identification code image, the lateral position deviation and the yaw angle deviation from the target object are calculated, including: Performing recognition according to the recognition code image to obtain recognition code corner point information; Correct the position of the target object according to the identification code corner point information and the camera parameters to obtain the position of the target object; Calculation is performed based on the target object position and the current position to obtain a lateral position deviation and a yaw angle deviation from the target object.

3. The underwater robot target tracking method according to claim 1, characterized in that: According to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the convergence of the yaw angle deviation as the goal, the longitudinal control force and the yaw control torque are calculated, including: Constructing a robot dynamics model according to the lateral position deviation, the yaw angle deviation and the robot physical parameters; According to the lateral position deviation, the yaw angle deviation, the preset controller proportional coefficient, the preset controller integral coefficient and the preset differential coefficient, with the convergence of the lateral position deviation and the yaw angle deviation as the goal, the robot dynamics model is solved to obtain the longitudinal control force and the yaw control torque.

4. The underwater robot target tracking method according to claim 1, characterized in that: Under the preset longitudinal speed constraint, the thruster mapping model is solved according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain thruster control parameters, including: Determining a corresponding lateral movement sub-controller output function according to the lateral movement position deviation, the yaw control torque and preset control parameters; Determining a corresponding yaw sub-controller output function according to the yaw angle deviation, the yaw control torque and preset control parameters; Calculating according to the lateral movement sub-controller output function, the yaw sub-controller output function and the preset longitudinal movement speed to obtain the first controller output result in the linear target tracking stage; Calculating according to the yaw sub-controller output function and the preset longitudinal speed to obtain the output result of the second controller in the steering target tracking phase; The thruster mapping model is solved according to the output result of the first controller and the output result of the second controller to obtain thruster control parameters.

5. The underwater robot target tracking method according to claim 4, characterized in that: The thruster control parameters include: a left side thruster frequency, a right side thruster frequency, a left tail thruster frequency, and a right tail thruster frequency.

6. The underwater robot target tracking method according to claim 5, characterized in that: Tracking and controlling the target object according to the thruster control parameters includes: In the case of a linear target tracking stage, tracking and controlling the target object according to the left side propeller frequency and the right side propeller frequency; In the case of the steering target tracking stage, comparing the yaw angle deviation with a preset deviation threshold; If the yaw angle deviation is less than a preset deviation threshold, tracking and controlling the target object according to the left side thruster frequency and the right side thruster frequency; If the yaw angle deviation is not less than a preset deviation threshold, the target object is tracked and controlled according to the left side thruster frequency, the right side thruster frequency, the left tail thruster frequency and the right tail thruster frequency.

7. An underwater robot target tracking device, characterized in that: include: A data acquisition module is used to obtain the current position, camera parameters, robot physical parameters and identification code images of several target objects; A deviation calculation module, used to calculate the lateral position deviation and yaw angle deviation from the target object according to the current position, the camera parameters and the identification code image; A control force and control torque calculation module, used for calculating the longitudinal control force and the yaw control torque according to the lateral position deviation, the yaw angle deviation and the physical parameters of the robot, with the convergence of the lateral position deviation and the yaw angle deviation as the goal; A thruster mapping model building module, used to build a thruster mapping model according to the longitudinal control force, the yaw control torque, the lateral position deviation, the yaw angle deviation and a preset longitudinal speed; A control parameter solving module, used for solving the thruster mapping model under a preset longitudinal speed constraint according to the longitudinal control force, the yaw control torque, the lateral position deviation and the yaw angle deviation to obtain thruster control parameters; The target tracking module is used to track and control the target object according to the thruster control parameters.

8. The underwater robot target tracking device according to claim 7, characterized in that: The deviation calculation module comprises: A corner point information recognition unit, used for performing recognition according to the recognition code image to obtain the recognition code corner point information; A target position determination unit, configured to perform target object position correction according to the identification code corner point information and the camera parameters to obtain the target object position; The position comparison unit is used to calculate according to the target object position and the current position to obtain the lateral position deviation and the yaw angle deviation from the target object.

9. A terminal device, characterized in that: The method comprises a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein when the processor executes the computer program, an underwater robot target tracking method as claimed in any one of claims 1 to 6 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes a stored computer program, wherein when the computer program is executed, the device where the computer-readable storage medium is located is controlled to execute the underwater robot target tracking method according to any one of claims 1 to 6.

Images