CN113189984A - Unmanned ship path planning method based on improved artificial potential field method - Google Patents

Abstract

The invention relates to an unmanned ship path planning method based on an improved artificial potential field method, which comprises the following steps: on the basis of a traditional artificial potential field method, aiming at a complex marine environment and most of obstacles with irregular shapes, the arrangement of the obstacles, which are regarded as particles, is changed, the obstacles are subjected to expansion treatment, and a steering area is reserved; when the attractive force and the repulsive force constructed by the traditional artificial potential field method are collinear and opposite in direction, the unmanned ship turns in a turning area in advance, and the problem that the unmanned ship falls into a local minimum point during path planning is solved; the method has the advantages that the practical application condition of the unmanned ship is considered, the problem of path mutation caused by combination force direction mutation and overlarge corner change in the path planning process is solved, the limitation of the maximum corner and the maximum angular acceleration of the unmanned ship is added, and therefore the planned path can be ensured to smoothly avoid obstacles and the effect of small corner change of the unmanned ship is achieved.

Description

Unmanned ship path planning method based on improved artificial potential field method

Technical Field

The invention relates to an unmanned ship path planning method based on an improved artificial potential field method, and belongs to the field of marine unmanned aerial vehicle motion control and path planning.

Background

At present, unmanned systems represented by unmanned ships are rapidly developing, and a large number of unmanned systems are developed and put into use. But the path planning research for unmanned ship motion control is not mature.

Common path planning methods include simulated annealing algorithm, fuzzy logic algorithm, tabu search algorithm, a-x algorithm, artificial potential field method, and the like. Compared with other algorithms, the artificial potential field method has the advantages of short operation time, strong real-time performance, good hardware adaptability and the like. However, the application of the traditional artificial potential field method in the fields of unmanned ship motion control and path planning has the following problems:

(1) the method has the advantages that the problem of obstacle modeling is solved, an unmanned ship sails on a complex sea surface, obstacles are considered as particles, and a planned path is difficult to achieve the optimal path;

(2) the problem of local minimum points is easily caused by local oscillation of the unmanned ship, so that the target can not be reached;

(3) the corner change is severe, and the corner change is continuous and cannot be suddenly changed and exceed the maximum corner of the unmanned ship in the actual sailing process of the unmanned ship.

In conclusion, how to solve the application of the traditional artificial potential field method under the condition of considering the actual navigation of the unmanned ship becomes a difficult point to be solved urgently.

Disclosure of Invention

The invention aims to provide an unmanned ship path planning method based on an improved artificial potential field method, and solves the problems that the traditional artificial potential field method is applied to the idealization of obstacle modeling, easy to fall into local minimum points, severe in corner change and the like in the unmanned ship path planning process.

The invention adopts the following technical scheme for solving the problems: an unmanned ship path planning method based on an improved artificial potential field method is designed, and on the basis of a traditional artificial potential field method, an obstacle is subjected to expansion processing, and a steering area is reserved; when the attractive force and the repulsive force constructed by the traditional manual potential field method are collinear and opposite in direction, the unmanned ship is enabled to turn in a turning area in advance, and the problem that the unmanned ship falls into a local minimum point is solved; the limitation of the maximum rotation angle and the maximum angular acceleration of the unmanned ship is added, and the problem of severe change of the rotation angle of the unmanned ship is solved. The method specifically comprises the following steps:

step 1:

establishing a circular equivalent model of the obstacles in the map, performing expansion treatment on the obstacles, reserving a turning area, setting relevant parameters, and then entering step 2;

step 2:

judging whether the unmanned ship reaches a target point, if so, ending, otherwise, entering the step 3;

and step 3:

constructing a gravity function:

F_g(x)＝k·d(x,x_goal) (1)

in the formula: f_g(x) Is a gravitation function, k is a gravitation coefficient, x is the current position of the unmanned ship, x_goalAs target point position, d (x, x)_goal) The model length is a vector, the model length is the distance between the current position of the unmanned ship and a target point, and the direction is that the unmanned ship points to the target point. Gravitation function F_g(x) The gravity is generated on the unmanned ship, and the unmanned ship goes to the target point under the action of the gravity.

