An underwater motion control method of an automatic-avoidance iguana robot

Abstract

The application relates to a robot control method, aiming to provide an underwater motion control method of an automatic-avoiding sea-lizard-imitating robot, which can control a four-legged sea-lizard-imitating robot to automatically avoid obstacles based on vision. The technical scheme is an underwater motion control method of an automatic-avoiding sea-lizard-imitating robot, which comprises the following steps: 1) the robot advances towards a target position according to a predetermined heading angle, and a binocular camera shoots underwater images; 2) an upper computer processes the images according to an avoidance algorithm, calculates the positions of the robot and obstacles, then obtains an avoidance heading angle according to path planning, and sends a control instruction to a single-chip microcomputer according to a motion model; and 3) the single-chip microcomputer feeds back according to the running states of a tail and four legs, controls the tail and the four legs to move, accurately controls underwater motion of the robot, and makes the robot automatically avoid obstacles and approach the target position.

Description

Technical Field

[0001] This invention relates to a robot control method, specifically an underwater motion control method for an automatically evasive, marine iguana-inspired robot. Background Technology

[0002] In the modern military field, unmanned combat platforms represent the future development direction of weapons and equipment, and military robots also have very good development prospects. Therefore, countries around the world are investing heavily in the research and development of new unmanned combat platforms.

[0003] Marine iguanas are amphibians and the only iguanas in the world adapted to marine life. They rely on their powerful tails for propulsion, while simultaneously using all four limbs to crawl on land. Therefore, marine iguanas possess excellent amphibious mobility, allowing them to adapt to complex amphibious environments. These characteristics can provide new ideas for developing more controllable, flexible, and capable biomimetic marine iguana-inspired robots that can detect and automatically avoid obstacles. For example, Chinese invention patent CN114683784A, entitled "A Bionic Amphibian Marine Iguana Robot," provides a purely mechanical robot with a two-degree-of-freedom crawling mechanism, capable of adapting to different terrains. However, this robot lacks a control method for automatically avoiding obstacles during movement, thus requiring further improvement. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings in the above-mentioned background technology and provide an underwater motion control method for an automatically avoidable marine iguana-like robot. This method should be able to control the quadrupedal marine iguana-like robot to automatically avoid obstacles based on vision.

[0005] The technical solution of this invention is:

[0006] An underwater motion control method for an automatically evasive, marine iguana-inspired robot includes the following steps:

[0007] 1) The robot moves towards the target position according to the predetermined heading angle, and the binocular camera captures underwater images;

[0008] 2) The host computer processes the image according to the avoidance algorithm and calculates the robot position and obstacle position. Then, it obtains the avoidance heading angle according to the path planning and sends control commands to the microcontroller according to the motion model.

[0009] 3) The microcontroller provides feedback based on the running status of the tail and four legs, controls the movement of the tail and four legs, precisely controls the robot's underwater movement, and the robot automatically avoids obstacles and approaches the target position;

[0010] The marine iguana-like robot includes a head, torso, tail, and four legs; the head is equipped with a binocular camera, and the torso is equipped with a host computer module and a microcontroller module; the host computer module is electrically connected to the binocular camera and the microcontroller module, and the microcontroller module is electrically connected to the tail and four legs.

[0011] The avoidance algorithm includes:

[0012] 1) Image preprocessing of underwater obstacles

[0013] The image is divided into multiple regions, and the saliency value of each region is obtained by calculating the region contrast. If an obstacle is identified, it is marked on the image and the process proceeds to the next step; otherwise, the process proceeds according to the predetermined heading angle.

[0014] 2) Dynamic recognition of underwater obstacle images

[0015] ① Obtain obstacles

[0016] The basic geometric elements of obstacles in the image are determined, and visual computing is used to minimize them through an iterative method. That is, given an initial circle, it is expanded outward through iteration. The expansion stops when it encounters the basic geometric elements of the obstacle in any direction, while other directions continue to expand and eventually converge to the target contour. The Snakes algorithm is used to select the obstacle to obtain the obstacle contour.

[0017] ② Obtain two-dimensional description

[0018] Compare the obstacle outlines in the different images acquired before and after to determine the obstacle's motion state. If the obstacle is moving, proceed to the next step.

