A path planning method for unmanned watercraft

Abstract

The invention discloses a method for path planning of an unmanned ship on water, and relates to the field of path planning. The method for path planning of an unmanned ship on water comprises the following steps: constructing a mathematical model of the motion of an unmanned ship on water surface; analyzing the optimization target of the unmanned ship on water surface in path planning; obtaining the dynamic and kinematic models of the unmanned ship according to the established mathematical model, and describing the motion law of the ship; studying the autonomous obstacle avoidance strategy of the local area of ​​the unmanned ship on water surface; studying the optimal control of the global path planning of the unmanned ship on water surface; global path planning refers to the path planning made after fully understanding the surrounding relevant environment. The global planning needs to master all the environmental information in the motion space, and arrange the path planning according to this information. Study a variety of global path planning methods, seek the optimal control algorithm, and finally perform simulation verification on the above obstacle avoidance strategy and path planning algorithm.

Description

Technical Field

[0001] The present invention relates to the field of path planning, and in particular to a method for path planning of an unmanned watercraft. Background Art

[0002] In the research on how unmanned surface ships can safely and effectively travel in complex and changeable sea environments, avoid potential risks by effective means, and reach the destination by the optimal path, the main research is on the technical routes of local autonomous obstacle avoidance and global path planning. In global path planning, the environmental information is completely known, including the location and shape of obstacles. Based on this, an environmental information model is established, and then the relevant path search algorithm is used to find the optimal solution. Global planning is based on known information in the environment, while local planning is real-time dynamic planning based on changes in the external environment. Local planning is the process of planning the path from the starting point or sub-target point to the next sub-target point after the research object perceives the current position and changes in the external environment through sensors and other equipment.

[0003] Current research mainly includes the following methods:

[0004] ① The Visibility Graph algorithm was proposed and first used by Michael A. Wesley in 1979. This algorithm connects the starting point with the vertices of all obstacles and the end point with a combination of straight lines, while ensuring that each connecting line cannot pass through obstacles, and then searches for the optimal path from the starting point to the end point. The advantage of this algorithm is that the algorithm itself is relatively simple, but the search time is long and when there are many obstacles, a combinatorial explosion is prone to occur.

[0005] ② The free space method was proposed by Professor Lozano-Perez of MIT in the 1980s. Its full name is the free space method based on C space. The algorithm divides the environment space into two parts: free space and obstacle space, and then uses a search algorithm to search for a path in the divided free space. The biggest feature of the free space method is that it is relatively flexible, but its complexity also increases with the increase in the number of obstacles.

[0006] ③ The unit decomposition method was proposed by Brooks. This algorithm divides the space into submodules, and regards each submodule as a unit, connecting each adjacent unit to form a connected graph, thus simplifying the path search to a search for the graph.

[0007] ④ The topological method is to divide the space to be planned into subspaces with topological characteristics. Since the spaces are connected to each other, a topological network can be established on this basis. The established topological network is used to search for paths, and the geometric path is solved based on the searched topological path. The main idea of ​​the topological method is dimensionality reduction. The advantage of this method is that it greatly reduces the search space, and only the number of obstacles determines the complexity of the algorithm. In addition, since this method does not require the exact position of the robot, it has better robustness. However, there are still shortcomings. It is more complicated to establish a topological network, especially when the number of obstacles increases, it will be difficult to correct the existing topological network.

[0008] ⑤ In the 1990s, Dr. Eberhart and Dr. Kennedy were inspired by the predatory behavior of birds in nature and proposed the particle swarm algorithm (PSO). The particle swarm algorithm is a type of swarm intelligence algorithm and can be regarded as a modern optimization algorithm. When birds are foraging, they do not know their own position or the distance between the food and themselves. At this time, the most effective strategy is to get close to the position of the bird closest to the food, so that the chance of finding food will be greatly increased. This mechanism is a sharing mechanism. In the continuous cognition, individuals take into account their own beliefs and those of other individuals. When they perceive that the beliefs of the surrounding individuals are better, they will make adaptive adjustments to achieve population optimization. The particle swarm algorithm has both cooperation and competition between individuals, and then seeks the optimal solution in a complex space.

[0009] ⑥Genetic algorithm is a bionic algorithm that simulates Darwin's theory of biological evolution. It uses the computational model of natural selection and genetics in the theory of evolution. It is a method of searching for the optimal solution by simulating the natural evolution process. Genetic algorithm includes the problems of chromosome encoding, population initialization, fitness value evaluation, population selection, population mating and mutation. When it is applied to mobile robot path planning, it adopts uncertain search rules and adaptively adjusts the search direction. The main feature of genetic algorithm is that it directly operates on structural objects, there is no limitation of derivative and function continuity, it has inherent implicit parallelism and better global optimization ability; it adopts probabilistic optimization method, can automatically obtain and guide the optimized search space, adaptively adjust the search direction, and does not require certain rules.

