A multi-constraint trajectory planning method for intelligent vehicles based on multi-dimensional laser radar point cloud information

Abstract

The application provides a kind of intelligent car multi-constraint trajectory planning method based on multi-dimensional laser radar point cloud information, it is related to intelligent car motion planning technical field, the method of the present application first constructs dense point cloud map using A-LOAM algorithm according to the point cloud information emitted by multi-dimensional laser radar, then on the basis of obtaining the dense point cloud map of surrounding environment, the method of curve fitting based on minimum jerk is used, the optimal trajectory is solved in state space by giving the planning starting point and end point position, velocity, acceleration, and the time is further discretized to generate front path point. The uniform B-spline curve without control points is used for curve fitting, and the unconstrained optimization problem about curve smoothness, intelligent car driving speed, intelligent car driving acceleration, obstacle avoidance distance, end point arrival distance is further constructed according to the existing dense occupancy grid map, the quasi-Newton method is used to solve the unconstrained optimization problem, and the optimal trajectory of the intelligent car in complex environment is obtained.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle motion planning technology, and more particularly to an intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional lidar point cloud information. Background Technology

[0002] In recent years, the development of intelligent vehicles has been rapid. Intelligent vehicles refer to a new generation of automobiles equipped with advanced sensors and utilizing simultaneous localization and real-time mapping or target detection technologies to enable autonomous decision-making and navigation in complex environments. Intelligent vehicles are also commonly known as intelligent connected vehicles or autonomous driving vehicles. In recent years, trajectory planning has become a hot research topic in the field of intelligent vehicles. Many countries and laboratories have invested significant human and financial resources in this area, greatly promoting the development of trajectory planning technology. The realization of trajectory planning technology relies heavily on high-precision maps constructed using advanced sensor technologies, such as point cloud maps constructed by LiDAR or cameras. During the autonomous driving process of intelligent vehicles, the vehicle's built-in perception system perceives the surrounding environment in real time and simultaneously constructs maps reflecting obstacle information and pedestrians on the road. It can also simultaneously perceive its own information such as speed, acceleration, and position during driving. Based on a designed finite state machine, it makes corresponding behavioral decisions and trajectory planning under different conditions or environments, and issues appropriate control commands to the vehicle chassis to ensure safe driving and autonomous obstacle avoidance during operation. The task of trajectory planning is to calculate a curve that avoids collisions and obstacles, and includes speed and path information. Essentially, this is a multi-objective mathematical optimization problem, primarily addressing the optimization of speed, acceleration, safety, and comfort during the autonomous vehicle's operation, and solving these problems to obtain an optimal trajectory suitable for the intelligent vehicle's driving.

[0003] In existing methods for intelligent vehicle trajectory planning, the vast majority rely on constructing a quadratic programming problem based on road boundaries or an unconstrained optimization problem based on Euclidean distance maps. These existing intelligent vehicle trajectory planning algorithms have the following problems:

[0004] Existing trajectory planning algorithms based on quadratic programming rely on the constraints of the left and right boundaries of the road to construct the quadratic programming problem. However, in practical applications, complex obstacle environments are not conducive to the construction of constraints, and the problem is prone to having no solution during the process of solving the quadratic programming problem, which has a certain impact on the safety of trajectory planning.

[0005] Existing methods for trajectory planning based on Euclidean distance maps construct an Euclidean distance map while building a 3D dense grid map, which prolongs the time required for map construction. Furthermore, constrained optimization problems based on Euclidean distance require the use of trilinear interpolation to calculate distances and gradients, further extending the solution time.

[0006] Most existing autonomous vehicle trajectory planning methods rely on point cloud maps constructed from visual information for trajectory planning. The path range of local planning is relatively narrow, and the point cloud maps constructed using cameras cannot fully represent obstacle information. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides a multi-constraint trajectory planning method for intelligent vehicles based on multi-dimensional LiDAR point cloud information. This invention constructs a three-dimensional dense grid map based on multi-dimensional LiDAR point cloud information, uses curve fitting based on minimum jerk to generate front-end path points, and constructs an unconstrained optimization problem concerning speed, acceleration, curve smoothness, obstacle avoidance distance, and destination arrival to solve for the optimal trajectory of the intelligent vehicle.

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

