Global path planning and homeward voyage method based on satellite map

Abstract

The invention relates to the technical field of automatic driving global path planning, and particularly discloses a global path planning and homeward voyage method based on a satellite map, and a global path planning module in an unmanned vehicle automatic driving system, and the global path planning module comprises the steps of constructing a road network according to the satellite map, performing global planning according to task points, performing re-planning, and performing homeward voyage. The global path planning module is provided with an I / O data interface and can receive vehicle position information and send a global planning result. For the satellite map, the drivable road sections are manually distinguished, and the road sections are manually marked through related software; according to a breadth-first algorithm, the optimal driving route is solved according to the occurrence sequence of the task points until the vehicle smoothly arrives at the task end point; after the vehicle arrives at the task end point, the program allows to select two return flight modes, i.e., return flight on the original path and return flight on the nearest path; the driving road network is drawn based on the satellite map, global path planning and homeward voyage are achieved, and the method is high in adaptability, low in operation difficulty and low in hardware requirement.

Description

Technical Field

[0001] This invention relates to the field of global path planning technology for autonomous driving, and specifically to a global path planning and return method based on satellite maps. Background Technology

[0002] At present, some listed companies have relatively mature civilian autonomous driving technology, which can provide relatively safe and reliable control solutions for vehicles on urban structured roads.

[0003] In civilian autonomous driving systems, such as Figure 1 As shown, the implementation of global path planning relies on high-precision map information, which is generally collected and marked by real vehicles. This high-precision map records detailed information such as distance, road conditions, and road signs, serving as an important reference factor for local planning. Combined with various positioning and perception modules, it comprehensively acquires real-time vehicle information, completes local planning and decision-making, and then generates control signals to send operation commands to the vehicle chassis, thus forming the general framework of autonomous driving.

[0004] Disadvantages of existing technology:

[0005] Existing technologies are relatively mature, possessing comprehensive technical reserves in positioning, perception, decision-making, and control, but all rely on manually collected high-precision map data. While these solutions are suitable for structured urban roads and can ensure the accuracy and safety of autonomous driving, they are unsuitable for the operational environment of military vehicles. For military unmanned driving applications, the complex and rugged off-road environment of battlefields and the special needs of material transfer, storage, and transportation must be considered. Civilian global planning technologies based on high-precision maps require preprocessing work such as data collection, annotation, and correction of the driving environment, making them unsuitable for global path planning in unknown environments. A more reasonable and feasible technical solution is needed for specific situations.

[0006] Therefore, it is necessary to design a global path planning and return method based on satellite maps to solve the problem that high-precision maps cannot be obtained in existing technologies. Summary of the Invention

[0007] In view of the problems existing in the prior art, the purpose of this invention is to provide a global path planning and return method based on satellite maps.

[0008] The technical solution adopted by this invention to solve its technical problem is: a global path planning and return method based on satellite maps, specifically including the following steps:

[0009] S1, a global path planning module in an automatic driving system of an unmanned vehicle, the global path planning module comprising constructing a road network according to a satellite map, globally planning according to task points, re-planning, and returning, the global path planning module having an I / O data interface, capable of receiving vehicle position information and sending global planning results;

[0010] S2, manually distinguishing drivable road segments from the satellite map, and marking the road segments by using related software, the step directly affecting the success of subsequent real vehicle testing;

[0011] S3, after the vehicle receives unmanned driving instructions, the program reads in road network information and given task point information, and solves an optimal driving route according to the order of the task points appearing according to a breadth-first algorithm, the program continuously receives vehicle position information during the driving of the vehicle, and updates vehicle control data in real time until the vehicle successfully reaches a task end point;

[0012] S4, after the vehicle reaches the task end point, an interactive box is popped up on a display interface, and the program allows selection of two return modes, i.e., returning by the original route and returning by the nearest route.

[0013] Preferably, the manually marked driving route in step S2 needs to be located as much as possible in the center of the road, and the calibration road segment needs to be processed by related software to complete point densification, smooth extraction, and correction, and the two roads at each intersection need to be disconnected to become an independent road, and the end point coordinates of the two connected roads need to be consistent.

[0014] Preferably, the driving route is a complex off-road route, and the main geographic information including drivable roads, large obstacles that significantly affect driving, and branches is manually distinguished, the road network is manually drawn, the software is used to complete point densification, smooth extraction, and correction of independent points, and the software calculates accurate longitude and latitude coordinate information of all road network points given the longitude and latitude coordinates of a plurality of points on the satellite map.