Constructing a repulsion function:

in the formula: f_o(x) Is a function of repulsion, n is the coefficient of repulsion, x_obsIs the position of an obstacle, d₀Is the radius of influence of the obstacle, d (x, x)_obs) Is a vector, the module length is the distance between the current position of the unmanned ship and the obstacle, the direction is the direction of the obstacle pointing to the unmanned ship, | d (x, x)_obs) L is d (x, x)_obs) Die length of (2). When the unmanned ship influences the radius d on the obstacle₀Inner, repulsive force function F_o(x) And a repulsive force is generated to make the obstacle far away. Then entering step 4;

and 4, step 4:

judging whether the attractive force and the repulsive force are collinear and have opposite directions, if so, entering a step 5, and otherwise, entering a step 8;

and 5:

at the moment, in order to get rid of local minimum points and avoid obstacles, the unmanned ship is in a steering area, and a corner formula is constructed:

in the formula: theta is the turning angle of the unmanned ship in the turning area, d_actIs the actual radius, θ, of the obstacle after it has been inflated_maxIndicating the maximum turning angle of the unmanned ship itself. Rotation angle formula enables unmanned ship to rotate by maximum rotation angle theta_maxSo as to achieve the effect of getting rid of local minimum points and avoiding obstacles. Then entering step 6;

step 6:

in order to avoid the path abrupt change caused by the overlarge angle theta conversion, an angular acceleration formula is constructed:

in the formula: theta_curr(t) represents the angle of rotation of the current unmanned ship, θ_curr(t- Δ t) represents a turning angle of the unmanned ship at a previous time, Δ t represents a sampling interval,

represents the maximum angular acceleration of the unmanned ship when theta_curr(t) is less than theta, theta is updated using equation (4)_curr(t) simultaneously calculating the next motion of the unmanned ship until theta_curr(t) is greater than θ. The angular acceleration formula limits the change in the angle of rotation of the drone at each step to within a maximum angular acceleration. Then entering step 7;

and 7:

judging whether the unmanned ship leaves a steering area, if so, entering a step 2, otherwise, entering a step 5;

and 8:

constructing a resultant force formula:

F_s(x)＝F_o(x)+F_g(x) (5)

in the formula: f_s(x) Is the resultant force.

Constructing a resultant force unit vector formula:

in the formula: theta_s(t) is the resultant force unit vector, | F_s(x) L is the resultant force F_s(x) Die length of (2). Resultant force unit vector theta is solved by a resultant force unit vector formula_s(t)，θ_sAnd (t) is the next movement direction of the unmanned ship. Then entering step 9;

and step 9:

judging whether the absolute value of the difference value between the next motion direction of the unmanned ship and the current direction of the unmanned ship is greater than the maximum angular acceleration or not, if so, entering a step 11, and otherwise, entering a step 10;

step 10:

calculating the next step of movement of the unmanned ship, and then entering the step 2;

step 11:

and (4) calculating the corner angle of the unmanned ship according to the formula (4), calculating the next step movement of the unmanned ship, and then entering the step 9.

The invention has the following beneficial effects:

1. the method disclosed by the invention escapes from local minimum points on the basis of not changing the traditional potential field function, and is simple, small in calculated amount and good in real-time performance;

2. compared with an improved method provided by a paper 'obstacle avoidance research of a mobile robot based on an improved artificial potential field method', the method has the following advantages:

(1) considering the limitation of the maximum rotation angle and the angular acceleration of the automobile;

(2) the planned path for avoiding the local minimum point is short and smooth;

(3) when the barrier is avoided, the unmanned ship is far away from the barrier, and the unmanned ship is safer.

3. Compared with an improved method provided by a paper unmanned ship path planning algorithm based on an improved artificial potential field method, the method has the following advantages:

(1) when the paper gets rid of local minimum points, an angle of 0-90 degrees is randomly selected for steering, and the turning angle is suddenly changed, but the invention designs a turning angle formula and an angular acceleration formula to ensure that the turning angle is smoothly changed;