[0019] ③ Obtain a three-dimensional description of the object's alignment.

[0020] By comparing the obstacle outlines in the images acquired before and after, the direction of the obstacle's movement can be determined.

[0021] 3) Underwater obstacle positioning

[0022] The image is converted to grayscale, and the average value of R, G, and B is calculated using the RGB model as the grayscale value.

[0023] I(x,y)=(R(x,y)+G(x,y)+B(x,y)) / 3;

[0024] Where I(x,y) represents the pixel located at (x,y), and R(x,y), G(x,y), and B(x,y) are the grayscale values ​​of the R, G, and B components of the pixel at that point, respectively.

[0025] Read the gray values ​​I of different detection points outside the outline of the obstacle selected in the image. If n consecutive neighboring detection points are greater than the threshold P, the detection point is determined to be a feature point. The feature point is compared with the marine image database. If the images match, the position of the obstacle and the position of the robot are obtained.

[0026] The path planning involves establishing a virtual potential field based on the robot's position, obstacle's position, and target position. The target position exerts an attractive force on the robot, while the obstacle exerts a repulsive force on the robot. The avoidance heading angle is determined based on the direction and magnitude of the resultant force, and the avoidance heading angle is continuously acquired to determine the obstacle avoidance path to the target position.

[0027] The gravity formula is:

[0028]

[0029]

[0030] Wherein: F att U represents the gravitational force exerted on the robot at the target position. att Let q be the gravitational potential field, and q be the position of the underwater machine in the water. goal Let ρ(q,q) be the target location, ε be the scale factor, and ρ(q,q) be the target location. gosl Let d be the Euclidean distance between the robot's position and the target position. goal The threshold for preventing excessively large gravitational potential fields can be modified based on the environmental conditions surrounding the marine iguana-like robot.

[0031] The repulsive force formula is:

[0032]

[0033]

[0034]

[0035]

[0036] Wherein: F rep For the resultant and repulsive forces, F rep 1. To prevent the repulsive force from collision, F rep 2. To prevent the repulsive component from falling into a local minimum, U rep For the improved repulsive potential field, n OR To be with F rep1 Unit vectors in the same direction, n RG To be with F rep2 Unit vectors in the same direction, where n is a positive number, η is a proportionality coefficient, and ρ(q,q) obs ) represents the distance between the object and the obstacle, ρ0 represents the radius of influence of the obstacle on the target point, p obsThis indicates the location of the obstacle.

[0037] The formula for calculating the avoidance heading angle is as follows:

[0038]

[0039]

[0040]

[0041] Where: the target location is (X) gaal ,Y goal The robot's position is (X,Y), and the position of the i-th obstacle is (X,Y). obsi ,Y obsi ), F att F is the gravitational force exerted on the robot by the target position. rep1i Let F be the component force of the robot to prevent collisions at the positions of i obstacles. rep2i Let α be the repulsive force exerted by the i-th obstacle position on the robot to prevent it from getting trapped in a local minimum, θ be the vector angle from the robot to the target value, and α be the repulsive force. i Let be the vector angle from the i-th obstacle to the robot. The heading angle for obstacle avoidance by the robot.

[0042] The motion model includes:

[0043] When the robot rises or dives in the water, it controls the tail to swing and the four legs to retract.

[0044] When the robot moves in a curved path, the tail is slightly swung to adjust its direction.

[0045] When the robot crawls in a straight line on the seabed, it controls its four legs to make a diagonal gait, and its tail drags without swaying.

[0046] When the robot encounters an obstacle while crawling in a straight line on the seabed, it controls the tail to swing to adjust its direction, while simultaneously moving its four legs.

[0047] When the robot is swimming near the seabed, it only swings its tail and tucks its four legs when it does not encounter an obstacle. When it encounters an obstacle, it controls the movement of its four legs to push off the obstacle.

[0048] When encountering waves, it controls the movement of its four legs and tail together to maintain stability.

[0049] The beneficial effects of this invention are:

[0050] The marine iguana-like robot acquires images through binocular cameras. The host computer calculates the positions of obstacles and the robot, obtains the avoidance heading angle, and sends control commands to the microcontroller to control the movement of the marine iguana-like robot. The robot moves towards the target position and automatically avoids obstacles, achieving full automation of recognition, localization, path planning, obstacle avoidance, and motion control. Attached Figure Description