[0010] ⑦ The simulated annealing algorithm is derived from the process of physical annealing. The algorithm was proposed by Kirkpatrick in 1983. The basic idea of ​​the simulated annealing algorithm is to control the temperature to keep decreasing by artificially setting the initial temperature, initial state and cooling rate of the system based on the similarity between the solution process of random optimization problems and the thermal equilibrium problem in statistical mechanics, and to make full use of the neighborhood range of the understanding space in combination with the jump characteristics for random search. The advantage of the simulated annealing algorithm in local path planning is that it can avoid the occurrence of local extreme values, but it has the defect of harsh convergence conditions, which makes it impossible to meet in practical applications. Under the condition of limited computational complexity, the convergence of the simulated annealing algorithm depends on its own parameters. Based on this situation, how to set the parameters becomes an important link in the application of the algorithm.

[0011] ⑧ Fuzzy logic algorithm avoids the trouble of modeling it because it simulates the inherent way of thinking of human beings. The algorithm does not require high accuracy of sensors, and is not sensitive to the uncertainty of the surrounding environment of the object being studied, so that the object being studied can show good continuity, consistency and stability. However, because both fuzzy membership and control rules need to be set, it is more difficult to summarize the rules. In addition, once the control rules are established, it is difficult to adjust them online, and the algorithm cannot meet the changes in the situation. Therefore, how to obtain the membership function, how to reasonably formulate control rules, and how to adjust the control rules are major problems that fuzzy logic algorithms have to face. In the face of these problems, the idea of ​​combining fuzzy logic algorithms with other algorithms can be used, and the controller can also be adjusted to enable the algorithm to have learning ability and autonomous adaptability.

[0012] ⑨ The artificial potential field APF (Artificial Potential Field) algorithm was proposed by Khatib in 1986 and was first introduced into the robot obstacle avoidance problem. The basic idea of ​​the artificial potential field method is to regard the movement of the research object in the environmental space as the movement caused by the virtual potential field. The target point has an attractive force on the research object, while the obstacle has a repulsive force on the research object, so that the research object moves towards the target point under the joint action of a group of forces. Most of the problems exposed by the artificial potential field method are caused by the special positional relationship between the target point and the obstacle of the robot in a special motion state in relatively complex environmental information. For example, it is difficult for the robot to find a path between close obstacles, it is easy to swing back and forth when walking in a narrow passage, and it is impossible to find the target point when there are obstacles near the target point. Summary of the invention

[0013] The purpose of the present invention is to address the above-mentioned shortcomings and propose a path planning method for an unmanned watercraft using an improved artificial potential field and fuzzy logic algorithm for local autonomous obstacle avoidance and an optimized particle swarm algorithm for global path planning.

[0014] The present invention specifically adopts the following technical solutions:

[0015] A method for planning a path for an unmanned watercraft comprises the following steps:

[0016] (1) Construct a mathematical model of the motion of the surface unmanned ship; the motion model of the surface unmanned ship includes the ground coordinate system η = [x, y, ψ] T and motion coordinate system v = [u,v,r] T , establish a corresponding relationship between the two coordinate systems, and through coordinate transformation, obtain the relationship between the two coordinate systems of the surface unmanned ship on the horizontal plane as shown in equations (1) and (2):

[0017] η=R(ψ)ν (1)

[0018]

[0019] Where x, y are the position coordinates of the surface unmanned ship; u is the longitudinal velocity, v is the transverse velocity, and r is the bow angular velocity; R(4) is the rotation matrix from the earth coordinate system to the motion coordinate system; ψ is the bow angle, ranging from 4∈(-π,m). The motion model of the surface unmanned ship is shown in formula (3):

[0020] M(ν)ν+C(ν)ν+D(ν)ν=τ (3)

[0021] The expressions of each term in formula (3) are as follows:

[0022] τ=[τ u 0 τ r ] T (4)

[0023]

[0024]

[0025]

[0026] C 23 =-C 32 =m 11 u (8)

[0027]

[0028] Where τ is the control force, R is the bow roll rotation matrix, M is the inertia coefficient matrix, C is the Coriolis force and centripetal force matrix, and D is the damping coefficient matrix.

[0029] (2) Analysis of optimization goals of surface unmanned vessels in path planning

[0030] According to the established mathematical model of the motion of the surface unmanned ship, the dynamic and kinematic models of the unmanned ship are obtained to describe the motion law of the ship. The control model for path tracking using the improved LOS method is shown in equations (10)-(13):

[0031]

[0032]

[0033]

[0034]

