A route planning method for high-speed unmanned boat

Abstract

The present application belongs to the technical field of unmanned boat path planning, and provides a route planning method for a high-speed unmanned boat, including the following steps: obtaining the latitude and longitude information of the mission area, starting point, end point, several must-pass points and at least one obstacle of the high-speed unmanned boat and converting them into UTM coordinates; converting the UTM coordinates into grid map coordinates, and executing the following steps on the grid map in sequence: connecting each must-pass point in sequence to obtain a first route; performing path expansion processing on each must-pass point to obtain an expanded path and a second route corresponding to each must-pass point, performing obstacle avoidance processing on the second route to obtain a third route; performing obtuse processing on the third route to obtain a planned route. The method of the present application can reasonably plan the route of a high-speed unmanned boat passing through several must-pass points in sequence.

Description

Technical Field

[0001] The present application belongs to the field of ship intelligent control and navigation technology, and specifically provides a route planning method for a high-speed unmanned boat. Background Art

[0002] High-speed Unmanned Surface Vehicle (High-speed USV) has the characteristics of rapid response and flexible maneuverability. It is usually used for tasks such as maritime rescue, disaster response, and environmental monitoring. Path planning is a key technology for unmanned surface vehicles to perform tasks. It involves how to plan the optimal path in a complex and changeable marine environment to reach the target location quickly and safely.

[0003] At present, when high-speed unmanned boats are performing various surface tasks, they often need to ensure that they pass through multiple predetermined positions in sequence. When performing the above-mentioned types of tasks, it is not only necessary to ensure that the high-speed unmanned boats pass through the above-mentioned predetermined positions (also known as must-pass points) while avoiding sea surface obstacles, but also to maintain the speed as much as possible during the process of passing through each must-pass point, so as to avoid frequent speed reduction or even stopping the ship, which will extend the mission time.

[0004] However, the existing unmanned boat path planning methods generally take the final destination as the target point, and focus on obstacle avoidance during the path planning process. They generally do not make corresponding judgments on how to pass through each must-pass point in a reasonable path while maintaining the speed. Summary of the invention

[0005] In order to solve the problems existing in the above-mentioned prior art, the present application provides a route planning method for a high-speed unmanned boat, which is used to plan a route for a high-speed unmanned boat to pass through several must-pass points in sequence. The method comprises the following steps:

[0006] S100, obtaining the mission area, the starting point, the ending point, several points that must be passed in sequence, and the longitude and latitude information of at least one obstacle of the high-speed unmanned boat;

[0007] S200, converting the latitude and longitude information of the mission area, the starting point, the ending point, each must-pass point and the obstacle into UTM coordinates;

[0008] S300, converting the UTM coordinates of the mission area, the starting point, the ending point, each must-pass point and the obstacle into grid map coordinates, and sequentially executing steps S400 to S700 on the grid map;

[0009] S400, sequentially connecting the necessary points between the starting point and the end point according to the passing order of the high-speed unmanned boat to obtain a first route;

[0010] S500, traversingly performing path expansion processing on each of the must-pass points to obtain an expanded path corresponding to each of the must-pass points, and then sequentially connecting the expanded paths corresponding to each of the must-pass points between the starting point and the end point in the order in which the high-speed unmanned boat passes, to obtain a second route, wherein each of the must-pass points is located at the midpoint of its corresponding expanded path;

[0011] S600, performing obstacle avoidance processing on the second route using a local A* algorithm based on the grid map coordinates of the obstacle to obtain a third route;

[0012] S700, traversing each section of the third route, performing obtuse processing on the third route, and obtaining a planned route for the high-speed unmanned boat to reach a terminal point after passing through a number of must-pass points in sequence from a starting point.

[0013] Preferably, the grid resolution of the grid map is adjusted based on the scale of the mission area.

[0014] Furthermore, when the length or width of the mission area is greater than or equal to 100 km, the grid resolution of the grid map is 500 m / grid, otherwise the resolution of the grid map is 20 m / grid.

[0015] Furthermore, in step S500, each necessary point is subjected to path expansion processing, specifically, for any necessary point , perform the following steps:

[0016] S501, calculating the first route The angle between two segments connected at a point ,if If it is an acute angle, jump to step S502, otherwise jump to step S504;

[0017] S502, The point is the midpoint, along the angle The expansion lengths on both sides of the vertical direction of the midline ,get The corresponding two expansion points , ,in, The lower limit is the speed at which the high-speed unmanned boat passes through the must-pass point Twice the turning radius when

[0018] S503, judgment , Is the position legal? If so, , The line connecting the two points is the necessary point The expansion path ends at the point If not, then give up this step and get , Then jump to step S504, wherein when the position of any point is legal, the point is within the task area and is not within the range of any obstacle;

[0019] S504, the must-pass point As the midpoint, along Expand the length to both sides To get the necessary point The corresponding two potential expansion points , ,in For the first route in A waypoint on a segment before the point;

[0020] S505, this must-pass point As the center of rotation, , The connection line is rotated according to the preset angle Perform multiple rotations to get the required point multiple potential expansion paths;

[0021] S506: If at least one of the multiple potential expansion paths is legal, determine an optimal potential expansion path from the at least one legal potential expansion path as the necessary point. The corresponding expansion path, and the two endpoints of the expansion path are determined as the must-pass points The corresponding two expansion points , , then end the path expansion process for the must-pass point, otherwise shrink And return to step S504.

[0022] Preferably, step S506 further comprises the following steps: if the reduced The speed of the high-speed unmanned boat passing the must-pass point at its estimated speed The turning radius of the high-speed unmanned boat when passing the must-pass point is reduced. Estimated speed at .

[0023] Preferably, step S600 further includes the following steps:

[0024] S610, extracting all flight segments intersecting with obstacles from the second flight route;

