An energy-optimal-based trajectory planning method for unmanned vehicle

Abstract

The application provides an unmanned vehicle trajectory planning method based on energy optimization, comprising: updating complex irregular obstacle information of surroundings in the unmanned vehicle based on point cloud information; calculating posterior probability of a grid according to point cloud information fed back each time, and constructing an occupancy grid map in real time; searching for path points for avoiding obstacles by using an A* algorithm based on vehicle kinematics according to terrain and environment information constructed by a sensor; obtaining a series of relatively dense path points by using a path backtracking method, and performing curve fitting by using a uniform B-spline curve without control points; constructing an Euclidean distance map to further construct an unconstrained optimization problem about minimum energy consumption, driving speed, driving acceleration and obstacle avoidance distance, and solving the optimization problem by using an open source solver to obtain an optimal trajectory with optimal energy consumption, satisfying dynamic feasibility and being able to avoid obstacles; and performing time redistribution on the optimal trajectory, so that the speed and acceleration values corresponding to the trajectory points do not exceed a threshold value.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle motion planning technology, and more particularly to an energy-optimal unmanned vehicle trajectory planning method. Background Technology

[0002] With the development of communication technology and simultaneous localization and mapping (SMR) technology, in recent years, intelligence, connectivity, sharing, and electrification have gradually become the main characteristics of autonomous vehicles. The emergence and development of autonomous vehicles have significantly enhanced the coupling relationship between vehicles and roads, posing newer and more complex requirements for the road infrastructure construction system closely related to them. Given the rapid development of autonomous driving, autonomous navigation based on visual information or point cloud information from LiDAR feedback has seen more and more prominent applications. Autonomous vehicles rely on environmental modeling information obtained from sensors to make autonomous decisions about their surroundings and plan their routes for a future period. The trajectory planning technology used in this process has seen significant development in recent years. More and more countries and experimental institutions have begun to study it, achieving considerable progress and results, greatly promoting the intelligentization trend of the automotive industry. Trajectory planning technology can be divided into trajectory planning methods based on the shortest time, trajectory planning methods based on optimal energy consumption, and trajectory planning methods based on simultaneous multi-objective optimization, depending on the trajectory optimization variables. Among these, trajectory planning methods based on simultaneous multi-objective optimization are more widely used. Most studies use variables such as trajectory energy consumption, dynamic feasibility, and obstacle avoidance distance as optimization objectives to achieve autonomous navigation of unmanned vehicles in complex obstacle environments.

[0003] Most existing methods for autonomous vehicle trajectory planning rely on road boundary construction sampling problems or quadratic programming problems. These existing algorithms have the following problems:

[0004] One approach is trajectory planning based on sampling. This involves calculating the cost function for each of the selected trajectories and choosing the optimal trajectory. While the sampling process is relatively simple, calculating the trajectory penalty function is time-consuming, making it unsuitable for autonomous vehicles to make real-time decisions and plans in complex environments.

[0005] Second, the trajectory planning algorithm uses a relatively simple quadratic programming algorithm, which is prone to no solution in some complex environments, that is, the trajectory cannot be obtained. This can pose a certain threat to vehicle safety. It does not consider the constraint of minimum energy consumption and lacks consideration of dynamic feasibility, and cannot better provide reasonable speed and acceleration for autonomous vehicles.

[0006] Third, most existing autonomous vehicle trajectory planning methods rely on curves fitted by fifth-order polynomials or on optimizing them to generate drivable trajectories. The calculation process is relatively complex and involves a large number of parameters, which prolongs the decision-making and planning time of autonomous vehicles. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides an energy-optimal unmanned vehicle trajectory planning method. This method constructs a grid map and an Euclidean distance map based on point cloud information, generates the front-end path based on the occupied grid map, and constructs an unconstrained optimization problem with respect to trajectory parameters based on the Euclidean distance map to solve for the optimal trajectory of the unmanned vehicle.

[0008] The technical means employed in this invention are as follows:

[0009] An energy-optimal autonomous vehicle trajectory planning method includes:

[0010] Based on point cloud information within a certain range, the information on complex and irregular obstacles around the autonomous vehicle is continuously updated. The posterior probability of the grid is calculated based on the point cloud information fed back each time, and the occupied grid map is constructed in real time.