[0035] Among them, x c ,y c , ψ c is the tracking error, x sf ,y sf ,ψ sf Represents the target point position information, s = ν r ,ν t and ν r It is the decomposition of the vertical and tangential velocity of the target path, K(s) is the curvature of the reference point, and we have:

[0036]

[0037]

[0038] ψ″ ff =ψ″ B +χ″ (16)

[0039]

[0040]

[0041]

[0042] Where ψ B It is the bow direction of the ship;

[0043] Unmanned surface ship is sailing c ,y c , ψ c The three tracking errors are controlled to control the speed so that the speed approaches the expected speed u. Therefore, the goal of the path tracking control of the surface unmanned ship is to make the error Pc minimum, as shown in formula (20);

[0044] P c =(x c ,y c ,ψ c ,ν c ) T →0 (20)

[0045] In order to make the planned sea sweeping path easy to track, when the surface unmanned ship maintains a stable heading and sails in a straight line, the angular velocity of the heading angle is 0, that is, formula (20) will be simplified to formula (21):

[0046] P c =(x c ,y c ,ν c ) T →0 (21)

[0047] When the unmanned surface ship maintains a uniform straight line navigation, the speed V remains unchanged, which can be further simplified to formula (22):

[0048] P c =(x c ,y c ) T →0 (22);

[0049] (3) Research on local area autonomous obstacle avoidance strategy for unmanned surface vessels

[0050] Based on the classical repulsive potential field function, the improved repulsive potential field function will reduce the repulsive force as the robot approaches the target point, as shown in formula (23):

[0051]

[0052] Among them, pp goal is a vector representing the Euclidean distance between the robot and the target point. The direction is on the line connecting the robot and the target point, and the robot points to the target point. Compared with the repulsive potential field function in the classical potential function, this formula adds pp goal Multiplier, to ensure that the potential field value of the robot is the global minimum when it reaches the target point;

[0053] (4) Research on optimal control of global path planning for unmanned surface vessels

[0054] A hybrid particle swarm algorithm based on simulated annealing is used to perform optimal control of global path planning for surface unmanned ships. The hybrid particle swarm algorithm based on simulated annealing is an operation that adds simulated annealing algorithm to the basic particle swarm algorithm. The factors added to the particle swarm algorithm mainly include initial temperature, cooling method and the ability to accept poor solutions with a certain probability.

[0055] (5) Simulation verification test.

[0056] Preferably, in the study of the local area autonomous obstacle avoidance strategy of the surface unmanned ship, a dynamic gravitational potential field model and a dynamic repulsive potential field model are used;

[0057] When the target point is moving, the robot needs to avoid obstacles while tracking them. The gravitational potential field model of the target point is established as shown in Equation (24). The model consists of two parts: the potential field function part based on relative position potential and the potential field function part based on relative velocity:

[0058]

[0059] Where v represents the current speed of the robot, v goal Indicates the current velocity of the target point. Both are vectors. vv goal is the relative speed between the robot and the target point, by adjusting k attp and k attv The size of k adjusts the ratio of relative position and relative velocity in the total gravitational potential field. attv When it takes the value of zero, it degenerates into the traditional gravitational field function;

[0060] The repulsive potential field function introduces the relative velocity term, as shown in equation (26). The repulsive potential field function of the relative position is integrated, and the dynamic repulsive potential field model is selected as shown in equation (27);

[0061]

[0062]

[0063]

[0064] is the repulsive potential field between the robot and the obstacle based on the relative position. is the repulsive potential field between the robot and the obstacle based on the relative velocity, k repp and k repv is the adjustable constant of the repulsive field, pp obsIt represents the Euclidean distance between the robot and the obstacle point. It is a vector whose direction is from the line connecting the robot and the obstacle to the current position of the robot. e is a unit vector whose direction is from the robot to the obstacle. V is the component of the relative velocity between the robot and the obstacle. Its direction is on the line connecting the robot and the obstacle. When V≤0, the obstacle moves away from the robot. When V>0, the obstacle moves toward the robot.

[0065] Preferably, in the study of global path planning optimal control of unmanned surface ships, the hybrid particle swarm algorithm based on simulated annealing has the following characteristics:

[0066] 1) Add “initial temperature” t0, t0 calculation formula is as follows (28):

[0067] t0=Fit(p g ) / ln 5 (28)

[0068] Among them, fit(x) represents the fitness function, p g is the global extreme value;

[0069] 2) Increase the "annealing coefficient" λ to control the cooling method. The cooling method is as shown in formula (29):

[0070] t k+1 =λt k (29)

[0071] 3) The probability calculation formula for the particle being selected as the global optimal particle at the current temperature is added, as shown in (30):

[0072]

[0073] Among them, t represents the current temperature, and N represents the number of particles in the particle group;

