USV collision avoidance method based on improved ant colony algorithm and improved artificial potential field method
By improving the combination method of ant colony algorithm and artificial potential field method, the problem of USV avoiding collisions and obstructions in unknown static environments is solved, and the real-time and effectiveness of collision avoidance are improved.
Patent Information
- Application Number
- CN202211054163.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-08-31
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2042-08-31
AI Technical Summary
The traditional USV collision avoidance planning method cannot effectively avoid collisions in a short distance under unknown static environments, and the algorithm converges slowly, affecting the safe collision avoidance time; the traditional artificial potential field method only considers the distance between the obstacle and the USV. It is impossible to tell whether the obstacles have an impact on navigation.
The USV collision avoidance method based on improved ant colony algorithm and improved artificial potential field method is adopted. By constructing coordinate systems, designing optimization windows for real-time collision avoidance planning, using the view method to build obstacle models, and selecting appropriate methods under different circumstances Avoid collisions, improve ant colony algorithm and artificial potential field method to improve real-time and effectiveness.
The real-time nature of collision avoidance is enhanced, the collision avoidance efficiency is improved, and the judgment conditions for whether obstacles have an impact on navigation are added by improving the artificial potential field method, which improves the effectiveness of collision avoidance, and makes up for the shortcomings of the traditional method.
Smart Images

