Emergency command vehicle path planning method and system, vehicle-mounted terminal and storage medium
By combining Floyd and A* algorithms in emergency command vehicle path planning, the path is dynamically optimized, and the problem of insufficient real-time and flexibility of traditional methods in emergency scenarios is solved, achieving more efficient path adjustment and rescue efficiency.
Patent Information
- Application Number
- CN202411042358.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-31
- Publication Date
- 2024-12-06
AI Technical Summary
Traditional path planning methods have poor real-time performance when dealing with complex and changing emergency scenarios, making them difficult to adapt to changes in dynamic environments, and are not considered enough for multiple factors, resulting in limited real-time dynamic adjustment capabilities of path planning, poor flexibility, and difficult to take into account dynamic global planning and Local path optimization.
By obtaining the global road information of traffic roads and the driving information of vehicles, using a search algorithm to determine the feasible path point set in the road network diagram, and recursively optimized based on the distance matrix and path matrix to generate the target driving path from the current point to the target point , dynamically avoid obstacle points, combine Floyd algorithm and A* algorithm to optimize path planning.
It enhances the real-time and reliability of path planning, improves the rescue efficiency of emergency command vehicles in emergencies, and can adjust the path in time when obstacles appear, ensuring that the obtained target driving path is globally optimal.
Smart Images

