Underwater robot autonomous obstacle avoidance method based on improved artificial potential field algorithm

Abstract

The invention relates to an autonomous obstacle avoidance method of an underwater robot based on an improved artificial potential field algorithm. The method comprises the following steps: modeling an autonomous obstacle avoidance problem environment of the underwater robot; according to the improved potential field function, the attraction force and repulsive force currently borne by the underwater robot are calculated, and then the magnitude and direction of the resultant force are calculated; the virtual obstacle circle construction method solves the local minimum problem; the speed potential field function solves the problem of dynamic obstacle avoidance. Introducing a logarithmic function on the basis of an attraction potential field function of the existing artificial potential field method, and designing the logarithmic function as a piecewise function; a balance item is added in a repulsion potential field function, the problem that the underwater robot cannot reach a target point when an obstacle is close to a target is avoided, a virtual obstacle circle construction method is provided, the speed repulsion potential field function is increased, the problem of dynamic obstacle avoidance is solved, autonomous obstacle avoidance in a complex environment is achieved, and the obstacle avoidance efficiency is improved. The method can be applied to an autonomous obstacle avoidance system of an autonomous underwater robot.

Description

Technical Field

[0001] The invention relates to the field of autonomous obstacle avoidance for an autonomous underwater robot, and in particular to an autonomous obstacle avoidance method for an underwater robot based on an improved artificial potential field algorithm. Background Art

[0002] Autonomous underwater vehicles (AUVs) are widely used in military and civilian fields, such as mine clearance, ocean sampling, geological sampling, and seabed exploration. They integrate technologies such as communications, networked systems, information fusion, and intelligent control, and can achieve functions such as autonomous navigation and intelligent obstacle avoidance. Autonomous obstacle avoidance is a strong guarantee for the safe navigation of autonomous underwater robots and the successful execution of tasks.

[0003] At present, there are many methods for autonomous underwater robots to avoid obstacles. Dynamic window algorithm and artificial potential field method are often used for autonomous obstacle avoidance. The artificial potential field method is simple to calculate, has good real-time performance, and can adapt to complex obstacles in unknown environments. However, when using the artificial potential field algorithm for autonomous obstacle avoidance, it has the following disadvantages:

[0004] 1. For complex unknown environments, when there are obstacles near the target, as the underwater robot approaches the target, the attractive potential field decreases and the repulsive potential field increases. The repulsive force on the underwater robot is much greater than the attractive force, and the resultant force always points away from the target point, so that it cannot reach the target point.

[0005] 2. When navigating toward the target, the underwater robot may pass through the potential field minimum position of non-target points. At these positions, the attractive and repulsive forces are almost equal in the collinear direction, but in opposite directions. The attractive and repulsive forces cancel each other out, causing the underwater robot to stagnate or oscillate locally.

[0006] 3. When an obstacle moves at a relatively low speed, it has little impact on the traditional artificial potential field algorithm. However, when the obstacle moves at a relatively high speed, the traditional artificial potential field does not consider the impact of the obstacle's movement speed, and there is a risk of collision. Summary of the invention

[0007] (I) Purpose of the invention

[0008] Aiming at the fact that the existing artificial potential field method has unreachable targets, local minima, and does not consider the influence of dynamic obstacle speed, an autonomous obstacle avoidance method for an autonomous underwater robot based on an improved artificial potential field method is proposed.

[0009] (II) Technical solution

[0010] In order to achieve the above object, the present invention provides an autonomous obstacle avoidance method for an autonomous underwater robot based on an improved artificial potential field method, and the specific steps are as follows:

[0011] Step 1: Establish a coordinate system, obtain the real-time position and speed information of the robot and obstacles through various sensors, and input the target position;

[0012] Step 2: Determine whether the underwater robot has reached the target point:

[0013] a. If the target point has not been reached, go to step 3;

[0014] b. If the target point is reached, the movement task is completed;

[0015] Step 3: Calculate the current attractive and repulsive forces on the underwater robot, and then calculate the magnitude and direction of the resultant force;

[0016] Step 4: Determine whether the resultant force calculated in step 3 is trapped in a local minimum according to its magnitude and direction:

[0017] a. If it falls into a local minimum, go to step 5;

[0018] b. If it does not fall into the local minimum, go to step 7;

[0019] Step 5: Select the obstacle closest to the underwater robot to construct a virtual obstacle circle. The constructed virtual obstacle will completely contain the original obstacle, ensuring that the underwater robot can safely avoid the obstacle and break the local minimum situation. ;