Figure CN115328147B_ABST
Abstract
Description
Technical Field
[0001] The present disclosure generally relates to the field of surface unmanned vehicle collision avoidance planning methods, and specifically to a USV collision avoidance method based on an improved ant colony algorithm and an improved artificial potential field method. Background Art
[0002] As an intelligent tool at sea, USV collision avoidance planning is not only an important symbol of automation, but also the core of autonomous navigation. Therefore, the premise for USV to complete its mission is that it can avoid collisions intelligently. Traditional collision avoidance planning methods include ant colony algorithm, particle swarm algorithm, genetic optimization algorithm, artificial potential field method, deep learning method, etc. For USV intelligent collision avoidance, in addition to safety and smoothness, real-time performance is also an important evaluation indicator for system methods. Ant colony algorithm is widely used as a conventional method for collision avoidance, but ant colony algorithm cannot make effective emergency collision avoidance for obstacles that suddenly appear in a short distance in an unknown static environment. At the same time, due to the slow convergence of the algorithm itself, it cannot perform emergency collision avoidance in emergencies, affecting the best safe collision avoidance time of USV; artificial potential field method can give a better response in emergency collision avoidance, but the traditional artificial potential field method only considers the distance between the obstacle and USV, and then judges the repulsive force between the obstacle and USV, and cannot judge whether the obstacle affects navigation, so the collision avoidance method needs to be improved urgently. Summary of the invention
[0003] In view of the above defects or deficiencies in the prior art, it is desired to provide a USV collision avoidance method based on an improved artificial potential field method of an improved ant colony algorithm to solve the above problems.
[0004] The present application provides a USV collision avoidance method based on an improved artificial potential field method of an improved ant colony algorithm, comprising the following steps:
[0005] S100, constructing a coordinate system, including a global coordinate system and a local coordinate system;
[0006] The local coordinate system includes a hull coordinate system and a navigator coordinate system. The hull coordinate system is a rectangular coordinate system with the USV as the origin and the USV navigation direction as the X-axis. The navigator coordinate system is a polar coordinate system with the USV as the origin and the USV navigation direction as the polar axis.
[0007] S200, design the optimization window for real-time collision avoidance planning:
[0008] Establish an optimization window centered on the USV and navigating with the USV, the size of the optimization window is set to the detection distance of the USV navigator, the optimization window is updated at a set time, and a collision avoidance plan is performed for the USV;
[0009] S300, using visual graph method to build obstacle model:
[0010] The visibility graph method includes a visibility graph expansion method and a visibility graph construction method, wherein the expansion method uses a vector area method to determine the concavity and convexity, and the construction method uses a relative position detection method to determine the visibility;
[0011] S400, judging the distance between the obstacle and the hull and selecting a real-time collision avoidance method:
[0012] If the obstacle is less than the set distance from the hull, the improved artificial potential field method is used for real-time collision avoidance. The specific method is as follows:
[0013] S411, setting the potential field to an ellipse, and determining the shape of the ellipse potential field;
[0014] S412. Calculate the potential value v of any visible vertex (x, y) of the obstacle model p ;
[0015] S413, v p Make a judgment, if v p ≥1, the USV will follow the original route. p <1, calculate the linear velocity and angular velocity of USV at the next moment;
[0016] If the obstacle is greater than the set distance from the hull, the improved ant colony algorithm is used to avoid collision.
[0017] According to the technical solution provided in the embodiment of the present application, the shape of the elliptical potential field is determined as follows:
[0018] A plane rectangular coordinate system is established with the hull as the origin of the coordinate system. The coordinates of the four vertices of the ellipse are (0, y r )、(0,y f )、(x f ,y xf )、(x f ,-y xf ), the shape of the elliptical potential field is V(y f ,y r ,y xf ,x f ).
[0019] According to the technical solution provided in the embodiment of the present application, v p The calculation formula is as follows:
[0020] v p =(y 2 +(ay+c)x 2 ) / (py+q)
[0021] Where p = y f -y r , q = y f -yr ,
[0022] According to the technical solution provided in the embodiment of the present application, the calculation formula of the linear velocity and angular velocity of the USV at the next moment is as follows:
[0023] γ=max(1-v p ,0)
[0024]
[0025]
[0026] In the formula, ω r and v l are the angular velocity and linear velocity obtained after USV planning, ω max and ω pre are the maximum turning angular velocity and current turning angular velocity of the USV, v pre and v max are the current linear velocity and maximum linear velocity of USV respectively, and α and β are intensity factors.
[0027] According to the technical solution provided in the embodiment of the present application, the steps of using the improved ant colony algorithm to avoid collision include:
[0028] S421, using a visibility judgment method based on relative position detection to count the visible points of all the puffed environment obstacle vertices including the starting point and the end point, each vertex g i Define an allowed i The linked list stores its visible point set;
[0029] S422, initialization τ ij (0) Set the historical best generation counter count = 0, the evolution generation counter G_count = 0, the historical best maximum generation Max, the maximum evolution generation Generation, the path cost ∞ of the initial historical best ant history_best, and the population number m of ants in each generation;
[0030] S423, setting the ant population number k=1;
[0031] S424, if k>m, go to S427, otherwise place ant k at the starting position g s Go up and go to S425;
[0032] S425, let the current position of the ant be g i , if g i The visible point list allowed iIf it is empty, the ant dies, k=k+1, and go to S424;
[0033] S426, if the ant's current position is the end point g e , then the ant finds the complete path, k = k + 1, and goes to S424, otherwise, the path point is changed from allowed i , to prevent the ants from searching back, and to find the next path point according to the state transition rule, and then go to S425;
[0034] S427, evolutionary generation counter G_count=G_count+1, if the best ant in this generation is better than the path found by the best ant in history, update the initial best ant in history history_best and set the best reserved generation counter count=0, otherwise, the best reserved generation counter count=count+1;
[0035] S428. If the historical optimal retained generation counter count> the historical optimal maximum retained generation Max or the evolution generation counter G_count≥ the maximum evolution generation Generation, the optimization method stops. Otherwise, the ant colony pheromone is globally updated based on the jungle principle and the maximum-minimum ant system, and the process goes to S423.
[0036] S429, inputting the navigator detection information and the target point information into the USV static collision avoidance planner based on the improved ant colony algorithm to obtain the USV's bow turning and speed adjustment command at the next moment.
[0037] According to the technical solution provided in the embodiment of the present application, the formula of the jungle principle is as follows:
[0038]
[0039]
[0040]
[0041] Where n is the number of moments after which an ant completes a cycle, Q represents the pheromone intensity, α is the global pheromone volatility coefficient, and L ALL is the length of the path taken by the ant in this cycle, is the pheromone increment of ants on path (i, j) in this cycle, m is the population of ants, L BAD is the worst path length taken by all ants in this cycle, Δτ ij(t) is the pheromone increment of the ant on the worst path in this cycle on the path (i, j).
[0042] According to the technical solution provided in the embodiment of the present application, the formula of the maximum-minimum ant system is as follows:
[0043]
[0044] In the formula, τ max , τ min The upper and lower limits of pheromone concentration are set by yourself.
[0045] Compared with the prior art, the beneficial effects of the present application are as follows: the present application combines the improved artificial potential field method and the improved ant colony algorithm to perform collision avoidance planning for the navigation of USV in an unknown static environment. Under normal circumstances, the improved ant colony algorithm is used for conventional collision avoidance, and the improved artificial potential field method is used for collision avoidance for obstacles that suddenly appear and affect navigation. At the same time, the improved ant colony algorithm is used to make up for the shortcomings of the artificial potential field method that can only be used for emergency collision avoidance but cannot perform path planning; by selecting targeted methods for collision avoidance in different situations, the real-time nature of collision avoidance is enhanced and the collision avoidance efficiency is improved; at the same time, by improving the artificial potential field method, the potential field is set to an elliptical shape, which avoids the problem that the traditional artificial potential field method can only make collision avoidance planning based on distance, adds a judgment condition on whether the obstacle affects navigation, and improves the effectiveness of collision avoidance. BRIEF DESCRIPTION OF THE DRAWINGS
[0046] Other features, objects and advantages of the present application will become more apparent by reading the detailed description of non-limiting embodiments made with reference to the following drawings:
[0047] Figure 1 A flow chart of the USV collision avoidance method based on the improved ant colony algorithm and the improved artificial potential field method provided in this application;
[0048] Figure 2 This is a schematic diagram of the USV coordinates of this application;
[0049] Figure 3 A schematic diagram of the navigator of the present application detecting unknown obstacles;
[0050] Figure 4 This is a schematic diagram of the polygonal obstacle expansion of the present application;
[0051] Figure 5 This is a schematic diagram of the vector area method of this application;
[0052] Figure 6 This is a schematic diagram of the puffing principle of this application;
[0053] Figure 7 A schematic diagram of determining visibility based on relative position for this application;
[0054] Figure 8 Schematic diagram of the effect of obstacles on the artificial potential field method in this application;
[0055] Fig. 9 This application is based on the artificial potential field model of the azimuthal elliptical potential field;
[0056] Fig.10 A schematic diagram of the definition of the potential value parameters of the artificial potential field law is provided for this application;
[0057] Fig.11 This is a comparison chart of the USV collision avoidance planning results for this application. DETAILED DESCRIPTION
[0058] The present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the relevant invention, rather than to limit the invention. It is also necessary to explain that, for ease of description, only the parts related to the invention are shown in the accompanying drawings.
[0059] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0060] Please refer to Figure 1 The present application provides a USV collision avoidance method based on an improved ant colony algorithm and an improved artificial potential field method, comprising the following steps:
[0061] S100, constructing a coordinate system, including a global coordinate system and a local coordinate system;
[0062] The local coordinate system includes a hull coordinate system and a navigator coordinate system. The hull coordinate system is a rectangular coordinate system with the USV as the origin and the USV navigation direction as the X-axis. The navigator coordinate system is a polar coordinate system with the USV as the origin and the USV navigation direction as the polar axis.
[0063] Specifically, in a certain embodiment, the global coordinate system adopts a north-east coordinate system, with the upper left corner of the map as the origin, the east direction as the X-axis, and the north direction as the Y-axis.
[0064] Specifically, Figure 1 As shown, the global coordinates of the USV location are (x u ,y u ), α u is the USV navigation direction and the east direction X G The navigator is installed in the hull coordinate system X U O U Y U Next point (x e ,0), that is, during the navigation process of USV, the navigator coordinate system P(d i ,θ i ) relative to the hull coordinate system XU O U Y U The position of is fixed, where Q i is a static obstacle detected by the navigator and in the navigator coordinate system P(d i ,θ i ) is the coordinate (d i ,θ i ).
[0065] S200, design the optimization window for real-time collision avoidance planning:
[0066] Establish an optimization window centered on the USV and navigating with the USV, the size of the optimization window is set to the detection distance of the USV navigator, the optimization window is updated at a set time, and a collision avoidance plan is performed for the USV;
[0067] Specifically, during collision avoidance planning, only obstacles in the current scrolling window are modeled. As the environment window continues to scroll forward, dynamic collision avoidance planning can be continuously performed on the USV. The optimization window has the following characteristics:
[0068] (1) Each time planning is performed, historically known obstacles are incorporated into the planning to add “memory” to the algorithm and prevent the USV collision avoidance planning from falling into the local optimum;
[0069] (2) The planning uses an unknown and newly discovered obstacle triggering mechanism. On the one hand, it can improve the algorithm's response speed to obstacle avoidance planning, and on the other hand, it can avoid wasting resources caused by frequent triggering of rules.
[0070] S300, using visual graph method to build obstacle model:
[0071] The navigation radar detects the surrounding environment by emitting electromagnetic waves and receiving waves reflected from obstacles. That is, the intersection of the electromagnetic wave and the surface of the obstacle is the reflection point or obstacle point. According to the distance and angle from the reflection point to the navigation radar, the position information of each obstacle vertex relative to the navigation radar can be known, and the position information is converted into the position information relative to the center of mass of the USV through coordinate transformation, thereby completing the detection of the local static unknown environment in the USV optimization window, and then the visualization method can be used to model the local static unknown environment in the optimization window.
[0072] The coordinate transformation is specifically:
[0073] like Figure 2 As shown, if the known obstacle is in the navigator coordinate system X R O R Y R The lower coordinate is (d, θ), which is converted into the hull coordinate system X according to the following formula: U O UY U Lower coordinates (x',y'):
[0074]
[0075] It can also be converted to the global coordinate system X according to the following formula G O G Y G Lower coordinates (x,y):
[0076]
[0077] Similarly, if the obstacle is known to be in the global coordinate system X G O G Y G The coordinates under are (x, y), which can be converted to the navigator coordinate system X according to the following formula R O R Y R Lower coordinate (d,θ):
[0078]
[0079] The visibility graph method includes a visibility graph expansion method and a visibility graph construction method. The expansion method uses a vector area method to determine the concavity and convexity, and the construction method uses a relative position detection method to determine the visibility.
[0080] Visible expansion method:
[0081] For an elliptical obstacle, a safety distance h is set according to actual needs, and the obstacle is expanded into an ellipse with the same elliptical center and longer major and minor radii (longer major and minor radii mean that the safety distance is taken into consideration), and the Pythagorean theorem is used to obtain the circumscribed rectangle of the expanded ellipse.
[0082] For polygonal obstacles, they are generally divided into convex polygonal obstacles and concave polygonal obstacles. The general method uses the puffing method of convex polygonal obstacles to puff the concave polygonal obstacles, such as Figure 4 As shown in (a), this expansion method may lose the optimal path in collision avoidance planning. The obstacles after the expansion of the visible graph are as follows Figure 4 (b) as shown.
[0083] First, use the vector area method to determine whether the polygon vertex is convex or concave, such as Figure 5 As shown, suppose polygon ABCDEFG forms a vector in clockwise order, and its area formula is as follows:
[0084] S ABCDEFG =S ABba +S BCcb +S DEed +S CDdc +S EFfe+S GFfg +S AGga
[0085] like Figure 5 As shown, S ABCDEFG and S BCD According to the vector directions, the area formula of polygon ABba is as follows:
[0086]
[0087] If the polygon vertex is a concave point, then S ABCDEFG ×S CDE <0;
[0088] If the polygon vertex is a convex point, then S ABCDEFG ×S BCD >0.
[0089] Secondly, design the expansion method according to the vertex concavity, such as Figure 6 As shown in (a), A is a convex point. According to its adjacent points F and B, the angle α between its angle bisector and the positive direction of the X-axis can be calculated as follows:
[0090]
[0091] The point obtained by expansion is the intersection of the reverse extension of the angle bisector of ∠BAF and the parallel line outside the polygon ABCDE and at a distance h from AB (or AF) as shown below:
[0092]
[0093]
[0094] Similarly, if Figure 6 As shown in (b), F is a concave point, and the point obtained by expansion is the intersection of the ∠AFE angle bisector outside the polygon ABCDE and the parallel line AF (or FE) at a distance h.
[0095] How to construct a visual graph:
[0096] The method based on relative position detection first calculates the distance d1, d2, d3, d4, d5 between P1 and each vertex of the obstacle, selects the two vertices closest to P1, and determines whether the boundary lines of the obstacle with the two vertices closest to P1 as endpoints intersect, such as Figure 7 As shown, it is to determine whether P1P2 intersects with AE, AB, and BC. Obviously, if they intersect, the road P1P2 collides with the obstacle, and if they do not intersect, the opposite is true. The problem of whether the USV route P1P2 collides with the obstacle is transformed into the problem of determining whether P1P2 intersects with AE, AB, and BC.
[0097] According to the distance formula from point (x, y) to a straight line, the distances from point A and B to P1 and P2 are calculated as d. a and d b , the distance between points P1 and P2 and AB is d p1 and d p2 , where the distance is signed, the formula is as follows:
[0098]
[0099] like Figure 7 As shown in , it is stipulated that if the point is above P1P2, the distance is less than 0, otherwise the distance is greater than 0. Therefore, when d a ×d b <0 and d p1 ×d p2 When d < 0, the boundary line AB of the obstacle intersects with P1P2; a ×d b <0 and d p1 ×d p2 ≥0, the boundary line AB of the obstacle does not intersect with P1P2; when d a ×d b > 0, the boundary line AB of the obstacle does not intersect with P1P2; when d a and d b When they are not 0 at the same time, it means that the vertex A or B of the obstacle is on P1P2. Since P1P2 and the obstacle boundary line AB only intersect at one vertex, and the obstacle has been expanded, it is considered that AB and P1P2 do not intersect. a and d b When both are 0, it means that the obstacle boundary line AB and P1P2 are on the same straight line, and the obstacle boundary line AB and P1P2 do not intersect. Similarly, it can be determined whether P1P2 intersects with AE and BC, thereby determining whether P1P2 conflicts with the obstacle.
[0100] S400, judging the distance between the obstacle and the hull and selecting a real-time collision avoidance method:
[0101] The distance to the obstacle is detected by the navigator. If the obstacle is closer to the hull than the set distance, the improved artificial potential field method is used to avoid collision in real time.
[0102] Specifically, in the traditional artificial potential field method, only the obstacle distance is considered, such as Figure 8As shown in the figure, the distance between obstacle A and USV is greater than obstacle B, and obstacle A is subject to a smaller repulsive force. However, the actual situation is just the opposite. Obstacle A is in the heading of USV, while obstacle B is on the right side of USV. Based on the orientation, obstacle A has a greater impact. In addition, since the improved artificial potential field method is only responsible for emergency collision avoidance, its focus is only on safely avoiding obstacles, so the impact of the target point is not considered. In summary, the improved artificial potential field model in this application takes the orientation factor into consideration, so that the potential field in the artificial potential field method becomes an elliptical distribution, such as Fig. 9 As shown, obstacle A and obstacle B are on the same potential field line, that is, their effects on the USV are the same.
[0103] The specific method to improve the artificial potential field method is:
[0104] S411, setting the potential field to an ellipse, and determining the shape of the ellipse potential field;
[0105] S412. Calculate the potential value v of any visible vertex (x, y) of the obstacle model p ;
[0106] S413, v p Make a judgment, if v p ≥1, the USV will follow the original route. p <1, calculate the linear velocity and angular velocity of USV at the next moment.
[0107] When the obstacle is greater than the set distance from the hull, the improved ant colony algorithm is used to avoid collision. The specific steps include:
[0108] S421, using a visibility judgment method based on relative position detection to count the visible points of all the puffed environment obstacle vertices including the starting point and the end point, each vertex g i Define an allowed i The linked list stores its visible point set;
[0109] S422, initialization τ ij (0) Set the historical best generation counter count = 0, the evolution generation counter G_count = 0, the historical best maximum generation Max, the maximum evolution generation Generation, the path cost ∞ of the initial historical best ant history_best, and the population number m of ants in each generation;
[0110] S423, setting the ant population number k=1;
[0111] S424, if k>m, go to S427, otherwise place ant k at the starting position g s Go up and go to S425;
[0112] S425, let the current position of the ant be g i , if g i The visible point list allowed i If it is empty, the ant dies, k=k+1, and go to S424;
[0113] S426, if the ant's current position is the end point g e , then the ant finds the complete path, k = k + 1, and goes to S424, otherwise, the path point is changed from allowed i , to prevent the ants from searching back, and to find the next path point according to the state transition rule, and then go to S425;
[0114] S427, evolutionary generation counter G_count=G_count+1, if the best ant in this generation is better than the path found by the best ant in history, update the initial best ant in history history_best and set the best reserved generation counter count=0, otherwise, the best reserved generation counter count=count+1;
[0115] S428. If the historical optimal retained generation counter count> the historical optimal maximum retained generation Max or the evolution generation counter G_count≥ the maximum evolution generation Generation, the optimization method stops. Otherwise, the ant colony pheromone is globally updated based on the jungle principle and the maximum-minimum ant system, and the process goes to S423.
[0116] S429, inputting the navigator detection information and the target point information into the USV static collision avoidance planner based on the improved ant colony algorithm to obtain the USV's bow turning and speed adjustment command at the next moment.
[0117] Based on the Qt platform, the improved ant colony algorithm and the improved artificial potential field method are combined to simulate the USV collision avoidance planning in a static unknown environment. Fig.11 shown.
[0118] Working principle: This application combines the improved artificial potential field method and the improved ant colony algorithm to perform collision avoidance planning for the navigation of USV in an unknown static environment. Under normal circumstances, the improved ant colony algorithm is used for conventional collision avoidance. For obstacles that suddenly appear and affect navigation, the improved artificial potential field method is used for collision avoidance. At the same time, the improved ant colony algorithm is used to make up for the shortcomings of the artificial potential field method that can only be used for emergency collision avoidance but cannot perform path planning; by selecting targeted methods for collision avoidance in different situations, the real-time nature of collision avoidance is enhanced and the collision avoidance efficiency is improved; at the same time, by improving the artificial potential field method, the potential field is set to an elliptical shape, which avoids the problem that the traditional artificial potential field method can only make collision avoidance planning based on distance, adds a judgment condition on whether the obstacle affects navigation, and improves the effectiveness of collision avoidance.
[0119] In a preferred embodiment, the shape of the elliptical potential field is determined as follows:
[0120] A plane rectangular coordinate system is established with the hull as the origin. The coordinates of the four vertices of the ellipse are (0, y r )、(0,y f )、(x f ,y xf )、(x f ,-y xf ), the shape of the elliptical potential field is V(y f ,y r ,y xf ,x f ),like Fig.10 shown.
[0121] In a preferred embodiment, v p The calculation formula is as follows:
[0122] v p =(y 2 +(ay+c)x 2 ) / (py+q)
[0123] Where p = y f -y r , q = y f -y r ,
[0124] In a preferred embodiment, the calculation formula of the linear velocity and angular velocity of the USV at the next moment is as follows:
[0125] γ=max(1-v p ,0)
[0126]
[0127]
[0128] In the formula, ω r and v l are the angular velocity and linear velocity obtained after USV planning, ω max and ω pre are the maximum turning angular velocity and current turning angular velocity of the USV, v pre and v max are the current linear velocity and maximum linear velocity of USV respectively, and α and β are intensity factors.
[0129] In a preferred embodiment, the formula of the jungle principle is as follows:
[0130]
[0131]
[0132]
[0133] Where n is the number of moments after which an ant completes a cycle, Q represents the pheromone intensity, α is the global pheromone volatility coefficient, and L ALL is the length of the path taken by the ant in this cycle, is the pheromone increment of ants on path (i, j) in this cycle, m is the population of ants, L BAD is the worst path length taken by all ants in this cycle, Δτ ij(t) is the pheromone increment of the ant on the worst path in this cycle on the path (i, j).
[0134] Specifically, we first use the jungle principle to remove the pheromones released by ants on the worst local path in each cycle, and then strengthen the pheromones on the top one-third of the paths (arranged from short to long) in each cycle according to the following formula:
[0135] τ ij (t+n)=λ×τ ij (t+n)
[0136]
[0137] Where D is the Euclidean distance from the starting point to the end point, L k is the length of the path taken by the kth ant in this cycle.
[0138] By updating the global pheromone based on the jungle principle, the disadvantage of slow convergence of the traditional ant colony algorithm is overcome, and the convergence speed of the ant colony algorithm is improved to improve the real-time performance of collision avoidance planning.
[0139] In a preferred embodiment, the formula of the maximum-minimum ant system is as follows:
[0140]
[0141] In the formula, τ max , τ min The upper and lower limits of pheromone concentration are set by yourself.
[0142] By updating the global pheromone based on the maximum-minimum ant system and the above-mentioned jungle principle, the real-time performance of collision avoidance planning can be further improved.
[0143] The above description is only a preferred embodiment of the present application and an explanation of the technical principles used. Those skilled in the art should understand that the scope of the invention involved in the present application is not limited to the technical solution formed by a specific combination of the above technical features, but should also cover other technical solutions formed by any combination of the above technical features or their equivalent features without departing from the inventive concept. For example, the above features are replaced with the technical features with similar functions disclosed in this application (but not limited to) by each other.
Claims
1. A USV collision avoidance method based on an improved ant colony algorithm and an improved artificial potential field method, characterized in that: The following steps are involved: S100, constructing a coordinate system, including a global coordinate system and a local coordinate system; The local coordinate system includes a hull coordinate system and a navigator coordinate system. The hull coordinate system is a rectangular coordinate system with the USV as the origin and the USV navigation direction as the X-axis. The navigator coordinate system is a polar coordinate system with the USV as the origin and the USV navigation direction as the polar axis. S200, design the optimization window for real-time collision avoidance planning: Establish an optimization window centered on the USV and navigating with the USV, the size of the optimization window is set to the detection distance of the USV navigator, the optimization window is updated at a set time, and a collision avoidance plan is performed for the USV; S300, using visual graph method to build obstacle model: The visibility graph method includes a visibility graph expansion method and a visibility graph construction method, wherein the expansion method uses a vector area method to determine the concavity and convexity, and the construction method uses a relative position detection method to determine the visibility; S400, judging the distance between the obstacle and the hull and selecting a real-time collision avoidance method: If the obstacle is less than the set distance from the hull, the improved artificial potential field method is used for real-time collision avoidance. The specific method is as follows: S411, setting the potential field to an ellipse, and determining the shape of the ellipse potential field; S412, Computational obstacle model of any visible vertex The potential value ; S413, yes Make a judgment, if ≥1, the USV will follow the original route. <1, calculate the linear velocity and angular velocity of USV at the next moment; If the obstacle is greater than the set distance from the hull, the improved ant colony algorithm is used to avoid collision; The steps of using the improved ant colony algorithm for collision avoidance include: S421, using a visibility judgment method based on relative position detection to count the visible points of all the vertices of the puffed environment obstacles including the starting point and the end point, each vertex Define one The linked list stores its visible point set; S422, Initialization , set the historical best retention generation counter count=0, the evolution generation counter G_count=0, the historical best maximum retention generation Max, the maximum evolution generation Generation, the path cost ∞ of the initial historical best ant history_best, and the population number m of ants in each generation; S423, set ant population number ; S424, if k>m, go to S427, otherwise place ant k at the starting position Go up and go to S425; S425, set the current position of the ant to ,like Visible point list If it is empty, the ant dies, k=k+1, and go to S424; S426: If the ant's current position is the end point , then the ant finds the complete path, k=k+1, and goes to S424, otherwise, the path point is changed from , to prevent the ants from searching back, and to find the next path point according to the state transition rule, and then go to S425; S427, evolutionary generation counter G_count=G_count+1, if the best ant in this generation is better than the path found by the best ant in history, update the initial best ant in history history_best and set the best history reserved generation counter count=0, otherwise, the best history reserved generation counter count=count+1; S428, if the historical optimal reserved generation counter count> the historical optimal maximum reserved generation Max or the evolutionary generation counter G_count≥ the maximum evolutionary generation Generation, the optimization method stops, otherwise the ant colony pheromone is globally updated based on the jungle principle and the maximum-minimum ant system, and the process goes to S423; S429, inputting the navigator detection information and the target point information into the USV static collision avoidance planner based on the improved ant colony algorithm to obtain the USV's bow turning and speed adjustment command at the next moment.
2. The USV collision avoidance method based on improved ant colony algorithm and improved artificial potential field method according to claim 1 is characterized in that: The shape of the elliptical potential field is determined as follows: A plane rectangular coordinate system is established with the hull as the origin of the coordinate system. The coordinates of the four vertices of the ellipse are , , , , the shape of the elliptical potential field is .
3. The USV collision avoidance method based on improved ant colony algorithm and improved artificial potential field method according to claim 2 is characterized in that: The calculation formula is as follows: in, , , , .
4. The USV collision avoidance method based on improved ant colony algorithm and improved artificial potential field method according to claim 3 is characterized in that: The calculation formulas for the linear velocity and angular velocity of the USV at the next moment are as follows: In the formula, and are the angular velocity and linear velocity obtained after USV planning, and are the maximum turning angular velocity and current turning angular velocity of the USV, and They are the current linear speed and maximum linear speed of USV respectively. and is the intensity factor.
5. The USV collision avoidance method based on improved ant colony algorithm and improved artificial potential field method according to claim 4, characterized in that: The formula of the jungle principle is as follows: ; In the formula, For passing In a moment, the ant completes a cycle. Indicates the pheromone intensity, is the global pheromone volatility coefficient, is the length of the path taken by the ant in this cycle, The number of ants on the path in this cycle The pheromone increment of the business, is the population of ants, is the worst path length taken by all ants in this cycle, The worst path of the ants in this cycle The pheromone increase on.
6. The USV collision avoidance method based on improved ant colony algorithm and improved artificial potential field method according to claim 5, characterized in that: The formula of the maximum-minimum ant system is shown in the figure below: In the formula, , The upper and lower limits of pheromone concentration are set by yourself.
Citation Information
Patent Citations
Cooperative autonomous navigation systems and methods for multiple unmanned surface vehicles considering communication
CA3067576A1
An improved ant colony optimization-based collision avoidance planning method for a USV in an unknown static obstacle environment
CN109685286A
Robot path planning method integrating artificial potential field and logarithm ant colony algorithm
CN110928295A
Unmanned aerial vehicle path planning method based on artificial potential field and ant colony algorithm
CN111784079A