[0025] S620, traverse each flight segment intersecting with an obstacle, and for each flight segment intersecting with an obstacle, execute steps S621 to S623 in sequence.

[0026] S621: Starting from the two intersection points of the flight segment and the obstacle, expand the length of the flight segment toward the outside of the obstacle. , get the obstacle avoidance starting point of this segment and obstacle avoidance endpoint ,in, The lower limit is twice the turning radius of the high-speed unmanned boat when it passes through the section at its expected speed;

[0027] S622, at the obstacle avoidance starting point and obstacle avoidance endpoint Execute the local A* algorithm to obtain the obstacle avoidance starting point and obstacle avoidance endpoint Multiple obstacle avoidance waypoints between;

[0028] S623, removing redundant waypoints from the plurality of obstacle avoidance waypoints, and sequentially connecting the obstacle avoidance starting point, the obstacle avoidance end point, and the remaining obstacle avoidance waypoints to obtain the flight segment after obstacle avoidance processing;

[0029] S630, connecting the segments after obstacle avoidance processing and other segments in the second route between the starting point and the ending point in sequence according to the passing order of the high-speed unmanned boat to obtain the third route.

[0030] Preferably, step S700 further includes the following steps:

[0031] S710, extracting all acute-angle segments in the third route, wherein each acute-angle segment includes two sequentially connected initial sub-segments and the angle between the two initial sub-segments is an acute angle;

[0032] S720, traverse each acute angle segment, and execute step S721 for each acute angle segment;

[0033] S721, determine whether the lengths of the two initial sub-segments of the acute angle segment are both greater than And whether the angle between the two is greater than 60 degrees, if so, the acute angle segment is cut to make it obtuse, if not, the acute angle segment is expanded to make it obtuse, wherein, The lower limit is twice the turning radius of the high-speed unmanned boat when it passes through the acute angle section at its expected speed;

[0034] S730, connecting the acute angle segments and other segments in the third route that have been blunt-angled in the order of the high-speed unmanned boat passing between the starting point and the end point in sequence to obtain the planned route.

[0035] Furthermore, the shearing operation on the acute angle segment in step S721 is specifically as follows: taking the intersection of the two initial sub-segments of the acute angle segment as the starting point, cutting a portion smaller than length to obtain two shear points , , delete the intersection of the two initial sub-segments of the acute angle segment and connect , , thus obtaining the acute angle segment after obtuse processing.

[0036] Furthermore, the expansion operation of the acute angle segment in step S721 is specifically as follows: starting from the intersection of the two initial sub-segments of the acute angle segment, continuously performing the extension-rotation operation to obtain multiple expansion points , and then connect the expansion sub-segments between the expansion points in sequence to obtain the acute angle segment after the blunt angle processing, wherein the first expansion point is located on the heading of one of the initial sub-segments, and in each extension-rotation operation, the extension length is not less than Furthermore, the included angle between the two expanded sub-segments obtained before and after the rotation is not less than 90°.

[0037] Preferably, step S200 also includes a step of expanding the obstacle.

[0038] The route planning method for a high-speed unmanned boat provided in an embodiment of the present application, after obtaining the initial route, sequentially performs path expansion, local obstacle avoidance and obtuse angle processing on it, wherein the path expansion enables the high-speed unmanned boat to pass the must-pass points at a higher speed, the local obstacle avoidance processing adjusts the sections near the obstacles with a smaller computational cost, and the obtuse angle processing further optimizes the acute-angle sections generated after the first two steps of route adjustment, so as to improve navigation stability, reduce energy consumption and reduce path tracking errors, and finally realize the reasonable planning of the high-speed unmanned boat to pass through each must-pass point in sequence. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A schematic diagram of a high-speed unmanned boat slowing down and passing through a must-pass point in some embodiments;

[0040] Figure 2 is a schematic diagram of a high-speed unmanned boat being unable to pass a must-pass point due to high-speed navigation in some embodiments;

[0041] Figure 3 A flow chart of a route planning method for a high-speed unmanned surface boat provided according to an embodiment of the present application;

[0042] Figure 4 This is a schematic diagram of the principle of UTM coordinate system transformation;

[0043] Figure 5 is a schematic diagram of a polygonal obstacle and its expansion operation in some embodiments;

[0044] Figure 6 is a flowchart of the implementation of step S500 in some embodiments;

[0045] Figure 7 A schematic diagram of generating expansion points on both sides of a must-pass point and performing path expansion in some embodiments;

[0046] Figure 8 A schematic diagram for generating multiple potential expansion paths in other embodiments;

[0047] Fig. 9 Based on Figure 8 Schematic diagram of path expansion based on multiple potential expansion paths in ;

[0048] Fig.10 is a flowchart of the implementation of step S600 in some embodiments;

[0049] Fig.11 A schematic diagram of the principle of removing redundant waypoints in some embodiments;

[0050] Fig.12 is a schematic diagram of a flight segment after local obstacle avoidance processing in some embodiments;

[0051] Fig.13 A schematic diagram of an acute angle segment caused by obstacle avoidance processing in some embodiments;

[0052] Fig.14 It is a schematic diagram of the principle of performing a shearing operation on an acute angle segment greater than 60 degrees in some embodiments;

[0053] Fig.15 It is a schematic diagram of the principle of performing an expansion operation on an acute angle segment less than or equal to 60 degrees in some embodiments;

[0054] Fig.16 It is a schematic diagram of the principle of expanding the acute angle segment less than or equal to 60 degrees in other embodiments;

[0055] Fig.17 A schematic diagram of a path planning scenario and a first route provided according to a specific embodiment of the present application;

[0056] Fig.18 A schematic diagram of a second route provided according to a specific embodiment of the present application;

[0057] Fig.19 A schematic diagram of a third route provided according to a specific embodiment of the present application;

[0058] Fig. 20A schematic diagram of a planned route provided according to a specific embodiment of the present application;