[0009] A multi-constraint trajectory planning method for intelligent vehicles based on multi-dimensional lidar point cloud information includes:

[0010] A three-dimensional dense grid map is constructed based on multi-dimensional lidar point cloud information.

[0011] A curve fitting method based on minimum jerk is used to generate front-end path points;

[0012] Curve fitting was performed using a uniform B-spline curve that did not pass through the control points.

[0013] For the fitted B-spline curve, an unconstrained optimization problem is constructed regarding curve smoothness, forward velocity, acceleration, obstacle avoidance distance, and endpoint arrival. The quasi-Newton method is used to solve the unconstrained optimization problem, and the optimal trajectory of the intelligent vehicle in complex environments is obtained.

[0014] Furthermore, the construction of a three-dimensional dense grid map based on multi-dimensional lidar point cloud information includes:

[0015] Obtain point cloud information based on the feedback from the Velide 16-line lidar;

[0016] Extract sharper edge points or flatter points from the point cloud as feature points;

[0017] The obtained feature points are matched to estimate the position and pose of the intelligent vehicle.

[0018] The estimated pose and the matched dense point cloud information are published as a topic.

[0019] Furthermore, the method of generating front-end path points using curve fitting based on minimum jerk includes:

[0020] Let A be the state matrix of the starting and ending points of the i-th trajectory segment. i The maximum velocity of the trajectory is v, the maximum acceleration of the trajectory is a, the trajectory time calculated based on the maximum velocity and maximum acceleration is t, the Euclidean distance threshold is set to d, the actual Euclidean distance between trajectories is set to s, and the calculated trajectory time is T. i The formulas for calculating the trajectory time t and the given distance d are as follows:

[0021]

[0022] If s < 2d, then calculate the trajectory at the given time using the following formula:

[0023]

[0024] If s > 2d, then calculate the trajectory at the given time using the following formula:

[0025]

[0026] For the i-th trajectory segment, construct the state matrix A. i The matrix is ​​shown below:

[0027]

[0028] Introducing variables Where m represents the start or end time of the trajectory, i represents the i-th segment of the trajectory, and k represents the k-th derivative of the trajectory, the variable matrix D of the multiple trajectories is represented as follows:

[0029]

[0030] Let P be the coefficient term of the fitted curve. i5 ,P i4 ,P i3 ,P i2 ,P i1 ,P i0 The fitted curve can be expressed as follows:

[0031] P i (T)=P i5 T 5 +P i4 T 4 +P i3 T3 +P i2 T 2 +P i1 T+P i0

[0032] Calculate the trajectory coefficient matrix P i The calculation formula is as follows:

[0033] P i =[P i5 P i4 P i3 P i2 P i1 P i0 ] T =A*D

[0034] By performing discrete sampling of the above curve over time, a series of front-end path points are obtained.

[0035] Furthermore, the curve fitting using a uniform B-spline curve that does not pass through the control points includes:

[0036] The path points {q0,q1,q2,...,q} are obtained based on a curve fitting method using minimum jerk. N Assume the B-spline curve has order p, and the path points are {q0, q1, q2, ..., q}. N There are M node vectors {t0, t1, t2, ..., t} M-1}, and node vector t i If ∈R, then the formula for calculating the number of nodes M is as follows:

[0037] M = N + p + 1

[0038] Where p = 3, for the parameterized fitting curve, each segment's node vector is set to have a fixed value Δt, and [t] i t i+1 The node vector t at a certain point between [ ] is transformed into a normalized variable u, and the calculation formula is shown below:

[0039]