[0015] Preferably, in the solving of the optimal driving route in step S3, if there is a drivable route, the route is marked on a road network display interface, if there are special cases where the given task points cannot be connected to each other, only the task points are marked on the road network display interface, the program allows mouse dragging to fine-tune the position of the task points until a reasonable planning route is displayed, for the corner at the intersection of two road segments in the route, a Bezier curve is used for smooth processing to facilitate the turning action of the vehicle, and at the same time, the program receives the current position information of the vehicle, when the nearest distance from the vehicle to the road network is not more than 20 meters, the nearest point relative to the current position of the vehicle is marked on the road network, and according to the obtained route information, the longitude and latitude coordinates of the next 200 points are given in order and sent to a local planning module as a basis for vehicle driving control.

[0016] Preferably, the local planning module is matched with a local planning, vehicle control and perception module, which cooperate with each other to finally be loaded on the vehicle to complete automatic driving control, and the communication among the local planning, vehicle control and perception module is realized based on ROS.

[0017] Preferably, the task point information in the step S3 is composed of the sequence number, latitude and longitude coordinates, speed and task attribute of the vehicle in sequence, and the program of the task point information uses a kd-tree nearest neighbor search algorithm to search the task point coordinates based on all road network points, and when the nearest distance from the task point to all road network points is not greater than 20 meters, the nearest neighbor point of the task point in all road network points is solved, which is a point that the vehicle must pass through, and the sequence number, speed and various attributes of the task of the task point are assigned to the nearest neighbor point.

[0018] Preferably, the width-first algorithm in the step S3 is to find the passable edge to the next task point each time from the task starting point, and the minimum cost edge is calculated as the planning route of the segment, and then the process is sequentially performed backward until the task ending point.

[0019] Preferably, the original route return in the step S3 is to arrange the task points in reverse order, and then use the width-first algorithm to solve the return route, when the nearest route return is selected, only the original task ending point is taken as the return starting point, and the original task starting point is taken as the return ending point, and the A* algorithm is used to solve the shortest return route, for the case that the return starting direction angle and the direction angle of the vehicle when arriving at the ending point are greater than 180 degrees, the program automatically takes four points near the position of the vehicle, generates a Bezier curve, and samples the points as the basis for the vehicle to turn around autonomously, and the two task points of the Bezier curve can be manually corrected by mouse dragging, and the program continuously receives the vehicle position information during the vehicle driving process, and the vehicle control data is updated in real time until the vehicle successfully returns to the task starting point.

[0020] Preferably, the comprehensive cost of the A* algorithm is composed of the actual length of the route passed and the Euclidean distance from the current position to the target ending point, and the route generated by triggering the nearest route return meets the strict shortest.

[0021] Preferably, the application program of the mouse dragging task point is built based on Qt, and the human-computer interaction is completed in the form of signals and keys.

[0022] The application has the following beneficial effects:

[0023] The global path planning and return method based on the satellite map designed in the application solves the problem that the automatic driving vehicle cannot drive on the off-road road in unknown environment, draws the driving road network based on the satellite map, and then realizes the global path planning and return, and among various existing processing methods, the application has the advantages of strong adaptability, low operation difficulty, low hardware requirement and the like.

[0024] The satellite map-based global path planning and return method designed by the application does not need to collect high-precision road networks manually, but forms a road network by manually marking drivable areas on a satellite map. An unmanned platform refers to relevant mandatory task points on the basis of the road network, and performs global path planning through a breadth-first algorithm. The return function can be triggered after reaching the target endpoint. The unmanned platform uses a breadth-first algorithm to obtain the original road return path or uses an A* algorithm to obtain the nearest return path, which can provide important reference for the transfer, storage and transportation of war materiel in complex unknown battlefield environments. BRIEF DESCRIPTION OF DRAWINGS

[0025] Figure 1 is a general framework diagram of an unmanned system.

[0026] Figure 2 is a logic diagram of signal exchange between the vehicle control module, global planning module and local planning module of the application.

[0027] Figure 3 is a flowchart of the basic logic of the satellite map-based global path planning.

[0028] Figure 4 is a return flowchart of the satellite map-based global path planning and return method. DETAILED DESCRIPTION

[0029] The technical solutions in the embodiments of the application will be further described in detail with reference to the accompanying drawings of the embodiments of the application. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of the application.

[0030] Key term definition:

[0031] Road network - in a certain area, a road system composed of various roads interconnecting and interweaving into a network distribution, in this patent, the road network is represented as a drivable trajectory topology displayed by a series of latitude and longitude coordinate points. Re-planning - in the process of action, unexpected situations occur, and action planning needs to constantly understand the environment and plan actions. In this patent, it specifically represents that obstacles cannot be avoided in the planning road, and a new path is obtained by re-planning.

[0032] Example 1.

[0033] As Figures 2-4As shown, a satellite map-based global path planning and return method is based on the battlefield cross-country environment, and is a global path planning module in an automatic driving system of an unmanned vehicle. The global path planning module includes several main parts of constructing a road network according to a satellite map, global planning according to a task point, re-planning, and returning. The global path planning module has an I / O data interface, can receive vehicle position information, and send a global planning result. A human-computer interaction interface is programmed, and real-time correction of planning requirements can be realized.

[0034] The map, road network, and task file of the global path planning are saved in a program subordinate file system in a txt format as a configuration file, are read in when the program is started, and the road network and the task file can be modified and saved through the human-computer interaction interface in the program. The global path planning module constructs an I / O data interface through ROS, receives a vehicle position signal sent by a vehicle control module, finds a corresponding nearest position of the vehicle in the road network as a starting reference point of sending a vehicle guiding path according to the data through a kd-tree nearest neighbor search algorithm, and when the vehicle is greater than 20 m away from any position in the road network, it is considered that the distance from the road network is too far, and the guiding path is not issued temporarily. When there is a nearest point of the current position of the vehicle in the road network, 200 points are intercepted in a forward direction along the direction of the global planning overall path from the point, and are sent to a local planning module as a basis for local planning. At the same time, when the vehicle passes through a key task point, relevant speed, task attribute, and other information are also transmitted to the local planning module, and the above information is updated in real time with the movement of the vehicle position. The local planning module plans a guiding path in a small range in front of the vehicle according to the global planning information and perception information, and when an obstacle cannot be avoided in front of the vehicle, the vehicle will be reversed after a certain period of time until it leaves the blocked road. For the global path planning, the road where the current vehicle is located is marked as a blocked road, and the blocked road is removed from the global road network, and the planning algorithm is triggered again to obtain a new guiding path, which is the implementation principle of the re-planning function.

[0035] For the satellite map, the drivable road section is manually distinguished, and the road section is manually marked through related software. This step directly affects the success or failure of subsequent real vehicle testing. The manually marked driving road section needs to be located on the road as much as possible, and the calibration road section needs to be completed through related software, including point densification, smoothing extraction, and correction. It is required that the two roads between the crossroads are disconnected from each other to become an independent road, and the endpoint coordinates of the connected two roads are consistent.

[0036] The driving road section is a complex cross-country road, and the main geographical information including the passable road, the large obstacle that obviously affects driving, and the branch road is manually distinguished. The road network is manually drawn, the point densification and smoothing extraction are completed with the help of software, the independent points are corrected, and the longitude and latitude coordinates of a plurality of points on the satellite map are given. The software can calculate the accurate longitude and latitude coordinate information of all road network points.

[0037] After the vehicle receives the unmanned driving instruction, the program reads in the road network information and the given task point information, and solves the optimal driving route according to the order of the task points according to the breadth-first algorithm. During the driving process of the vehicle, the program continuously receives the vehicle position information, and updates the vehicle control data in real time until the vehicle successfully arrives at the task end point. If there is a passable route in the solved optimal driving route, the route is marked on the road network display interface. If there is a special case that the given task points cannot be connected to each other, only the task points are marked on the road network display interface. The program allows the mouse to be dragged to fine-tune the position of the task point until a reasonable planning route is displayed. For the corner at the intersection of two roads in the route, a Bezier curve is used for smoothing processing to facilitate the vehicle to complete the turning action. At the same time, the program receives the current position information of the vehicle. When the nearest distance from the vehicle to the road network is not more than 20 meters, the nearest point relative to the current position of the vehicle is marked on the road network, and the latitude and longitude coordinates of the first 200 points in front are given in order according to the obtained route information, and are sent to the local planning module as the basis for vehicle driving control.