[0051] Figure 1 This is a schematic diagram illustrating the principle of the present invention.

[0052] Figure 2 This is a schematic diagram of the connection relationship of the marine iguana-like robot of the present invention.

[0053] Figure 3 This is a schematic diagram of the present invention for acquiring obstacles.

[0054] Figure 4 This is a schematic diagram of the tail of the marine iguana-inspired robot of the present invention.

[0055] Figure 5 This is a schematic diagram of the internal structure of the tail of the marine iguana-inspired robot of the present invention.

[0056] Figure 6 This is a schematic diagram of the tail swinging mechanism of the marine iguana-inspired robot of the present invention.

[0057] Figure 7 This is a schematic diagram of the movement mode of the marine iguana-like robot of the present invention.

[0058] Figure 8 This is a schematic diagram of the swinging of the pull rope at the tail of the marine iguana-like robot of the present invention. Detailed Implementation

[0059] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0060] An underwater motion control method for an automatically evasive, marine iguana-inspired robot includes the following steps:

[0061] 1) The robot moves towards the target position according to the predetermined heading angle, and the binocular camera captures underwater images;

[0062] The target location is preset, the robot's direction of travel toward the target location is the predetermined heading angle, and the marine image database already stores images, feature points, and location information of the robot's movement area;

[0063] 2) The host computer processes the image according to the avoidance algorithm and calculates the robot position and obstacle position. Then, it obtains the avoidance heading angle according to the path planning and sends control commands to the microcontroller according to the motion model.

[0064] 3) The microcontroller provides feedback based on the running status of the tail and four legs, controls the movement of the tail and four legs, and precisely controls the underwater movement of the robot. The robot automatically avoids obstacles and approaches the target position.

[0065] The avoidance algorithm includes:

[0066] 1) Underwater obstacle image preprocessing (using different color channels to clearly distinguish obstacles from the blue ocean background)

[0067] The image is segmented into multiple regions, and the saliency value of each region is obtained by calculating the region contrast (RC), which is the similarity to the blue channel and the difference from the ocean background. If an obstacle is identified, it is marked on the image and the process proceeds to the next step; otherwise, the process proceeds according to the predetermined heading angle.

[0068] Let I be the pixel value of a point in the image, denoted as I. i (i = 1, 2, ..., N, where N is the number of pixels in the image), calculate the saliency value of all pixels in the image:

[0069]

[0070] Where I j It is the preset blue area pixel value, D d (I i I j The contrast is the pixel value of a point in the image and that of the blue region. The greater the contrast, the higher the significance. Points with similar significance are considered to be in the same region.

[0071] To enhance contrast, the regions other than the blue region are weighted more heavily to calculate region significance values.

[0072]

[0073]

[0074] Finally, select the areas with high salience as obstacles.

[0075] in:

[0076] W(R n ): for region R n The pixel values ​​in the region R are used to... n Weighting is applied (the larger the region, the greater the weight).

[0077] D c(R n Region R n The color weighting value within the area (the larger the non-blue area, the larger the value).

[0078] p(c n ,k): represents the k-th color in region n across all C n The probability of getting the middle color.

[0079] D(c n ,k): is the weighted value of the difference between the k-th color and blue in region n (1 for blue, 10 for non-blue);

[0080] 2) Dynamic recognition of underwater obstacles (used to acquire obstacle dynamics)

[0081] ① Obtain obstacles

[0082] The basic geometric elements of obstacles in the image (including edge points, line segments, curves, etc.) are determined, and then minimized using visual computation and an iterative method, given an initial circle ( Figure 3 As shown), after iteration, it expands outward. In any direction, it stops expanding after encountering the basic geometric elements of the image obstacle, while in other directions it continues to expand and eventually converges to the target contour. The Snakes algorithm is used to select the obstacle to obtain the obstacle contour.

[0083] ② Obtain two-dimensional description

[0084] Compare the obstacle outlines in the images acquired before and after to determine the obstacle's motion state (moving or stationary). If the obstacle is moving, proceed to the next step.

[0085] ③ Obtain a three-dimensional description of the object's alignment.

[0086] By comparing the obstacle outlines in the images acquired before and after, the direction of the obstacle's movement can be determined (if an obstacle moves in a certain direction, the robot needs to move in the opposite direction to avoid the obstacle).

[0087] 3) Underwater obstacle positioning