[0011] Based on the terrain and environmental information constructed by the sensors, the A* algorithm based on vehicle kinematics is used to search for path points to avoid obstacles.

[0012] A series of relatively dense path points are obtained by using the path backtracking method, and curve fitting is performed using a uniform B-spline curve that does not pass through the control points.

[0013] We construct an Euclidean distance map to further develop an unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance. We then use an open-source solution library to solve the problem and obtain an optimal trajectory that is energy-efficient, meets dynamic feasibility requirements, and can avoid obstacles.

[0014] The optimal trajectory is time-redistributed so that the velocity and acceleration values ​​corresponding to the trajectory points do not exceed the threshold.

[0015] Furthermore, the process of continuously updating the information on complex and irregular obstacles surrounding the autonomous vehicle based on point cloud information within a certain range, calculating the posterior probability of the grid based on each feedback of point cloud information, and constructing an occupied grid map in real time includes:

[0016] Let the grid size be m i The occupancy of the grid is modeled using binary random variables;

[0017] The grid state is divided into three types: occupied, idle, and unknown, with corresponding probabilities of 1, 0, and 0.5, respectively. This means that the random variables of each grid are independent of each other. According to the Markov assumption, the current observation is independent of the previous observations. Therefore, the update formula is as follows:

[0018]

[0019] For a specific sensing instrument, the point cloud information updated each time is a fixed value. The sensor data of the tth time will always complete the update of the occupied grid map based on the existing grid state of the t-1th time.

[0020] Furthermore, the step of searching for path points to avoid obstacles using the A* algorithm based on vehicle kinematics, based on the terrain and environmental information constructed by the sensors, includes:

[0021] The starting point is converted into the address corresponding to the grid map and stored. Each time a node search is performed, the vehicle's maximum turning radius, travel speed and total movement time are considered.

[0022] By sampling the three values ​​for different reasons, a series of nodes are obtained, maintaining both an open set and a closed set. The underlying vehicle kinematics formula is shown below:

[0023]

[0024] Let the current state of the node where the vehicle is located be After performing a neighbor node search, the node state is: The formula for node expansion is shown below:

[0025]

[0026] The two-dimensional Euclidean distance between the expanded neighbor nodes and the target node is calculated as the basis for selecting path points from the expanded points, thereby obtaining a series of optimal path points.

[0027] Furthermore, the open set stores a series of nodes obtained from the sampling. After each node search is completed, the node with the smallest total value is found from the open set as the current node. Then, the node search is repeated from the current node to obtain the node to be checked, and this process is repeated until the end. The closed set stores nodes that are no longer checked. Each time a current node is selected and added to the open set, this current node becomes a node that is no longer checked, so it must be added to the closed set to prevent it from being checked again. If the open set is empty, it means that the entire map has been searched and no path has been found. If the open set is not empty, the path points are backtracked along the parent node of the current node according to the set time to obtain the path point data of the front end avoiding obstacles.

[0028] Furthermore, the method of path backtracking is used to obtain a series of relatively dense path points, and curve fitting is performed using a uniform B-spline curve that does not pass through control points, including:

[0029] Based on vehicle kinematics A*, N+1 path points {Q0, Q1, Q2, ..., Q} are obtained to avoid obstacles. N Let the order of the B-spline curve be p. b (p b =3), and for N+1 control points, there are M node vectors {t0,t1,t2,...,t... M And the node vector t i For any node ∈ R, the formula for calculating the number of nodes M is as follows:

[0030] M = N + p b +1

[0031] For a uniform B-spline curve, each segment's nodal vector Δt = t i+1 -t i Given a unique value, set a normalization variable u, and calculate using the following formula:

[0032]

[0033] Let the parametric expression of the B-spline curve be p(u), the normalized variable matrix be U, the coefficient matrix be M3, and the corresponding control point matrix be q. m The corresponding expression is shown below:

[0034]

[0035] The fitted 3rd-order B-spline curve described above is converted into the following expression:

[0036]

[0037] Where p = 3, the velocity and acceleration of the trajectory point can be further solved using the following derivative formula:

[0038]

[0039] The derivative of the velocity at the trajectory point is shown in the following formula:

[0040]

[0041] The velocity of the trajectory points is calculated using the following expression:

[0042]

[0043] The velocities of the trajectory points are shown below:

[0044]

[0045] Similarly, taking the second derivative of the given trajectory expression, we obtain the following expression:

[0046]

[0047] The acceleration of the trajectory point is calculated as follows:

[0048]

[0049] Since the B-spline curve is a uniform B-spline curve and the nodal vectors between the control points are the same, the expression for the velocity acceleration is as follows:

[0050]

[0051] Similarly, the expression for jerk is as follows:

[0052]

[0053] Furthermore, the construction of the Euclidean distance map further constructs an unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance. This problem is solved using an open-source solution library to obtain an optimal trajectory that is energy-efficient, dynamically feasible, and capable of avoiding obstacles, including:

[0054] The unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance is constructed as follows:

[0055] f total =λ1f s +λ2f c +λ3(f v +f a )

[0056] Among them, regarding the minimum energy consumption f s The form of the construction problem and the expression for the first derivative are shown below:

[0057]

[0058] Regarding obstacle avoidance distance f c The form of the construction problem and the expression for the first derivative are shown below:

[0059]

[0060] Among them, the derivative of the distance function It can be obtained by trilinear interpolation on a Euclidean distance map;

[0061] Regarding velocity f v The form of the construction problem and the expression for the first derivative are shown below:

[0062]

[0063] Regarding acceleration f a The form of the construction problem and the expression for the first derivative are shown below:

[0064]

[0065] The above unconstrained optimization problem is solved to obtain the optimized trajectory.

[0066] Furthermore, the step of time-redistributing the optimal trajectory so that the velocity and acceleration values ​​corresponding to the trajectory points do not exceed the threshold includes:

[0067] Let the velocity and acceleration values ​​outside the trajectory be v, respectively. m ,a m The maximum speed is set to v. max The corresponding node vector intervals are [u i+1 ,u i+p+1 ],[u i+2 ,u i+p+1 ], where the adjusted time interval is [u1 i+1 u1 i+p+1 ],[u1 i+2 u1 i+p+1 The adjusted velocity and acceleration are v. f ,a f The adjusted velocity ratio and acceleration ratio are μ, where p is the curve order, which is equal to 3. The adjustment formula is as follows:

[0068]

[0069] Compared with the prior art, the present invention has the following advantages:

[0070] 1. Compared with existing trajectory planning algorithms for autonomous vehicles that use quadratic programming with road boundaries as constraints, this invention uses a method of solving unconstrained optimization problems to find the optimal trajectory, which can also obtain a trajectory that satisfies the constraints in complex environments.

[0071] 2. Compared with existing trajectory planning algorithms for unmanned vehicles that select the optimal trajectory based on sampling, this invention only optimizes and solves one trajectory planned by the unmanned vehicle, which greatly reduces the solution time and provides more safety assurance for the real-time planning of unmanned vehicles in local environments.

[0072] 3. Compared with existing trajectory planning algorithms for autonomous vehicles based on grid maps, this invention further constructs an Euclidean distance map and uses gradient information to construct an unconstrained optimization problem about obstacle avoidance and collision avoidance distance, enriching and improving map information and providing more usable conditions for subsequent trajectory planning.

[0073] Based on the above reasons, this invention can be widely applied in fields such as motion planning for unmanned vehicles. Attached Figure Description

[0074] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0075] Figure 1 This is a flowchart of the method of the present invention.

[0076] Figure 2 This is a schematic diagram of the A* algorithm based on vehicle kinematics of the present invention.

[0077] Figure 3 This is a schematic diagram of the B-spline curve fitting of the obtained path points according to the present invention.

[0078] Figure 4 This is a schematic diagram illustrating the trajectory planning of the unmanned vehicle in a simulation environment according to the present invention.

[0079] Figure 5 This is a schematic diagram of the speed obtained after trajectory planning for the unmanned vehicle of the present invention.

[0080] Figure 6 This is a schematic diagram of the acceleration obtained after trajectory planning for the unmanned vehicle of the present invention.

[0081] Figure 7 This is a schematic diagram showing the comparison of obstacle avoidance distances before and after optimization, obtained by the unmanned vehicle of this invention after trajectory planning.

[0082] Figure 8 This is a schematic diagram comparing the trajectories before and after optimization obtained by the unmanned vehicle of this invention after trajectory planning. Detailed Implementation

[0083] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0084] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the present invention or its application or use. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0085] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0086] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps described in these embodiments do not limit the scope of the invention. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following figures denote similar items; therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0087] In the description of this invention, it should be understood that the orientation or positional relationship indicated by directional terms such as "front, back, up, down, left, right", "horizontal, vertical, horizontal" and "top, bottom" is generally based on the orientation or positional relationship shown in the accompanying drawings, and is only for the convenience of describing this invention and simplifying the description. Unless otherwise stated, these directional terms do not indicate or imply that the device or element referred to must have a specific orientation or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on the scope of protection of this invention. The directional terms "inner" and "outer" refer to the inner and outer contours relative to the outline of each component itself.