[0038] After the vehicle arrives at the task end point, an interactive box is popped up on the display interface, and two kinds of return modes, i.e. the original route return and the nearest route return, are allowed to be selected. If the original route return is selected, the task points are arranged in reverse order, and the return route is solved again by using the breadth-first algorithm. If the nearest route return is selected, only the original task end point is taken as the return starting point, and the original task starting point is taken as the return end point, and the shortest return route is solved by using the A* algorithm. For the case that the return starting direction angle is greater than 180° from the direction angle when the vehicle arrives at the end point, the program automatically takes four points near the position of the vehicle, generates a Bezier curve, and samples the points as the basis for the vehicle to turn around. The two task points of the Bezier curve can be dragged by the mouse for manual correction of the turning route. During the driving process of the vehicle, the program continuously receives the vehicle position information, and updates the vehicle control data in real time until the vehicle successfully returns to the task starting point.

[0039] Embodiment two, the technical solutions of the present application are described in detail with specific examples.

[0040] 1. The invention mainly uses three algorithms: kd-tree nearest neighbor search algorithm, taking all road network points as a whole, completing tree building, traversing each task point to find the nearest neighbor point, marking the found nearest neighbor point as a vehicle must pass point, and task points with a distance greater than 20 meters from the road network are considered too far and are not marked. BFS breadth-first search algorithm, the connection relationship of the road in the road network is represented in the road network file as connected roads containing road points with the same coordinates. According to this attribute, the connection relationship of each road is established, including the road number connected and whether the connection is the start or end of the road. Finally, it is represented as map{(id, start)(id, start)…(id, end)}. Combined with the road topological connection relationship, BFS algorithm is applied from the start point (sequence number 0) to find the connection between the start point and the first sub-target point with sequence number 1, and then sequentially to the end. That is, global path planning is realized. a_star algorithm, as above, for the existing road topological connection relationship, a_star algorithm is used, the actual length of the road already passed is the actual g_cost, and the Euclidean distance to reach the road is the expected cost h_cost, and the sum of the two is the total cost a_cost. Starting from the start point, gradually approaching the end point, the actual nearest global driving guide path is obtained.

[0041] 2. The program combines the actual application requirements to design several practical functions. Task point addition, deletion and modification: the program is realized based on Qt, in the human-computer interaction system, the position of the task point can be changed by holding the left mouse button and dragging the task point position to change the position of the key point passed by the vehicle, the task attribute can be modified to some specified options by selecting the task point, and unnecessary task points can be deleted by pressing the DELETE key on the keyboard. The above operations can be performed synchronously during driving. Road network correction: in actual driving, if it is detected that a certain road network deviates due to drawing precision and other problems, the mouse can be dragged to frame the offset position of the road network while pressing the ALT key on the keyboard. After framing, pressing the up, down, left and right keys can correct the road network. Return mode selection: the program designs two return modes: original road return and nearest road return. The interface pops up the mode selection human-computer interaction button when the vehicle is about 5 meters away from the end point, waits for the program to control the vehicle to stop at the end point, selects any return mode, and if the difference between the current heading of the vehicle and the expected heading of the return is greater than 180°, the vehicle will trigger the U-turn program. The U-turn trajectory is fitted by a Bezier curve, which can be interacted with the mouse to modify the actual U-turn trajectory by dragging the control points of the Bezier curve while pressing the SHIFT key on the keyboard. During this process, the vehicle always remains stationary. After all adjustments are completed, press the right mouse button to start the vehicle and implement the return according to the selected scheme.

[0042] A kind of global path planning and return technology for complex battlefield environment, not relying on high-precision road network information completion, mainly for the material transfer strategy of complex battlefield cross-country environment.It does not need to collect high-precision road network manually, but forms a road network by manually marking drivable area on satellite map, and on the basis of the road network, referring to relevant must-pass task points, global path planning is carried out by breadth-first algorithm, and return function can be triggered after reaching the target terminal, and unmanned platform can use breadth-first algorithm to obtain the original road return path or obtain the nearest return path by A* algorithm.The present application can provide important reference for the transfer, storage, transportation and other needs of war materiel in complex unknown battlefield environment.

[0043] The present application is not limited to the above-mentioned embodiments, and any person should know that the structural changes made under the inspiration of the present application fall within the scope of protection of the present application.

[0044] The techniques, shapes and configurations not described in detail in the present application are well-known techniques.

Claims