[0088] The image is converted to grayscale, and the average value of R, G, and B is calculated using the RGB model as the grayscale value:

[0089] I(x,y)=(R(x,y)+G(x,y)+B(x,y)) / 3

[0090] Where I(x,y) represents the pixel located at (x,y), and R(x,y), G(x,y), and B(x,y) are the grayscale values ​​of the R, G, and B components of the pixel at that point, respectively.

[0091] The outlines of obstacles selected in the image are removed. The gray values ​​I of different detection points on the image are read. If n consecutive neighboring detection points are greater than the threshold P (P is 120% of I), the detection points are determined to be feature points. The feature points are compared with the marine image database. If the images match (matching degree is higher than 80%), the positions of the obstacles and the robot are obtained. When there are no obstacles, the robot position can be obtained in this way.

[0092] Images in the marine image database correspond to different latitudes and longitudes. After image matching, the latitude and longitude of the image can be determined, thereby determining the position of the marine iguana robot. Then, a coordinate system is established with the binocular camera as the origin, and the latitude and longitude of the obstacle is calculated to obtain the position of the obstacle (3-3).

[0093] The path planning includes:

[0094] A virtual potential field is established based on the robot's position, the obstacle's position, and the target's position. The target position exerts an attractive force on the robot, while the obstacle exerts a repulsive force on the robot. The robot determines its avoidance heading angle based on the direction and magnitude of the net force acting on it, and continuously acquires the heading angle to determine the obstacle avoidance path to reach the target position.

[0095] Specifically, the current robot position and obstacle position are obtained through image comparison, while the target position is preset by the operator and transmitted to the robot. The robot position, obstacle position, and target position are all determined within a preset coordinate system.

[0096] Considering that in an underwater environment, robots need to avoid multiple obstacles to reach their target location, which is a complex process, a virtual potential field is established in the underwater environment. This allows the robot to accurately avoid obstacles and move towards the target location with the optimal heading angle when it approaches multiple obstacles and needs to move towards the target point.

[0097] Specifically, a virtual potential field is established based on the robot's environment. This virtual potential field comprises both attractive and repulsive potential fields. The robot is situated within the attractive potential field of the target, meaning the target exerts an attractive force on the robot. The marine iguana-like robot is situated within the repulsive potential fields of the obstacles, meaning the obstacles exert a repulsive force on the robot. When encountering an obstacle, within the established virtual potential field, the target point exerts an attractive force on the robot, while the obstacle exerts a repulsive force. The robot determines its avoidance heading angle based on the direction and magnitude of the net force acting on it; otherwise, it is only subject to the attractive force of the target position and travels in a straight line.

[0098] Specifically, the gravitational potential field of the marine iguana-like robot is generated by the attraction of the target position to the robot, and is related to the Euclidean distance between the current robot position and the target position. The gravitational force exerted by the target position on the robot can be obtained by differentiating the gravitational potential field. In a preset coordinate system, the Euclidean distance from the robot to the target can be calculated based on the obtained robot position and the initially input target position. A preset threshold of 2 is set for the Euclidean distance between the robot and the target, which can be modified according to the surrounding environment of the marine iguana-like robot. The gravitational potential field is calculated based on the Euclidean distance between the robot and the target and the preset threshold. The gravitational force is calculated by differentiating the gravitational potential field. The gravitational force formula is as follows:

[0099]

[0100]

[0101] Wherein: F att U represents the gravitational force exerted on the robot at the target position. att Let q be the gravitational potential field, and q be the position of the underwater machine in the water. goal Let ρ(q,q) be the target location, ε be the scale factor, and ρ(q,q) be the target location. gosl Let d be the Euclidean distance between the robot's position and the target position. goal The threshold for preventing excessively large gravitational potential fields can be modified based on the environmental conditions surrounding the marine iguana-like robot.