[0088] For ease of description, spatial relative terms such as "above," "over," "on the upper surface of," "above," etc., are used herein to describe the spatial positional relationship of a device or feature as shown in the figures to other devices or features. It should be understood that spatial relative terms are intended to encompass different orientations in use or operation besides the orientation of the device as described in the figures. For example, if the device in the figures is inverted, a device described as "above" or "above" other devices or structures would subsequently be positioned as "below" or "under" other devices or structures. Thus, the exemplary term "above" can include both "above" and "below." The device may also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatial relative descriptions used herein will be interpreted accordingly.

[0089] Furthermore, it should be noted that the use of terms such as "first" and "second" to define components is merely for the purpose of distinguishing the corresponding components. Unless otherwise stated, the above terms have no special meaning and therefore should not be construed as limiting the scope of protection of this invention.

[0090] like Figure 1 As shown, this invention provides an energy-optimal autonomous vehicle trajectory planning method, including:

[0091] S1. Based on point cloud information within a certain range, continuously update the information of complex and irregular obstacles around the unmanned vehicle, calculate the posterior probability of the grid based on the point cloud information fed back each time, and construct the occupied grid map in real time.

[0092] S2. Based on the terrain and environmental information constructed by the sensors, the A* algorithm based on vehicle kinematics is used to search for path points to avoid obstacles.

[0093] S3. A series of relatively dense path points are obtained by using the path backtracking method, and curve fitting is performed using a uniform B-spline curve that does not pass through the control points.

[0094] S4. Construct an Euclidean distance map to further construct an unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance. Use an open-source solution library to solve the problem and obtain an optimal trajectory that has the best energy consumption, satisfies dynamic feasibility, and can avoid obstacles.

[0095] S5. Time reallocation is performed on the optimal trajectory so that the velocity and acceleration values ​​corresponding to the trajectory points do not exceed the threshold.

[0096] In a specific implementation, as a preferred embodiment of the present invention, in step S1, based on point cloud information within a certain range, the information on complex and irregular obstacles surrounding the unmanned vehicle is continuously updated, and the posterior probability of the grid is calculated based on the point cloud information fed back each time, and an occupied grid map is constructed in real time, including:

[0097] Let the grid size be m i The occupancy of the grid is modeled using binary random variables;

[0098] The grid state is divided into three types: occupied, idle, and unknown, with corresponding probabilities of 1, 0, and 0.5, respectively. This means that the random variables of each grid are independent of each other. According to the Markov assumption, the current observation is independent of the previous observations. Therefore, the update formula is as follows:

[0099]

[0100] For a specific sensing instrument such as a camera or lidar, the point cloud information updated each time is a fixed value. The sensor data of the tth time will always complete the update of the occupied grid map based on the existing grid state of the t-1th time.

[0101] In a specific implementation, as a preferred embodiment of the present invention, in step S2, based on the terrain and environmental information constructed by the sensors, the A* algorithm based on vehicle kinematics is used to search for path points to avoid obstacles, such as... Figure 2 As shown, it includes:

[0102] The starting point is converted into the address corresponding to the grid map and stored. Each time a node search is performed, the vehicle's maximum turning radius, travel speed and total movement time are considered.