1. A global path planning and return method based on a satellite map, characterized in that, Specifically comprising the following steps: S1, a global path planning module in the automatic driving system of the unmanned vehicle, the global path planning module comprising constructing a road network according to a satellite map, globally planning according to task points, re-planning, and returning, the global path planning module having an I / O data interface, capable of receiving vehicle position information and sending global planning results; S2, manually distinguishing drivable road segments from the satellite map, and marking the road segments by using related software, the step directly affecting the success or failure of subsequent real vehicle testing; S3, after the vehicle receives the unmanned driving instruction, the program reads in the road network information and the given task point information, and solves the optimal driving route according to the order of the task points in the breadth-first algorithm, during the driving of the vehicle, the program continuously receives the vehicle position information, and updates the vehicle control data in real time, until the vehicle successfully arrives at the task end point; S4, after the vehicle arrives at the task end point, an interactive box is popped up on the display interface, and the program allows selection of two return modes, i.e., returning by the original route and returning by the nearest route. 2.The satellite map based global path planning and return method according to claim 1, wherein, The manually marked driving route in the step S2 needs to be located as much as possible in the road center, and the calibration road segment needs to be completed by related software, including point densification, smooth extraction, and correction, and the two cross roads are required to be disconnected from each other to become an independent road, and the end point coordinates of the two connected roads are consistent. 3.The satellite map based global path planning and return method of claim 2, wherein, The driving route is a complex off-road surface, and the main geographic information including drivable roads, large obstacles that obviously affect driving, and branches are manually distinguished, the road network is manually drawn, the software is used to complete point densification, smooth extraction, and correction of independent points, and the software can calculate accurate longitude and latitude coordinate information of all road network points given longitude and latitude coordinates of a plurality of points on the satellite map. 4.The satellite map based global path planning and return method of claim 1, wherein, If there is a drivable route in the solved optimal driving route in the step S3, the route is marked on the road network display interface, if there is a special case that the given task points cannot be connected to each other, only the task points are marked on the road network display interface, the program allows mouse dragging to fine-tune the task point position until a reasonable planning route is displayed, for the corner at the intersection of the two road segments in the route, a Bezier curve is used for smooth processing to facilitate the turning action of the vehicle; at the same time, the program receives the current position information of the vehicle, when the distance between the vehicle and the road network is not more than 20 meters, the nearest point relative to the current position of the vehicle is marked on the road network, and according to the obtained route information, the longitude and latitude coordinates of the front 200 points are given in order and sent to the local planning module as the basis for vehicle driving control.

5. The satellite map based global path planning and homing method of claim 4, wherein, The local planning module has a matching local planning module, a vehicle control module, and a perception module, which cooperate with each other to finally be loaded on the vehicle to complete automatic driving control, and the communication between the local planning module, the vehicle control module, and the perception module is realized based on ROS. 6.The satellite map based global path planning and homing method of claim 1, wherein, The task point information in the step S3 is composed of the sequence number, the longitude and latitude coordinates, the speed, and the sequence of the task attributes that the vehicle needs to pass through in turn, and the program of the task point information uses the kd-tree nearest neighbor search algorithm to search the task point coordinates based on all road network points as the main body, and when the nearest distance from the task point to all road network points is not greater than 20 meters, the nearest neighbor point of the task point in all road network points is solved to be a necessary point for the vehicle, and the sequence number, the speed, and the various attributes of the task of the task point are assigned to the nearest neighbor point. 7.The satellite map based global path planning and homing method of claim 1, wherein, The width-first algorithm in the step S3 is to find the passable edge to the next task point from the task starting point each time, and the minimum cost edge is calculated to be the planning route, and the process is repeated until the task ending point. 8.The satellite map based global path planning and homing method of claim 1, wherein, The original route return in the step S3 is to arrange the task points in reverse order, and then use the width-first algorithm to solve the return route, and when the nearest route return is selected, only the original task ending point is taken as the return starting point, and the original task starting point is taken as the return ending point, and the A* algorithm is used to solve the shortest return route; for the case that the return starting direction angle and the vehicle arrival ending direction angle are different by more than 180°, the program automatically takes four points near the vehicle position to generate a Bezier curve, and the sampling points are used as the basis for the vehicle to turn around autonomously, and the two task points of the Bezier curve can be dragged by the mouse to manually correct the turning route; During the vehicle driving process, the program continuously receives the vehicle position information, and updates the vehicle control data in real time until the vehicle successfully returns to the task starting point. 9.The satellite map based global path planning and return method of claim 8, wherein, The comprehensive cost of the A* algorithm is composed of the actual length of the passed route and the Euclidean distance from the current position to the target ending point, and the route triggered by the nearest route return meets the strict shortest.

10. The satellite map based global path planning and homing method of claim 8, wherein, The application program of the mouse dragging task point is based on Qt, and the signal and key mode is used to complete the human-computer interaction.

Images