[0020] Step 6: Recalculate the magnitude and direction of the resultant force based on the constructed virtual obstacle;

[0021] Step 7: Move to the next path point calculated based on the resultant force and the set step length, and return to step 2

[0022] In step 3, the current position coordinates of the underwater robot are defined as X(x, y) T , the target position coordinate is X(x t ,y t ) T , the distance between the underwater robot and the target is d(x,x t ), and the distance between it and the obstacle is d(x,x o ). Considering that at a distance far from the target, the attraction should increase as the distance between the two increases to avoid the risk of collision with obstacles, while at a distance close to the target, the attraction should increase as the distance decreases to avoid the problem of unreachable targets. Therefore, the attraction potential field function is designed as a piecewise function, and a logarithmic function is introduced. The improved attraction potential field function is designed as:

[0023]

[0024] Where k att1 , k att2 is the attractive potential field parameter, dt is the limited distance between the AUV and the target. The attraction function is the negative gradient of the attraction potential field function:

[0025]

[0026] in is the unit vector along the direction of the AUV pointing to the target, indicating the direction of attraction.

[0027] On the other hand, a balance term pointing to the target direction is added to the repulsive potential field function to balance the increase of the repulsive force. When the AUV approaches the target, the repulsive force gradually weakens to ensure that it reaches the target point. The improved repulsive potential field function is:

[0028]

[0029] Where n is any real number greater than 0. The improved repulsive force function is the negative gradient of the repulsive potential field function:

[0030]

[0031] in is the unit vector pointing toward the AUV from the center of the obstacle, indicating the direction of the repulsive force.

[0032] In step 3, the obstacle speed information is introduced and defined as V o , define the relative speed between the underwater robot and the obstacle as V a -V o , α is the angle between the relative velocity direction and the relative position direction. The velocity repulsive potential field function is added on the basis of the repulsive potential field function:

[0033]

[0034] Where k v is the dynamic obstacle rejection parameter, V ao is the relative speed between the AUV and the dynamic obstacle.

[0035] The repulsive force function is:

[0036] F rep =F rep (X)+F rep (V) (7)

[0037]

[0038] Where Vi represents the direction of the velocity repulsion function.

[0039] In step 5, in order to solve the local minimum problem, when the AUV navigates to the local minimum area, a virtual obstacle circle is constructed so that its center is offset from the center of the original obstacle and completely includes the original obstacle to ensure navigation safety. The center O' and radius of the virtual obstacle circle are:

[0040] O'(x',y')=(x+o i ,y+o i ) (9)

[0041]

[0042] Where O'(x',y') represents the coordinates of the center of the virtual obstacle circle, x and y represent the coordinates of the center of the original obstacle, R' represents the radius of the virtual obstacle circle, R represents the radius of the original obstacle, and O i is the eccentricity parameter of the virtual obstacle circle.

[0043] (III) Beneficial effects:

[0044] Compared with the prior art, the present invention has the following beneficial effects:

[0045] 1. A logarithmic function is introduced on the basis of the attractive potential field function of the existing artificial potential field method and designed as a piecewise function; and a balance term is added to the repulsive potential field function to avoid the problem that the underwater robot cannot reach the target point when the obstacle is close to the target.

[0046] 2. A virtual obstacle circle construction method is proposed. The obstacle closest to the underwater robot is selected to construct a virtual obstacle circle. The constructed virtual obstacle will completely contain the original obstacle, ensuring that the underwater robot can safely avoid the obstacle and break the local minimum situation.

[0047] 3. Introduce obstacle speed information, add speed repulsion potential field function on the basis of repulsion potential field function, and solve the dynamic obstacle avoidance problem. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 This is a flow chart of the underwater robot autonomous obstacle avoidance method based on the improved artificial potential field algorithm of the present invention;

[0049] Figure 2 A schematic diagram of constructing a virtual obstacle circle method of the present invention;

[0050] Figure 3 This is a diagram of the obstacle avoidance simulation effect of the underwater robot autonomous obstacle avoidance method based on the improved artificial potential field algorithm in a static obstacle environment.

[0051] Figure 4 This is a diagram of the obstacle avoidance simulation effect of the underwater robot autonomous obstacle avoidance method based on the improved artificial potential field algorithm in a dynamic obstacle environment. DETAILED DESCRIPTION

[0052] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0053] like Figure 1 As shown, the underwater robot autonomous obstacle avoidance method of the present invention improves the artificial potential field algorithm, comprising the following steps:

[0054] Step 1: Establish a coordinate system, obtain the real-time position and speed information of the robot and obstacles through various sensors, and input the target position;

[0055] Step 2: Determine whether the underwater robot has reached the target point:

[0056] a. If the target point has not been reached, go to step 3;

[0057] b. If the target point is reached, the movement task is completed;

[0058] Step 3: Calculate the current attractive and repulsive forces on the underwater robot, and then calculate the magnitude and direction of the resultant force;

[0059] Step 4: Determine whether the resultant force calculated in step 3 is trapped in a local minimum according to its magnitude and direction:

[0060] a. If it falls into a local minimum, go to step 5;

[0061] b. If it does not fall into the local minimum, go to step 7;

[0062] Step 5: Select the obstacle closest to the underwater robot to construct a virtual obstacle circle. The constructed virtual obstacle will completely contain the original obstacle, ensuring that the underwater robot can safely avoid the obstacle and break the local minima.

[0063] Step 6: Recalculate the magnitude and direction of the resultant force based on the constructed virtual obstacle;

[0064] Step 7: Move to the next path point calculated based on the resultant force and the set step length, and return to step 2

[0065] In step 3, the current position coordinates of the underwater robot are defined as X(x, y) T , the target position coordinate is X(x t ,y t ) T , the distance between the underwater robot and the target is d(x,x t ), and the distance between it and the obstacle is d(x,x o). Considering that at a distance far from the target, the attraction should increase as the distance between the two increases to avoid the risk of collision with obstacles, while at a distance close to the target, the attraction should increase as the distance decreases to avoid the problem of unreachable targets. Therefore, the attraction potential field function is designed as a piecewise function, and a logarithmic function is introduced. The improved attraction potential field function is designed as:

[0066]

[0067] Where k att1 , k att2 is the attractive potential field parameter, d t is the limited distance between the AUV and the target. The attraction function is the negative gradient of the attraction potential field function:

[0068]

[0069] in is the unit vector along the direction of the AUV pointing to the target, indicating the direction of attraction.

[0070] On the other hand, a balance term pointing to the target direction is added to the repulsive potential field function to balance the increase of the repulsive force. When the AUV approaches the target, the repulsive force gradually weakens to ensure that it reaches the target point. The improved repulsive potential field function is:

[0071]

[0072] Where n is any real number greater than 0. The improved repulsive force function is the negative gradient of the repulsive potential field function:

[0073]

[0074] in is the unit vector pointing toward the AUV from the center of the obstacle, indicating the direction of the repulsive force.

[0075] In step 3, the obstacle speed information is introduced and defined as V o , define the relative speed between the underwater robot and the obstacle as V a -V o , α is the angle between the relative velocity direction and the relative position direction. The velocity repulsive potential field function is added on the basis of the repulsive potential field function:

[0076]

[0077] Where k v is the dynamic obstacle rejection parameter, V ao is the relative speed between the AUV and the dynamic obstacle.

[0078] The repulsive force function is:

[0079] F rep =F rep (X)+F rep (V) (17)

[0080]

[0081] Where Vi represents the direction of the velocity repulsion function.

[0082] In step 5, in order to solve the local minimum problem, when the AUV navigates to the local minimum area, a virtual obstacle circle is constructed so that its center is offset from the center of the original obstacle and completely includes the original obstacle to ensure navigation safety. The center O' and radius of the virtual obstacle circle are:

[0083] O'(x',y')=(x+o i ,y+o i ) (19)

[0084]

[0085] Where O'(x',y') represents the coordinates of the center of the virtual obstacle circle, x and y represent the coordinates of the center of the original obstacle, R' represents the radius of the virtual obstacle circle, R represents the radius of the original obstacle, and O i is the eccentricity parameter of the virtual obstacle circle.

[0086] The significant difference between the present invention and the existing methods is that: first, a logarithmic function is introduced on the basis of the attraction potential field function of the existing artificial potential field method, and it is designed as a piecewise function; and a balance term is added to the repulsion potential field function to avoid the problem that the underwater robot cannot reach the target point when the obstacle is close to the target. Second, a virtual obstacle circle construction method is proposed, and a virtual obstacle circle is constructed by selecting the obstacle closest to the underwater robot. The constructed virtual obstacle will completely contain the original obstacle, ensuring that the underwater robot can safely avoid the obstacle and breaking the local minimum situation. Third, the obstacle speed information is introduced, and the speed repulsion potential field function is added on the basis of the repulsion potential field function to solve the dynamic obstacle avoidance problem.