Figure CN119085672A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of special vehicle path planning, and in particular to a path planning method, system, vehicle-mounted terminal and storage medium for an emergency command vehicle. Background Art
[0002] The frequent occurrence of emergencies such as natural disasters, fires, and production accidents pose a serious threat to people's lives and property. In order to respond quickly and effectively to such incidents, the emergency command vehicle plays an important role as a mobile command center. The emergency command vehicle is a mobile command center specially used to deal with various emergencies. The vehicle needs to be able to move flexibly and deeply into every corner of the city to handle and resolve emergencies in a timely and effective manner. In order for the emergency command vehicle to arrive at the scene of the incident as soon as possible, it is necessary to use intelligent driving technology to plan an optimal path. Path planning is one of the key technologies to improve the rescue efficiency of the emergency command vehicle.
[0003] However, traditional path planning methods have obvious shortcomings when dealing with complex and changeable emergency scenarios, which are mainly reflected in the following aspects: poor real-time performance, a single path planning algorithm is difficult to adapt to dynamic environmental changes, such as sudden road obstacles or traffic jams, resulting in limited real-time dynamic adjustment capabilities of path planning; insufficient consideration of multiple factors, the selection of the optimal path is often based only on a single indicator of distance or time, while ignoring the influence of multiple factors such as the number of road intersections, road conditions, traffic congestion, and temporary obstacles, resulting in low actual driving efficiency; poor flexibility, a single path planning algorithm is difficult to take into account dynamic global planning and local path optimization, especially in the special application scenario of emergency command vehicles, which requires more flexible and intelligent path planning strategies.
[0004] Therefore, it is necessary to provide an intelligent path planning solution that can comprehensively consider multiple factors and has high real-time performance. Summary of the invention
[0005] In view of the above-mentioned shortcomings of the prior art, the present invention discloses a path planning method, system, vehicle-mounted terminal and storage medium for an emergency command vehicle, which are used to solve at least one of the above-mentioned technical problems.
[0006] In the first aspect, the present application provides a path planning method for an emergency command vehicle, which obtains global road information of traffic roads and driving information of vehicles, wherein the global road information includes a road network diagram and a distance matrix and a path matrix corresponding to the road network diagram, the distance matrix includes the shortest weighted distance between any two nodes, the path matrix includes the shortest path nodes between any two nodes, and the driving information includes an initial shortest path from a starting point to a target point; if an obstacle point is detected in the initial shortest path, the position of the vehicle is determined as the current point; a node search is performed in the road network diagram according to a search algorithm to determine a set of feasible path points from the current point to the target point; based on the distance matrix and the path matrix, each node in the feasible path point set is recursively optimized to generate a target driving path from the current point to the target point.
[0007] In one embodiment of the present invention, the node search is performed in the road network graph according to the search algorithm to determine a set of feasible path points from the current point to the target point, including: searching for candidate nodes from the current point to the next node in the road network graph, and removing the obstacle points and the traveled nodes from the candidate nodes; for each remaining candidate node, according to the cost function, calculating the first cost from the current point to the target point, and determining the remaining candidate node corresponding to the minimum cost as the first feasible path node; continuing to search and determine the second feasible path node corresponding to the first feasible path node until the next node is the target point and stopping the search; arranging the current point, each feasible path node and the target point in sequence to generate the feasible path point set.
[0008] In one embodiment of the present invention, the recursive optimization of each node in the feasible path point set is performed based on the distance matrix and the path matrix, including: determining multiple adjacent node pairs according to the arrangement order of the nodes in the feasible path point set; determining the first path length of each adjacent node pair according to the distance matrix, and calculating the second path length of each adjacent node pair according to the cost function; comparing the first path length with the second path length, and determining the adjacent node pairs whose first path length is less than the second path length as optimizable node pairs; reconstructing the target driving path between the optimizable node pairs according to the path matrix, and replacing all the optimizable node pairs in the feasible path point set with the target driving path.
[0009] In one embodiment of the present invention, the generating of the target driving path from the current point to the target point includes: determining whether the recursive current point is the penultimate node in the feasible path point set; if so, generating the target driving path; if not, determining whether the recursive target point is the last node in the feasible path point set; if not, moving the recursive current point back one node toward the feasible path point set and continuing to perform recursive optimization until the target driving path is generated when the recursive current point is the penultimate node in the feasible path point set; if so, moving the recursive current point back one node toward the feasible path point set, setting the recursive target point as the first node in the feasible path point set, and moving the recursive target point back one node toward the feasible path point set and continuing to perform recursive optimization until the target driving path is generated when the recursive current point is the penultimate node in the feasible path point set.
[0010] In one embodiment of the present invention, before continuing to search to determine the second feasible path node corresponding to the first feasible path node, it also includes: determining the number of remaining candidate nodes corresponding to the first feasible path node, and comparing the number with a preset number threshold; if the number is less than the number threshold, returning to the previous node of the first feasible path node, and re-determining the third drivable path node corresponding to the previous node.
[0011] In one embodiment of the present invention, the method for determining the distance matrix and the path matrix includes: obtaining the path length, road condition and traffic congestion level of the road section corresponding to each edge in the road network graph, and determining the weighted distance of each edge according to the path length, the road condition, the traffic congestion level and the preset weight coefficient; creating a matrix according to each node in the road network graph, and determining the first node pair directly connected and the second node pair not directly connected in the matrix; assigning distance weights to each of the node pairs according to the weighted distance to obtain a first matrix, and assigning path points to each of the node pairs to obtain a second matrix; traversing each node in the road network graph as an intermediate point, and updating the first matrix with the shortest weighted distance according to the calculation result of the state transfer formula to obtain the distance matrix; when the shortest weighted distance of any node pair is updated, the path points of the arbitrary node pair are updated according to the corresponding intermediate point to obtain the path matrix.
[0012] In one embodiment of the present invention, the method for determining the initial shortest path includes: taking the starting point and the target point as a third node pair, determining from the path matrix a first shortest path node on the shortest path corresponding to the starting point; continuing to take the first shortest path node and the target point as a fourth node pair, determining from the path matrix a second shortest path node on the shortest path corresponding to the first shortest path node; until the second shortest path node is the target point, arranging the starting point, each of the shortest path nodes and the target point in sequence to generate the initial shortest path.
[0013] In the second aspect, the present application provides an emergency command vehicle path planning system, the system comprising: an information acquisition module, used to acquire global road information of traffic roads and driving information of vehicles, wherein the global road information includes a road network diagram and a distance matrix and a path matrix corresponding to the road network diagram, the distance matrix includes the shortest weighted distance between any two nodes, the path matrix includes the shortest path node between any two nodes, and the driving information includes an initial shortest path from a starting point to a target point; an obstacle point detection module, used to determine the position of the vehicle as the current point if an obstacle point is detected in the initial shortest path; a feasible path determination module, used to perform node search in the road network diagram according to a search algorithm to determine a set of feasible path points from the current point to the target point; a path optimization module, used to recursively optimize each node in the feasible path point set according to the distance matrix and the path matrix to generate a target driving path from the current point to the target point.
[0014] In a third aspect, a vehicle-mounted terminal is provided, characterized in that the vehicle-mounted terminal comprises: one or more processors; a storage device for storing one or more programs, and when the one or more programs are executed by the one or more processors, the vehicle-mounted terminal implements the emergency command vehicle path planning method described in the first aspect.
[0015] In a fourth aspect, the present application provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor of a computer, the computer executes the emergency command vehicle path planning method described in the first aspect.
[0016] As described above, the emergency command vehicle path planning method, system, vehicle-mounted terminal and storage medium provided by the embodiments of the present invention have the following beneficial effects:
[0017] By acquiring global road information of traffic roads and driving information of vehicles, the global road information includes a road network diagram and a distance matrix and a path matrix corresponding to the road network diagram, and the driving information includes an initial shortest path from a starting point to a target point. When an obstacle point is detected in the initial shortest path, the position of the vehicle is determined as the current point, and a node search is performed in the road network diagram according to a search algorithm to determine a feasible path point set from the current point to the target point. Finally, each node in the feasible path point set is recursively optimized according to the distance matrix and the path matrix to generate a target driving path from the current point to the target point. When an obstacle is detected, a feasible path is first dynamically planned according to a search algorithm to avoid the obstacle point, and then a suboptimal path in the feasible path is replaced by a recursive search of a local optimal path. Finally, a global optimal path is obtained when the obstacle point blocks the current road, thereby enhancing the reliability of the obtained target driving path.
[0018] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS
[0019] The drawings herein are incorporated into the specification and constitute a part of the specification, showing embodiments consistent with the present application, and together with the specification, are used to explain the principles of the present application. Obviously, the drawings described below are only some embodiments of the present application, and for those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative work. In the drawings:
[0020] Figure 1 is a schematic diagram of an implementation environment of an emergency command vehicle path planning system shown in an exemplary embodiment of the present application;
[0021] Figure 2 is a flow chart of a method for path planning of an emergency command vehicle shown in an exemplary embodiment of the present application;
[0022] Figure 3 is a schematic diagram of a road network diagram shown in an exemplary embodiment of the present application;
[0023] Figure 4 is a schematic diagram of an initial shortest path shown in an exemplary embodiment of the present application;
[0024] Figure 5 is a schematic diagram of a target driving path shown in an exemplary embodiment of the present application;
[0025] Figure 6 is a flowchart of a specific emergency command vehicle path planning method shown in an exemplary embodiment of the present application;
[0026] Figure 7is a block diagram of a path planning system for an emergency command vehicle shown in an exemplary embodiment of the present application;
[0027] Figure 8 It is a schematic diagram of the structure of a vehicle-mounted terminal shown in an exemplary embodiment of the present application. DETAILED DESCRIPTION
[0028] The following will describe the embodiments of the present invention with reference to the accompanying drawings and preferred embodiments. Those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be understood that the preferred embodiments are only for illustrating the present invention, not for limiting the scope of protection of the present invention.
[0029] It should be noted that the illustrations provided in the following embodiments are only used to illustrate the basic concept of the present invention in a schematic manner, and thus the illustrations only show components related to the present invention rather than being drawn according to the number, shape and size of components in actual implementation. In actual implementation, the type, quantity and proportion of each component may be changed arbitrarily, and the component layout may also be more complicated.
[0030] In the following description, numerous details are discussed to provide a more thorough explanation of the embodiments of the present invention. However, it is obvious to those skilled in the art that the embodiments of the present invention can be implemented without these specific details. In other embodiments, well-known structures and devices are shown in the form of block diagrams rather than in detail to avoid making the embodiments of the present invention difficult to understand.
[0031] For emergency command vehicles, in order to arrive at the scene of an incident as soon as possible when facing an emergency, it is often necessary to use intelligent driving technology to plan an optimal path to improve the rescue efficiency of the emergency command vehicle. However, the inventors of this application have found that the traditional path planning method has obvious shortcomings when dealing with complex and changeable emergency scenarios, which are mainly reflected in the following aspects: poor real-time performance, a single path planning algorithm is difficult to adapt to dynamic environmental changes, such as sudden road obstacles or traffic congestion, resulting in limited real-time dynamic adjustment capabilities of path planning; insufficient consideration of multiple factors, the selection of the optimal path is often based only on a single indicator of distance or time, and ignores the influence of multiple factors such as the number of road intersections, road conditions, traffic congestion, and temporary obstacles, resulting in low actual driving efficiency; poor flexibility, a single path planning algorithm is difficult to take into account dynamic global planning and local path optimization, especially in the special application scenario of emergency command vehicles, which requires a more flexible and intelligent path planning strategy.
[0032] Therefore, see Figure 1 , Figure 1 FIG. 1 is a schematic diagram of an implementation environment of an emergency command vehicle path planning system shown in an exemplary embodiment of the present application. Figure 1 As shown, the implementation environment includes a vehicle 110 and a path planning system 120, wherein the path planning system 120 is embedded in the vehicle 110 and is used to implement path planning of the vehicle 110. The path planning system 120 includes but is not limited to a vehicle computer system, an on-board computer, etc. When an obstacle is detected, a feasible path is first dynamically planned according to a search algorithm to avoid the obstacle point, and then a suboptimal path in the feasible path is replaced by a recursive search for a local optimal path, and finally a global optimal path is obtained when the obstacle point blocks the current road, thereby enhancing the reliability of the obtained target driving path.
[0033] See also Figure 2 , Figure 2 is a flowchart of an emergency command vehicle path planning method shown in an exemplary embodiment of the present application. The method can be applied to Figure 1 It should be understood that the method can also be applied to other exemplary implementation environments and be specifically executed by devices in other implementation environments, and this embodiment does not limit the implementation environment to which the method is applicable.
[0034] like Figure 2 As shown, in an exemplary embodiment, the emergency command vehicle path planning method includes at least steps S210 to S240, which are described in detail as follows:
[0035] Step S210, obtaining global road information of traffic roads and driving information of vehicles.
[0036] Among them, the global road information includes the road network graph and the distance matrix and path matrix corresponding to the road network graph. The distance matrix includes the shortest weighted distance between any two nodes, the path matrix includes the shortest path nodes between any two nodes, and the driving information includes the initial shortest path from the starting point to the target point.
[0037] In this embodiment, ArcGIS (Geographic Information System) network dataset tools can be used to construct a dataset using urban traffic roads as the source, and the dataset is abstracted into an urban road traffic network graph structure. Figure 3 , Figure 3 is a schematic diagram of a road network diagram shown in an exemplary embodiment of the present application. Figure 3As shown, each intersection of urban roads is interrupted through the network dataset tool of ArcGIS, so that the entire traffic road network consists of intersections and road sections. The intersections in the traffic road network are abstracted as nodes in the graph structure, and the roads are abstracted as edges in the graph structure, and the urban traffic road network diagram is established based on this.
[0038] As a possible embodiment, each edge in the road network graph carries information on the path length, road conditions and traffic congestion level of the corresponding road section, wherein the road conditions include but are not limited to factors affecting communication time such as road type and lane width.
[0039] Please continue to see Figure 3 The distance matrix is a matrix composed of the shortest weighted distances between any two nodes, where the shortest weighted distance refers to the shortest distance value determined by considering the actual path length, road conditions and traffic congestion information; the path matrix is a matrix composed of the shortest path nodes between any two nodes, where the shortest path node refers to the node with the shortest path distance between the two points; the driving information includes the initial shortest path from the starting point to the target point, where the starting point and the target point are the starting point and the destination of the vehicle, and the initial shortest path refers to the shortest path from the starting point to the target point without considering the obstacle point, that is, including the starting point, the target point and the nodes on the shortest path that need to be passed.
[0040] In an embodiment of the present application, the Floyd algorithm (multi-source shortest path algorithm, also known as the Floyd algorithm) may be used to calculate the distance matrix and the path matrix, and to determine the initial shortest path.
[0041] In one embodiment, a method for determining a distance matrix and a path matrix includes: obtaining the path length, road conditions and traffic congestion level of the road sections corresponding to each edge in the road network graph, and determining the weighted distance of each edge based on the path length, road conditions, traffic congestion level and a preset weight coefficient; creating a matrix based on each node in the road network graph, and determining the first node pairs that are directly connected and the second node pairs that are not directly connected in the matrix; assigning distance weights to each node pair based on the weighted distance to obtain a first matrix, and assigning path points to each node pair to obtain a second matrix; traversing each node in the road network graph as an intermediate point, and updating the first matrix with the shortest weighted distance based on the calculation result of the state transfer formula to obtain a distance matrix; when the shortest weighted distance of any node pair is updated, the path points of any node pair are updated based on the corresponding intermediate point to obtain a path matrix.
[0042] It should be noted that the matrix is an n×n matrix, where n is the number of nodes in the road network graph; each element D[i][j] in the distance matrix (set as D) represents the shortest path length from any node i to any node j, that is, the shortest weighted distance; each element P[i][j] in the path matrix (set as P) stores the information on the shortest path from any node i to any node j. Specifically, P[i][j] usually stores the index of the last vertex before reaching j on this shortest path from i, that is, between i and j, which is the next node of i. In the Floyd algorithm, the distance matrix is usually initialized to the adjacency matrix of the graph, and then updated iteratively through the algorithm until the shortest path lengths between all node pairs are correctly calculated.
[0043] In addition, in the road network graph, any two nodes may be directly connected, then there is a direct edge, and any two nodes may not be directly connected, then there is no direct edge. Any two nodes directly connected in the matrix are taken as the first node pair, and any two nodes not directly connected are taken as the second node pair. In the matrix, the node pairs on the diagonal (such as D[i][i] in the distance matrix) are actually the node pairs composed of node i itself. Therefore, the first node pair is assigned a weighted distance, the second node pair is assigned a preset value, and the node pairs on the diagonal are assigned zero to obtain the first matrix, that is, the initialized distance matrix, where the preset value can be an infinite value or a value greater than the maximum path sum in the road network graph; then if there is a direct edge between i and j, P[i][j] is initialized to j, if there is no direct edge between i and j, P[i][j] can be initialized to -1 or j, and for the node pairs on the diagonal, no setting is required to obtain the second matrix, that is, the initialized path matrix.
[0044] Next, the initialized distance matrix D and the initialized path matrix P are iterated to obtain the shortest distance and the shortest path point between any two nodes. First, determine each node k in the road network graph (k ranges from 1 to n, where n is the total number of nodes), then check and update the distance matrix D. For each node pair i and j, check whether there is a path through k as the middle point, so that the total distance from i to k plus from k to j is less than the current D[i][j]. If there is a shorter path, update D[i][j] to the new shorter distance, and further update the path matrix P. If D[i][j] is updated, the corresponding P[i][j] is also updated to k, that is, on the shortest path, the next vertex from i to j is k. When all nodes as middle points k have been checked, the final distance matrix and path matrix are obtained.
[0045] In obtaining the distance matrix, the state transfer formula of the Floyd algorithm is used to iterate the shortest distance. The state transfer formula is as follows:
[0046] G[s][g]=min {G[s][g], G[s][i]+G[i][g]} Formula (1)
[0047] Among them, G represents the distance matrix, s represents any node, g represents any node except s, and i represents any intermediate point except s and g.
[0048] In this embodiment, the weighted distance of each edge in the road network diagram is determined by the path length, road condition, traffic congestion level and weight coefficient of the corresponding road section of each edge, wherein the weight coefficient of each factor can be set according to the specific situation, and the weight cost of multiple factors such as the actual path length, road condition, traffic congestion level, etc. is comprehensively considered to obtain the global shortest distance of each edge in the road network diagram, and then based on the global shortest distance distance matrix and the path matrix, the problem that the selection of the shortest path is often based only on a single indicator of distance or time is avoided, and the reliability of path planning is improved based on the consideration of multiple factors.
[0049] In one embodiment, a method for determining an initial shortest path includes: taking a starting point and a target point as a third node pair, determining a first shortest path node on the shortest path corresponding to the starting point from a path matrix; continuing to take the first shortest path node and the target point as a fourth node pair, determining a second shortest path node on the shortest path corresponding to the first shortest path node from the path matrix; until the second shortest path node is the target point, arranging the starting point, each shortest path node and the target point in sequence to generate an initial shortest path.
[0050] In this embodiment, an empty path set is created to store nodes on the shortest path, the starting point s is added to the path set, and then the current node is set to the starting point s. When s is not the target point g, the path matrix P is queried to find the next node next on the shortest path from s to g, and next is added to the path set. The current node is updated to next, and the above steps are repeated to find the next node until the current node is the target point g, and g is added to the path set. The path set contains all nodes on the shortest path from the starting point to the target point, arranged in order of access, to obtain the initial shortest path.
[0051] See also Figure 4 , Figure 4 FIG. 1 is a schematic diagram of an initial shortest path shown in an exemplary embodiment of the present application. Figure 4 As shown, the thick solid line shown is an example of the shortest path before the obstacle point is retrieved.
[0052] In addition, by querying the distance matrix D based on the starting point and the target point, the shortest distance between s and g can be found, that is, D[s][g] will give the total distance of the shortest path from s to g.
[0053] Step S220: If it is detected that an obstacle point exists in the initial shortest path, the position of the vehicle is determined as the current point.
[0054] In this embodiment, the vehicle will detect obstacles on the driving path in real time during driving, and dynamically plan the subsequent driving path according to the obstacles in time, ensuring the real-time nature of path planning, thereby improving the rescue efficiency. In addition, the obstacle point here refers to the obstacle point on the untraveled road section ahead when the vehicle is driving on the initial shortest path.
[0055] See also Figure 5 , Figure 5 is a schematic diagram of a target driving path shown in an exemplary embodiment of the present application. Figure 5 As shown in the figure, the dots next to the bold solid line and on the original initial shortest path are examples of obstacle points.
[0056] Step S230, performing a node search in the road network graph according to a search algorithm to determine a set of feasible path points from the current point to the target point.
[0057] In an embodiment of the present application, the search algorithm may be an A* algorithm (A-Star Algorithm, A-star search algorithm), which performs node search in a road network graph and determines a set of feasible path points from a current point to a target point while avoiding obstacles, thereby improving the efficiency of determining feasible paths.
[0058] Specifically, a node search is performed in a road network graph according to a search algorithm to determine a set of feasible path points from a current point to a target point, including: searching for candidate nodes from the current point to the next node in the road network graph, and removing obstacle points and traveled nodes from the candidate nodes; for each remaining candidate node, according to a cost function, calculating a first cost from the current point to the target point, and determining the remaining candidate node corresponding to the minimum cost as a first feasible path node; continuing to search to determine a second feasible path node corresponding to the first feasible path node, and stopping the search when the next node is the target point; arranging the current point, each feasible path node and the target point in sequence to generate a set of feasible path points.
[0059] In this embodiment, the current point, target point, traveled nodes and obstacle points are imported, and the Open set, Close set and Path set corresponding to the feasible path points are created and initialized, that is, the current point and the target point are placed in the Path set, and the traveled nodes and obstacle points are placed in the Close set, then the candidate nodes from the current point to the next node are searched from the road network graph, and the candidate nodes are placed in the Open set, and the nodes in the Close set are removed from the Open set, then the costs of the remaining candidate nodes are calculated, and the remaining candidate nodes corresponding to the minimum cost (indicating the closest distance to the current point) are determined as feasible path nodes and placed in the Path set, and then the feasible path node is taken as the current point, and the above steps are repeated until the search is stopped when the next node is the target point. In this way, the global shortest feasible path point set is determined by the A* algorithm.
[0060] In addition, in this embodiment, the cost function for calculating the cost in the A* algorithm is improved, namely:
[0061] dis A* (k) = α × dis Floyd (x,k +h(k,y) Formula (2)
[0062] Where k represents the candidate node; x represents the current point; y represents the target point; A* () represents the cost of candidate node k; Floyd () represents the weighted distance from the current point x to the candidate node k; h() represents the Manhattan distance from the candidate node k to the target point y; α represents the preset weight coefficient.
[0063] In one embodiment, before continuing to search to determine the second feasible path node corresponding to the first feasible path node, it also includes: determining the number of remaining candidate nodes corresponding to the first feasible path node, and comparing the number with a preset number threshold; if the number is less than the number threshold, returning to the previous node of the first feasible path node, and re-determining the third drivable path node corresponding to the previous node.
[0064] In this embodiment, when selecting the next drivable path node according to the currently corresponding remaining candidate nodes, the number of remaining candidate nodes is first determined, and the number is compared with a preset number threshold, that is, it is determined whether the number of remaining candidate nodes is zero. If so, to avoid no feasible path at the current point, return to the previous node and redetermine the current point. At this time, the current point is determined to be the point with the second smallest cost. If the above situation still exists, the current point is selected in turn according to the smaller cost until the number of remaining candidate nodes is greater than zero. In this way, the reliability of the determined feasible path point set is guaranteed.
[0065] Step S240, recursively optimize each node in the feasible path point set according to the distance matrix and the path matrix to generate a target driving path from the current point to the target point.
[0066] In this embodiment, the distance matrix and path matrix obtained by the Floyd algorithm are used to locally optimize the global feasible path point set determined by the A* algorithm based on the obstacle points, so as to obtain the final target driving path. When the emergency command vehicle detects that an obstacle cannot be passed, the above two algorithms are combined to recursively search for the local optimal path, replace the suboptimal path in the feasible path, and obtain the global optimal path when the obstacle points block the current road, thereby realizing the path planning of the fusion algorithm.
[0067] Specifically, according to the distance matrix and the path matrix, each node in the feasible path point set is recursively optimized, including: determining multiple adjacent node pairs according to the arrangement order of the nodes in the feasible path point set; determining the first path length of each adjacent node pair according to the distance matrix, and calculating the second path length of each adjacent node pair according to the cost function; comparing the first path length with the second path length, and determining the adjacent node pairs whose first path length is less than the second path length as optimizable node pairs; reconstructing the target driving path between the optimizable node pairs according to the path matrix, and replacing all the optimizable node pairs in the feasible path point set with the target driving path.
[0068] In this embodiment, the feasible path point set obtained by the A* algorithm is traversed to obtain multiple adjacent node pairs (a, b). For each adjacent node pair (a, b) on the feasible path (including the current point and the target point), the distance matrix D[p][q] obtained by the Floyd algorithm is queried, and the shortest weighted distance of the adjacent node pair (a, b) is used as the first path length, and the corresponding cost calculated when determining the feasible path node set is used as the second path length. The first path length is compared with the second path length. If the first path length is less than the second path length, it indicates that there is a shortest path node before the adjacent node pair (a, b), and optimization can be performed. Therefore, the adjacent node pair (a, b) is determined as an optimizable node pair (a, b), where the optimizable node pair (a, b) may exist, may not exist, or may be multiple pairs. Then, the target driving path between the optimizable node pairs (a, b) is reconstructed according to the path matrix, and the optimizable node pairs in the feasible path point set are replaced with the target driving path. Specifically, if the shortest path of P[a][b] is not b (that is, the next node of the shortest distance of the feasible path node a is not a directly connected node), the shortest path from a to b is reconstructed according to the path matrix, and the reconstructed shortest path replaces the node pair (a, b) in the original path. In this way, a better target driving path is obtained. The obtained target driving path is the global optimal path after considering the obstacle point occlusion and the global shortest path information of the Floyd algorithm, which ensures the timeliness and reliability of the path planning after the vehicle encounters an obstacle point.
[0069] In one embodiment, a target driving path from a current point to a target point is generated, including: determining whether the recursive current point is the penultimate node in a feasible path point set; if so, generating the target driving path; if not, determining whether the recursive target point is the last node in the feasible path point set; if not, moving the recursive current point back one node in the feasible path point set and continuing to perform recursive optimization until the target driving path is generated when the recursive current point is the penultimate node in the feasible path point set; if so, moving the recursive current point back one node in the feasible path point set, setting the recursive target point as the first node in the feasible path point set, and moving the recursive target point back one node in the feasible path point set and continuing to perform recursive optimization until the target driving path is generated when the recursive current point is the penultimate node in the feasible path point set.
[0070] In this embodiment, first, the number of nodes Len on the feasible path is determined through the set of feasible path points, including the current point and the target point. The recursive starting point is initialized as Path(i), and the recursive target point is Path(j), where i = 0, i < Len - 1, j = 1, i < j < Len. During the recursive optimization process, it is continuously determined whether Path(i) is the second-to-last node in the set of feasible path points (i.e., i = Len - 1). If so, the target driving path is generated. If not, it is determined whether Path(j) is the last node in the set of feasible path points. If not, Path(i) is shifted one node backward in the set of feasible path points and the recursive optimization continues until the target driving path is generated when Path(i) is the second-to-last node in the set of feasible path points; if so, Path(i) is shifted one node backward in the set of feasible path points, Path(j) is set as the first node in the set of feasible path points, and Path(j) is shifted one node backward in the set of feasible path points to continue the recursive optimization until the target driving path is generated when Path(i) is the second-to-last node in the set of feasible path points. In this way, it is further ensured that the obtained target driving path is the final shortest path.
[0071] In a possible embodiment, after all adjacent node pairs are traversed once, the recursive optimization operation is repeated until there are no optimizable node pairs, and then the recursive optimization operation ends and the target driving path is generated. In this way, it is also further ensured that the obtained target driving path is the final shortest path.
[0072] Please refer to Figure 6 , Figure 6 which is a flowchart of a specific emergency command vehicle path planning method shown in an exemplary embodiment of the present application. As Figure 6 shown, the steps of a specific emergency command vehicle path planning method are described in detail as follows:
[0073] Load the road network graph, input the current point, target point, obstacle points, and traveled nodes; create and initialize the Open set, Close set, and Path set, put the current point and target point into the Path set, and put the obstacle points and traveled nodes into the Close set; determine the candidate nodes and put them into the Open set; remove all nodes in the Open set that exist in the Close set; determine whether the Open set is empty. If so, backtrack to the previous node and re-perform the steps of determining the candidate nodes and putting them into the Open set; if not, calculate the costs of each node in the Open set; select the node with the minimum cost in the Open set as the expansion of the next node; determine whether the next node is the target point. If not, continue with the next node as the current point and perform the steps of determining the candidate nodes and putting them into the Open set; if so, output the Path set and its node number Len; initialize the recursive current point as Path(i), i = 0, i < Len - 1, and initialize the recursive target point as Path(j), j = 1, i < j < Len; calculate the Floyd path length and A* path length between Path(i) and Path(j); determine whether the Floyd path length is less than the A* path length. If less, replace the path between nodes i and j with the shortest path in the path matrix planned by the Floyd algorithm, and then determine whether i = Len - 1 holds. If greater than or equal, directly determine whether i = Len - 1 holds; if it holds, obtain the optimized Path set and generate the target driving path; if it does not hold, determine whether j = Len holds; if it does not hold, directly set j = j + 1 and continue with the steps of calculating the Floyd path length and A* path length between Path(i) and Path(j); if it holds, first set i = i + 1, j = 1, then set j = j + 1, and then continue with the steps of calculating the Floyd path length and A* path length between Path(i) and Path(j); until i = Len - 1, obtain the optimized Path set and generate the target driving path.
[0074] The above-mentioned emergency command vehicle path planning method obtains global road information of traffic roads and driving information of vehicles. The global road information includes a road network diagram and a distance matrix and a path matrix corresponding to the road network diagram. The driving information includes an initial shortest path from a starting point to a target point. When an obstacle point is detected in the initial shortest path, the position of the vehicle is determined as the current point, and a node search is performed in the road network diagram according to a search algorithm to determine a feasible path point set from the current point to the target point. Finally, each node in the feasible path point set is recursively optimized according to the distance matrix and the path matrix to generate a target driving path from the current point to the target point. When an obstacle is detected, a feasible path is first dynamically planned according to a search algorithm to avoid the obstacle point, and then a suboptimal path in the feasible path is replaced by a recursive search of a local optimal path. Finally, a global optimal path is obtained when the obstacle point blocks the current road, thereby enhancing the reliability of the obtained target driving path.
[0075] See also Figure 7 , Figure 7 is a block diagram of an emergency command vehicle path planning system shown in an exemplary embodiment of the present application. The system can be applied to Figure 1 The implementation environment shown is a schematic diagram. It should be understood that the system may also be applicable to other exemplary implementation environments, and this embodiment does not limit the implementation environment to which the system is applicable.
[0076] like Figure 7 As shown, in an exemplary embodiment, the emergency command vehicle path planning system 700 includes at least an information acquisition module 710, an obstacle point detection module 720, a feasible path determination module 730 and a path optimization module 740, which are described in detail as follows:
[0077] The information acquisition module 710 is used to acquire global road information of traffic roads and driving information of vehicles, wherein the global road information includes a road network diagram and a distance matrix and a path matrix corresponding to the road network diagram, the distance matrix includes the shortest weighted distance between any two nodes, the path matrix includes the shortest path nodes between any two nodes, and the driving information includes an initial shortest path from a starting point to a target point;
[0078] The obstacle point detection module 720 is used to determine the position of the vehicle as the current point if an obstacle point is detected in the initial shortest path;
[0079] A feasible path determination module 730 is used to perform node search in the road network graph according to a search algorithm to determine a set of feasible path points from a current point to a target point;
[0080] The path optimization module 740 is used to recursively optimize each node in the feasible path point set according to the distance matrix and the path matrix to generate a target driving path from the current point to the target point.
[0081] It should be noted that the emergency command vehicle path planning system provided in the above embodiment and the emergency command vehicle path planning method provided in the above embodiment belong to the same concept, and the contents of the operations performed by each module have been described in detail in the method embodiment and will not be repeated here.
[0082] See also Figure 8 , Figure 8 It is a schematic diagram of the structure of a vehicle-mounted terminal shown in an exemplary embodiment of the present application. Figure 8 The structure diagram of the computer system of the vehicle terminal suitable for implementing the embodiment of the present application is shown. It should be noted that: Figure 8 The computer system 800 of the vehicle-mounted terminal shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.
[0083] like Figure 8 As shown, the computer system 800 includes a central processing unit (CPU) 801, which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 802 or the program loaded from the storage part 808 to the random access memory (RAM) 803, such as executing the method in the above embodiment. In RAM 803, various programs and data required for system operation are also stored. CPU 801, ROM 802 and RAM 803 are connected to each other through bus 804. Input / output (I / O) interface 805 is also connected to bus 804.
[0084] The following components are connected to the I / O interface 805: an input section 806 including a keyboard, a mouse, etc.; an output section 807 including a cathode ray tube (CRT), a liquid crystal display (LCD), etc., and a speaker; a storage section 808 including a hard disk, etc.; and a communication section 809 including a network interface card such as a LAN (Local Area Network) card, a modem, etc. The communication section 809 performs communication processing via a network such as the Internet. A drive 810 is also connected to the I / O interface 805 as needed. A removable medium 811, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., is installed on the drive 810 as needed so that a computer program read therefrom is installed into the storage section 808 as needed.
[0085] In particular, according to an embodiment of the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present application includes a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a computer program for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication section 809, and / or installed from a removable medium 811. When the computer program is executed by a central processing unit (CPU) 801, various functions defined in the system of the present application are executed.
[0086] The present invention also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor of a computer, the computer executes the above-mentioned emergency command vehicle path planning method. The computer-readable storage medium may be included in the vehicle-mounted terminal described in the above-mentioned embodiment, or may exist independently without being assembled into the vehicle-mounted terminal.
[0087] It should be noted that the computer-readable medium shown in the embodiment of the present application can be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium can be, for example, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples of computer-readable storage media can include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM), a flash memory, an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present application, a computer-readable signal medium may include a data signal propagated in a baseband or as part of a carrier wave, wherein a computer-readable computer program is carried. This propagated data signal can take a variety of forms, including but not limited to an electromagnetic signal, an optical signal, or any suitable combination of the above. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium, which may send, propagate or transmit a program for use by or in conjunction with an instruction execution system, apparatus or device. A computer program contained on a computer-readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wired, etc., or any suitable combination of the above.
[0088] The flowcharts and block diagrams in the accompanying drawings illustrate the possible architecture, functions and operations of the systems, methods and computer program products according to various embodiments of the present invention. Among them, each box in the flowchart or block diagram can represent a module, a program segment, or a part of the code, and the above-mentioned module, program segment, or a part of the code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram or flowchart, and the combination of boxes in the block diagram or flowchart, can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.
[0089] The units involved in the embodiments of the present invention may be implemented by software or hardware, and the units described may also be arranged in a processor. The names of these units do not, in some cases, limit the units themselves.
[0090] The above embodiments are merely illustrative of the principles and effects of the present invention, and are not intended to limit the present invention. Anyone familiar with the technology may modify or change the above embodiments without violating the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by a person of ordinary skill in the art without departing from the spirit and technical ideas disclosed by the present invention shall still be covered by the claims of the present invention.
Claims
1. A method for planning a path for an emergency command vehicle, characterized in that: The method comprises: Acquire global road information of traffic roads and driving information of vehicles, wherein the global road information includes a road network graph and a distance matrix and a path matrix corresponding to the road network graph, the distance matrix includes the shortest weighted distance between any two nodes, the path matrix includes the shortest path nodes between any two nodes, and the driving information includes an initial shortest path from a starting point to a target point; If an obstacle point is detected in the initial shortest path, the position of the vehicle is determined as the current point; Performing node search in the road network graph according to a search algorithm to determine a set of feasible path points from the current point to the target point; According to the distance matrix and the path matrix, each node in the feasible path point set is recursively optimized to generate a target driving path from the current point to the target point.
2. The method for emergency command vehicle path planning according to claim 1, characterized in that: The performing node search in the road network graph according to the search algorithm to determine a feasible path point set from the current point to the target point includes: Searching for candidate nodes from the current point to the next node in the road network graph, and removing the obstacle points and the traveled nodes from the candidate nodes; For each remaining candidate node, calculate the first cost from the current point to the target point according to the cost function, and determine the remaining candidate node corresponding to the minimum cost as the first feasible path node; Continue searching to determine a second feasible path node corresponding to the first feasible path node, and stop searching when the next node is the target point; The current point, each feasible path node and the target point are arranged in sequence to generate the feasible path point set.
3. The method for emergency command vehicle path planning according to claim 1, characterized in that: The recursive optimization of each node in the feasible path point set according to the distance matrix and the path matrix includes: Determining a plurality of adjacent node pairs according to an arrangement order of nodes in the feasible path point set; Determine a first path length of each of the adjacent node pairs according to the distance matrix, and calculate a second path length of each of the adjacent node pairs according to a cost function; comparing the first path length with the second path length, and determining the adjacent node pairs whose first path length is smaller than the second path length as the optimizable node pairs; The target driving path between the optimizable node pairs is reconstructed according to the path matrix, and the optimizable node pairs in the feasible path point set are all replaced with the target driving path.
4. The method for emergency command vehicle path planning according to claim 3, characterized in that: The generating a target driving path from the current point to the target point includes: Determine whether the recursive current point is the second to last node in the feasible path point set; If yes, then generate the target driving path; if no, then determine whether the recursive target point is the last node in the feasible path point set; If not, the recursive current point is moved back one node toward the feasible path point set and the recursive optimization is continued until the target driving path is generated when the recursive current point is the penultimate node in the feasible path point set; If so, the recursive current point is moved back one node toward the feasible path point set, the recursive target point is set as the first node in the feasible path point set, and the recursive target point is moved back one node toward the feasible path point set to continue recursive optimization until the recursive current point is the second to last node in the feasible path point set and the target driving path is generated.
5. The method for emergency command vehicle path planning according to claim 2, characterized in that: Before continuing to search and determine the second feasible path node corresponding to the first feasible path node, the method further includes: Determine the number of the remaining candidate nodes corresponding to the first feasible path node, and compare the number with a preset number threshold; If the number is less than the number threshold, return to the previous node of the first feasible path node and re-determine the third drivable path node corresponding to the previous node.
6. The method for emergency command vehicle path planning according to any one of claims 1 to 5, characterized in that: The distance matrix and the path matrix are determined by: Obtaining the path length, road condition and traffic congestion degree of each road section corresponding to each edge in the road network graph, and determining the weighted distance of each edge according to the path length, the road condition, the traffic congestion degree and a preset weight coefficient; Creating a matrix according to each node in the road network graph, and determining a first node pair that is directly connected and a second node pair that is not directly connected in the matrix; Assigning distance values to each of the node pairs according to the weighted distance to obtain a first matrix, and assigning path point values to each of the node pairs to obtain a second matrix; Each node in the road network graph is traversed as an intermediate point, and the first matrix is updated with the shortest weighted distance according to the calculation result of the state transfer formula to obtain the distance matrix; When the shortest weighted distance of any node pair is updated, the path points of the arbitrary node pair are updated according to the corresponding intermediate points to obtain the path matrix.
7. The method for emergency command vehicle path planning according to any one of claim 6, characterized in that: The method for determining the initial shortest path includes: Taking the starting point and the target point as a third node pair, determining a first shortest path node on the shortest path corresponding to the starting point from the path matrix; Continue to use the first shortest path node and the target point as a fourth node pair, and determine a second shortest path node on the shortest path corresponding to the first shortest path node from the path matrix; When the second shortest path node is the target point, the starting point, each of the shortest path nodes and the target point are arranged in sequence to generate the initial shortest path.
8. A path planning system for an emergency command vehicle, characterized in that: The system comprises: An information acquisition module, used to acquire global road information of traffic roads and driving information of vehicles, wherein the global road information includes a road network diagram and a distance matrix and a path matrix corresponding to the road network diagram, the distance matrix includes the shortest weighted distance between any two nodes, the path matrix includes the shortest path nodes between any two nodes, and the driving information includes an initial shortest path from a starting point to a target point; An obstacle point detection module is used to determine the position of the vehicle as the current point if an obstacle point is detected in the initial shortest path; A feasible path determination module, used to perform node search in the road network graph according to a search algorithm to determine a set of feasible path points from the current point to the target point; The path optimization module is used to recursively optimize each node in the feasible path point set according to the distance matrix and the path matrix, and generate a target driving path from the current point to the target point.
9. A vehicle-mounted terminal, characterized in that: The vehicle-mounted terminal comprises: one or more processors; A storage device for storing one or more programs, when the one or more programs are executed by the one or more processors, enables the vehicle-mounted terminal to implement the emergency command vehicle path planning method as described in any one of claims 1 to 7.
10. A computer-readable storage medium, characterized in that: Computer-readable instructions are stored thereon, and when the computer-readable instructions are executed by a processor of a computer, the computer is enabled to execute the emergency command vehicle path planning method according to any one of claims 1 to 7.
Citation Information
Cited By
Path planning method and device, computer equipment and storage medium
CN120913432A