[0102] Specifically, the repulsive potential field surrounding the robot is generated by the repulsion exerted on the robot by each obstacle and is related to the Euclidean distance from the current robot position to each obstacle position. Considering that if the combined repulsive force of several obstacles is equal in magnitude and opposite in direction to the gravitational force at the target point when the robot is at a certain position, the resultant force is zero, causing the robot to no longer experience any force and thus unable to search for an obstacle avoidance path. To prevent the magnitude of the repulsive force from falling into a local minimum, we use an improved repulsive potential field function. Differentiating the repulsive potential field yields the repulsive force exerted on the marine iguana robot by each obstacle, including a collision-prevention repulsive component and a local minimum repulsive component. In a preset coordinate system, the Euclidean distance from the current robot position to each obstacle can be calculated based on the current robot position and the positions of each obstacle. Based on the Euclidean distances from the current robot to the obstacles and from the current robot to the target, the repulsive potential field can be calculated. Differentiating the repulsive field yields the collision-prevention repulsive component and the local minimum repulsive component. The formula for calculating repulsive force is as follows:

[0103]

[0104]

[0105]

[0106]

[0107] Wherein: F rep For the resultant and repulsive forces, F rep1 To prevent the repulsive force from the collision, F rep 2. To prevent the repulsive component from falling into a local minimum, U rep For the improved repulsive potential field, n OR To be with F rep1 Unit vectors in the same direction, n RG To be with F rep2 Unit vectors in the same direction, where n is a positive number, η is a proportionality coefficient, and ρ(q,q) obs ) represents the distance between the object and the obstacle, ρ0 represents the radius of influence of the obstacle on the target point, p obs This indicates the location of the obstacle.

[0108] Specifically, based on the calculated gravitational and repulsive forces acting on the robot, the direction of the resultant force can be obtained by vector summation, thus yielding the robot's avoidance heading angle. By continuously acquiring the avoidance heading angle, an obstacle avoidance path can be planned for the marine iguana-like robot to continuously approach the target value. The formula for calculating the avoidance heading angle is as follows:

[0109]

[0110]

[0111]

[0112] Where: the target location is (X) gaal ,Y goal The robot's position is (X,Y), and the position of the i-th obstacle is (X,Y). obsi ,Y obsi ), F att F is the gravitational force exerted on the robot by the target position. rep1i Let F be the component force of the robot to prevent collisions at the positions of i obstacles. rep2i Let α be the repulsive force exerted by the i-th obstacle position on the robot to prevent it from getting trapped in a local minimum, θ be the vector angle from the robot to the target value, and α be the repulsive force. i Let be the vector angle from the i-th obstacle to the robot. The heading angle for obstacle avoidance by the robot.

[0113] The motion model includes:

[0114] like Figure 7As shown in (a) and (b), when the robot rises or dives in the water, it controls the tail to swing and the four legs to retract.

[0115] like Figure 7 As shown in (c) and (d), when the robot is crawling in a straight line on the seabed, it controls its four legs to perform a diagonal gait, with the tail dragging without swaying. When the robot is moving in a curved path, it controls the tail to sway slightly to adjust its direction.

[0116] like Figure 7 As shown in (d), when the robot encounters an obstacle while crawling in a straight line on the seabed, it controls the tail to swing to adjust its direction, while moving its four legs. The four legs are raised to the corresponding height according to the height of the obstacle and then move forward to avoid the obstacle.

[0117] like Figure 7 As shown in (a), (b), and (c), when the robot is swimming near the seabed, it only swings its tail and retracts its four legs when it does not encounter an obstacle. When it encounters an obstacle, it controls the movement of its four legs to push off the obstacle.

[0118] like Figure 7 As shown in (d), when encountering waves, the four legs and tail are controlled to move together to maintain stability and prevent the bionic robot from being swept away by the waves. If the waves are in the same direction as the original movement of the bionic robot, the tail of the bionic robot is controlled to swing and turn until it is in the opposite direction to the waves. When the waves are in the opposite direction to the original movement of the bionic robot, the four legs are controlled to paddle forward in a diagonal trotting motion, and the tail moves with a certain swing amplitude and frequency according to the size of the waves to provide power to resist the waves.

[0119] The marine iguana-like robot includes a head, torso, tail, and four legs.

