Path planning method and device of engineering vehicle and engineering vehicle
By considering the number of inflection points and obstacle size in the construction vehicle path planning, calculating the cost of each sub-path and selecting the path point with the lowest cost, the problem of low driving efficiency of the construction vehicle is solved, and a straighter and more efficient path planning is achieved.
Patent Information
- Application Number
- CN202510428570.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-04-07
- Publication Date
- 2025-06-27
AI Technical Summary
The driving efficiency of engineering vehicles in autonomous driving mode is mainly due to the failure of the existing path planning method to effectively consider the size of the engineering vehicles and the size of obstacles, resulting in frequent obstacle avoidance operations and unstable driving.
A path planning method for engineering vehicles is proposed. By determining multiple paths and calculating the cost of each sub-path, the cost is positively correlated with the number of inflection points and the size of obstacles, and then the sub-path with the smallest cost is selected as the next path point.
This method can reduce unnecessary obstacle avoidance operations, improve the straightness of the path, and thus improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
Smart Images
Figure CN120213073A_ABST
Abstract
Description
Technical Field
[0001] The present disclosure relates to the technical field of path planning, and in particular to a path planning method, apparatus, and engineering vehicle for an engineering vehicle. Background Art
[0002] With the rapid development of technology, autonomous driving technology has become a research hotspot in the field of vehicles. During the process of autonomous driving, path planning of vehicles is a very important link. Summary of the Invention
[0003] In the related art, the driving efficiency of an engineering vehicle in the autonomous driving mode is low. Here, the engineering vehicle includes large vehicles. Large vehicles include, for example, heavy trucks, cranes, or excavators.
[0004] After analysis, it is found that in the related art, the engineering vehicle uses the same path planning method as other vehicles (for example, small vehicles such as sedans).
[0005] In this case, in the related art, when detecting any obstacle (for example, a rock) that may hinder the driving of a small vehicle, the small vehicle needs to spend extra time to perform an obstacle avoidance operation and re-plan the driving path.
[0006] However, considering the large size of the engineering vehicle, the obstacles that hinder the driving of small vehicles do not necessarily hinder the driving of the engineering vehicle, and frequent obstacle avoidance operations will cause the driving of large vehicles with a higher center of gravity, such as engineering vehicles, to be unstable. Therefore, the path planned by relying on the related technology is not suitable for the driving of the engineering vehicle, resulting in low driving efficiency of the engineering vehicle in the autonomous driving mode.
[0007] To solve the above problems, the embodiments of the present disclosure propose the following solutions.
[0008] According to an aspect of the embodiments of the present disclosure, there is provided a path planning method for an engineering vehicle, including: determining, according to reference information, multiple paths of the engineering vehicle from a starting point to an ending point, where the reference information includes a static map, each path has multiple path points, and the multiple path points include inflection points, the starting point, and the ending point; when there are multiple sub-paths between a first path point where the engineering vehicle is currently located and the ending point, determining the cost of each sub-path of the engineering vehicle from the first path point to the ending point, where the cost of each sub-path is positively correlated with the number of inflection points of the sub-path and positively correlated with the size of the obstacle between the first path point and a second path point, and the second path point is the next path point of the first path point on the sub-path; determining, according to the cost of each sub-path, the next path point that the engineering vehicle is to travel to, where the next path point that the engineering vehicle is to travel to is the second path point on the sub-path with the minimum cost.
[0009] In some embodiments, the reference information further includes at least one set of information, the at least one set of information including at least one of a first set of information, a second set of information, a third set of information, and a fourth set of information. The first set of information includes the width of the engineering vehicle and the width limit value of the road corresponding to each alternative path. The second set of information includes the height of the engineering vehicle and the height limit value of the road. The third set of information includes the weight of the engineering vehicle and the maximum load-bearing weight of the road. The fourth set of information includes the minimum turning radius of the engineering vehicle and the minimum turning radius of the road. Determining, according to the reference information, multiple paths for the engineering vehicle to travel from a starting point to an ending point includes: determining multiple alternative paths from the starting point to the ending point according to the starting point, the ending point, and the static map; and determining the multiple paths from the multiple alternative paths according to the at least one set of information.
[0010] In some embodiments, the at least one set of information includes the fourth set of information.
[0011] In some embodiments, the at least one set of information further includes at least one of the first set of information, the second set of information, and the third set of information.
[0012] In some embodiments, the sub-path with the minimum cost is the sub-path with the minimum total cost f(n), where: f(n) = a * h1(n) + b * h2(n) + g(n), n represents the first path point, g(n) represents the cost of the engineering vehicle traveling from the starting point to the first path point. For each sub-path, h1(n) represents a first estimation of the cost of the sub-path, h2(n) represents a second estimation of the cost of the sub-path, a is positively correlated with the number of inflection points of the sub-path, b is positively correlated with the size of the obstacle. Among them, the first estimation is related to the number of inflection points of the sub-path, and the second estimation is not related to the number of inflection points of the sub-path.
[0013] In some embodiments, for each sub-path, h2(n) satisfies at least one of a first condition, a second condition, and a third condition, where: the first condition is that h2(n) is positively correlated with the straight-line distance from the first path point to the ending point; the second condition is that h2(n) is positively correlated with the congestion degree of the road corresponding to the sub-path; and the third condition is that h2(n) is positively correlated with the angle between a first orientation and a second orientation, the first orientation being the orientation of the front of the engineering vehicle when it travels to the first path point, and the second orientation being the preset orientation of the front of the engineering vehicle when it stops at the ending point.
[0014] In some embodiments, h2(n) satisfies the first condition.
[0015] In some embodiments, h2(n) also satisfies at least one of the second condition and the third condition.
[0016] In some embodiments, h2(n) = v1*c(n) + v2*d(n) + v3*θ(n), where c(n) represents the degree of congestion, d(n) represents the straight-line distance, θ(n) represents the included angle, v1 represents the weight of c(n), v2 represents the weight of d(n), v3 represents the weight of θ(n), where v1 is positively correlated with c(n), v2 is positively correlated with d(n), and v3 is positively correlated with θ(n), and v1, v2, and v3 are normalized.
[0017] In some embodiments, the trajectory between the first path point and the next path point that the engineering vehicle is to travel to is optimized by the Reeds-Shepp curve algorithm.
[0018] According to another aspect of the embodiments of the present disclosure, there is provided a path planning device for an engineering vehicle, including: a module configured to execute the method described in any one of the above embodiments.
[0019] According to another aspect of the embodiments of the present disclosure, there is provided a path planning device for an engineering vehicle, including: a memory; and a processor coupled to the memory, configured to execute the method described in any one of the above embodiments based on instructions stored in the memory.
[0020] According to another aspect of the embodiments of the present disclosure, there is provided an engineering vehicle, including: the path planning device for an engineering vehicle described in any one of the above embodiments.
[0021] According to another aspect of the embodiments of the present disclosure, there is provided a computer-readable storage medium, including computer program instructions, where the steps of the method described in any one of the above embodiments are implemented when the computer program instructions are executed by a processor.
[0022] According to another aspect of the embodiments of the present disclosure, there is provided a computer program product, including a computer program, where the steps of the method described in any one of the above embodiments are implemented when the computer program is executed by a processor.
[0023] In the embodiments of the present disclosure, multiple paths of the engineering vehicle from the starting point to the ending point are first determined. When there are multiple sub-paths between the first path point where the engineering vehicle is currently located and the ending point, the cost of the sub-paths related to the number of inflection points and the size of obstacles is determined, and the next path point of the engineering vehicle is determined according to the cost of each sub-path. In this way, when performing path planning for the engineering vehicle, the number of inflection points and the size of obstacles can be comprehensively considered, unnecessary obstacle avoidance operations can be reduced, and the planned path can be made straighter, which helps to improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0024] The technical solutions of the present disclosure will be further described in detail below with reference to the accompanying drawings and embodiments. Description of the Drawings
[0025] In order to more clearly illustrate the technical solutions in the embodiments of the present disclosure or in the prior art, the accompanying drawings required for use in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present disclosure. For those of ordinary skill in the art, other accompanying drawings can be obtained based on these drawings without creative efforts.
[0026] Figure 1 It is a schematic flowchart of a path planning method for an engineering vehicle according to some embodiments of the present disclosure.
[0027] Figure 2 It is a schematic diagram of trajectory optimization according to some embodiments of the present disclosure.
[0028] Figure 3 It is a schematic structural diagram of a path planning device for an engineering vehicle according to some embodiments of the present disclosure.
[0029] Figure 4 It is a schematic structural diagram of a path planning device for an engineering vehicle according to some other embodiments of the present disclosure. Detailed Embodiments
[0030] The technical solutions in the embodiments of the present disclosure will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present disclosure. Obviously, the described embodiments are only some embodiments of the present disclosure, rather than all embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present disclosure without creative efforts fall within the scope of protection of the present disclosure.
[0031] Unless otherwise specifically stated, the relative arrangements, numerical expressions, and numerical values of the components and steps set forth in these embodiments do not limit the scope of the present disclosure.
[0032] At the same time, it should be understood that for the convenience of description, the sizes of the various parts shown in the accompanying drawings are not necessarily drawn according to the actual proportional relationship.
[0033] For technologies, methods, and devices known to those of ordinary skill in the relevant field, they may not be discussed in detail, but where appropriate, the technologies, methods, and devices should be regarded as part of the specification.
[0034] In all the examples shown and discussed here, any specific value should be construed as merely exemplary, rather than as a limitation. Therefore, other examples of the exemplary embodiments may have different values.
[0035] It should be noted that like reference numerals and letters refer to like items in the following figures, and thus, once an item is defined in one figure, further discussion thereof is not required in subsequent figures.
[0036] In addition, in the description of the present disclosure, the terms "first", "second", "third", etc. are used for descriptive purposes only and should not be construed as indicating or implying relative importance or order. Similarly, although operations are depicted in the figures in a particular order, this should not be construed as requiring that the operations be performed in the particular order shown or in sequential order, or that all illustrated operations be performed to achieve the desired result. In some cases, multitasking and parallel processing may be advantageous.
[0037] Figure 1 is a schematic flowchart of a path planning method for a construction vehicle according to some embodiments of the present disclosure. As some embodiments, the construction vehicle is a wheeled heavy vehicle. For example, a wheeled heavy fire truck.
[0038] In step 102, according to the reference information, multiple paths of the construction vehicle from the starting point to the ending point are determined. Here, the reference information includes a static map, and each path has multiple path points, and the multiple path points include inflection points, the starting point, and the ending point. The number of inflection points on each path can be one or more.
[0039] As some embodiments, the static map can be a road map of the environment where the construction vehicle is located. For example, the road map can be obtained by querying the network.
[0040] As some embodiments, after the construction vehicle travels to the ending point, a reverse parking operation can be performed at the ending point so that the construction vehicle can be parked regularly.
[0041] In step 104, when there are multiple sub-paths between the first path point where the construction vehicle is currently located and the ending point, the cost of each sub-path of the construction vehicle from the first path point to the ending point is determined.
[0042] Here, the cost of each sub-path is positively correlated with the number of inflection points of the sub-path and positively correlated with the size of the obstacle between the first path point and the second path point, and the second path point is the next path point of the first path point on the sub-path. For example, the first path point where the construction vehicle is currently located can be determined by the Global Positioning System (GPS).
[0043] For example, assume that there are two sub-paths between the first path point where the engineering vehicle is currently located and the end point, the number of inflection points of the first sub-path is 3, and the number of inflection points of the second sub-path is 2. Then, when there are no obstacles between the first path point and the second path points on each sub-path, the cost of the second sub-path is less than the cost of the first sub-path.
[0044] For another example, assume that there are two sub-paths between the first path point where the engineering vehicle is currently located and the end point, the number of inflection points of both the first sub-path and the second sub-path is 2, and there is an obstacle only between the first path point and the second path point on the first sub-path. Then, the cost of the second sub-path is less than the cost of the first sub-path.
[0045] For still another example, assume that there are two sub-paths between the first path point where the engineering vehicle is currently located and the end point, the number of inflection points of the first sub-path is 2, the number of inflection points of the second sub-path is 3, and there is an obstacle between the first path point and the second path point on the first sub-path. Then, it is necessary to consider both the number of inflection points and the size of the obstacle to determine the magnitude relationship between the cost of the second sub-path and the cost of the first sub-path. How to determine the cost of the sub-path by considering the number of inflection points and the size of the obstacle will be introduced later.
[0046] As some embodiments, obstacles can be classified into static obstacles and dynamic obstacles. For example, static obstacles include static objects such as walls, fences, and barriers; for another example, dynamic obstacles include moving objects such as pedestrians and vehicles.
[0047] As some embodiments, the data collected by sensors and a trained obstacle target detection model can be used to determine whether there is an obstacle. For example, data of static obstacles (such as a point cloud map) can be collected by a lidar; for another example, data of dynamic obstacles (such as a video) can be collected by a camera.
[0048] For example, the data collected by sensors can be input into a trained obstacle target detection model to detect in real time whether there are obstacles on the driving path during the driving of the engineering vehicle, and output the position of the obstacle and the distance from the engineering vehicle when there is an obstacle.
[0049] For example, a trained obstacle target detection model can determine the distance of an obstacle by using the data collected by a millimeter-wave radar.
[0050] As some embodiments, the cross-sectional area of the obstacle can be used as the size of the obstacle. For example, a trained obstacle target detection model can also output the cross-sectional area of the obstacle. The cross-sectional area of the obstacle can be understood as the cross-sectional area in the direction perpendicular to the extension direction of the road.
[0051] In step 106, according to the cost of each sub-path, determine the next path point that the engineering vehicle will travel to. Here, the next path point that the engineering vehicle will travel to is the second path point on the sub-path with the minimum cost.
[0052] For example, assume that there are two sub-paths between the first path point where the engineering vehicle is currently located and the end point, and the cost of the second sub-path is less than that of the first sub-path. Then, the next path point of the first path point is the second path point on the second sub-path.
[0053] It should be understood that after determining the next path point that the engineering vehicle will travel to, the engineering vehicle can travel from the first path point where it is currently located to the next path point. After traveling to the next path point, the next path point becomes the first path point where it is currently located. By repeating steps 104 and 106, the engineering vehicle finally travels to the end point to achieve the autonomous driving of the engineering vehicle.
[0054] In the above embodiment, first determine multiple paths from the starting point to the end point of the engineering vehicle. When there are multiple sub-paths between the first path point where the engineering vehicle is currently located and the end point, determine the cost of the sub-paths related to the number of inflection points and the size of the obstacles, and determine the next path point of the engineering vehicle according to the cost of each sub-path. In this way, when performing path planning for the engineering vehicle, the number of inflection points and the size of the obstacles can be comprehensively considered, unnecessary obstacle avoidance operations can be reduced, and the planned path can be made straighter, which helps to improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0055] In some embodiments, the reference information further includes at least one set of information, and the at least one set of information includes at least one of the first set of information, the second set of information, the third set of information, and the fourth set of information. The first set of information includes the width of the engineering vehicle and the width limit value of the road corresponding to each alternative path. The second set of information includes the height of the engineering vehicle and the height limit value of the road. The third set of information includes the weight of the engineering vehicle and the maximum load-bearing weight of the road. The fourth set of information includes the minimum turning radius of the engineering vehicle and the minimum turning radius of the road.
[0056] Multiple alternative paths from the starting point to the end point can be determined according to the starting point, the end point, and the static map; and multiple paths of the engineering vehicle from the starting point to the end point can be determined from the multiple alternative paths according to at least one set of information.
[0057] For example, the at least one set of information includes any one, any two, any three, or all of the first set of information, the second set of information, the third set of information, and the fourth set of information.
[0058] As some implementation manners, the multiple alternative paths can be all paths from the starting point to the end point in the static map.
[0059] In some embodiments, when the content of at least one set of information matches, a corresponding alternative path is determined as one of the multiple paths from the starting point to the ending point.
[0060] For example, when at least one set of information includes the first set of information, if the width of the engineering vehicle matches the width limit value of a certain alternative path among the multiple alternative paths, then this alternative path is a path for the engineering vehicle from the starting point to the ending point. For example, when the width limit value of the road is greater than or equal to the product of the first coefficient and the width of the engineering vehicle, it can be regarded as a match. The first coefficient is greater than 1, for example, it is 1.2.
[0061] Again, for example, when at least one set of information includes the second set of information, if the height of the engineering vehicle matches the height limit value of a certain alternative path among the multiple alternative paths, then this alternative path is a path for the engineering vehicle from the starting point to the ending point. For example, when the height limit value of the road is greater than or equal to the product of the second coefficient and the height of the engineering vehicle, it can be regarded as a match. The second coefficient is greater than 1, for example, it is 1.2.
[0062] Also, for example, when at least one set of information includes the third set of information, if the weight of the engineering vehicle matches the maximum load-bearing weight of a certain alternative path among the multiple alternative paths, then this alternative path is a path for the engineering vehicle from the starting point to the ending point. For example, when the maximum load-bearing weight of the road is greater than or equal to the product of the third coefficient and the weight of the engineering vehicle, it can be regarded as a match. The third coefficient is greater than 1, for example, it is 1.1.
[0063] Furthermore, for example, when at least one set of information includes the fourth set of information, if the minimum turning radius of the engineering vehicle matches the minimum turning radius of a certain alternative path among the multiple alternative paths, then this alternative path is a path for the engineering vehicle from the starting point to the ending point. For example, when the minimum turning radius of the road is greater than or equal to the product of the fourth coefficient and the minimum turning radius of the engineering vehicle, it can be regarded as a match. The fourth coefficient is greater than 1, for example, it is 1.05.
[0064] In some embodiments, the first coefficient, the second coefficient, the third coefficient, and the fourth coefficient are all greater than 1. In this way, a certain redundancy can be reserved to improve the safety of the engineering vehicle when driving on the road corresponding to the matched path.
[0065] In some embodiments, the first coefficient is the same as the second coefficient, and the first coefficient is greater than the third coefficient, and the third coefficient is greater than the fourth coefficient.
[0066] In some embodiments, at least one of the first set of information, the second set of information, the third set of information, and the fourth set of information can be determined by querying the construction materials of the road and the usage instructions of the engineering vehicle.
[0067] It should be understood that in the case where at least one set of information includes multiple sets of information, multiple paths of the engineering vehicle from the starting point to the ending point need to satisfy that the content of these multiple sets of information all matches.
[0068] In the above embodiments, by using at least one attribute information of the width, height, weight, and minimum turning radius of the engineering vehicle, as well as the information of the road corresponding to the attribute information, multiple paths that can satisfy the passing conditions of the engineering vehicle as much as possible are determined from multiple alternative paths from the starting point to the ending point for path planning, thereby reducing the time of path planning and making the planned path more suitable for the driving of the engineering vehicle, which helps to further improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0069] In some embodiments, the at least one set of information includes a fourth set of information. In this way, multiple paths that satisfy the U-turn of the engineering vehicle can be determined from multiple alternative paths from the starting point to the ending point for path planning. Furthermore, when an unexpected situation occurs on the current path and it is impassable, it is convenient for the engineering vehicle to make a U-turn and drive away from this path and re-plan the path, which helps to further improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0070] In some embodiments, in addition to including the fourth set of information, the at least one set of information further includes at least one of the first set of information, the second set of information, and the third set of information.
[0071] For example, the at least one set of information further includes any one, any two, or all of the first set of information, the second set of information, and the third set of information.
[0072] In the above embodiments, multiple paths that simultaneously satisfy the U-turn of the engineering vehicle and other passing conditions can be determined from multiple alternative paths from the starting point to the ending point, which helps to further improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0073] In some embodiments, the sub-path with the minimum cost is the sub-path with the minimum total cost f(n).
[0074] Here, f(n) = a * h1(n) + b * h2(n) + g(n), where n represents the first path point where the engineering vehicle is currently located, and g(n) represents the cost of the engineering vehicle from the starting point to the first path point.
[0075] For each sub-path, h1(n) represents the first estimation of the cost of the sub-path, h2(n) represents the second estimation of the cost of the sub-path, a is positively correlated with the number of inflection points of the sub-path, b is positively correlated with the size of the obstacle, the first estimation is related to the number of inflection points of the sub-path, and the second estimation is not related to the number of inflection points of the sub-path.
[0076] The path planning method in this case can be called comprehensive planning. In some embodiments, comprehensive planning is applicable to the situation of comprehensively considering obstacles and the straightness of the path.
[0077] For example, the more the number of inflection points of the sub-path, the larger h1(n) is.
[0078] For example, a can be called the complex parameter of the map environment, and the value range of a is, for example, [0, 1]; for another example, b can be called the environmental obstacle parameter, and the value range of b is, for example, [0, 1].
[0079] As some implementation manners, f(n) can be called the evaluation function, and h1(n) and h2(n) can be called the heuristic functions.
[0080] As some implementation manners, in the case where a straighter path is desired to be planned, f(n) can be deformed into f(n) = h1(n) + g(n). The path planning method in this case can be called global planning. For example, global planning is applicable to the situation where it is known in advance that there are no obstacles or all the obstacles known to exist in advance are relatively small obstacles.
[0081] As some other implementation manners, in the case where there are no obstacles in the path to be planned, f(n) can be deformed into f(n) = h2(n) + g(n). The path planning method in this case can be called local planning. For example, local planning is applicable to the situation where all obstacles need to be avoided or all the obstacles known to exist in advance are relatively large obstacles.
[0082] In the above embodiments, when determining the next path point of the engineering vehicle, in addition to considering the cost of each sub-path, the cost of the engineering vehicle from the starting point to the first path point is also considered. The total cost f(n) is calculated by combining the formula, and the sub-path with the minimum total cost f(n) is determined as the sub-path with the minimum cost. In this way, using the total cost f(n) can more comprehensively consider the cost of the engineering vehicle from the starting point to the first path point and then from the first path point to the end point, making the obtained total cost f(n) more accurate, thereby helping to further improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0083] In some embodiments, for each sub-path, h2(n) satisfies at least one of the first condition, the second condition, and the third condition. For example, h2(n) satisfies any one, two, or all of the first condition, the second condition, and the third condition.
[0084] Here, the first condition is that h2(n) is positively correlated with the straight-line distance from the first path point to the end point, the second condition is that h2(n) is positively correlated with the congestion level of the road corresponding to the sub-path, and the third condition is that h2(n) is positively correlated with the angle between the first orientation and the second orientation. Here, the first orientation is the orientation of the front of the engineering vehicle when it travels to the first path point, and the second orientation is the preset orientation of the front of the engineering vehicle when it stops at the end point.
[0085] For example, when h2(n) satisfies the first condition, if the straight-line distance from the first path point to the end point is smaller, then h2(n) is smaller, that is, the second estimate of the cost of the sub-path is smaller.
[0086] For another example, when h2(n) satisfies the second condition, if the congestion level of the road corresponding to the sub-path is smaller, then h2(n) is smaller, that is, the second estimate of the cost of the sub-path is smaller.
[0087] For still another example, when h2(n) satisfies the third condition, if the angle between the first orientation and the second orientation is smaller, then h2(n) is smaller, that is, the second estimate of the cost of the sub-path is smaller.
[0088] As some embodiments, the congestion level of the road can be obtained by querying the network. For example, the congestion situation of the road displayed in the map navigation application software can be used as the congestion level of the road.
[0089] In the above embodiments, for each sub-path, at least one of the first condition, the second condition, and the third condition is used to constrain h2(n), so as to more reasonably calculate the second estimate of the cost of the sub-path, making the obtained total cost f(n) more accurate, which helps to further improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0090] In some embodiments, h2(n) satisfies the first condition. That is, h2(n) is positively correlated with the straight-line distance from the first path point to the end point. In this case, the straight-line distance from the first path point to the end point affects the proportion of b*h2(n) in f(n).
[0091] That is to say, if the straight-line distance from the first path point to the end point is larger, then the proportion of b*h2(n) in f(n) is larger, that is, when calculating f(n), the influence of b*h2(n) increases.
[0092] In the above embodiments, for each sub-path, the first condition is used to constrain h2(n), so as to consider the proportion of the straight-line distance from the first path point to the end point when calculating f(n), making the total cost f(n) more inclined to consider the straight-line distance from the first path point to the end point, which helps the engineering vehicle to travel from the first path point to the end point faster, thereby further improving the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0093] In some embodiments, in addition to satisfying the first condition, h2(n) also satisfies at least one of the second condition and the third condition. For example, h2(n) also satisfies any one or all of the second condition and the third condition.
[0094] In the above embodiments, for each sub-path, in addition to using the first condition to constrain h2(n), the second condition and / or the third condition are also used to constrain h2(n), so as to consider the proportion of different factors when calculating f(n), which helps to further improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0095] In some embodiments, h2(n) = v1*c(n) + v2*d(n) + v3*θ(n). Here, c(n) represents the congestion degree of the road, d(n) represents the straight-line distance from the first path point to the end point, θ(n) represents the included angle between the first orientation and the second orientation, v1 represents the weight of c(n), v2 represents the weight of d(n), and v3 represents the weight of θ(n).
[0096] Here, v1 is positively correlated with c(n), v2 is positively correlated with d(n), and v3 is positively correlated with θ(n), and v1, v2, and v3 are normalized.
[0097] For example, assume that the initial coefficient corresponding to the current c(n) is 10, the initial coefficient corresponding to the current d(n) is 50, and the initial coefficient corresponding to the current θ(n) is 40. Then v1 is equal to 10 / (10 + 50 + 40), that is, v1 is equal to 0.1. Similarly, v2 is equal to 0.5, and v3 is equal to 0.4.
[0098] In the above embodiments, when calculating h2(n), the congestion degree of the road, the straight-line distance from the first path point to the end point, and the included angle between the first orientation and the second orientation can be comprehensively considered, and the corresponding weights can be dynamically adjusted according to the relative magnitude relationship among the congestion degree, the straight-line distance, and the included angle, so that the calculated h2(n) comprehensively reflects the influence of the congestion degree, the straight-line distance, and the included angle between the first orientation and the second orientation, which helps to further improve the driving efficiency of the engineering vehicle in the autonomous driving mode.
[0099] In some embodiments, the hybrid A* algorithm is used to determine the next path point of the engineering vehicle.
[0100] Next, in combination with some embodiments of the present disclosure, introduce how to use the hybrid A* algorithm to determine the next path point of the first path point.
[0101] In step S1, determine the starting point and the end point of the engineering vehicle in the hybrid A* algorithm.
[0102] In step S2, initialize the open list and the closed list, and put the starting point of the engineering vehicle into the open list. Here, the open list is used to store the path points that need to be explored or will be explored next, and the closed list is used to store the path points that have been explored and the positions of obstacles.
[0103] In step S3, determine whether the open list is empty. If the open list is empty, it indicates that all path points have been explored, and the planning is directly terminated. If the open list is not empty, take the point with the minimum cost in the open list as the first path point (initially the starting point of the engineering vehicle), and remove it and put it into the closed list, indicating that this path point has been explored and will not be explored subsequently. Here, the evaluation function of the hybrid A* algorithm is f(n) mentioned above, and the heuristic functions are h1(n) and h2(n) mentioned above.
[0104] In step S4, evaluate whether the coincidence degree between the coverage range of the first path point and the expected coverage range of the end point of the engineering vehicle reaches a preset threshold to determine whether the engineering vehicle will reach the end point. If it is determined that the engineering vehicle will reach the end point, jump to step S5; if it is determined that the engineering vehicle has not reached the end point, jump to step S6. For example, the preset threshold range is 0.5.
[0105] As some embodiments, a coverage range of a path point can be obtained by taking the path point as the center and the minimum turning radius of the engineering vehicle as the radius.
[0106] In step S5, generate a path from the first path point to the end point and stop the path planning. As some embodiments, the trajectory of this path can be optimized by filtering so that the engineering vehicle can park at the end point more smoothly.
[0107] In step S6, take the first path point as the parent node and expand child nodes outward (i.e., the second path points), and calculate the cost from the first path point to each child node and then from the child node to the end point. Then, the step jumps to step S7.
[0108] In step S7, detect whether each child node is in the open list in sequence. If the child node is in the open list, update the cost corresponding to the child node in the open list and jump to step S3; if each child node is not in the open list, jump to step S8.
[0109] In step S8, each child node is sequentially detected to see if it is in the Closelist. If at least one child node is in the Closelist, then the at least one child node is not repeatedly added to the Openlist, and the process jumps to step S3.
[0110] If at least one child node is not in the Closelist, then the child node with the minimum cost among the at least one child node is added to the Openlist, and the process jumps to step S3. Thus, the process of path planning using the hybrid A* algorithm is completed.
[0111] In some embodiments, the trajectory between the first path point where the engineering vehicle is currently located and the next path point that the engineering vehicle is to travel to is optimized by the Reeds-Shepp curve algorithm. In this way, the trajectory between these two path points can be made smoother, which helps the engineering vehicle to travel smoothly in the autonomous driving mode.
[0112] Next, in combination with Figure 2 , it is introduced how to optimize the trajectory between the first path point and the next path point of the first path point by using the Reeds-Shepp curve algorithm.
[0113] Figure 2 is a schematic diagram of trajectory optimization according to some embodiments of the present disclosure.
[0114] In some embodiments, as Figure 2 shown, the first path point of the engineering vehicle is A, and the coverage range of the first path point is A1. Here, A1 is a circle with A as the center and the minimum turning radius of the engineering vehicle as the radius; B, C, D, E, F, and G are respectively the second path points planned by the Reeds-Shepp curve algorithm according to the position coordinates, heading angle of the engineering vehicle, and the minimum turning radius of the engineering vehicle.
[0115] D1 is a circle with D as the center and the minimum turning radius of the engineering vehicle as the radius, F1 is a circle with F as the center and the minimum turning radius of the engineering vehicle as the radius, and G1 is a circle with G as the center and the minimum turning radius of the engineering vehicle as the radius.
[0116] Assume that C is the next path point of the first path point. When optimizing using the Reeds-Shepp curve, it is determined that the circular arc L2 is the trajectory of the engineering vehicle from A to C. Here, C1 is a circle with C as the center and the minimum turning radius of the engineering vehicle as the radius.
[0117] From Figure 2 it can be seen that compared with the straight line L1 between A and C, the trajectory of the circular arc L2 is smoother and more suitable for the engineering vehicle to travel.
[0118] Similarly, when the next waypoint after the first waypoint is B, D, E, F, or G, an arc trajectory can be used to replace the straight-line trajectory based on the Reeds-Shepp curve algorithm.
[0119] It should be understood that when the engineering vehicle does not need to change its driving direction, the optimization using the Reeds-Shepp curve algorithm can be omitted.
[0120] It should be understood that only B, C, D, E, F, and G are exemplarily shown here as the second waypoints, and the Reeds-Shepp curve algorithm can plan more or fewer second waypoints according to the actual situation.
[0121] Each embodiment in this specification is described in a progressive manner. The key point of each embodiment is to illustrate the differences from other embodiments. For the same or similar parts among the embodiments, reference can be made to each other. For the device embodiments, since they basically correspond to the method embodiments, the description is relatively simple, and for the relevant parts, reference can be made to the description of the method embodiments.
[0122] In some embodiments, the path planning device of the engineering vehicle includes a module for executing the method of any one of the above embodiments.
[0123] Figure 3 It is a schematic structural diagram of a path planning device of an engineering vehicle according to some embodiments of the present disclosure.
[0124] As Figure 3 shown, the path planning device of the engineering vehicle includes a first determination module 301, a second determination module 302, and a third determination module 303.
[0125] The first determination module 301 is configured to determine multiple paths of the engineering vehicle from the starting point to the ending point according to reference information, where the reference information includes a static map, each path has multiple waypoints, and the multiple waypoints include inflection points, a starting point, and an ending point.
[0126] The second determination module 302 is configured to determine the cost of each sub-path of the engineering vehicle from the first waypoint to the ending point when there are multiple sub-paths between the first waypoint where the engineering vehicle is currently located and the ending point. Among them, the cost of each sub-path is positively correlated with the number of inflection points of the sub-path and positively correlated with the size of the obstacle between the first waypoint and the second waypoint, and the second waypoint is the next waypoint of the first waypoint on the sub-path.
[0127] The third determination module 303 is configured to determine the next waypoint that the engineering vehicle is to travel to according to the cost of each sub-path, where the next waypoint that the engineering vehicle is to travel to is the second waypoint on the sub-path with the minimum cost.
[0128] In some embodiments, the path planning device of the engineering vehicle may further include other modules to execute the path planning method of the engineering vehicle in any of the foregoing embodiments.
[0129] Figure 4 FIG. 4 is a schematic structural diagram of a path planning device of an engineering vehicle according to some other embodiments of the present disclosure.
[0130] As Figure 4 shown, the path planning device 400 of the engineering vehicle includes a memory 401 and a processor 402 coupled to the memory 401. The processor 402 is configured to execute the method in any of the foregoing embodiments based on instructions stored in the memory 401.
[0131] The memory 401 may include, for example, a system memory, a fixed non-volatile storage medium, etc. The system memory may store, for example, an operating system, application programs, a boot loader, and other programs.
[0132] In some embodiments, the path planning device 400 of the engineering vehicle may further include an input / output interface 403, a network interface 404, a storage interface 405, etc. The input / output interface 403, the network interface 404, and the storage interface 405, as well as between the memory 401 and the processor 402, may be connected through a bus 406, for example. The input / output interface 403 provides a connection interface for input / output devices such as a display, a mouse, a keyboard, and a touch screen. The network interface 404 provides a connection interface for various networking devices. The storage interface 405 provides a connection interface for external storage devices such as an SD card and a USB flash drive.
[0133] An embodiment of the present disclosure further provides an engineering vehicle, including: the path planning device of the engineering vehicle in any of the foregoing embodiments.
[0134] An embodiment of the present disclosure further provides a computer-readable storage medium, including computer program instructions, and when the computer program instructions are executed by a processor, the steps of the method in any of the foregoing embodiments are implemented.
[0135] An embodiment of the present disclosure further provides a computer program product, including a computer program, and when the computer program is executed by a processor, the steps of the method in any of the foregoing embodiments are implemented.
[0136] So far, the embodiments of the present disclosure have been described in detail. To avoid obscuring the concept of the present disclosure, some details well known in the art are not described. Those skilled in the art can clearly understand how to implement the technical solutions disclosed herein based on the above description.
[0137] Those skilled in the art should understand that the embodiments of the present disclosure can be provided as methods, systems, or computer program products. Therefore, the present disclosure can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present disclosure can take the form of a computer program product implemented on one or more computer-usable non-transitory storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.
[0138] The present disclosure is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to the embodiments of the present disclosure. It should be understood that the functions specified in one or more processes in the flowchart and / or one or more blocks in the block diagram can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate a means for implementing the functions specified in Figure 1 one or more processes and / or blocks Figure 1 one or more blocks.
[0139] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including an instruction means that implements the functions specified in Figure 1 one or more processes and / or blocks Figure 1 one or more blocks.
[0140] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in Figure 1 one or more processes and / or blocks Figure 1 one or more blocks.
[0141] Although some specific embodiments of the present disclosure have been described in detail by way of examples, those skilled in the art should understand that the above examples are only for illustration and not for limiting the scope of the present disclosure. Those skilled in the art should understand that the above embodiments can be modified or partial technical features can be equivalently replaced without departing from the scope and spirit of the present disclosure. The scope of the present disclosure is defined by the appended claims.
Claims
1. A path planning method for an engineering vehicle, comprising: Determine multiple paths of the engineering vehicle from a starting point to an end point according to reference information, wherein the reference information includes a static map, each path has multiple path points, and the multiple path points include an inflection point, the starting point, and the end point; In the case where there are multiple sub-paths between a first path point where the engineering vehicle is currently located and the end point, determining the cost of each sub-path of the engineering vehicle from the first path point to the end point, wherein the cost of each sub-path is positively correlated with the number of turning points of the sub-path and with the size of an obstacle between the first path point and a second path point, the second path point being the next path point of the first path point on the sub-path; According to the cost of each sub-path, the next path point to which the engineering vehicle is to travel is determined, wherein the next path point to which the engineering vehicle is to travel is the second path point on the sub-path with the minimum cost.
2. The method according to claim 1, wherein: The reference information further includes at least one group of information, the at least one group of information including at least one of a first group of information, a second group of information, a third group of information and a fourth group of information, the first group of information including the width of the engineering vehicle and the width limit of the road corresponding to each alternative path, the second group of information including the height of the engineering vehicle and the height limit of the road, the third group of information including the weight of the engineering vehicle and the maximum load-bearing weight of the road, and the fourth group of information including the minimum turning radius of the engineering vehicle and the minimum turning radius of the road; Determining the multiple paths of the engineering vehicle from the starting point to the end point according to the reference information includes: Determine a plurality of alternative paths from the starting point to the end point according to the starting point, the end point and the static map; The multiple paths are determined from the multiple candidate paths according to the at least one set of information.
3. The method according to claim 2, wherein: The at least one set of information includes the fourth set of information.
4. The method according to claim 3, wherein: The at least one set of information also includes at least one of the first set of information, the second set of information, and the third set of information.
5. The method according to any one of claims 1 to 4, wherein: The subpath with the minimum cost is the subpath with the minimum total cost f(n), where: f(n)=a*h1(n)+b*h2(n)+g(n), n represents the first path point, g(n) represents the cost of the engineering vehicle from the starting point to the first path point, For each sub-path, h1(n) represents the first estimate of the cost of the sub-path, h2(n) represents the second estimate of the cost of the sub-path, a is positively correlated with the number of inflection points of the sub-path, and b is positively correlated with the size of the obstacle, wherein the first estimate is related to the number of inflection points of the sub-path, and the second estimate is not related to the number of inflection points of the sub-path.
6. The method according to claim 5, wherein: For each subpath, h2(n) satisfies at least one of the first condition, the second condition, and the third condition, where: The first condition is that h2(n) is positively correlated with the straight-line distance from the first path point to the end point; The second condition is that h2(n) is positively correlated with the congestion level of the road corresponding to the sub-path; and The third condition is that h2(n) is positively correlated with the angle between the first orientation and the second orientation, wherein the first orientation is the orientation of the front of the vehicle when the engineering vehicle travels to the first path point, and the second orientation is the preset orientation of the front of the vehicle when the engineering vehicle is parked at the end point.
7. The method according to claim 6, wherein: h2(n) satisfies the first condition.
8. The method according to claim 7, wherein: h2(n) also satisfies at least one of the second condition and the third condition.
9. The method according to claim 8, wherein: h2(n)=v1*c(n)+v2*d(n)+v3*θ(n), Among them, c(n) represents the congestion degree, d(n) represents the straight-line distance, θ(n) represents the angle, v1 represents the weight of c(n), v2 represents the weight of d(n), and v3 represents the weight of θ(n). Among them, v1 is positively correlated with c(n), v2 is positively correlated with d(n), and v3 is positively correlated with θ(n). v1, v2 and v3 are normalized.
10. The method according to any one of claims 1 to 4, wherein: The trajectory between the first path point and the next path point to which the engineering vehicle is to travel is optimized via a Reeds-Shepp curve algorithm.
11. A path planning device for an engineering vehicle, comprising: A module configured to execute the method according to any one of claims 1 to 10.
12. A path planning device for an engineering vehicle, comprising: Memory; as well as A processor coupled to the memory, configured to execute the method according to any one of claims 1 to 10 based on instructions stored in the memory.
13. An engineering vehicle, comprising: A path planning device for an engineering vehicle as described in claim 11 or 12.
14. A computer-readable storage medium comprising a computer program, wherein: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 10 are implemented.
15. A computer program product comprising a computer program, wherein: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 10 are implemented.