[0040] Let the parametric expression of the cubic B-spline curve be P(u), the matrix of normalized variables be U, the coefficient matrix before the normalized variables be M3, and the control point matrix corresponding to each segment of the cubic B-spline curve be q. m The expression for each segment of the cubic B-spline curve is as follows:

[0041]

[0042] Based on the obtained trajectory, the velocity V of the i-th segment of the trajectory is determined using mathematical induction. i and acceleration A i The following expression is obtained through derivation and solution:

[0043]

[0044] Among them, u i+1 ,u i+p+1 ,u i+2 q is the normalized variable represented by the corresponding trajectory. i+1 ,q i For the (i+1)th and i-th control points, since the node vectors of each segment are equal, the above formula can be further simplified to the following formula:

[0045]

[0046] Similarly, the jerk J of the trajectory can be represented. i As shown below:

[0047]

[0048] Furthermore, the unconstrained optimization problem concerning curve smoothness, forward velocity, acceleration, obstacle avoidance distance, and endpoint arrival is constructed for the fitted B-spline curve. A quasi-Newton method is used to solve the unconstrained optimization problem to obtain the optimal trajectory of the intelligent vehicle in complex environments, including:

[0049] The problem is constructed in the following form:

[0050] f total =λ1f sm +λ2f c +λ3(f v +f a )+λ4f g

[0051] Where λ1, λ2, λ3, λ4 are the weights of the corresponding optimization terms, f total ,f sm ,f c ,f v ,f a ,f g These correspond to the overall constraint optimization terms, curve smoothness term, obstacle avoidance distance term, forward velocity term, acceleration term, endpoint arrival constraint term, and curve smoothness term f, respectively. sm Construct the form of the problem and the first derivative J sm The expression is as follows:

[0052]

[0053] Where, q i+3 ,q i+2 ,q i+1 ,q i Let f be the control point corresponding to the i-th trajectory segment, and N represent the number of trajectory control points. Regarding the obstacle avoidance distance term f... c The construction involves discrete sampling over time based on the obtained B-spline curve, resulting in a series of path points. Collision detection is performed on each path point using a densely occupied grid map to determine point A before crossing an obstacle and point B after leaving the obstacle. The A* algorithm is then used to determine the obstacle outline between these two points, and the path point q crossing the obstacle is then identified. i Draw a perpendicular line from the tangent direction to the obstacle profile at a point p. i The outline is set from point q i Point p i unit vector v i Therefore, the distance d from the path point to the obstacle can be calculated. i :

[0054] d i =(q i -p i ) T v i

[0055] Regarding the obstacle avoidance distance constraint term f c and the first derivative J c The construction formula is as follows:

[0056]

[0057] Where s is the set distance threshold, and the forward velocity term f v Construct the form of the problem and the first derivative J v The expression is as follows:

[0058]

[0059] Where a2, b2, c2 are coefficients, v max ,v j Given the velocity threshold and the intersection of the quadratic and cubic curves, the derivative of the velocity with respect to the control point is shown below:

[0060]

[0061] Regarding acceleration f a Construct the form of the problem and the first derivative J a The expression is as follows:

[0062]

[0063] Among them, a max ,a j Given the velocity threshold and the intersection of the quadratic and cubic curves, the derivative of the velocity with respect to the control point is shown below:

[0064]

[0065] Regarding the destination arrival item f g Construct the form of the problem and the first derivative J g The expression is as follows:

[0066]

[0067] Here, G represents a defined target point, and the optimal trajectory is obtained by solving for the defined target point G using the quasi-Newton method.

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

[0069] 1. The intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional lidar point cloud information provided by this invention, compared with the existing unmanned vehicle trajectory planning algorithm based on quadratic planning with the left and right boundaries of the road as constraints, can perform trajectory planning in any complex obstacle environment. The constraint conditions for constructing obstacles do not depend on the left and right boundaries, thus reducing the difficulty of the construction optimization problem.