[0120] The head is equipped with a binocular camera, and the torso is equipped with a host computer module and a microcontroller module. The host computer module is electrically connected to the binocular camera via a serial port or network port, and the host computer module is electrically connected to the microcontroller module via a data cable. The microcontroller module is electrically connected to the tail and four legs.

[0121] During underwater movement, the marine iguana-inspired robot maintains stability through the coordinated movement of its four legs and tail. By controlling the tail's swing angle and speed, as well as the movement speed and gait of its four legs, it can change its forward momentum and direction. It exhibits high flexibility and controllability in complex underwater environments, adapts well to various underwater conditions, and reduces the impact of complex underwater environments on its underwater movement.

[0122] The binocular camera first obtains underwater environment images, the host computer processes the images to obtain obstacle avoidance paths, and then sends motion control commands to the microcontroller. The microcontroller interprets the motion control commands into corresponding timing signals and directly controls the robot's tail and four legs to perform relevant actions, thereby realizing precise underwater motion control of the marine iguana-like robot.

[0123] The angle and frequency of the tail's left and right swings need to be adjusted according to the state of the bionic robot.

[0124] like Figure 4 As shown, the tail section includes several elliptical tail joints 1, a pull rope 2, a spring 3, a tail tip 4, and a tail servo 5. The tail swings left and right to provide forward propulsion. The greater the swing frequency and the greater the swing angle, the greater the propulsion. When the tail swings to the left and right at the same angle, the robot moves in a straight line; when the tail swings to the left at a greater angle than to the right, the robot moves to the right; when the tail swings to the right at a greater angle than to the right, the robot moves to the left. The tail servo is as follows: Figure 5 As shown, the swing angle and speed are controlled by the tail servo, or by the swing arm 5.1 driven by the tail servo. The swing arm has two pairs of limiting holes 5.11. When the rope is tied to the outer limiting hole, the tail swings faster and the swing angle is larger. The rotation angle and speed of the tail servo can be adjusted according to actual needs to achieve tail swing motion control.

[0125] The tail control method in this invention involves the tail swinging left or right from its base to generate forward momentum. During the swinging process, each joint of the tail also bends accordingly, thereby increasing the swing amplitude and thus increasing forward momentum. The tail motion control method for underwater swimming mainly employs a left-right swaying motion. The tail swaying can be divided into four stages. Here, the change of the robot's tail swaying angle with time t conforms to the following equation:

[0126]

[0127] Choosing an appropriate method to simplify the calculation yields more accurate and realistic data. The tail's swing amplitude is used to accurately describe the change function with time t, and its equation is as follows:

[0128]

[0129] like Figure 8 As shown, l is the tail length, r is half the distance between the nodes at the base of the two pull ropes, h is the vertical distance from the center point of the swing arm to the plane at the base of the tail, and ω is the angular velocity of the servo motor.

[0130] In this invention, the quadrupedal motion control of the robot is basically the same as that of a quadrupedal robot, and a diagonal gait control method is adopted.

[0131] The accompanying drawings illustrate preferred embodiments of the invention. However, the invention can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a thorough and complete understanding of the disclosure of the invention.

Claims