(2) the thesis judges that the situation that the unmanned ship sinks into the local minimum point is that three times of oscillation occurs on the path after the unmanned ship sinks into the local minimum point, and the invention turns in advance when the attraction force and the repulsion force are collinear and the directions are opposite, so that the efficiency of getting rid of the local minimum point is high.

Drawings

FIG. 1 is a flow chart of an unmanned ship path planning method based on an improved artificial potential field method;

FIG. 2 is a circular equivalent model of an obstacle;

FIG. 3 is a stress analysis diagram for unmanned ship path planning;

FIG. 4 is a diagram of the maximum turning angle of the unmanned ship;

FIG. 5 is a force analysis diagram of the unmanned ship when the attractive force and the repulsive force are collinear;

FIG. 6 is a unmanned ship path planning diagram;

FIG. 7 is an angle change diagram of unmanned ship path planning;

fig. 8 is a diagram of the angular acceleration change of the unmanned ship path planning.

Detailed Description

FIG. 1 is a flow chart of unmanned ship path planning based on an improved artificial potential field method, which comprises the following steps:

step 1:

establishing a circular equivalent model of the obstacles in the map, expanding the obstacles to reserve a turning area, and expanding the obstacles according to the edges of the obstacles as shown in figure 2, wherein x in the figure_obsIs the position of an obstacle, d_actThe length of the radius after the expansion treatment of the obstacle, the part enclosed by the solid line circle in the figure, is the result after the expansion of the obstacle, d₀To constructAnd establishing the influence radius of the function of the repulsion force of the obstacle, wherein the part enclosed by a dotted line circle in the figure is the influence range of the repulsion force of the obstacle, namely the unmanned ship can be subjected to the repulsion force in the range. In the figure, the ring part between the dotted line circle and the solid line circle is defined as a steering area, and if the attraction force and the repulsion force applied to the unmanned ship are collinear and the directions are opposite, the unmanned ship moves forwards again, and a local minimum point can be trapped, so that the unmanned ship enters the steering area to steer. Setting relevant parameters, and then entering the step 2;

step 2:

judging whether the unmanned ship reaches a target point, if so, ending, otherwise, entering the step 3;

and step 3:

constructing a gravity function:

F_g(x)＝k·d(x,x_goal) (1)

in the formula: f_g(x) Is a gravitation function, k is a gravitation coefficient, x is the current position of the unmanned ship, x_goalAs target point position, d (x, x)_goal) The model length is a vector, the model length is the distance between the current position of the unmanned ship and a target point, and the direction is that the unmanned ship points to the target point. Gravitation function F_g(x) The gravity is generated on the unmanned ship, and the unmanned ship goes to the target point under the action of the gravity.

Constructing a repulsion function:

in the formula: f_o(x) Is a function of repulsion, n is the coefficient of repulsion, x_obsIs the position of an obstacle, d₀Is the radius of influence of the obstacle, d (x, x)_obs) Is a vector, the module length is the distance between the current position of the unmanned ship and the obstacle, the direction is the direction of the obstacle pointing to the unmanned ship, | d (x, x)_obs) L is d (x, x)_obs) Die length of (2). When the unmanned ship influences the radius d on the obstacle₀Inner, repulsive force function F_o(x) And a repulsive force is generated to make the obstacle far away. Then entering step 4;

and 4, step 4:

judging the gravitation F_g(x) And repulsive force F_o(x) Whether collinear and opposite, if, as shown in fig. 5, the unmanned ship is subjected to attractive force F_g(x) And repulsive force F_o(x) Collinear and opposite in direction, enter step 5 at this moment, otherwise enter step 8;

and 5:

at the moment, in order to get rid of local minimum points and avoid obstacles, the unmanned ship is in a steering area, and a corner formula is constructed:

in the formula: theta is the turning angle of the unmanned ship in the turning area, d_actIs the actual radius, θ, of the obstacle after it has been inflated_maxIndicating the maximum rotation angle of the unmanned ship itself, the unmanned ship can be rotated to the left or right by the maximum rotation angle theta as shown in fig. 4_max. Rotation angle formula enables unmanned ship to rotate by maximum rotation angle theta_maxSo as to achieve the effect of getting rid of local minimum points and avoiding obstacles. Then entering step 6;

step 6:

in order to avoid the path abrupt change caused by the overlarge angle theta conversion, an angular acceleration formula is constructed:

in the formula: theta_curr(t) represents the angle of rotation of the current unmanned ship, θ_curr(t- Δ t) represents a turning angle of the unmanned ship at a previous time, Δ t represents a sampling interval,

represents the maximum angular acceleration of the unmanned ship when theta_curr(t) is less than theta, theta is updated using equation (4)_curr(t) simultaneously calculating the next motion of the unmanned ship until theta_curr(t) is greater than θ. The angular acceleration formula limits the change in the angle of rotation of the drone at each step to within a maximum angular acceleration. Then entering step 7;

and 7:

judging whether the unmanned ship leaves a steering area, if so, entering a step 2, otherwise, entering a step 5;

and 8:

constructing a resultant force formula:

F_s(x)＝F_o(x)+F_g(x) (5)

in the formula: f_s(x) Is the resultant force.

Constructing a resultant force unit vector formula:

in the formula: theta_s(t) is the resultant force unit vector, | F_s(x) L is the resultant force F_s(x) Die length of (2). Resultant force unit vector theta is solved by a resultant force unit vector formula_s(t)，θ_sAnd (t) is the next movement direction of the unmanned ship. As shown in FIG. 3, x is the current position of the unmanned ship, x_obsIs the position of the obstacle, x_goalAs target point position, F_g(x) The unmanned ship is subjected to the attraction of the target point, F_o(x) Repulsion of obstacles to unmanned ships, F_s(x) For unmanned ship under the gravitation F_g(x) And repulsive force F_o(x) Resultant force under action, resultant force F_s(x) Unit vector theta_sAnd (t) is the next movement direction of the unmanned ship. Then entering step 9;

and step 9:

judging whether the absolute value of the difference value between the next motion direction of the unmanned ship and the current direction of the unmanned ship is greater than the maximum angular acceleration or not, if so, entering a step 11, and otherwise, entering a step 10;

step 10:

calculating the next step of movement of the unmanned ship, and then entering the step 2;

step 11:

and (4) calculating the corner angle of the unmanned ship according to the formula (4), calculating the next step movement of the unmanned ship, and then entering the step 9.

Simulating the improved artificial potential field method by utilizing Matlab according to the flow chart to obtain a graph 67, results shown in FIG. 8. FIG. 6 is a planning diagram of unmanned ship path, setting the length of the map to be 5km and the width to be 5km, the coordinates x of the departure point of the unmanned ship to be (3, 0), and the coordinates x of the target point_goalIs (3, 5), the attraction coefficient k is 9, and the coordinate x of the obstacle_obsIs (3, 2.5), the obstacle expands to a radius d_obs0.48km, obstacle radius of influence d₀0.8km, a repulsion coefficient n of 0.3, and a maximum turning angle theta of the unmanned ship_max0.550rad, maximum angular acceleration

Is 0.088rad/s²The planned path is short and smooth; FIG. 7 shows the unmanned ship path planning at the maximum rotation angle θ_maxThe angle change graph is 0.550rad, the maximum corner can be rotated after the unmanned ship enters a steering area in order to avoid a formula (4) designed by the obstacle, and the angle does not change suddenly due to the limitation of angular acceleration, so that the corner change of the planned path is smooth and the fluctuation is small; FIG. 8 illustrates unmanned ship path planning at maximum angular acceleration

Is 0.088rad/s²At maximum angular acceleration

Is 0.088rad/s²Under the limit of (2), the angular acceleration change is-0.088 rad/s²～0.088rad/s²Without exceeding this range.

Claims (1)