[0074] 4) Imitating the sudden jump characteristics of the simulated annealing algorithm, select a p from all individuals i As the global optimal p' g Instead of the true global optimal p g , that is, to select the worse solution as the global optimal value with a certain probability, p' g The selection of is carried out by the roulette method, where the probability of each individual being selected as the global optimal is calculated by formula (30);

[0075] 5) Update the velocity and position of each particle according to equations (31) to (33):

[0076] ν i,j (t+1)=φ{ν i,j (t)+c1r1[p i,j -x i,j (t)]+c2r2[pg,j -x i,j (t)]} (31)

[0077] x i,j (t+1)=x i,j (t)+ν i,j (t+1),j=1,2,...,d (32)

[0078]

[0079] Among them, φ represents the compression factor, and d represents the dimension of the solution corresponding to the particle.

[0080] The present invention has the following beneficial effects:

[0081] This path planning method uses the sudden jump characteristics of the simulated annealing algorithm to solve the problem. The sudden jump characteristics of the simulated annealing algorithm can jump out of the local optimal solution when the particle swarm algorithm falls into the local optimal solution, thereby enhancing its global search ability. At the same time, the rules of the simulated annealing algorithm and the particle swarm algorithm are relatively simple and easy to implement, and the algorithm efficiency of both is relatively high. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 This is a schematic diagram of the local autonomous obstacle avoidance technology route analysis;

[0083] Figure 2 Provides a schematic diagram of the technical route analysis for global path planning;

[0084] Figure 3 This is the force analysis diagram for the target unreachable problem;

[0085] Figure 4 This is a schematic diagram of a simulation example of the target unreachable problem;

[0086] Figure 5 This is the schematic diagram of step 43 of dynamic obstacle avoidance simulation example 2;

[0087] Figure 6 This is the schematic diagram of step 103 of dynamic obstacle avoidance simulation example 2;

[0088] Figure 7 It is the particle swarm algorithm process based on simulated annealing;

[0089] Figure 8 This is a schematic diagram of the general particle swarm algorithm planning;

[0090] Fig. 9 To improve the planning diagram of ordinary particle swarm algorithm;

[0091] Fig.10 It is a schematic diagram of the number of iterations of the common particle swarm algorithm;

[0092] Fig.11 Schematic diagram of improving the number of iterations of ordinary particle swarm algorithm;

[0093] Fig.12 Planning for ordinary particle swarm algorithm;

[0094] Fig.13 To improve the planning of ordinary particle swarm algorithm;

[0095] Fig.14 is the number of iterations of the ordinary particle swarm algorithm;

[0096] Fig.15 To improve the number of iterations of ordinary particle swarm algorithm. DETAILED DESCRIPTION

[0097] The specific implementation of the present invention is further described below in conjunction with the accompanying drawings and specific embodiments:

[0098] Combination Figure 1-7 , a method for planning a path for an unmanned watercraft, comprising the following steps:

[0099] (1) Construct a mathematical model of the motion of the surface unmanned ship (the mathematical model includes a dynamic model and a motion model); the motion model of the surface unmanned ship includes the ground coordinate system η = [x, y, ψ] T and motion coordinate system v = [u,v,r] T , establish a corresponding relationship between the two coordinate systems, and through coordinate transformation, obtain the relationship between the two coordinate systems of the surface unmanned ship on the horizontal plane as shown in equations (1) and (2):

[0100] η=R(ψ)ν (1)

[0101]

[0102] Where x, y are the position coordinates of the surface unmanned ship; u is the longitudinal velocity, v is the transverse velocity, and r is the bow angular velocity; R(4) is the rotation matrix from the earth coordinate system to the motion coordinate system; ψ is the bow angle, ranging from 4∈(-π,m). The motion model of the surface unmanned ship is shown in formula (3):

[0103] M(ν)ν+C(ν)ν+D(ν)ν=τ (3)

[0104] The expressions of each term in formula (3) are as follows:

[0105] τ=[τ u 0 τ r ] T (4)

[0106]

[0107]

[0108]

[0109] C 23 =-C 32 =m 11 u (8)

[0110]

[0111] Where τ is the control force, R is the bow roll rotation matrix, M is the inertia coefficient matrix, C is the Coriolis force and centripetal force matrix, and D is the damping coefficient matrix.

[0112] (2) Analysis of optimization goals of surface unmanned vessels in path planning

[0113] According to the established mathematical model of the motion of the surface unmanned ship, the dynamic and kinematic models of the unmanned ship are obtained to describe the motion law of the ship. In order to ensure the organic balance of the heading stability, speed stability and path length of the surface unmanned ship in the path considering energy consumption, it is necessary to simulate the environmental variables in the path planning as realistically as possible, determine the optimization target, and use the improved LOS method for path tracking The control model is shown in equations (10)-(13):