[0087] It should be clear that the present invention is not limited to the specific configuration and processing described above and shown in the figures. For the sake of simplicity, a detailed description of the known method is omitted here. In the above embodiments, several specific steps are described and shown as examples. However, the method process of the present invention is not limited to the specific steps described and shown, and those skilled in the art can make various changes, modifications and additions, or change the order between the steps after understanding the spirit of the present invention.

[0088] Finally, it should be noted that the above-described embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An autonomous obstacle avoidance method for an autonomous underwater robot based on an improved artificial potential field method, the specific steps of which are as follows: Step 1: Establish a coordinate system, obtain the real-time position and speed information of the robot and obstacles through various sensors, and input the target position; Step 2: Determine whether the underwater robot has reached the target point: a. If the target point has not been reached, go to step 3; b. If the target point is reached, the movement task is completed; Step 3: Calculate the current attraction and repulsion of the underwater robot, and then calculate the magnitude and direction of the resultant force; Step 4: Determine whether the resultant force calculated in step 3 is in a local minimum according to its magnitude and direction: a. If it falls into a local minimum, go to step 5; b. If it does not fall into the local minimum, go to step 7; Step 5: Select the obstacle closest to the underwater robot to construct a virtual obstacle circle. The constructed virtual obstacle will completely contain the original obstacle, ensuring that the underwater robot can safely avoid the obstacle and break the local minimum situation. ; Step 6: Recalculate the magnitude and direction of the resultant force based on the constructed virtual obstacle; Step 7: Move to the next path point calculated based on the resultant force and the set step length, and return to step 2 The autonomous obstacle avoidance method for an autonomous underwater robot based on an improved artificial potential field method according to claim 1 is characterized in that: In step 3, the current position coordinates of the underwater robot are defined as X(x, y) T , the target position coordinate is X(x t ,y t ) T , the distance between the underwater robot and the target is d(x,x t ), and the distance between it and the obstacle is d(x,x o ). Considering that at a distance far from the target, the attraction should increase as the distance between the two increases to avoid the risk of collision with obstacles, while at a distance close to the target, the attraction should increase as the distance decreases to avoid the problem of unreachable targets. Therefore, the attraction potential field function is designed as a piecewise function, and a logarithmic function is introduced. The improved attraction potential field function is designed as: Where k att1 , k att2 is the attractive potential field parameter, d t is the limited distance between the AUV and the target. The attraction function is the negative gradient of the attraction potential field function: in is the unit vector in the direction of the AUV pointing to the target, indicating the direction of attraction. On the other hand, a balance term pointing to the target direction is added to the repulsive potential field function to balance the increase of the repulsive force. When the AUV approaches the target, the repulsive force gradually weakens to ensure that it reaches the target point. The improved repulsive potential field function is: Where n is any real number greater than 0. The improved repulsive force function is the negative gradient of the repulsive potential field function: in is the unit vector pointing toward the AUV from the center of the obstacle, indicating the direction of the repulsive force. The autonomous obstacle avoidance method for an autonomous underwater robot based on an improved artificial potential field method according to claim 1 is characterized in that: In step 3, the obstacle speed information is introduced and defined as V o , define the relative speed between the underwater robot and the obstacle as V a -V o , α is the angle between the relative velocity direction and the relative position direction. The velocity repulsive potential field function is added on the basis of the repulsive potential field function: Where k v is the dynamic obstacle rejection parameter, V ao is the relative speed between the AUV and the dynamic obstacle. The repulsive force function is: F rep =F rep (X)+F rep (V) (7) Where Vi represents the direction of the velocity repulsion function. The autonomous obstacle avoidance method for an autonomous underwater robot based on an improved artificial potential field method according to claim 1 is characterized in that: In step 5, in order to solve the local minimum problem, when the AUV navigates to the local minimum area, a virtual obstacle circle is constructed so that its center is offset from the center of the original obstacle and completely includes the original obstacle to ensure navigation safety. The center O' and radius of the virtual obstacle circle are: O'(x',y')=(x+o i ,and+or i ) (9) Where O'(x',y') represents the coordinates of the center of the virtual obstacle circle, x and y represent the coordinates of the center of the original obstacle, R' represents the radius of the virtual obstacle circle, R represents the radius of the original obstacle, and O i is the eccentricity parameter of the virtual obstacle circle.

Images