1. The invention designs an unmanned ship path planning method based on an improved artificial potential field method, which solves the problems of idealized obstacle modeling, easy falling into local minimum points, severe corner change and the like of the traditional artificial potential field method applied to unmanned ship path planning, and is characterized in that:

step 1:

establishing a circular equivalent model of the obstacles in the map, performing expansion treatment on the obstacles, reserving a turning area, setting relevant parameters, and then entering step 2;

step 2:

judging whether the unmanned ship reaches a target point, if so, ending, otherwise, entering the step 3;

and step 3:

constructing a gravity function:

F_g(x)＝k·d(x,x_goal) (1)

in the formula: f_g(x) Is a gravitation function, k is a gravitation coefficient, x is the current position of the unmanned ship, x_goalAs target point position, d (x, x)_goal) Is a vector, the modular length is the distance between the current position of the unmanned ship and the target point, the direction is that the unmanned ship points to the target point, and the gravitation function F_g(x) The gravity is generated on the unmanned ship, and the unmanned ship goes to a target point under the action of the gravity;

constructing a repulsion function:

in the formula: f_o(x) Is a function of repulsion, n is the coefficient of repulsion, x_obsIs the position of an obstacle, d₀Is the radius of influence of the obstacle, d (x, x)_obs) Is a vector, the module length is the distance between the current position of the unmanned ship and the obstacle, the direction is the direction of the obstacle pointing to the unmanned ship, | d (x, x)_obs) L is d (x, x)_obs) The length of the model (d) when the unmanned ship affects the radius of the obstacle₀Inner, repulsive force function F_o(x) Generating repulsive force to the barrier, and enabling the barrier to be far away from the barrier, and then entering the step 4;

and 4, step 4:

judging whether the attractive force and the repulsive force are collinear and have opposite directions, if so, entering a step 5, and otherwise, entering a step 8;

and 5:

at the moment, in order to get rid of local minimum points and avoid obstacles, the unmanned ship is in a steering area, and a corner formula is constructed:

in the formula: theta is the turning angle of the unmanned ship in the turning area, d_actIs the actual radius, θ, of the obstacle after it has been inflated_maxThe maximum rotation angle of the unmanned ship is shown, and the unmanned ship is rotated by a rotation angle formula to the maximum rotation angle theta_maxThe effect of getting rid of local minimum points and avoiding obstacles is achieved, and then the step 6 is carried out;

step 6:

in order to avoid the path abrupt change caused by the overlarge angle theta conversion, an angular acceleration formula is constructed:

in the formula: theta_curr(t) represents the angle of rotation of the current unmanned ship, θ_curr(t- Δ t) represents a turning angle of the unmanned ship at a previous time, Δ t represents a sampling interval,

represents the maximum angular acceleration of the unmanned ship when theta_curr(t) is less than theta, theta is updated using equation (4)_curr(t) simultaneously calculating the next motion of the unmanned ship until theta_curr(t) if the angular acceleration formula is larger than theta, limiting the change of the rotation angle of each step of the unmanned ship to be within the maximum angular acceleration, and then entering the step 7;

and 7:

judging whether the unmanned ship leaves a steering area, if so, entering a step 2, otherwise, entering a step 5;

and 8:

constructing a resultant force formula:

F_s(x)＝F_o(x)+F_g(x) (5)

in the formula: f_s(x) For the resultant force, a resultant force unit vector formula is constructed:

in the formula: theta_s(t) is the resultant force unit vector, | F_s(x) L is the resultant force F_s(x) The resultant unit vector theta is obtained by the formula of the modulus length and the resultant unit vector_s(t)，θ_s(t) the next movement direction of the unmanned ship, and then the step 9 is carried out;

and step 9:

judging whether the absolute value of the difference value between the next motion direction of the unmanned ship and the current direction of the unmanned ship is greater than the maximum angular acceleration or not, if so, entering a step 11, and otherwise, entering a step 10;

step 10:

calculating the next step of movement of the unmanned ship, and then entering the step 2;

step 11:

and (4) calculating the corner angle of the unmanned ship according to the formula (4), calculating the next step movement of the unmanned ship, and then entering the step 9.

Images