[0114]

[0115]

[0116]

[0117]

[0118] Among them, x c ,y c , ψ c is the tracking error, x sf ,y sf ,ψ sf Represents the target point position information, s = ν r ,ν t and ν r It is the decomposition of the vertical and tangential velocity of the target path, K(s) is the curvature of the reference point, and we have:

[0119]

[0120]

[0121] ψ″ ff =ψ″ B +χ″ (16)

[0122]

[0123]

[0124]

[0125] Where ψ B It is the bow direction of the ship;

[0126] Unmanned surface ship is sailing c ,y c , ψ c The three tracking errors are controlled to control the speed so that the speed approaches the expected speed u. Therefore, the goal of the path tracking control of the surface unmanned ship is to make the error P c minimum, as shown in formula (20);

[0127] P c =(x c ,y c ,ψ c ,ν c ) T →0 (20)

[0128] In order to make the planned sea sweeping path easy to track, when the surface unmanned ship maintains a stable heading and sails in a straight line, the angular velocity of the heading angle is 0, that is, formula (20) will be simplified to formula (21):

[0129] P c =(x c ,y c ,ν c ) T →0 (21)

[0130] When the unmanned surface ship maintains a uniform straight line navigation, the speed V remains unchanged, which can be further simplified to formula (22):

[0131] P c =(x c ,y c ) T →0 (22).

[0132] (3) Research on local area autonomous obstacle avoidance strategy for unmanned surface vessels

[0133] Local planning is a real-time dynamic planning based on changes in the external environment. Local planning is to plan the path from the starting point or sub-target point to the next sub-target point after the research object perceives the current location and changes in the external environment through sensors and other equipment. Obstacle avoidance is the focus of local planning research. When the surface unmanned ship is driving normally, it suddenly encounters unknown static or dynamic obstacles that interfere with the route. At this time, the unmanned ship will make adjustments to avoid danger in time.

[0134] Disadvantages of the Classical Artificial Potential Field Method

[0135] Most of the problems exposed by the artificial potential field method are caused by the special positional relationship between the target point and the obstacle in the robot's special motion state in relatively complex environmental information. For example, it is difficult for the robot to find a path between close obstacles, it is easy to swing back and forth when walking in a narrow passage, and it is impossible to find the target point when there are obstacles near the target point. The following is a study and discussion of the most prominent problems of the potential field method.

[0136] (1) Target unreachable problem

[0137] If there is an obstacle near the target point, and the robot is also within the influence range of the obstacle, the repulsive force increases as the robot approaches the obstacle, and the closer it gets, the more rapidly it increases. The gravitational force is very small compared to the repulsive force, which makes the robot unable to reach the target point in the end. This is the unreachable target problem of the artificial potential field method, also called the global minimum problem in some literature, which means that the target point is not the lowest point of the global total potential field. Figure 2 As shown, the robot and the target point are within the influence range of the obstacle, pp obs If the value decreases, The value increases, so Increase; and pp goal Reduce, thus decreases, and finally causes the robot to be repelled by obstacles and move away from the target point.

[0138] Figure 4 This is a simulation diagram of the unreachable target problem of the classical potential field method. In this simulation, four obstacles are set, as shown in the figure, of which there are two obstacles near the target point. It is obvious from the simulation diagram that when the robot is about to reach the target point, it enters the influence range of the two obstacles. The repulsive force is too large, causing the robot to circle back and forth at this point and fail to reach the target point.

[0139] (2) Local Minimum Problem

[0140] In a multi-obstacle environment, there are many possible distributions of obstacles. At a certain point before the robot reaches the target, the virtual gravitational force of the target point on the robot and the virtual repulsive force of the obstacle on the robot are exactly equal in magnitude and opposite in direction. At this time, the resultant force is zero, and the robot mistakenly thinks that it has reached the target point. It stops or wanders here, not knowing the next direction of movement, causing the robot to be unable to reach the target point. This is the local minimum problem of the artificial potential field method.

[0141] (3) Path oscillation problem

[0142] In addition, when the robot passes through a position with many obstacles, local oscillation will occur. The reason for the oscillation is that the gravitational force on the robot at this position is zero, but the speed is not zero, so the robot continues to move forward under the action of inertia and leaves the point. At the same time, it is affected by an opposite repulsive force, which reduces the robot's speed to zero, and then moves backward under the action of repulsion, and so on. It can be seen that the root cause of the oscillation also comes from the local minimum problem. During the oscillation process, the robot can sometimes get out of the local minimum by itself. The oscillation wastes a lot of time and obviously slows down the robot's speed, but in most cases it still cannot get out of the local minimum alone.

