Unmanned surface vessel path planning method based on improved beetle antennae search algorithm
By improving the Tianniu search algorithm, optimizing the search direction and step length, combining path optimization strategies and obstacle expansion technology, the problems of low search efficiency and low path quality in unmanned surface boat path planning are solved, and more efficient and Higher quality path planning.
Patent Information
- Application Number
- CN202411017625.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-29
- Publication Date
- 2024-11-08
AI Technical Summary
The traditional search algorithm for tycoons has problems such as inefficient search efficiency and low path quality when planning unmanned surface boats. Especially in complex environments and multi-constraint conditions, resulting in tortuous paths, many redundant nodes, navigation time and Increased energy consumption reduces the practicality and safety of the path.
By improving the Tianniu search algorithm, the vector pointed to the end point direction of the current position as a reference is used, the constraint angle is set to optimize the search direction, dynamically update the search step size, introduce path optimization strategies, eliminate redundancy and oscillation nodes, and convert impassable areas Puff into circular areas to simplify obstacle handling.
It improves the efficiency and quality of path planning, reduces the path angle and the number of algorithm iterations, simplifies the path point inspection process, and significantly improves the efficiency of unmanned surface boat path planning and the smoothness and practicality of paths.
Smart Images

Figure CN118915755A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of path planning, and in particular to an unmanned surface vessel path planning method based on an improved beetle whisker search algorithm. Background Art
[0002] With the continuous deepening of ocean exploration and application, unmanned surface vessels are playing an increasingly important role in military reconnaissance, environmental monitoring, resource exploration and other fields. Path planning of unmanned surface vessels is one of the core technologies for achieving autonomous navigation and efficient mission execution. Its main task is to find a mobile path that meets specific constraints (such as safety) and evaluation indicators (such as path length) based on the passable and impassable areas in the map, combined with information such as the starting point and the end point.
[0003] For the path planning problem of mobile intelligent bodies such as unmanned surface vessels, traditional algorithms such as A* and Dijkstra can find a feasible path, but when faced with complex environments and multiple constraints, they often have high computational complexity and poor real-time performance. Therefore, developing a bionic algorithm with fast computational speed, low complexity and suitable for unmanned surface vessels has positive significance for improving the mobility and intelligence level of unmanned surface vessels.
[0004] Among the bionic optimization algorithms, the beetle whisker search (BAS) algorithm simulates the search behavior of beetles when looking for food. It has the advantages of simple calculation and few parameters, and is especially suitable for low-dimensional optimization objectives. However, the traditional BAS algorithm has obvious shortcomings when applied to unmanned surface vehicle path planning. First, the random selection of search directions and fixed step size make the search process lack of pertinence and flexibility, affecting the efficiency and accuracy of the search. Secondly, due to the limitations of the algorithm itself, there are often many redundant nodes and oscillating nodes in the generated path, resulting in a tortuous path, which not only increases the navigation time and energy consumption, but also reduces the practicality and safety of the path. Summary of the invention
[0005] In view of this, the purpose of the present invention is to propose an unmanned surface vessel path planning method based on an improved longhorn beetle whisker search algorithm to solve the problems of low search efficiency and low path quality in traditional algorithms during path planning.
[0006] Based on the above purpose, the present invention provides an unmanned surface vehicle path planning method based on an improved beetle whisker search algorithm, comprising the following steps:
[0007] S1. Read map information to obtain the specific locations of passable and impassable areas;
[0008] S2. Based on the starting point, the beetle whisker search algorithm is used to find the path. When finding the path, the direction vector pointing from the current position to the end point is used as a reference to set the constraint angle so that the random search direction of the beetle satisfies the constraint.
[0009] S3, based on the current direction of the longicorn beetle, calculating the positions of its left and right tentacles;
[0010] S4, evaluating the estimated cost of the tentacle position, selecting the side with the smaller cost as the search direction, and excluding the situation where the searched path point is an obstacle or two path points cross an obstacle;
[0011] S5, dynamically update the search step size and update the path nodes;
[0012] S6. Introduce a path optimization strategy, optimize the path points, connect the optimized path points, form the final path, and realize path planning.
[0013] Preferably, in step S2, the constraint angle is adjusted according to the position of the longicorn, and the constraint is strengthened when it is far away from the obstacle and the end point, and the constraint is relaxed when it is close to the obstacle.
[0014] Preferably, the method further comprises, in the process of path finding using the beetle whisker search algorithm, expanding the inaccessible area into a circular area with its maximum influence distance as the radius.
[0015] Preferably, excluding the searched path points as obstacles includes:
[0016] After calculating the next position of the longicorn, the relationship between the distance from the position to the nearby obstacle and the expansion radius of the obstacle is determined. If it is less than, it means that the position is an obstacle. Otherwise, the position can be used as the next path node.
[0017] Preferably, eliminating obstacles that cross over between two path points includes:
[0018] Calculate the position of the projection point of the nearby obstacle on the line connecting the two path points and the distance to the line connecting the two path points, and determine whether the two path points cross the obstacle based on the calculation results.
[0019] Preferably, step S5 further comprises:
[0020] During the iteration process, the search step size is dynamically updated and a step size attenuation mechanism is incorporated. Based on the initial step size, a step size attenuation factor is multiplied. The step size attenuation factor forms a mathematical exponential relationship with the iteration factor. A larger step size is used at the beginning of the iteration, and the step size is gradually and smoothly reduced when approaching the target.
[0021] Preferably, the path optimization strategy includes: starting from the starting point, taking the path node farthest from the starting point and directly connected without crossing obstacles as the preferred path point, connecting the preferred path point with the current node, and removing the intermediate nodes as redundant nodes.
[0022] Preferably, in step S2, taking the direction vector from the current position to the end position as a reference, setting the constraint angle so that the random search direction of the longicorn meets the constraint specifically includes:
[0023] According to the current position and the end point position information, the reference direction vector is obtained
[0024]
[0025] Among them, (x n ,y n ) indicates the current position of the longicorn, (x e ,y e ) represents the end point coordinates;
[0026] Direction vector For reference, set a certain constraint angle and make the beetle's orientation meet the constraint, including:
[0027]
[0028] θ min ≤θ≤θ max
[0029] Among them, θ represents the angle between two vectors, θ min With θ max represents the minimum and maximum constraint angles, Represents the normalized beetle heading vector.
[0030] Beneficial effects of the present invention:
[0031] 1. Improve path planning efficiency: In the iterative process, the present invention eliminates the sharp corners or edges of obstacles through the obstacle expansion strategy, and simplifies the inspection process of path points; at the same time, by optimizing the search direction and step size selection mechanism, the path angle and the number of algorithm iterations are effectively reduced, thereby improving the path finding efficiency.
[0032] 2. Improve the quality of the path: The path optimization strategy removes redundant nodes in the path, effectively reduces the path length and tortuosity, and improves the smoothness and practicality of the path. While keeping the algorithm simple and the amount of calculation small, the present invention significantly improves the efficiency and path quality of the unmanned surface vehicle path planning. BRIEF DESCRIPTION OF THE DRAWINGS
[0033] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings in the following description are only for the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0034] Figure 1 A block diagram of a path planning method according to an embodiment of the present invention;
[0035] Figure 2 A detailed flowchart of a path planning method according to an embodiment of the present invention is shown;
[0036] Figure 3 Path planned for traditional BAS algorithms.
[0037] Figure 4 The path planned for the method of the present invention.
[0038] Figure 5 The figure shows the comparison between the method of the present invention and the traditional BAS algorithm in terms of the number of iterations.
[0039] Figure 6 The figure shows the comparison between the method of the present invention and the traditional BAS algorithm in terms of path length. DETAILED DESCRIPTION
[0040] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with specific embodiments.
[0041] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the present invention should be understood by people with ordinary skills in the field to which the present invention belongs. The "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.
[0042] like Figure 1 and Figure 2 As shown, the embodiment of this specification provides a path planning method for an unmanned surface vehicle based on an improved beetle whisker search algorithm, see Figure 1,This method mainly includes six steps of S1 map information reading and preprocessing, S2 constraining the beetle search direction, S3 beetle position change, S4 dynamic ,update step size, update path, S5 path node optimization, S6 output optimized path, and drawing path curve. The detailed ,description of each step is as follows.
[0043] Step S1, read the map information to determine the specific locations of the starting point and the end point, the passable and impassable areas. This step specifically includes the following sub-steps:
[0044] Step S101, according to the map environment, select a suitable coordinate scale, establish a coordinate system, and obtain the starting point S(x0, y0), the end point E(x e ,y e ) and obstacles O(x oi ,y oi ), i represents the i-th obstacle;
[0045] Step S102: taking the maximum impact distance r of the obstacle as oi is the radius. When calculating, the obstacle is expanded into a circular area to eliminate the sharp corners or edges of the obstacle and reduce the difficulty of handling the obstacle. In practical applications, r can be appropriately increased. oi When there is noise or uncertainty in the sensor data, circular puffing can serve as a fault-tolerance mechanism. Even if the sensor measurements are slightly off, collisions can be avoided as long as the moving object remains outside the puffed circular area.
[0046] Step S2, based on the position of the starting point, the longhorn beetle search algorithm is used for path finding. When finding the path, the direction vector pointing from the current position to the end position is used as a reference to constrain the longhorn beetle to search randomly. Since the direction of the longhorn beetle is arbitrary, there is a high degree of blindness when searching for the path, which causes the generated path to be very tortuous. Constraining the direction of the longhorn beetle can effectively reduce the path angle and increase the convergence speed. This step specifically includes the following sub-steps:
[0047] Step S201, starting from the starting point S(x0, y0), the path is found using the beetle whisker search algorithm, and the direction of the beetle's head is arbitrary:
[0048]
[0049] in, represents the normalized orientation vector of the longhorn beetle, which refers to the orientation of the longhorn beetle in the motion space, represented by the direction vector from the right whisker to the left whisker; rands(k,1) is used to generate a random vector, k represents the dimension of the vector, and ||·||2 represents the vector's binorm;
[0050] Step S202: Set the reference direction to point from the current position to the destination. Based on the current position and the destination position information, the reference direction vector
[0051]
[0052] Among them, (x n ,y n ) indicates the current position of the longicorn, (x e ,y e ) represents the end point coordinates;
[0053] Step S203, using direction vector For reference, set a certain constraint angle and make the beetle's orientation meet the constraint. The specific solution is as follows:
[0054]
[0055] θ min ≤θ≤θ max
[0056] Among them, θ represents the angle between two vectors, θ min With θ max Indicates the minimum and maximum constraint angles. The set constraint angle can be adjusted according to the position of the beetle. When it is far away from obstacles and far away from the end point, the restriction is strengthened to speed up the search. When it is close to obstacles, the restriction is relaxed to avoid collision.
[0057] Step S3, based on the direction of the longhorn beetle, calculate the positions of the left and right antennae of the longhorn beetle. According to the constrained direction of the longhorn beetle and the current center of mass position of the longhorn beetle, the positions of the left and right antennae can be calculated:
[0058]
[0059] Among them, X left , X right They represent the positions of the left and right antennae respectively; l represents the distance between the two antennae and is defined as l=step / C, where step is the search step of the longicorn beetle and C is a fixed constant representing the ratio of the longicorn beetle step to the distance between the two antennae.
[0060] Step S4, calculate the estimated cost of the tentacle position, select the side with the smaller cost to search for the next path point, and exclude the situation where the next path point is an obstacle and the situation where two path points cross obstacles. This step includes the following sub-steps:
[0061] Step S401, after obtaining the positions of the left and right antennae of the longhorn beetle, the odor concentration at the positions of the two antennae can be calculated. The odor concentration is often abstracted as a distance representation, and the higher the odor concentration, the shorter the distance. Therefore, the odor concentration received by the two antennae can be expressed as:
[0062]
[0063] Step S402, according to the odor concentration of the left and right whiskers (estimated cost), determine the location of the next path point of the longhorn beetle:
[0064]
[0065] Among them, X n represents the position of the longicorn at the nth iteration, X n+1 Indicates the next step of the beetle; step is the search step length of the beetle. is the normalized direction vector, which refers to the direction of the beetle in the motion space, represented by the direction vector from the right whisker to the left whisker; f left With f right represents the path cost of the left and right whiskers of the longicorn beetle, defined as Where X left , X right represents the position of the left and right tentacles; sign is the sign function, if f left <f right , it searches for the next path point in the left direction, otherwise it searches in the right direction;
[0066] Step S403, determine whether the next position of the longhorn beetle is within the obstacle range. Since the obstacle has been expanded into a circular area in advance, the difficulty of handling the obstacle is greatly reduced. After obtaining the next position of the longhorn beetle, the distance d between the position and the nearby obstacles can be calculated. i , we only need to compare d i Is it smaller than the radius r of these puffing obstacles? oi If it is less than, it means that the position is an obstacle and it is necessary to search for the next step again. If it is greater than, it means that the position can be used as the next path node.
[0067] Step S404, determine whether the line connecting the next path node and the current node crosses an obstacle, that is, calculate the position of the projection point of the nearby obstacle on the line connecting the two path points and the distance d to the line connecting the two path points. j If the projection point is outside the line segment (i.e., the extension line of the line segment), it means that the obstacle is not crossed; if the projection point is on the line segment, further judgment d j The maximum impact distance r from these obstacles oi If d j <r oi , indicating that the obstacle has been crossed and the location of the next path node needs to be searched again. If d j >r oi , indicating that the longhorn beetle moves forward to the next path node without crossing the obstacle.
[0068] Step S5, dynamically update the search step length and update the path nodes. This step includes the following sub-steps:
[0069] Step S501, improve the fixed step size strategy, and incorporate a step size attenuation mechanism, that is, introduce a carefully designed step size attenuation factor, which forms a mathematical exponential relationship with the iteration factor in the iterative process. In the early stage, a larger step size is used to speed up the search speed. When approaching the target, the step size is gradually reduced to improve the accuracy and avoid oscillation. The specific scheme is:
[0070] step n =step0×η α
[0071]
[0072] Among them, step n and step0 represent the current step size and the initial step size, η is the step size factor and η∈[0,1]; α represents the iteration factor, which is used to gradually reduce the search step size; d n and d0 represent the distance from the current position and the starting point to the end point respectively; D is used to control the minimum step length, that is, the minimum step length
[0073] Step S502, following the above steps, until the end point appears in the path node, the path finding is completed.
[0074] Step S6, according to the path optimization strategy, the path points are optimized, the optimized path points are connected to form a final path, and the path planning is realized. This step includes the following sub-steps:
[0075] Step S601, use the path optimization strategy to optimize the path points. Using the principle that the straight line between two points is the shortest, starting from the starting point, the path node farthest from the starting point and directly connected without crossing obstacles is used as the "preferred path point", which is connected to the current node, and the intermediate nodes are removed as redundant nodes.
[0076] Step S602, according to the above steps, after eliminating redundant nodes and oscillating nodes, the starting point, each "optimal path point" and the end point are connected in sequence to obtain the final path.
[0077] Following the above steps, the path planning of the unmanned surface vessel is completed.
[0078] Table 1
[0079] starting point end Iterations Path length Number of inflection points Traditional BAS (10,10) (50,50) 85 83 83 Method of the present invention (10,10) (50,50) 54 57.3781 3
[0080] Figure 3 The path planned for the traditional BAS algorithm, Figure 4is the path planned by the method of this embodiment, wherein the dotted line is the path before optimization, and the solid line is the path after optimization. Table 1 is the comparison result of the improved beetle whisker search algorithm of the present invention and the traditional BAS algorithm applied to the path planning of unmanned surface craft. Figure 5 and Figure 6 The comparison between the method of the present invention and the traditional BAS algorithm in terms of the number of iterations and the path length is shown respectively. It can be seen that the present invention has the following advantages:
[0081] The present invention can effectively overcome the shortcomings of the traditional BAS algorithm when applied to unmanned surface vessel path planning, constrain the direction of the longhorn beetle to reduce the blindness of the path search, and adopt a dynamic update step size strategy to accelerate the search speed and improve the search accuracy.
[0082] Compared with the solution of directly using BAS for unmanned surface vessel path planning, this method not only optimizes obstacle handling, optimizes the search direction and step size selection mechanism, but also introduces path optimization technology. It uses the principle of the shortest straight line between two points and starts from the starting point. The "preferred path point" that is farthest from the starting point and directly connected without crossing obstacles is connected to the current node. The intermediate nodes are removed as redundant nodes, which further shortens the path length and improves the practicality of the path.
[0083] The present invention adopts an obstacle puffing technology. During calculation, the obstacle is puffed into a circular area, eliminating the sharp corners or edges of the obstacle and simplifying subsequent processing. At the same time, it is equivalent to adding a safe area around the obstacle. In this way, when there is noise or uncertainty in the sensor data, the circular puffing can be used as a fault-tolerant mechanism. As long as the moving object remains outside the puffed circular area, collision can be avoided, effectively improving the safety of the path.
[0084] Those skilled in the art should understand that the discussion of any of the above embodiments is only exemplary and is not intended to imply that the scope of the present invention is limited to these examples; under the concept of the present invention, the technical features in the above embodiments or different embodiments can also be combined, the steps can be implemented in any order, and there are many other changes in different aspects of the present invention as described above, which are not provided in detail for the sake of simplicity. Any omissions, modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.
Claims
1. A path planning method for an unmanned surface vehicle based on an improved beetle whisker search algorithm, characterized in that: The method comprises the following steps: S1. Read map information to obtain the specific locations of passable and impassable areas; S2. Based on the starting point, the beetle whisker search algorithm is used to find the path. When finding the path, the direction vector pointing from the current position to the end point is used as a reference to set the constraint angle so that the random search direction of the beetle satisfies the constraint. S3, based on the current direction of the longicorn beetle, calculating the positions of its left and right tentacles; S4, evaluating the estimated cost of the tentacle position, selecting the side with the smaller cost as the search direction, and excluding the situation where the searched path point is an obstacle or two path points cross an obstacle; S5, dynamically update the search step size and update the path nodes; S6. Introduce a path optimization strategy, optimize the path points, connect the optimized path points, form the final path, and realize path planning.
2. The unmanned surface vehicle path planning method based on the improved beetle whisker search algorithm according to claim 1 is characterized in that: In step S2, the constraint angle is adjusted according to the position of the longicorn beetle, and the constraint is strengthened when it is far away from the obstacle and the end point, and the constraint is relaxed when it is close to the obstacle.
3. The unmanned surface vehicle path planning method based on the improved beetle whisker search algorithm according to claim 1 is characterized in that: The method also includes, in the process of path finding using the beetle whisker search algorithm, expanding the inaccessible area into a circular area with its maximum influence distance as the radius.
4. The unmanned surface vehicle path planning method based on the improved beetle whisker search algorithm according to claim 3 is characterized in that: The step of excluding the searched path points as obstacles includes: After calculating the next position of the longicorn, the relationship between the distance from the position to the nearby obstacle and the expansion radius of the obstacle is determined. If it is less than, it means that the position is an obstacle. Otherwise, the position can be used as the next path node.
5. The unmanned surface vehicle path planning method based on the improved beetle whisker search algorithm according to claim 1 is characterized in that: Eliminating obstacles that cross between two path points includes: Calculate the position of the projection point of the nearby obstacle on the line connecting the two path points and the distance to the line connecting the two path points, and determine whether the two path points cross the obstacle based on the calculation results.
6. The unmanned surface vehicle path planning method based on the improved beetle whisker search algorithm according to claim 1 is characterized in that: Step S5 further comprises: During the iteration process, the search step size is dynamically updated and a step size attenuation mechanism is incorporated. On the basis of the initial step size, a step size attenuation factor is multiplied. The step size attenuation factor forms a mathematical exponential relationship with the iteration factor in the iteration process. A larger step size is used at the beginning of the iteration, and the step size is gradually and smoothly reduced when approaching the target.
7. The unmanned surface vehicle path planning method based on the improved beetle whisker search algorithm according to claim 1 is characterized in that: The path optimization strategy includes: starting from the starting point, taking the path node farthest from the starting point and directly connected without crossing obstacles as the preferred path point, connecting the preferred path point with the current node, and removing the intermediate nodes as redundant nodes.
8. The unmanned surface vehicle path planning method based on the improved beetle whisker search algorithm according to claim 1 is characterized in that: In step S2, the step of setting a constraint angle with reference to the direction vector pointing from the current position to the end position so that the random search direction of the longicorn meets the constraint specifically includes: According to the current position and the end point position information, the reference direction vector is obtained Among them, (x n ,y n ) indicates the current position of the longicorn, (x e ,y e ) represents the end point coordinates; With direction vector For reference, set a certain constraint angle and make the beetle's orientation meet the constraint, including: i min ≤θ≤θ max Among them, θ represents the angle between two vectors, θ min With θ max represents the minimum and maximum constraint angles, Represents the normalized beetle heading vector.