[0059] Fig.21 It is a schematic diagram of a planned route provided according to a comparative embodiment. DETAILED DESCRIPTION

[0060] Hereinafter, the present application will be further described based on preferred embodiments with reference to the accompanying drawings.

[0061] In the description of the embodiments of the present application, it should be noted that if the terms "upper", "lower", "inside", "outside", etc. indicate an orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship in which the products of the embodiments of the present application are usually placed when in use, it is only for the convenience of describing the present application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present application. In addition, in the description of the present application, in order to distinguish different units, the words first, second, etc. are used in this specification, but these are not limited by the order of manufacture, nor can they be understood as indicating or implying relative importance, and their names may be different in the detailed description and claims of the present application.

[0062] The vocabulary in this specification is used to illustrate the embodiments of the present application, but is not intended to limit the present application. It should also be noted that, unless otherwise clearly specified and limited, the terms "disposed", "connected", and "connected" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection, a direct connection, an indirect connection through an intermediate medium, or a connection between the two elements. For those skilled in the art, the specific meanings of the above terms in this application can be specifically understood.

[0063] The present application provides a route planning method for a high-speed unmanned surface boat through an embodiment. The planning method is used to reasonably plan a route for a high-speed unmanned boat to pass through several must-pass points in sequence. In order to fully illustrate the technical solution of the present application, the application scenario targeted by the planning method is first introduced.

[0064] As described in the background technology, high-speed unmanned boats can be used to perform various surface navigation and operation tasks. For different types of tasks, different routes need to be planned for high-speed unmanned boats to give full play to their characteristics of rapid response and flexible maneuverability.

[0065] Among the surface operation tasks performed by high-speed unmanned boats, there is a type of task that requires the high-speed unmanned boat to start from a starting point, pass through multiple preset fixed positions in a predetermined order and finally reach an end point. This type of task can be, for example, a task of sampling seawater at multiple predetermined locations or a task of measuring marine environmental parameters at certain locations. At the same time, in order to speed up the completion of the task, it is generally necessary to ensure that the high-speed unmanned boat passes the above-mentioned must-pass points at the maximum speed that can ensure safe navigation.