[0143] Based on the classical repulsive potential field function, the improved repulsive potential field function will reduce the repulsive force as the robot approaches the target point, as shown in formula (23):

[0144]

[0145] Among them, pp goal is a vector representing the Euclidean distance between the robot and the target point. The direction is on the line connecting the robot and the target point, and the robot points to the target point. Compared with the repulsive potential field function in the classical potential function, this formula adds pp goal Multiplier is used to ensure that the potential field value of the robot is the global minimum when it reaches the target point.

[0146] In the study of local area autonomous obstacle avoidance strategy for surface unmanned ships, dynamic gravitational potential field model and dynamic repulsive potential field model are used;

[0147] When the target point is moving, the robot needs to avoid obstacles while tracking them. The gravitational potential field model of the target point is established as shown in Equation (24). The model consists of two parts: the potential field function part based on relative position potential and the potential field function part based on relative velocity:

[0148]

[0149] Where v represents the current speed of the robot, v goal Indicates the current velocity of the target point. Both are vectors. vv goal is the relative speed between the robot and the target point, by adjusting k attp and k attv The size of k adjusts the ratio of relative position and relative velocity in the total gravitational potential field. attv When it takes the value of zero, it degenerates into the traditional gravitational field function;

[0150] The repulsive potential field function introduces the relative velocity term, as shown in equation (26). The repulsive potential field function of the relative position is integrated, and the dynamic repulsive potential field model is selected as shown in equation (27);

[0151]

[0152]

[0153]

[0154] is the repulsive potential field between the robot and the obstacle based on the relative position. is the repulsive potential field between the robot and the obstacle based on the relative velocity, k repp and k repv is the adjustable constant of the repulsive field, pp obs It represents the Euclidean distance between the robot and the obstacle point. It is a vector whose direction is from the line connecting the robot and the obstacle to the current position of the robot. e is a unit vector whose direction is from the robot to the obstacle. V is the component of the relative velocity between the robot and the obstacle. Its direction is on the line connecting the robot and the obstacle. When V≤0, the obstacle moves away from the robot. When V>0, the obstacle moves toward the robot.

[0155] (4) Research on optimal control of global path planning for unmanned surface vessels

[0156] A hybrid particle swarm algorithm based on simulated annealing is used for optimal control of global path planning of surface unmanned ships. The hybrid particle swarm algorithm based on simulated annealing is to add the operation of simulated annealing algorithm to the basic particle swarm algorithm. The factors added to the particle swarm algorithm mainly include initial temperature, cooling method and the ability to accept poor solutions with a certain probability.

[0157] In the study of optimal control of global path planning for surface unmanned ships, the hybrid particle swarm algorithm based on simulated annealing has the following characteristics:

[0158] 1) Add “initial temperature” t0, t0 calculation formula is as follows (28):

[0159] t0=Fit(p g ) / ln 5 (28)

[0160] Among them, fit(x) represents the fitness function, p g is the global extreme value;

[0161] 2) Increase the "annealing coefficient" λ to control the cooling method. The cooling method is as shown in formula (29):

[0162] t k+1 =λt k (29)

[0163] 3) The probability calculation formula for the particle being selected as the global optimal particle at the current temperature is added, as shown in (30):

[0164]

[0165] Among them, t represents the current temperature, and N represents the number of particles in the particle group;

[0166] 4) Imitating the sudden jump characteristics of the simulated annealing algorithm, select a p from all individuals i As the global optimal p' g Instead of the true global optimal p g , that is, to select the worse solution as the global optimal value with a certain probability, p' g The selection of is carried out by the roulette method, where the probability of each individual being selected as the global optimal is calculated by formula (30);

[0167] 5) Update the velocity and position of each particle according to equations (31) to (33):

[0168] ν i,j (t+1)=φ{ν i,j (t)+c1r1[p i,j -x i,j (t)]+c2r2[p g,j -x i,j (t)]} (31)

[0169] x i,j (t+1)=x i,j (t)+ν i,j (t+1),j=1,2,...,d (32)

[0170]

[0171] Among them, φ represents the compression factor, and d represents the dimension of the solution corresponding to the particle.

[0172] (5) Simulation verification test, combined with Figure 8-15 , Figure 8 In the figure, environment map example 1, under the ordinary particle swarm algorithm, the environment size is set to 20*20, and the path planning situation. The upper left corner box is the starting point, and the lower right corner star is the destination point. After the ordinary particle swarm algorithm path planning, the starting point can be reached from the destination point.

[0173] Fig. 9 In the example 1 of environment map, under the improved particle swarm algorithm, the environment size is set to 20*20, and the path planning situation. The upper left corner box is the starting point, and the lower right corner star is the destination point. After the improved particle swarm algorithm path planning, the starting point can be reached from the destination point, and the path planned by the improved particle swarm algorithm is shorter than that of the ordinary particle swarm algorithm.