[0070] 2. The intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional lidar point cloud information provided by this invention, compared with the existing unmanned vehicle trajectory planning algorithm based on Euclidean distance map, only uses the A* algorithm to construct obstacle boundaries on the original three-dimensional densely occupied grid map to determine the distance of obstacles, eliminating the need to construct the Euclidean distance map and reducing the trajectory planning time.

[0071] 3. The intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional LiDAR point cloud information provided by this invention, compared with the existing trajectory planning algorithm for unmanned vehicles based on visual information, relies on the point cloud information of LiDAR to construct the environmental map, has a large local planning path range, better constructs obstacle information in the environment, and is more conducive to trajectory planning of unmanned vehicles in complex environments.

[0072] Based on the above reasons, this invention can be widely applied in fields such as intelligent vehicle motion planning. Attached Figure Description

[0073] 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.

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

[0075] Figure 2 A simulation environment map constructed for embodiments of the present invention.

[0076] Figure 3 This invention provides a three-dimensional dense point cloud map constructed using a laser SLAM algorithm, as shown in the embodiments of the present invention.

[0077] Figure 4 This is a schematic diagram illustrating the method for generating front-end path points based on curve fitting using minimum jerk, as provided in an embodiment of the present invention.

[0078] Figure 5 This is a schematic diagram of B-spline curve fitting for front-end path points provided in an embodiment of the present invention.

[0079] Figure 6 This is a schematic diagram illustrating how an intelligent vehicle uses the A* algorithm to construct obstacle distances, as provided in an embodiment of the present invention.

[0080] Figure 7 This is a schematic diagram illustrating trajectory planning for an intelligent vehicle in a simulation environment, as provided in an embodiment of the present invention.

[0081] Figure 8 This is a schematic diagram of the forward velocity obtained after trajectory planning for the intelligent vehicle provided in an embodiment of the present invention.

[0082] Figure 9 This is a schematic diagram of acceleration obtained after trajectory planning for an intelligent vehicle provided in an embodiment of the present invention. 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 a multi-constraint trajectory planning method for intelligent vehicles based on multi-dimensional lidar point cloud information, including:

[0091] S1. Construct a three-dimensional dense grid map based on multi-dimensional LiDAR point cloud information;

[0092] S2. Generate front-end path points using a curve fitting method based on minimum jerk.

[0093] S3. Curve fitting is performed using a uniform B-spline curve that does not pass through the control points.

[0094] S4. For the fitted B-spline curve, construct an unconstrained optimization problem concerning curve smoothness, forward velocity, acceleration, obstacle avoidance distance, and endpoint arrival. Use the quasi-Newton method to solve the unconstrained optimization problem and obtain the optimal trajectory of the intelligent vehicle in a complex environment.

[0095] In a specific implementation, as a preferred embodiment of the present invention, in step S1, a three-dimensional dense grid map is constructed based on multi-dimensional lidar point cloud information, including:

[0096] S11. Obtain point cloud information based on the feedback from the Velide 16-line lidar;

[0097] S12. Extract the sharper edge points or the flatter points in the point cloud as feature points;

[0098] S13. Perform feature point matching on the obtained feature points to estimate the position and attitude of the intelligent vehicle.

[0099] S14. Publish the estimated pose and the matched dense point cloud information as a topic.

[0100] In this invention, in the constructed Gazebo simulation scene, such as Figure 2 As shown, based on the point cloud information collected by multi-dimensional lidar, the ALOAM algorithm is used for pose estimation and the construction of a 3D dense point cloud map, as follows: Figure 3 As shown.

[0101] In a specific implementation, as a preferred embodiment of the present invention, in step S2, a curve fitting method based on minimum jerk is used to generate the front-end path points, including:

[0102] S21. Set the state matrix of the starting and ending points of the i-th trajectory segment as A. i The maximum velocity of the trajectory is v, the maximum acceleration of the trajectory is a, the trajectory time calculated based on the maximum velocity and maximum acceleration is t, the Euclidean distance threshold is set to d, the actual Euclidean distance between trajectories is set to s, and the calculated trajectory time is T. i The formulas for calculating the trajectory time t and the given distance d are as follows:

[0103]

[0104] S22. If s < 2d, then calculate the trajectory at the given time using the following formula:

[0105]

[0106] S23. If s > 2d, then calculate the trajectory given time using the following formula:

[0107]

[0108] S24. For the i-th trajectory segment, construct the state matrix A. i The matrix is ​​shown below:

[0109]

[0110] S25, Introducing Variables Where m represents the start or end time of the trajectory, i represents the i-th segment of the trajectory, and k represents the k-th derivative of the trajectory, the variable matrix D of the multiple trajectories is represented as follows:

[0111]

[0112] S26. Let P be the coefficient term of the fitted curve. i5 ,P i4 ,P i3 ,P i2 ,P i1 ,P i0 The fitted curve can be expressed as follows:

[0113] P i (T)=P i5 T 5 +P i4 T 4 +P i3 T 3 +P i2 T 2 +P i1 T+P i0

[0114] S27. Calculate the trajectory coefficient matrix P i The calculation formula is as follows:

[0115] P i =[P i5 P i4 P i3 P i2 P i1 P i0 ] T =A*D

[0116] S28. By performing discrete sampling of the above curve over time, a series of front-end path points are obtained, such as... Figure 4 As shown.

[0117] In a specific implementation, as a preferred embodiment of the present invention, in step S3, a uniform B-spline curve that does not pass through the control point is used for curve fitting, such as... Figure 5 As shown, it includes:

[0118] S31. Path points {q0,q1,q2,...,q} obtained based on curve fitting using minimum jerk. N Assume the B-spline curve has order p, and the path points are {q0, q1, q2, ..., q}. N There are M node vectors {t0, t1, t2, ..., t} M-1}, and node vector t i If ∈R, then the formula for calculating the number of nodes M is as follows:

[0119] M = N + p + 1

[0120] Where p = 3, for the parameterized fitting curve, each segment's node vector is set to have a fixed value Δt, and [t]i t i+1 The node vector t at a certain point between [ ] is transformed into a normalized variable u, and the calculation formula is shown below:

[0121]

[0122] S32. Let the parametric expression of a cubic B-spline curve be P(u), the matrix of normalized variables be U, the coefficient matrix before the normalized variables be M3, and the control point matrix corresponding to each segment of the cubic B-spline curve be q. m The expression for each segment of the cubic B-spline curve is as follows:

[0123]

[0124] S33. Based on the obtained trajectory, use mathematical induction to determine the velocity V of the i-th segment of the trajectory. i and acceleration A i The following expression is obtained through derivation and solution:

[0125]

[0126] Among them, u i+1 ,u i+p+1 ,u i+2 q is the normalized variable represented by the corresponding trajectory. i+1 ,q i For the (i+1)th and i-th control points, since the node vectors of each segment are equal, the above formula can be further simplified to the following formula:

[0127]

[0128] S34. Similarly, the jerk J of the trajectory can be represented. i As shown below:

[0129]

[0130] In a specific implementation, as a preferred embodiment of the present invention, in step S4, an unconstrained optimization problem is constructed for the fitted B-spline curve regarding curve smoothness, forward velocity, acceleration, obstacle avoidance distance, and endpoint arrival. A quasi-Newton method is used to solve the unconstrained optimization problem to obtain the optimal trajectory of the intelligent vehicle in a complex environment, including:

[0131] S41. The form of the construction problem is as follows:

[0132] f total =λ1f sm +λ2f c +λ3(f v +fa )+λ4f g

[0133] Where λ1, λ2, λ3, λ4 are the weights of the corresponding optimization terms, f total ,f sm ,f c ,f v ,f a ,f g These correspond to the overall constraint optimization terms, curve smoothness term, obstacle avoidance distance term, forward velocity term, acceleration term, endpoint arrival constraint term, and curve smoothness term f, respectively. sm Construct the form of the problem and the first derivative J sm The expression is as follows:

[0134]

[0135] Where, q i+3 ,q i+2 ,q i+1 ,q i Let f be the control point corresponding to the i-th trajectory segment, and N represent the number of trajectory control points. Regarding the obstacle avoidance distance term f... c The construction involves discrete sampling over time based on the obtained B-spline curve, resulting in a series of path points. Collision detection is performed on each path point using a densely occupied grid map to determine point A before crossing an obstacle and point B after leaving the obstacle. The A* algorithm is then used to determine the obstacle outline between these two points, and the path point q crossing the obstacle is then identified. i Draw a perpendicular line from the tangent direction to the obstacle profile at a point p. i The outline is set from point q i Point p i unit vector v i Therefore, the distance d from the path point to the obstacle can be calculated. i :

[0136] d i =(q i -p i ) T v i

[0137] S42. Regarding the obstacle avoidance distance constraint term f c and the first derivative J c The construction formula is as follows:

[0138]

[0139] Where s is the set distance threshold, and the forward velocity term f v Construct the form of the problem and the first derivative J vThe expression is as follows:

[0140]

[0141] Where a2, b2, c2 are coefficients, v max ,v j Given the velocity threshold and the intersection of the quadratic and cubic curves, the derivative of the velocity with respect to the control point is shown below:

[0142]

[0143] S43. Regarding acceleration f a Construct the form of the problem and the first derivative J a The expression is as follows:

[0144]

[0145] Among them, a max ,a j Given the velocity threshold and the intersection of the quadratic and cubic curves, the derivative of the velocity with respect to the control point is shown below:

[0146]

[0147] S44. Regarding the destination arrival item f g Construct the form of the problem and the first derivative J g The expression is as follows:

[0148]

[0149] Here, G represents a defined target point. The optimal trajectory is obtained by solving for the target point G using a quasi-Newton method. The velocity and yaw angle information obtained from solving the trajectory are then transmitted to the trajectory tracking controller to drive the intelligent vehicle's movement. Figure 6 , 7 As shown. Figure 8 , Figure 9 This is a schematic diagram of the forward velocity and acceleration obtained after the intelligent vehicle performs trajectory planning.

[0150] In summary, the intelligent vehicle initializes itself based on the pose and dense 3D map obtained from the ALOAM algorithm. Then, it selects the target point and begins trajectory planning, continuously performing collision detection on the optimized trajectory during operation. If a collision is detected at a generated trajectory point, the intelligent vehicle replans and generates an executable trajectory, ensuring that it reaches the destination under the constraint of zero velocity and acceleration. Due to the constraint of curve smoothing on the trajectory, the intelligent vehicle maintains low energy consumption during operation.

[0151] 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. A multi-constraint trajectory planning method for intelligent vehicles based on multi-dimensional lidar point cloud information, characterized in that, include: A three-dimensional dense grid map is constructed based on multi-dimensional LiDAR point cloud information. A curve fitting method based on minimum jerk is used to generate front-end path points; Curve fitting was performed using a uniform B-spline curve that did not pass through the control points. For the B-spline curve obtained from the fitting, an unconstrained optimization problem is constructed regarding curve smoothness, forward velocity, acceleration, obstacle avoidance distance, and endpoint arrival. A quasi-Newton method is used to solve the unconstrained optimization problem, yielding the optimal trajectory of the intelligent vehicle in complex environments, including: The problem is constructed in the following form: in, For the weights of the corresponding optimization terms, These correspond to the overall constraint optimization terms, curve smoothness term, obstacle avoidance distance term, forward velocity term, acceleration term, endpoint arrival constraint term, and curve smoothness term, respectively. Construct the form of the problem and the first derivative. The expression is as follows: in, For the first i Control points corresponding to the segment trajectory Represents the number of trajectory control points, regarding the obstacle avoidance distance item. The construction involves discrete sampling over time based on the obtained B-spline curve, resulting in a series of path points. Collision detection is then performed on each path point using a densely occupied grid map to determine point A before crossing an obstacle and point B after leaving the obstacle. A value is then used between these two points. The algorithm determines the outline of the obstacle and identifies the path points that traverse the obstacle. Draw a perpendicular line from the tangent direction that intersects the outline of the obstacle at a point. , outline from point Point of view unit vector This allows us to calculate the distance between the path point and the obstacle. : Regarding obstacle avoidance distance constraints and the first derivative The construction formula is as follows: in, For the set distance threshold, regarding the forward velocity term Construct the form of the problem and the first derivative. The expression is as follows: in, For coefficients, Given the velocity threshold and the intersection of the quadratic and cubic curves, the derivative of the velocity with respect to the control point is shown below: Regarding acceleration Construct the form of the problem and the first derivative. The expression is as follows: in, Given the velocity threshold and the intersection of the quadratic and cubic curves, the derivative of the velocity with respect to the control point is shown below: Regarding the destination arrival item Construct the form of the problem and the first derivative. The expression is as follows: in, G Representing a defined target point, a quasi-Newton method is used to determine the target point. G The optimal trajectory is obtained by solving the problem.