[0066] However, since the control of the unmanned boat generally adopts the method of correcting the course and speed in real time according to the deviation between the navigation status and the planned path, if the unmanned boat directly turns at each must-pass point along a broken line or a course close to a broken line, its speed must be reduced to avoid the danger of capsizing when it makes a small radius turn at high speed. Therefore, if Figure 1 As shown, the high-speed unmanned boat reaches the must-pass point The previous segment L1 starts to slow down and arrives When the speed is very low, it passes through a section L3 so that it can turn safely and return to section L2. (This problem must be passed at the point The segment angle at Figure 1 This will make the unmanned boat need to frequently accelerate and decelerate during navigation, which is not only not conducive to shortening the mission time, but also will increase the power system consumption; otherwise, it can only Figure 2 As shown in FIG. 1 , the ship turns ahead at segment L1 at a higher speed and enters segment L2 along segment L3 with a larger turning radius, thus failing to pass the must-pass point. In addition, when the unmanned boat makes a sharp turn at high speed, the path tracking error may increase due to excessive deviation from the route, and the probability of collision with obstacles is greatly increased.

[0067] To this end, it is necessary to plan the route of the high-speed unmanned boat more reasonably when performing this type of mission, so that the high-speed unmanned boat can pass through each must-pass point while maintaining its speed as much as possible.

[0068] Figure 3 The flowchart of a route planning method for a high-speed unmanned surface boat according to an embodiment of the present application is used to plan a route for a high-speed unmanned boat to pass through several must-pass points in sequence, such as Figure 3 As shown, the method comprises the following steps:

[0069] S100, obtaining the mission area, the starting point, the ending point, several points that must be passed in sequence, and the longitude and latitude information of at least one obstacle of the high-speed unmanned boat;

[0070] S200, converting the latitude and longitude information of the mission area, the starting point, the ending point, each must-pass point and the obstacle into UTM coordinates;

[0071] S300, converting the UTM coordinates of the mission area, the starting point, the ending point, each must-pass point and the obstacle into grid map coordinates, and sequentially executing steps S400 to S700 on the grid map;

[0072] S400, sequentially connecting the necessary points between the starting point and the end point according to the passing order of the high-speed unmanned boat to obtain a first route;

[0073] S500, traversingly performing path expansion processing on each of the must-pass points to obtain an expanded path corresponding to each of the must-pass points, and then sequentially connecting the expanded paths corresponding to each of the must-pass points between the starting point and the end point in the order in which the high-speed unmanned boat passes, to obtain a second route, wherein each of the must-pass points is located at the midpoint of its corresponding expanded path;

[0074] S600, performing obstacle avoidance processing on the second route using a local A* algorithm based on the grid map coordinates of the obstacle to obtain a third route;

[0075] S700, traversing each section of the third route, performing obtuse processing on the third route, and obtaining a planned route for the high-speed unmanned boat to reach a terminal point after passing through a number of must-pass points in sequence from a starting point.

[0076] In each of the above steps, firstly, coordinate transformation and rasterization are performed through steps S100 to S300 to preprocess the subsequent route planning, and then the initial route is generated through step S400, and then the initial route is sequentially expanded, locally avoided, and blunted through steps S500 to S700, wherein the path expansion enables the high-speed unmanned boat to pass through the must-pass point at a higher speed, and the local obstacle avoidance process adjusts the section near the obstacle with a lower computational cost, and the blunted process optimizes the acute-angle section generated after the first two steps of route adjustment, so as to further improve navigation stability, reduce energy consumption and reduce path tracking errors, and finally realize reasonable planning of the high-speed unmanned boat to pass through each must-pass point in sequence.

[0077] The specific implementation of the method is described in detail below with reference to the accompanying drawings.

[0078] Data preprocessing:

[0079] In an embodiment of the present application, step S100 is used to obtain raw data, specifically, including the mission area of ​​the high-speed unmanned boat, the starting point, the end point, several points that must be passed in sequence, and the longitude and latitude information of at least one obstacle. Obviously, depending on the type of data to be processed, the form of the longitude and latitude information and the data size are also different. For example, for the mission area, it can be a polygonal area surrounded by multiple points. Therefore, its longitude and latitude information is the longitude and latitude of each vertex of the mission area. For the starting point, the end point and each point that must be passed, they can be represented by a combination of longitude and latitude. For obstacles, when they are roughly polygonal, they can be represented by the longitude and latitude of each vertex. When they are roughly circular, they can be identified by the longitude and latitude of the center of the circle combined with its radius. The above-mentioned different types of areas, the representation method and data format of the longitude and latitude information of objects are already known to those skilled in the art, and will not be repeated here.

[0080] In the embodiment of the present application, the above latitude and longitude information is first converted to the UTM coordinate system through coordinate transformation. This projection method has been proposed since 1938 and is currently widely used as the projection basis for geodetic surveying and topographic maps. Figure 4 As shown, in a geometric sense, the UTM projection belongs to the transverse conformal elliptical cylindrical projection. The projection length ratio through the central meridian is not equal to 1 but equal to 0.9996, so that there is no deformation on the two secant lines after projection, and there is a simple proportional relationship with the Gauss projection coordinates.

[0081] Specifically, for any point, the latitude and longitude of the point can be transformed into UTM coordinates by following the steps below:

[0082] S201: Determine the UTM area code according to the longitude of the point:

[0083] ,

[0084] in, , They are the longitude and UTM area code of the point respectively;

[0085] S202: Calculate the arc corresponding to the central meridian of the UTM zone where the point is located:

[0086] ,

[0087] in, is the arc corresponding to the central meridian of the UTM zone where the point is located;

[0088] S203: Convert the latitude and longitude of the point to radians:

[0089] ,

[0090] in, is the latitude of the point, , are the arcs corresponding to the longitude and latitude of the point respectively;

[0091] S204: Calculate the eccentricity of the ellipsoid in the WGS84 coordinate system e and the radius of curvature of the ellipsoid :

[0092] ;

[0093] in, , are the major axis radius and minor axis radius of the ellipsoid in the WGS84 coordinate system, and their values ​​are =6378137.0, =6356752.3142;

[0094] S205: Calculate the meridian arc length M :

[0095] ;

[0096] S206: Calculate the longitude difference of the point :

[0097] ;

[0098] S207: Calculate the UTM coordinates of the point:

[0099] ,

[0100] in, , They are the horizontal and vertical coordinate values ​​of the point in the UTM coordinate system. is the second eccentricity, and its expression is , , is the scale factor of the UTM coordinate transformation, and its expression is .

[0101] In some preferred embodiments of the present application, after the transformation of longitude and latitude to UTM coordinates is performed through the above steps, a step of performing an expansion operation on each obstacle is also included.

[0102] The expansion operation is used to expand the real boundary of the obstacle outward, so that the obstacle after the expansion operation is enlarged to a certain extent compared to its original size. Using the obstacles after the expansion operation for route planning can leave a more sufficient safety distance for the high-speed unmanned boat to avoid obstacles.

[0103] As mentioned above, in the embodiments of the present application, the obstacle may be regarded as a polygon or a circle, and therefore, different expansion operation steps may be performed for the polygonal obstacle and the circular obstacle, respectively.

[0104] Specifically, when the obstacle is a polygon, the safety distance can be expanded outward along the vertical direction of the obstacle based on each side of the obstacle. , get the virtual edges corresponding to each edge, and then connect the virtual edges to complete the expansion operation of the polygonal obstacle. When the obstacle is circular, the original radius of the obstacle can be directly increased by Thereby achieving the expansion operation thereon.

[0105] Figure 5 In a specific embodiment, a polygonal obstacle is shown with a safe distance. Schematic diagram of the expansion operation (the dotted line is the boundary after expansion). Considering the need to leave enough space for the high-speed unmanned boat to turn without losing speed, the safe distance Preferably, it can be set to the turning radius corresponding to when the high-speed unmanned boat navigates at the minimum speed it needs to maintain.

[0106] In the present application, step S300 is used to convert the mission area, starting point, end point, various must-pass points and obstacles in the UTM coordinate system to the grid map coordinate system, so as to perform subsequent route planning processing on the grid map.

[0107] The reason for using a grid map for route planning is that obstacle avoidance requires the use of a gridding algorithm such as the A* algorithm described later. In some preferred embodiments of the present application, in order to comprehensively consider the planning time and planning precision, the grid resolution of the grid map can be adjusted based on the scale of the mission area. For example, two different gridding resolutions can be designed according to different mission area sizes. For a mission area with a length or width greater than or equal to 100 kilometers, the map gridding resolution is set to 500m per grid; for other cases, the map gridding resolution is set to 20m per grid, and the conversion from UTM coordinates to grid map coordinates is performed based on the following steps:

[0108] First calculate the resolution, height and width of the raster map:

[0109] ,

[0110] ,

[0111] ,

[0112] in, is the resolution of the raster map, H , W are the height and width of the grid map, , are the maximum and minimum values ​​of the horizontal coordinate of the mission area in the UTM coordinate system, , are the maximum and minimum values ​​of the vertical coordinates of the mission area in the UTM coordinate system, is the floor rounding function;

[0113] Then traverse each UTM coordinate point and calculate the grid coordinates corresponding to each UTM coordinate point:

[0114] ,

[0115] ,

[0116] in, , It is the horizontal and vertical coordinate values ​​of the UTM coordinate point on the grid map.

[0117] Generate the first route:

[0118] Step S400 is used to generate the first route. In the embodiment of the present application, the first route in the form of a broken line is obtained by connecting each necessary point with a straight line between the initial point and the end point according to a predetermined navigation order. Obviously, any turning point of the first route, that is, the intersection of any two adjacent segments (in the embodiment of the present application, the straight line connecting any two points is called a segment) is a necessary point. Therefore, the first route is the initial route without any optimization and obstacle avoidance processing.

[0119] As analyzed above, using the must-pass point as the turning point for the high-speed unmanned boat to turn will cause the unmanned boat to be unable to pass the must-pass point at high speed, or the turning radius at the must-pass point is too large. At the same time, the route section directly connecting each must-pass point may also collide with obstacles. Therefore, the first route needs to be optimized.

[0120] In the embodiment of the present application, starting from the first route, path expansion, local obstacle avoidance and obtuse processing are performed in sequence through steps S500 to S700 to obtain the final planned path. Steps S500 to S700 are described in detail below.

[0121] Path expansion processing:

[0122] Path expansion is used to expand the segments at one or both ends of each must-pass point as the center, and the expansion point after path expansion is used as the turning point of the segment. Since the must-pass point must be located on the segment formed after path expansion, it can ensure that the high-speed unmanned boat passes the must-pass point without reducing the speed. At the same time, the applicant found that different path expansion methods can be used for each segment on the first route according to whether the angle between the segments is an acute angle, so as to achieve a better path expansion.

[0123] In the embodiment of the present application, in step S500, path expansion processing is performed on each necessary point, specifically, as follows: Figure 6 As shown, for any must-pass point , perform the following steps:

[0124] S501, calculating the first route The angle between two segments connected at a point ,if If it is an acute angle, jump to step S502; otherwise, jump to step S504.

[0125] S502, The point is the midpoint, along the angle The expansion lengths on both sides of the vertical direction of the midline ,get The corresponding two expansion points , ,in, The lower limit is the speed at which the high-speed unmanned boat passes through the must-pass point Twice the turning radius.

[0126] Specifically, step S502 is used to expand the path of two adjacent segments that form an acute angle in the first route. Figure 7 In one embodiment, when passing through a certain point When the two flight segments J1 and J2 form an acute angle, a schematic diagram of path expansion is performed in step S502, as shown in FIG. Figure 7 As shown, after the path expansion, it must pass through the point Must be located , On the expansion path formed by the connection In the second route obtained by connecting the expansion paths, the original flight segments J1 and J2 will become three flight segments. , , Connection, it turns out that you must pass The turning point will become , The obtuse angle at the point allows the high-speed unmanned boat to pass the must-pass point at a higher speed. .

[0127] S503, judgment , Is the position legal? If so, , The line connecting the two points is the necessary point The expansion path ends at the point If not, then give up this step and get , And jump to step S504, wherein, when the position of any point is legal, the point is within the task area and is not within the range of any obstacle.

[0128] Step S503 is used to determine whether the result of path expansion for the acute angle segment falls into the obstacle range. If so, it means that the path expansion has failed. At this time, the process goes to step S504 to search for the expanded path together with the right angle and obtuse angle situations.

[0129] S504, the must-pass point As the midpoint, along Expand the length to both sides To get the necessary point The corresponding two potential expansion points , ,in For the first route in A waypoint on a segment before the point;

[0130] S505, this must-pass point As the center of rotation, , The connection line is rotated according to the preset angle Perform multiple rotations to get the required point multiple potential expansion paths;

[0131] S506: If at least one of the multiple potential expansion paths is legal, determine an optimal potential expansion path from the at least one legal potential expansion path as the necessary point. The corresponding expansion path, and the two endpoints of the expansion path are determined as the must-pass points The corresponding two expansion points , , then end the path expansion process for the must-pass point, otherwise shrink And return to step S504.

[0132] Specifically, Figure 8 As shown, in step S504, The point is extended in the positive and negative directions along the direction of its previous segment, so that point and one of two potential expansion points ( ) is located on its previous segment J1, another potential expansion point ( ) is located in the extension direction of the previous flight segment, and then in step S505, it is used as a potential expansion path, and As the center of the circle, the potential expansion path is rotated according to the angle Perform multiple rotations, and then in step S506, if Fig. 9 As shown in the figure, find the best potential expansion path from multiple potential expansion paths that do not fall into the obstacle range and do not exceed the task area. As a must-go point The corresponding expansion path, at this time, is The corresponding two expansion points , It was also confirmed that the new second route also consists of three segments at this location. , , Connection formed.

[0133] If S506 determines that no potential expansion path is legal, then it is necessary to reduce and search the expansion path again.

[0134] In some preferred embodiments, if the reduced The speed of the high-speed unmanned boat passing the must-pass point at its estimated speed The turning radius is twice that of the ship at that speed, indicating that the ship must pass through the point at that speed. There is a risk of collision with obstacles when the high-speed unmanned boat passes the necessary point. The estimated speed at The lower limit of is further reduced to ensure that the search can safely pass the must-pass point expansion path.

[0135] Through the above steps, we can get all the necessary points After the corresponding expansion path is obtained, the expansion points corresponding to each expansion path are connected end to end in the order of the high-speed unmanned boat passing, and the second route can be obtained. At this time, all the must-pass points are on each route segment and are not turning points.

[0136] Local obstacle avoidance:

[0137] In an embodiment of the present application, the local obstacle avoidance processing of step S600 is used to perform obstacle avoidance processing on the portion of the second route that intersects with the obstacle. The reason for using local obstacle avoidance rather than global obstacle avoidance is that the general global obstacle avoidance algorithm does not consider how to ensure that the unmanned boat passes through the must-pass points at high speed. Therefore, if the global obstacle avoidance algorithm is adopted, it may cause the subsequent processing algorithm to be more complicated. For this reason, in the present application, the segment at the must-pass point is first optimized through path expansion processing, and then local obstacle avoidance processing is only performed on the segment within the obstacle range in each optimized segment.

[0138] Specifically, in the embodiments of the present application, Fig.10 As shown, step S600 further includes the following steps:

[0139] S610, extracting all flight segments intersecting with obstacles from the second flight route;

[0140] S620, traverse each flight segment intersecting with an obstacle, and for each flight segment intersecting with an obstacle, execute steps S621 to S623 in sequence.

[0141] S621: Starting from the two intersection points of the flight segment and the obstacle, expand the length of the flight segment toward the outside of the obstacle. , get the obstacle avoidance starting point of this segment and obstacle avoidance endpoint ,in, The lower limit is twice the turning radius of the high-speed unmanned boat when it passes through the section at its expected speed;

[0142] S622, at the obstacle avoidance starting point and obstacle avoidance endpoint Execute the local A* algorithm to obtain the obstacle avoidance starting point and obstacle avoidance endpoint Multiple obstacle avoidance waypoints between;

[0143] S623, removing redundant waypoints from the plurality of obstacle avoidance waypoints, and sequentially connecting the obstacle avoidance starting point, the obstacle avoidance end point, and the remaining obstacle avoidance waypoints to obtain the flight segment after obstacle avoidance processing;

[0144] S630, connecting the segments after obstacle avoidance processing and other segments in the second route between the starting point and the ending point in sequence according to the passing order of the high-speed unmanned boat to obtain the third route.

[0145] Specifically, in the embodiment of the present application, in order to maintain the speed of the high-speed unmanned boat as much as possible, the present application only performs a safety turn on the portion of the segment intersecting with the obstacle where the high-speed unmanned boat cannot avoid the obstacle by safe steering (i.e., the distance to the boundary of the obstacle falls within the range of 100 km / h). local obstacle avoidance processing is performed.

[0146] In some optional embodiments, the above local avoidance processing can be performed using algorithms such as the A* algorithm well known to those skilled in the art. At the same time, in order to further optimize the obstacle avoidance path, such as Fig.11 As shown, in step S623, redundant waypoints in the plurality of obstacle avoidance waypoints may be removed in the following manner:

[0147] Set the obstacle avoidance starting point Connect each obstacle avoidance waypoint forward in sequence and determine whether the line connecting it and each obstacle avoidance waypoint intersects with the obstacle. Keep the obstacle avoidance waypoint corresponding to the last non-intersecting line, the obstacle avoidance starting point Each obstacle avoidance waypoint between the reserved obstacle avoidance waypoint is deleted as a redundant waypoint, and then the above operation is continued with the reserved obstacle avoidance waypoint as the starting point until the obstacle avoidance end point is reached. . Fig.12 The middle dotted line portion shows the new flight segment obtained after removing the redundant waypoints.

[0148] After performing the above operations, in step S630, the segments after obstacle avoidance processing in the second route and other segments are connected end to end between the starting point and the end point in the order of the high-speed unmanned boat passing, so as to obtain a third route after local obstacle avoidance processing.

[0149] Global blunting:

[0150] After the path expansion and local obstacle avoidance processing in the first two steps, the problem of the high-speed unmanned boat passing through the must-pass points and avoiding obstacles can be solved. However, at the same time, acute angle segments may be generated in each segment of the third route, that is, the high-speed unmanned boat turns at a sharp angle at two sub-segments connected in front and behind, for example, Fig.13 In the embodiment shown, due to the need to avoid obstacles, the local obstacle avoidance results in a sharp angle when the unmanned boat needs to bypass the obstacle. Therefore, it is also necessary to perform global obtuse processing on the third route through step S700 to meet the needs of high-speed navigation of the unmanned boat.

[0151] In some embodiments of the present application, step S700 specifically includes the following steps:

[0152] S710, extracting all acute-angle segments in the third route, wherein each acute-angle segment includes two sequentially connected sub-segments and the angle between the two sub-segments is an acute angle;

[0153] S720, traverse each acute angle segment, and execute step S721 for each acute angle segment;

[0154] S721, determine whether the lengths of the two sub-segments of the acute angle segment are both greater than And whether the angle between the two is greater than 60 degrees, if so, the acute angle segment is cut to make it obtuse, if not, the acute angle segment is expanded to make it obtuse, wherein, The lower limit is twice the turning radius of the high-speed unmanned boat when it passes through the acute angle section at its expected speed;

[0155] S730, connecting the acute angle segments and other segments in the third route that have been blunt-angled in the order of the high-speed unmanned boat passing between the starting point and the end point in sequence to obtain the planned route.

[0156] In step S721, for acute angle segments with relatively large angles, the shearing operation is preferentially used to simplify the complexity of the path as much as possible while ensuring the speed of the unmanned boat, such as Fig.14 As shown, in some preferred embodiments, the shearing operation for the acute angle segment greater than 60 degrees in step S721 is specifically as follows: taking the intersection of the two initial sub-segments (initial sub-segment 1 and initial sub-segment 2) of the acute angle segment as the starting point, cutting a segment less than 60 degrees from each of the two initial sub-segments. length to obtain two shear points , , delete the intersection of the two initial sub-segments of the acute angle segment, and connect , A new cut sub-segment is formed and connected end to end with the original initial sub-segment 1 and initial sub-segment 2, thereby obtaining the acute angle segment after the blunt angle processing (obviously, the acute angle segment has become a non-acute angle segment at this time).

[0157] For acute-angle segments with sharp angles or short lengths, even if the shearing operation is performed, the length of the obtained sheared sub-segment will be short, and it cannot be guaranteed that the unmanned boat can pass through at high speed without slowing down. Therefore, in the embodiment of the present application, it is necessary to expand the acute-angle segment so that the unmanned boat can pass through the segment while maintaining the original speed. Fig.15 As shown, in some preferred embodiments, the expansion operation of the acute angle segment less than or equal to 60 degrees in step S721 is specifically: starting from the intersection of the two initial sub-segments (initial sub-segment 1 and initial sub-segment 2) of the acute angle segment, continuously performing the extension-rotation operation to obtain multiple expansion points , and then connect the expansion sub-segments between the expansion points in sequence to obtain the acute angle segment after the obtuse angle treatment, wherein the first expansion point is located on the heading of one of the initial sub-segments (initial sub-segment 1), and in each extension-rotation operation, the extension length is not less than Furthermore, the included angle between the two expanded sub-segments obtained before and after the rotation is not less than 90°.

[0158] In some preferred embodiments, in order to achieve the high-speed unmanned boat returning to the original section by the optimal path while maintaining the original speed, in the expansion operation of step S721, Fig.15 As shown, the number of expansion points and expansion sub-segments are both 4, and the rotation angle of the second expansion sub-segment relative to the first expansion sub-segment is ( The rotation angles of the third expanded sub-segment relative to the second expanded sub-segment and the fourth expanded sub-segment relative to the third expanded sub-segment are both 90°, and the fourth expansion point Coincident with the intersection of the two initial sub-segments, the above setting can make the fourth expanded sub-segment remain perpendicular to the initial sub-segment 2, so that the high-speed unmanned boat can enter the initial sub-segment 2 from the initial sub-segment 1 with the minimum turning radius it can achieve without slowing down.

[0159] It should be noted that, in the embodiment of the present application, the obtuse angle processing in step S700 does not require that all angles of each newly generated sub-segment are obtuse angles, but can be performed in a better way without generating new acute angle segments. For example, Fig.15 After the expansion process of the illustrated embodiment, an obtuse turning angle and three right-angle turning angles are generated.

[0160] because Fig.15 The obtuse angle method shown in the figure expands the initial acute angle segment outward, so it is still possible to conflict with obstacles. Therefore, in some preferred embodiments, step S721 further includes the following steps:

[0161] If the result after the expansion operation is illegal (ie, at least one expansion path intersects with an obstacle), the expansion operation is performed again, and the first expansion point is located on the heading of another initial sub-segment. Fig.16 The expanded path formed based on the initial sub-segment 2 is shown. By using this step, it can be further ensured that the path expansion process will not cause new obstacle avoidance problems. Specific embodiment 1:

[0163] Specific embodiment 1 is used to illustrate the effect of the planning method provided by the present application. The path planning scene after the coordinate system conversion is as follows: Fig.17 As shown in the figure (the grid is no longer shown in the figure), the mission area is the range shown in the outermost red frame, each must-pass point is represented by a green dot, and obstacles are represented by multiple red circles and blue rectangles. Connect each must-pass point in sequence from the starting point to the end point according to the order in which the high-speed unmanned boat passes. Fig.17 The first route is shown by the green line.

[0164] The total length of the first route is 9.27km, but because the high-speed unmanned boat needs to pass through the necessary points at each turning point, it must slow down. At the same time, due to the existence of areas where it intersects with obstacles, the route cannot actually be implemented and requires subsequent optimization.

[0165] Figures 18 to 20 The second route after path expansion, the third route after local obstacle avoidance, and the final planned route after global obtuse processing (green lines in each figure) are shown respectively.

[0166] Although the length of the planned route obtained by the above steps increases to 10.86 km, it can ensure that the high-speed unmanned boat completes the entire route at a speed of no less than 25 knots, and the time taken is 14 minutes and 4 seconds.

[0167] As a comparison, Fig.21 The figure shows a comparison route (green line) obtained using the global A* algorithm. The total length of the comparison route is 9.98 km. Although it is shorter than the planned route obtained by the method of the present application, it can be seen from the figure that there are still multiple sharp-angle broken line segments on the comparison route, resulting in a speed reduction problem when the high-speed unmanned boat travels along the route, and the time taken is 15 minutes and 56 seconds.

[0168] It can be seen that the method provided by the present application can effectively ensure that the high-speed unmanned boat passes through each must-pass point while maintaining speed and safety, making the route planning more reasonable.

[0169] The above is a detailed introduction to the specific implementation methods of the present application. For those skilled in the art, several improvements and modifications may be made to the present application without departing from the principles of the present application. These improvements and modifications also fall within the scope of protection of the claims of the present application.

Claims

1. A route planning method for a high-speed unmanned boat, used to plan a route for a high-speed unmanned boat to pass through a number of must-pass points in sequence, characterized in that: The following steps are involved: S100, obtaining the mission area, the starting point, the ending point, several points that must be passed in sequence, and the longitude and latitude information of at least one obstacle of the high-speed unmanned boat; S200, converting the latitude and longitude information of the mission area, the starting point, the ending point, each must-pass point and the obstacle into UTM coordinates; S300, converting the UTM coordinates of the mission area, the starting point, the ending point, each must-pass point and the obstacle into grid map coordinates, and sequentially executing steps S400 to S700 on the grid map; S400, sequentially connecting the must-pass points between the starting point and the end point according to the passing order of the high-speed unmanned boat to obtain a first route; S500, traversingly performing path expansion processing on each of the must-pass points to obtain an expanded path corresponding to each of the must-pass points, and then sequentially connecting the expanded paths corresponding to each of the must-pass points between the starting point and the end point in the order in which the high-speed unmanned boat passes, to obtain a second route, wherein each of the must-pass points is located at the midpoint of its corresponding expanded path; S600, performing obstacle avoidance processing on the second route using a local A* algorithm based on the grid map coordinates of the obstacle to obtain a third route; S700, traversing each segment of the third route, performing obtuse processing on the third route, and obtaining a planned route for the high-speed unmanned boat to reach a terminal point after passing through a plurality of necessary points in sequence from a starting point; In step S500, path expansion processing is performed on each necessary point. Specifically, for any necessary point p, the following steps are performed: S501, calculating the angle ∠p formed by two segments connected at point p of the first route, if ∠p is an acute angle, jump to step S502, otherwise jump to step S504; S502, taking point p as the midpoint, expanding by a length length1 on both sides in a direction perpendicular to the midline of the angle ∠p, to obtain two expansion points pex1 and pex2 corresponding to point p, wherein the lower limit of length1 is twice the turning radius of the high-speed unmanned boat when it passes the must-pass point p at its estimated speed; S503, judging whether the positions of pex1 and pex2 are legal, if so, taking the line connecting pex1 and pex2 as the expansion path of the necessary point p, and ending the path expansion process of the necessary point p, if not, abandoning this step to obtain pex1 and pex2 and jumping to step S504, wherein, when the position of any point is legal, the point is within the task area and is not within the range of any obstacle; S504, taking the must-pass point p as the midpoint, expanding along p1→p to both sides by a length of length1 to obtain two potential expansion points pex'1 and pex'2 corresponding to the must-pass point p, wherein p1 is a waypoint on a segment of the first route before point p; S505, taking the necessary point p as the rotation center, rotating the line connecting pex'1 and pex'2 multiple times according to a preset rotation angle θ, to obtain multiple potential expansion paths of the necessary point p; S506, if at least one of the multiple potential expansion paths is legal, determine an optimal potential expansion path from the at least one legal potential expansion path as the expansion path corresponding to the must-pass point p, and determine the two endpoints of the expansion path as the two expansion points pex1 and pex2 corresponding to the must-pass point p, and then end the path expansion processing of the must-pass point, otherwise reduce length1 and return to step S504.

2. The route planning method for a high-speed unmanned boat according to claim 1, characterized in that: The grid resolution of the grid map is adjusted based on the size of the mission area.

3. The route planning method for a high-speed unmanned boat according to claim 1, characterized in that: Step S506 also includes the following steps: If the reduced length1 is less than twice the turning radius of the high-speed unmanned boat when passing the must-pass point p at its estimated speed, the estimated speed of the high-speed unmanned boat when passing the must-pass point p is reduced.

4. The route planning method for a high-speed unmanned boat according to claim 1, characterized in that: Step S600 further includes the following steps: S610, extracting all flight segments intersecting with obstacles from the second flight route; S620, traverse each flight segment intersecting with an obstacle, and for each flight segment intersecting with an obstacle, execute steps S621 to S623 in sequence. S621, taking the two intersection points of the route segment and the obstacle as the starting point, expanding the route segment to the outside of the obstacle by a length of length2, respectively, to obtain the obstacle avoidance starting point pex"1 and obstacle avoidance end point pex"2 of the route segment, wherein the lower limit of length2 is twice the turning radius of the high-speed unmanned boat when passing through the route segment at its expected speed; S622, executing a local A* algorithm between the obstacle avoidance starting point pex"1 and the obstacle avoidance end point pex"2 to obtain a plurality of obstacle avoidance waypoints between the obstacle avoidance starting point pex"1 and the obstacle avoidance end point pex"2; S623, removing redundant waypoints from the plurality of obstacle avoidance waypoints, and sequentially connecting the obstacle avoidance starting point, the obstacle avoidance end point, and the remaining obstacle avoidance waypoints to obtain the flight segment after obstacle avoidance processing; S630, connecting the segments after obstacle avoidance processing and other segments in the second route between the starting point and the ending point in sequence according to the passing order of the high-speed unmanned boat to obtain the third route.

5. The route planning method for a high-speed unmanned boat according to claim 1, characterized in that: Step S700 further includes the following steps: S710, extracting all acute-angle segments in the third route, wherein each acute-angle segment includes two sequentially connected initial sub-segments and the angle between the two initial sub-segments is an acute angle; S720, traverse each acute angle segment, and execute step S721 for each acute angle segment; S721, determining whether the lengths of the two initial sub-segments of the acute angle segment are both greater than length3 and whether the angle between the two sub-segments is greater than 60 degrees; if so, performing a shearing operation on the acute angle segment to make it obtuse; if not, performing an expansion operation on the acute angle segment to make it obtuse, wherein the lower limit of length3 is twice the turning radius of the high-speed unmanned boat when passing through the acute angle segment at its expected speed; S730, connecting the acute angle segments and other segments in the third route that have been blunt-angled in the order of the high-speed unmanned boat passing between the starting point and the end point in sequence to obtain the planned route.

6. The route planning method for a high-speed unmanned boat according to claim 5, characterized in that: The specific operation of cutting the acute angle segment in step S721 is as follows: Taking the intersection of the two initial sub-segments of the acute-angle segment as the starting point, cut a length less than length3 on the two initial sub-segments to obtain two cutting points pex"'1 and pex"'2, delete the intersection of the two initial sub-segments of the acute-angle segment and connect pex"'1 and pex"'2 to obtain the acute-angle segment after obtuse processing.

7. The route planning method for a high-speed unmanned boat according to claim 5, characterized in that: The expansion operation of the acute angle segment in step S721 is specifically as follows: Taking the intersection of the two initial sub-segments of the acute angle segment as the starting point, the extension-rotation operation is continuously performed to obtain multiple expansion points pex""1,...,pex"" n , and then connect the expanded sub-segments between the expansion points in turn to obtain the acute-angle segment after obtuse processing, wherein the first expansion point is located on the heading of one of the initial sub-segments, and in each extension-rotation operation, the extension length is not less than length3 and the angle between the two expanded sub-segments obtained before and after the rotation is not less than 90°.

8. The route planning method for a high-speed unmanned boat according to any one of claims 1 to 7, characterized in that: Step S200 also includes the step of performing an expansion operation on the obstacle.

Images