[0174] Fig.10 and Fig.11 are the number of path planning iterations in environment map example 1 under the common particle swarm algorithm and the improved particle swarm algorithm. Fig.10 and Fig.11 As shown in the figure, the ordinary particle swarm algorithm ( Fig.10 ) and improved particle swarm optimization ( Fig.11 ) reached their local optimal solutions after 5 iterations. After that, the improved particle swarm algorithm found a better solution than before after 7 iterations and stabilized at the optimal solution until the end of the search; the ordinary particle swarm algorithm reached a better solution after 54 iterations until the end of the search. It can be seen that in terms of convergence speed, according to the experimental results, it is obvious that the improved particle swarm algorithm is faster than the ordinary particle swarm algorithm; and in terms of planning path length, the path length of the improved particle swarm algorithm is also smaller than that of the ordinary particle swarm algorithm.

[0175] Fig.12 This is the environment map example 2. Under the ordinary particle swarm algorithm, the environment size is set to 20*20, and the path planning situation. The upper left corner box is the starting point, and the lower right corner star is the destination point. After the ordinary particle swarm algorithm path planning, the path from the starting point to the destination is not planned, but it falls into the local optimal value and fails to reach the end point.

[0176] Fig.13 This is the environment map example 2. Under the improved particle swarm algorithm, the environment size is set to 20*20, and the path planning situation. The upper left corner box is the starting point, and the lower right corner star is the destination point. After the improved particle swarm algorithm path planning, it is possible to reach the destination point from the starting point, and the Fig.12 The local optimum in .

[0177] Fig.14 and Fig.15 are the number of path planning iterations in environment map example 2 under the common particle swarm algorithm and the improved particle swarm algorithm. Fig.14 and Fig.15 As shown in the figure, the ordinary particle swarm algorithm ( Fig.14 ) and improved particle swarm optimization ( Fig.15 ) reached their respective local optimal solutions after 3 iterations. After that, the improved particle swarm algorithm found a better solution than before after 7 iterations and stabilized at the optimal solution until the end of the search; the ordinary particle swarm algorithm reached the local optimal position after 10 iterations, but could not "break free" from it to continue searching, so that the final search result was stuck near the local value of 7100. It can be seen that the improved particle swarm algorithm can overcome the inherent deficiencies of the ordinary particle swarm algorithm, break through the local optimal value barrier, and make the planning result finally reach the global optimal solution.

[0178] The MATLAB simulation software is used to simulate and verify the above obstacle avoidance strategy and path planning algorithm.

[0179] Of course, the above description is not a limitation of the present invention, and the present invention is not limited to the above examples. Changes, modifications, additions or substitutions made by technicians in this technical field within the essential scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A method for planning a path for an unmanned watercraft, characterized in that: The following steps are involved: (1) Construct a mathematical model of the motion of the surface unmanned ship; the motion model of the surface unmanned ship includes the ground coordinate system η = [x, y, ψ] T and motion coordinate system v = [u,v,r] T , establish a corresponding relationship between the two coordinate systems, and through coordinate transformation, obtain the relationship between the two coordinate systems of the surface unmanned ship on the horizontal plane as shown in equations (1) and (2): η=R(ψ)ν (1) Among them, x, y are the position coordinates of the surface unmanned ship; u is the longitudinal velocity, v is the transverse velocity, and r is the bow angular velocity; R(ψ) is the rotation matrix from the earth coordinate system to the motion coordinate system; ψ is the bow angle, ranging from ψ∈(-π,π). The motion model of the surface unmanned ship is shown in formula (3): M(ν)ν+C(ν)ν+D(ν)ν=τ (3) The expressions of each term in formula (3) are as follows: τ=[τ u 0 t r ] T (4) C 23 =-C 32 =m 11 u (8) Where τ is the control force, R is the bow roll rotation matrix, M is the inertia coefficient matrix, C is the Coriolis force and centripetal force matrix, and D is the damping coefficient matrix; (2) Analysis of the optimization objectives of surface unmanned vessels in path planning According to the established mathematical model of the motion of the surface unmanned ship, the dynamic and kinematic models of the unmanned ship are obtained to describe the motion law of the ship. The control model for path tracking using the improved LOS method is shown in equations (10)-(13): Among them, x c ,y c , ψ c is the tracking error, x sf ,y sf ,ψ sf Represents the target point position information, s = ν r ,ν t and ν r is the decomposition of the vertical and tangential velocities of the target path, K(s) is the curvature of the reference point, and we have: Where ψ B It is the bow direction of the ship; Unmanned surface ship is sailing c ,y c , ψ c The three tracking errors are controlled to control the speed so that the speed approaches the expected speed u. Therefore, the goal of the path tracking control of the surface unmanned ship is to make the error P c minimum, as shown in formula (20); P c =(x c ,y c ,ψ c ,n c ) T →0 (20) In order to make the planned sea sweeping path easy to track, when the surface unmanned ship maintains a stable heading and sails in a straight line, the angular velocity of the heading angle is 0, that is, formula (20) will be simplified to formula (21): P c =(x c ,y c ,n c ) T →0 (21) When the unmanned surface ship maintains a uniform straight line navigation, the speed V remains unchanged, which can be further simplified to formula (22): P c =(x c ,y c ) T →0 (22); (3) Research on local area autonomous obstacle avoidance strategy for unmanned surface vessels Based on the classical repulsive potential field function, the improved repulsive potential field function will reduce the repulsive force as the robot approaches the target point, as shown in formula (23): Among them, pp goal is a vector representing the Euclidean distance between the robot and the target point. The direction is on the line connecting the robot and the target point, and the robot points to the target point. Compared with the repulsive potential field function in the classical potential function, this formula adds pp goal Multiplier, to ensure that the potential field value of the robot is the global minimum when it reaches the target point; (4) Research on optimal control of global path planning for unmanned surface vessels A hybrid particle swarm algorithm based on simulated annealing is used to perform optimal control of global path planning for surface unmanned ships. The hybrid particle swarm algorithm based on simulated annealing is an operation that adds simulated annealing algorithm to the basic particle swarm algorithm. The factors added to the particle swarm algorithm mainly include initial temperature, cooling method and the ability to accept poor solutions with a certain probability. (5) Simulation verification test.