[0103] Sampling is performed on the three values ​​with different values ​​to obtain a series of nodes. An open set and a closed set are maintained. The open set stores the series of nodes obtained from the sampling. After each node search, the node with the smallest total value is selected from the open set as the current node. Then, the node search is repeated from the current node to obtain the node to be checked, and this process continues until the end. The closed set stores nodes that are no longer checked. Each time a current node is selected and added to the open set, it becomes a node that is no longer checked, so it must be added to the closed set to prevent it from being checked again. If the open set is empty, it means that the entire map has been searched without finding a path. If the open set is not empty, path point backtracking is performed along the parent node of the current node according to the set time to obtain the path point data for the front end to avoid obstacles. The vehicle kinematics formulas used are as follows:

[0104]

[0105] Let the current state of the node where the vehicle is located be After performing a neighbor node search, the node state is: The formula for node expansion is shown below:

[0106]

[0107] The two-dimensional Euclidean distance between the expanded neighbor nodes and the target node is calculated as the basis for selecting path points from the expanded points, thereby obtaining a series of optimal path points.

[0108] In a specific implementation, as a preferred embodiment of the present invention, in step S3, a series of relatively dense path points are obtained using a path backtracking method, and curve fitting is performed using a uniform B-spline curve that does not pass through control points, such as... Figure 3 As shown, the fitted path curve is obtained at the front end, specifically including:

[0109] Based on vehicle kinematics A*, N+1 path points {Q0, Q1, Q2, ..., Q} are obtained to avoid obstacles. N Let the order of the B-spline curve be p. b (p b =3), and for N+1 control points, there are M node vectors {t0,t1,t2,...,t... M And the node vector t i For any node ∈ R, the formula for calculating the number of nodes M is as follows:

[0110] M = N + p b +1

[0111] For a uniform B-spline curve, each segment's nodal vector Δt = t i+1 -t i Given a unique value, set a normalization variable u, and calculate using the following formula:

[0112]

[0113] Let the parametric expression of the B-spline curve be p(u), the normalized variable matrix be U, the coefficient matrix be M3, and the corresponding control point matrix be q. m The corresponding expression is shown below:

[0114]

[0115] The fitted 3rd-order B-spline curve described above is converted into the following expression:

[0116]

[0117] Where p = 3, the velocity and acceleration of the trajectory point can be further solved using the following derivative formula:

[0118]

[0119] The derivative of the velocity at the trajectory point is shown in the following formula:

[0120]

[0121] The velocity of the trajectory points is calculated using the following expression:

[0122]

[0123] The velocities of the trajectory points are shown below:

[0124]

[0125] Similarly, taking the second derivative of the given trajectory expression, we obtain the following expression:

[0126]

[0127] The acceleration of the trajectory point is calculated as follows:

[0128]

[0129] Since the B-spline curve is a uniform B-spline curve and the nodal vectors between the control points are the same, the expression for the velocity acceleration is as follows:

[0130]

[0131] Similarly, the expression for jerk is as follows:

[0132]

[0133] In a preferred embodiment of the present invention, step S4 involves constructing an Euclidean distance map to further develop an unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance. This problem is solved using an open-source solution library to obtain an optimal trajectory that is energy-efficient, meets dynamic feasibility requirements, and can avoid obstacles. This includes:

[0134] The unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance is constructed as follows:

[0135] f total =λ1f s +λ2f c +λ3(f v +f a )

[0136] Among them, regarding the minimum energy consumption f s The form of the construction problem and the expression for the first derivative are shown below:

[0137]

[0138] Regarding obstacle avoidance distance f c The form of the construction problem and the expression for the first derivative are shown below:

[0139]

[0140] Among them, the derivative of the distance function It can be obtained by trilinear interpolation on a Euclidean distance map;

[0141] Regarding velocity f v The form of the construction problem and the expression for the first derivative are shown below:

[0142]

[0143] Regarding acceleration f a The form of the construction problem and the expression for the first derivative are shown below:

[0144]

[0145] Solving the above unconstrained optimization problem yields the optimized trajectory. For example... Figure 4 As shown, the two lines represent the trajectory of the unmanned vehicle fitted with the B-spline curve and the optimized trajectory, respectively. Figure 5 , Figure 6 , Figure 7 , Figure 8 This is a diagram showing the speed, acceleration, obstacle avoidance distance before and after optimization, and trajectory comparison before and after optimization obtained after the autonomous vehicle performs trajectory planning.

[0146] In a specific implementation, as a preferred embodiment of the present invention, step S5, which involves time reallocation of the optimal trajectory to ensure that the velocity and acceleration values ​​corresponding to the trajectory points do not exceed a threshold, includes:

[0147] Let the velocity and acceleration values ​​outside the trajectory be v, respectively. m ,a m The maximum speed is set to v. max The corresponding node vector intervals are [u i+1 ,u i+p+1 ],[u i+2 ,u i+p+1 ], where the adjusted time interval is [u1 i+1 u1 i+p+1 ],[u1 i+2u1 i+p+1 The adjusted velocity and acceleration are v. f ,a f The adjusted velocity ratio and acceleration ratio are μ, where p is the curve order, which is equal to 3. The adjustment formula is as follows:

[0148]

[0149] Before trajectory planning, the autonomous vehicle initializes its position and attitude based on the feedback point cloud information, determining the origin of its coordinates. Then, a target point is selected, and the vehicle begins trajectory planning, continuously performing local planning based on the perceived area and performing collision detection on the generated optimized trajectory. If a collision is detected, the trajectory is replanned at the current position, and a new executable trajectory is generated. When the vehicle's position is close to the target point, trajectory planning stops, and boundary constraints are applied to ensure that the vehicle reaches the destination at a near-zero speed. Because the unconstrained optimization problem optimizes the vehicle's minimum energy consumption, energy loss during travel is minimized.

[0150] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An energy-optimal trajectory planning method for unmanned vehicles, characterized in that, include: Based on point cloud information within a certain range, the information on complex and irregular obstacles around the autonomous vehicle is continuously updated. The posterior probability of the grid is calculated based on the point cloud information fed back each time, and the occupied grid map is constructed in real time. Based on the terrain and environmental information constructed by the sensors, the A* algorithm based on vehicle kinematics is used to search for path points to avoid obstacles. A series of relatively dense path points are obtained by using the path backtracking method, and curve fitting is performed using a uniform B-spline curve that does not pass through the control points. We construct an Euclidean distance map to further develop an unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance. We then use an open-source solution library to solve the problem and obtain an optimal trajectory that is energy-efficient, meets dynamic feasibility requirements, and can avoid obstacles. The optimal trajectory is time-redistributed so that the velocity and acceleration values ​​corresponding to the trajectory points do not exceed the threshold.

2. The energy-optimal unmanned vehicle trajectory planning method according to claim 1, characterized in that, The process of continuously updating the information on complex and irregular obstacles surrounding the autonomous vehicle based on point cloud information within a certain range, calculating the posterior probability of the grid based on each feedback of point cloud information, and constructing an occupied grid map in real time includes: Let the grid size be m i The occupancy of the grid is modeled using binary random variables; The grid state is divided into three types: occupied, idle, and unknown, with corresponding probabilities of 1, 0, and 0.5, respectively. This means that the random variables of each grid are independent of each other. According to the Markov assumption, the current observation is independent of the previous observations. Therefore, the update formula is as follows: For a specific sensing instrument, the point cloud information updated each time is a fixed value. The sensor data of the tth time will always complete the update of the occupied grid map based on the existing grid state of the t-1th time.

3. The energy-optimal unmanned vehicle trajectory planning method according to claim 1, characterized in that, The step of searching for path points to avoid obstacles using the A* algorithm based on vehicle kinematics, based on terrain and environmental information constructed by sensors, includes: The starting point is converted into the address corresponding to the grid map and stored. Each time a node search is performed, the vehicle's maximum turning radius, travel speed and total movement time are considered. By sampling the three values ​​for different reasons, a series of nodes are obtained, maintaining both an open set and a closed set. The underlying vehicle kinematics formula is shown below: Let the current state of the node where the vehicle is located be After performing a neighbor node search, the node state is: The formula for node expansion is shown below: The two-dimensional Euclidean distance between the expanded neighbor nodes and the target node is calculated as the basis for selecting path points from the expanded points, thereby obtaining a series of optimal path points.

4. The energy-optimal unmanned vehicle trajectory planning method according to claim 3, characterized in that, The open set stores a series of nodes obtained from the sampling. After each node search is completed, the node with the smallest total value is found from the open set as the current node. Then, the node search is repeated from the current node to obtain the node to be checked. This process is repeated until the end. The closed set stores nodes that are no longer checked. Each time a current node is selected and added to the open set, this current node becomes a node that is no longer checked. Therefore, it must be added to the closed set to prevent it from being checked again. If the open set is empty, it means that all maps have been searched and no path has been found; If the open set is not empty, then backtrack along the parent node of the current node according to the set time to obtain the path point data of the front end avoiding obstacles.

5. The energy-optimal unmanned vehicle trajectory planning method according to claim 1, characterized in that, The method of path backtracking is used to obtain a series of relatively dense path points, and curve fitting is performed using a uniform B-spline curve that does not pass through control points, including: Based on vehicle kinematics A*, N+1 path points {Q0, Q1, Q2, ..., Q} are obtained to avoid obstacles. N Let the order of the B-spline curve be p. b (p b =3), and for N+1 control points, there are M node vectors {t0,t1,t2,...,t... M And the node vector t i For any node ∈ R, the formula for calculating the number of nodes M is as follows: M=N+p b +1 For a uniform B-spline curve, each segment's nodal vector Δt = t i+1 -t i Given a unique value, set a normalization variable u, and calculate using the following formula: Let the parametric expression of the B-spline curve be p(u), the normalized variable matrix be U, the coefficient matrix be M3, and the corresponding control point matrix be q. m The corresponding expression is shown below: The fitted 3rd-order B-spline curve described above is converted into the following expression: Where p = 3, the velocity and acceleration of the trajectory point can be further solved using the following derivative formula: The derivative of the velocity at the trajectory point is shown in the following formula: The velocity of the trajectory points is calculated using the following expression: The velocities of the trajectory points are shown below: Similarly, taking the second derivative of the given trajectory expression, we obtain the following expression: The acceleration of the trajectory point is calculated as follows: Since the B-spline curve is a uniform B-spline curve and the nodal vectors between the control points are the same, the expression for the velocity acceleration is as follows: Similarly, the expression for jerk is as follows:

6. The energy-optimal unmanned vehicle trajectory planning method according to claim 1, characterized in that, The construction of the Euclidean distance map further establishes an unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance. An open-source solution library is used to solve this problem, resulting in an optimal trajectory that is energy-efficient, dynamically feasible, and capable of avoiding obstacles. This includes: The unconstrained optimization problem concerning minimum energy consumption, driving speed, driving acceleration, and obstacle avoidance distance is constructed as follows: f total =λ1f s +λ2f c +λ3(f v +f a ) Among them, regarding the minimum energy consumption f s The form of the construction problem and the expression for the first derivative are shown below: Regarding obstacle avoidance distance f c The form of the construction problem and the expression for the first derivative are shown below: Among them, the derivative of the distance function It can be obtained by trilinear interpolation on a Euclidean distance map; Regarding velocity f v The form of the construction problem and the expression for the first derivative are shown below: Regarding acceleration f a The form of the construction problem and the expression for the first derivative are shown below: The above unconstrained optimization problem is solved to obtain the optimized trajectory.

7. The energy-optimal unmanned vehicle trajectory planning method according to claim 1, characterized in that, The step of time-redistributing the optimal trajectory so that the velocity and acceleration values ​​corresponding to the trajectory points do not exceed the threshold includes: Let the velocity and acceleration values ​​outside the trajectory be v, respectively. m ,a m The maximum speed is set to v. max The corresponding node vector intervals are [u i+1 ,u i+p+1 ],[u i+2 ,u i+p+1 ], where the adjusted time interval is [u1 i+1 u1 i+p+1 ],[u1 i+2 u1 i+p+1 The adjusted velocity and acceleration are v. f ,a f The adjusted velocity ratio and acceleration ratio are μ, where p is the curve order, which is equal to 3. The adjustment formula is as follows:

Images