2. The intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional lidar point cloud information according to claim 1, characterized in that, The construction of a three-dimensional dense grid map based on multi-dimensional lidar point cloud information includes: Obtain point cloud information based on the feedback from the Velide 16-line lidar; Extract sharper edge points or flatter points from the point cloud as feature points; The obtained feature points are matched to estimate the position and pose of the intelligent vehicle. The estimated pose and the matched dense point cloud information are then published as a topic.

3. The intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional lidar point cloud information according to claim 1, characterized in that, The method of generating front-end path points using curve fitting based on minimum jerk includes: Setting the first The state matrices of the starting and ending points of the trajectory segment are as follows: The maximum speed of the trajectory is The maximum acceleration of the trajectory is The trajectory time calculated based on the maximum trajectory velocity and maximum trajectory acceleration is: Set the Euclidean distance threshold to The actual Euclidean distance between the trajectories is set as follows: The calculated trajectory is given at time . Then calculate the trajectory time. and a given distance The formula is shown below: if The trajectory is calculated at a given time using the following formula: if The trajectory is calculated at a given time using the following formula: For the Segment trajectory, construct state matrix The matrix is ​​shown below: Introducing variables ,in, Represents the start or end time of the trajectory. Representing the Segment trajectory, The first representing the trajectory The first derivative gives the variable matrix of the multi-segment trajectory. It is represented as follows: Let the coefficients of the fitted curve be... The fitted curve can be expressed as follows: Calculate the trajectory coefficient matrix The calculation formula is as follows: By performing discrete sampling of the above curve over time, a series of front-end path points are obtained.

4. The intelligent vehicle multi-constraint trajectory planning method based on multi-dimensional lidar point cloud information according to claim 1, characterized in that, The method of curve fitting using a uniform B-spline curve that does not pass through control points includes: The path points are obtained based on a curve fitting method using minimum jerk. Assume the order of the B-spline curve is... Assuming path point { }have node vectors { }, and node vectors Then the number of nodes The calculation formula is as follows: in, For parametric fitting curves, the node vectors of each segment are set to have fixed values. ,Will The node vector of a certain point between them Transform into normalized variables The calculation formula is as follows: Let the parametric expression of a cubic B-spline curve be: Let the matrix formed by the normalized variables be denoted as The coefficient matrix before normalization is set as follows: The control point matrix corresponding to each cubic B-spline curve segment is set as follows: The expression for each segment of the cubic B-spline curve is as follows: Based on the obtained trajectory, mathematical induction is used to analyze the first... velocity of segment trajectory and acceleration The following expression is obtained through derivation and solution: in, The normalized variable represents the corresponding trajectory. For the first and the With control points, since the node vectors of each segment are equal, the above formula can be further simplified to the following formula: Similarly, the jerk of the trajectory can be represented. As shown below: 。

Images