2. A method for planning a path for an unmanned watercraft as claimed in claim 1, characterized in that: In the study of local area autonomous obstacle avoidance strategy for surface unmanned ships, dynamic gravitational potential field model and dynamic repulsive potential field model are used; When the target point is moving, the robot needs to avoid obstacles while tracking them. The gravitational potential field model of the target point is established as shown in Equation (24). The model consists of two parts: the potential field function part based on relative position potential and the potential field function part based on relative velocity: Where v represents the current speed of the robot, v goal Indicates the current velocity of the target point. Both are vectors. vv goal is the relative speed between the robot and the target point, by adjusting k attp and k attv The size of k adjusts the ratio of relative position and relative velocity in the total gravitational potential field. attv When it takes the value of zero, it degenerates into the traditional gravitational field function; The repulsive potential field function introduces the relative velocity term, as shown in equation (26). The repulsive potential field function of the relative position is integrated, and the dynamic repulsive potential field model is selected as shown in equation (27); is the repulsive potential field between the robot and the obstacle based on the relative position. is the repulsive potential field between the robot and the obstacle based on the relative velocity, k repp and k repv is the adjustable constant of the repulsive field, pp obs It represents the Euclidean distance between the robot and the obstacle point. It is a vector whose direction is from the line connecting the robot and the obstacle to the current position of the robot. e is a unit vector whose direction is from the robot to the obstacle. V is the component of the relative velocity between the robot and the obstacle. Its direction is on the line connecting the robot and the obstacle. When V≤0, the obstacle moves away from the robot. When V>0, the obstacle moves toward the robot.

3. A method for planning a path for an unmanned watercraft as claimed in claim 1, characterized in that: In the study of optimal control of global path planning for surface unmanned ships, the hybrid particle swarm algorithm based on simulated annealing has the following characteristics: 1) Add "initial temperature" t0, t0 calculation formula is as follows (28): t0=Fit(p g ) / ln5 (28) Among them, Fit(x) represents the fitness function, p g is the global extreme value; 2) Increase the "annealing coefficient" λ to control the cooling method. The cooling method is as shown in formula (29) t k+1 =λt k (29) 3) The probability calculation formula for the particle being selected as the global optimal particle at the current temperature is added, as shown in (30): Among them, t represents the current temperature, and N represents the number of particles in the particle group; 4) Imitating the sudden jump characteristics of the simulated annealing algorithm, select a p from all individuals i As the global optimal p' g Instead of the true global optimal p g , that is, to select the worse solution as the global optimal value with a certain probability, p' g The selection of is carried out by the roulette method, where the probability of each individual being selected as the global optimal is calculated by formula (30); 5) Update the velocity and position of each particle according to equations (31) to (33): n i,j (t+1)=φ{ν i,j (t)+c1r1[p i,j -x i,j (t)]+c2r2[p g,j -x i,j (t)]} (31) x i,j (t+1)=x i,j (t)+ν i,j (t+1),j=1,2,...,d (32) Among them, φ represents the compression factor, and d represents the dimension of the solution corresponding to the particle.

Images