1. An underwater motion control method of an automatic-avoidance iguana-like robot, comprising the following steps: 1) The robot advances towards a target position according to a predetermined heading angle, and a binocular camera captures underwater images; 2) An upper computer processes the images according to an avoidance algorithm, calculates the position of the robot and the position of the obstacle, then obtains an avoidance heading angle according to path planning, and sends a control instruction to a single-chip microcomputer according to a motion model; 3) The single-chip microcomputer makes feedback according to the running state of the tail and the four legs, controls the tail and the four legs to move, and accurately controls the underwater motion of the robot, so that the robot automatically avoids the obstacle and approaches the target position; The iguana-like robot comprises a head, a trunk, a tail and four legs; the head is provided with a binocular camera, and the trunk is provided with an upper computer module and a single-chip microcomputer module; the upper computer module is electrically connected with the binocular camera and the single-chip microcomputer module, and the single-chip microcomputer module is electrically connected with the tail and the four legs; The path planning comprises: establishing a virtual potential field according to the position of the robot, the position of the obstacle and the target position; the target position generates an attractive force on the robot, and the obstacle generates a repulsive force on the robot; the avoidance heading angle is determined according to the direction and size of the resultant force; and the avoidance heading angle is continuously obtained to determine an obstacle-avoiding path to the target position; The attractive force formula is: Where: F att is the attractive force of the robot to the target position, U att is the attractive potential, q is the position of the underwater robot in water, q goal is the target position, ε is the scale factor, ρ(q, q gosl ) is the Euclidean distance between the position of the robot and the target position, d goal is the threshold value for preventing the attractive potential from being too large, which can be modified according to the environmental conditions around the iguana robot. The repulsive force formula is: where: F rep is the repulsive force, F rep1 is the repulsive force to prevent collision, F rep2 is the repulsive force to prevent falling into local minimum, U rep is the improved repulsive potential field, n OR is the unit vector in the same direction as F rep1 , n RG is the unit vector in the same direction as F rep2 , n is positive, η is a positive proportional coefficient, ρ(q, q obs ) represents the distance between the object and the obstacle, ρ0 represents the influence radius of the obstacle on the target point, p obs is the obstacle position; The calculation formula of the avoidance heading angle is as follows: Wherein: target position is (X gaal ,Y goal ), robot position is (X,Y), the i-th obstacle position is (X obsi ,Y obsi ), F att is the attraction of the target position to the robot, F rep1i is the anti-collision repulsive component of the i-th obstacle position to the robot, F rep2i is the anti-trapping-in-local-minimum repulsive component of the i-th obstacle position to the robot, θ is the vector angle of the robot to the target value, α i is the vector angle of the i-th obstacle to the robot, is the robot obstacle avoidance heading angle.

2. The underwater motion control method of an automatic-avoidance iguana-like robot according to claim 1, wherein: The avoidance algorithm comprises: 1) Underwater obstacle image preprocessing The image is divided into multiple regions, the saliency value of each region is obtained by calculating the region contrast, if the obstacle is identified, the image is marked, and the next step is entered, otherwise the robot advances according to the predetermined heading angle; 2) Underwater obstacle image dynamic identification ①Obstacle acquisition The basic geometric elements of the obstacle in the image are determined, visual computing is used, and the method of iteration is used for minimization, that is, an initial circle is given, and after iteration, it is expanded outward, and stops expanding in any direction after encountering the basic geometric elements of the image obstacle, while the other directions continue to expand and finally converge to the target contour; the Snakes algorithm is adopted to frame the obstacle to obtain the obstacle contour; ②Two-dimensional description acquisition The obstacle contours in the different images obtained before and after are compared to determine the motion state of the obstacle, and if the obstacle is moving, the next step is entered; ③Three-dimensional description acquisition of object positioning The obstacle contours in the images obtained before and after are compared to determine the motion direction of the obstacle; 3) Underwater positioning of the obstacle The image is processed by grayscale, and the average values of R, G and B are calculated as the grayscale value by using the RGB model: I(x, y) = (R(x, y) + G(x, y) + B(x, y)) / 3; Where I(x, y) represents a pixel located at (x, y), R(x, y), G(x, y) and B(x, y) are the R component, G component and B component values of the pixel, respectively, and the grayscale is obtained; The grayscale values I of different detection points outside the framed obstacle contour on the image are read, if the grayscale values of n continuous neighborhood detection points are greater than a threshold P, the detection point is determined as a feature point, and the feature point is compared with a sea image database, if the image matches, the position of the obstacle and the position of the robot are obtained.

3. The underwater motion control method of an automatic-avoidance iguana-like robot according to claim 2, wherein: The motion model comprises: When the robot is ascending or diving in water, control the tail to swing, the four legs to retract; When the robot is moving in curve, control the tail to swing slightly to adjust the direction; When the robot is moving in straight line on the sea bottom, control the four legs to move in diagonal gait, the tail to drag without swing; When the robot is moving in straight line on the sea bottom and meets an obstacle, control the tail to swing to adjust the direction, meanwhile control the four legs to move; When the robot is moving near the sea bottom, only the tail swings without the four legs moving when there is no obstacle, control the four legs to move to kick off the obstacle when there is an obstacle; When the robot meets a wave, control the four legs and the tail to move together to keep stable.

Images