Unmanned vehicle trajectory planning method in unstructured environment

Abstract

The invention discloses an unmanned vehicle trajectory planning method in an unstructured environment, and belongs to the technical field of unmanned driving. The trajectory planning method comprises the following steps: constructing a motion primitive tree based on a vehicle kinematic model under a Cartesian coordinate system, and searching the motion primitive tree to obtain an initial trajectory; discretizing the initial trajectory into a series of state points, projecting the state points into an xyt configuration space, and constructing a drivable area of the vehicle in a time-space domain on the basis of the state points; and optimizing the initial trajectory by using a segmented Bezier curve in a travelable region of the vehicle in a time-space domain to obtain a final travelable trajectory. Compared with a traditional space-time decoupling trajectory planning method, time and space dimensions are considered at the same time, transverse and longitudinal combined movement of the intelligent vehicle can be achieved, and the method better fits the driving habits of human drivers. In addition, the concept of a space-time corridor is introduced, and the safety and flexibility of trajectory planning are further improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned driving, and in particular relates to a trajectory planning method for an unmanned vehicle in an unstructured environment. Background Art

[0002] Unmanned driving systems usually consist of perception, decision-making, planning, and control modules. The core task of the planning module is to generate a drivable trajectory that satisfies the vehicle's kinematic constraints and is collision-free. The trajectory planning scheme in the planning module directly reflects the intelligence level of the unmanned vehicle.

[0003] At present, the commonly used trajectory planning method in unstructured environments is the traditional spatiotemporal decoupling planning method. The core idea of ​​this method is to first plan the path under the constraints of static obstacles, and then plan the speed for dynamic obstacles based on the generated path. In unstructured environments with high uncertainty and complexity, the traditional spatiotemporal decoupling method has weak interaction with dynamic obstacles, which makes the generated trajectory easily fall into local suboptimal, thus limiting the obstacle avoidance performance of driverless cars in unstructured environments. Summary of the invention

[0004] In view of the above shortcomings of the prior art, the present invention provides a trajectory planning method for an unmanned vehicle in an unstructured environment.

[0005] To achieve the above purpose, the technical solution adopted by the present invention is: a method for trajectory planning of an unmanned vehicle in an unstructured environment, which uses a laser radar to perceive the surrounding environment, uses an IMU to provide positioning information for the vehicle, and then obtains the final trajectory through initial trajectory search, construction of the drivable area of ​​the vehicle in the spatiotemporal domain, and trajectory optimization. The steps are as follows:

[0006] S1. Construct a motion primitive tree based on the vehicle kinematic model in a Cartesian coordinate system, search the motion primitive tree, and obtain the initial trajectory. The steps are as follows:

[0007] S1.1. According to the two-degree-of-freedom kinematic model of the vehicle, the vehicle differential equation can be obtained, which is expressed as follows:

[0008]

[0009] In the formula, x and y are the coordinates of the center point of the rear axle of the vehicle, respectively, where x is the horizontal coordinate of the center point and y is the vertical coordinate of the center point; v is the longitudinal speed of the vehicle; l is the wheelbase of the vehicle; is the vehicle body direction angle; δ is the front wheel turning angle; a is the vehicle acceleration; They represent the derivatives of the coordinates of the center point of the rear axle of the vehicle, the direction angle of the vehicle body, and the longitudinal speed of the vehicle respectively;

[0010] S1.2, the vehicle acceleration a and the front wheel steering angle δ are used as the longitudinal control variable and the lateral control variable of the vehicle, respectively, where a∈[a min ,a max ],δ∈[-δ min ,δ max ]; Discretize the longitudinal control amount and lateral control amount of the vehicle into a∈{a 1 ,a 2 ,...,a n},δ∈{δ 1 ,δ 2 ,...,δ n}, the value of n is positively correlated with the trajectory, and the forward simulation time length is τ; in the node expansion process, with a fixed time interval Δt and a minimum expansion step length d min Perform step-by-step node expansion, the expression is as follows:

[0011]

[0012] definition is the vehicle state space, and the state at a given time T is X T , the corresponding control quantity is a T and δ T , then the state X at the next moment T+1 It can be expressed as:

[0013]

[0014] Where d is the distance traveled by the vehicle during the forward simulation, R is the turning radius of the vehicle, and x T ,y T is the coordinate of the rear axle center point of the vehicle at a given time T, is the direction angle of the vehicle at a given time T, v T is the longitudinal velocity of the vehicle at a given time T, a T is the acceleration of the vehicle at time T, t T is the time of the vehicle at a given time T, is the direction angle of the vehicle at the T+1th moment;

[0015] By discretizing the control quantity {a 1 ,a 2 ,...,a n} and {δ 1 ,δ 2 ,...,δ n} to form a multi-layer motion primitive, i.e., a motion primitive tree; then, an initial trajectory search is performed on the motion primitive tree to obtain an initial trajectory;

[0016] S2, discretize the initial trajectory into state points and project them into the xyt configuration space, and construct the drivable area of ​​the vehicle in the space-time domain based on the state points;

[0017] Status point X 1 ,X 2 ,...,X n Positively correlated with trajectory;

[0018] The build steps include:

[0019] S2.1. Construct a space-time grid map to describe the distribution information of obstacles in space and time in the future;

[0020] S2.2, based on the space-time grid map and the initial trajectory, construct the drivable area of ​​the vehicle in the space-time domain, that is, the three-dimensional space-time corridor;

[0021] In the process of building the spatiotemporal grid map, in order to simplify the construction process, obstacles (environmental complexity) are classified into two categories: static obstacles and dynamic obstacles, and they are processed separately;

[0022] For static obstacles, since their positions are fixed, in order to improve the efficiency of the algorithm, they can be regarded as obstacles occupying the same space on the entire discretized time axis t, and there is no need to perform trajectory prediction;

[0023] For dynamic obstacles, since their positions change over time, trajectory prediction is required; that is, a two-dimensional constant speed model is used to predict the motion trajectory of the surrounding dynamic obstacles, and the predicted trajectory is projected along the discretized time axis to generate occupancy information in the spatiotemporal dimension, where the discrete time interval is Δt, which is adjusted according to the complexity of the surrounding environment;

[0024] Furthermore, the process of constructing the drivable area of ​​the vehicle in the time and space domain specifically includes the following steps:

[0025] S2.2.1. Generate seed points: To ensure that the seed points are collision-free, first discretize them into state points X based on the collision-free nature of the initial trajectory. 1 ,X 2 ,...,X n ; Project these state points into the xyt configuration space as seed points for corridor generation;

[0026] S2.2.2, Cube expansion: This step is the core link of constructing the drivable area of ​​the ego-vehicle in the spatiotemporal domain. First, traverse each seed point and generate an initial cube between two adjacent seed points. Specifically, two adjacent seed points are used as vertices of the cube. By connecting these two seed points, an initial solution space is formed. Secondly, based on the initial cube, expansion is performed in five directions, namely, the positive and negative directions of the x and y axes and the positive direction of the t axis. The expansion is stopped when the expansion boundary of the cube intersects the boundaries of static obstacles and dynamic obstacles. In order to improve computational efficiency, during the expansion process, the seed points already included in the expansion area can be merged into the current expansion area. After each expansion of a cube is completed, the next nearest cube that is not in the expansion area is expanded, and so on, to construct a three-dimensional spatiotemporal grid map and the drivable area of ​​the ego-vehicle in the spatiotemporal domain.

[0027] The drivable area of ​​the ego-vehicle in the space-time domain defines a convex free space suitable for subsequent trajectory optimization, which provides clear constraints for the optimization algorithm, ensures that the trajectory avoids collisions with obstacles during the optimization process, and provides the necessary space-time guarantee for trajectory optimization;

[0028] S3. Use piecewise Bezier curves to optimize the initial trajectory in the drivable area of ​​the vehicle in the time and space domain to obtain the final drivable trajectory; the steps are as follows:

[0029] S3.1. Generate an L-order Bezier curve f(t) consisting of L+1 control points within the drivable area of ​​the vehicle in the space-time domain. The expression for generating the Bezier curve is as follows:

[0030]

[0031] Where t represents time, t∈[0,1]; p i is the i-th control point of the Bezier curve; is the Bernstein polynomial (Bernsteinbasis);

[0032] The Bezier curve has two properties: Convex Hull and Hodograph, which can ensure that the optimized trajectory strictly follows the constraints, thereby improving the smoothness and safety of the trajectory;

[0033] S3.2. Divide the drivable area of ​​the vehicle in the time and space domain according to the complexity of the environment to improve the trajectory optimization effect;

[0034] Specifically, when the drivable area of ​​the ego vehicle in the space-time domain encounters an obstacle during the expansion phase, the expansion stops. Therefore, the drivable area of ​​the ego vehicle in the space-time domain is a segmented structure.

[0035] Each expanded cube constitutes an independent drivable area of ​​the ego vehicle in the space-time domain. The trajectory is segmented in the space-time domain according to the segmented structure of the drivable area of ​​the ego vehicle in the space-time domain. This process ensures that each segment of the drivable area of ​​the ego vehicle in the space-time domain is described by an independent piecewise Bezier curve. In this way, the trajectory can be optimized separately in each local drivable area of ​​the ego vehicle in the space-time domain, improving the flexibility of trajectory optimization.

[0036] If the time allocated to each segmented trajectory described in S3.2 is different, it is necessary to define a corresponding scaling factor s according to the time allocated to each segmented trajectory; if the time allocated to each segmented trajectory described in S3.2 is the same, then the scaling factor s=1 is defined; thus, the m-segment piecewise Bezier curve in the one-dimensional space μ∈{x,y} can be expressed as:

[0037]

[0038] In the formula, represents the hth control point in the mth trajectory, h∈L; t 0 ,t 1 ,...,t m Indicates the start and end time of each trajectory, t 0 ,t 1 ,...,t m ∈t;

[0039] S3.3. Optimize the initial trajectory with minimizing jerk as the optimization goal to avoid sudden acceleration and deceleration of the vehicle and improve the smoothness of the trajectory; define the optimization objective function of the j-th Bezier curve, j∈m; the expression is as follows:

[0040]

[0041] In the formula, f x (t) and f y (t) represents the Bezier curve on the x and y axes respectively, t j-1 and t j represents the start and end time of the jth trajectory, t j-1 and t j ∈t;

[0042] Define f μj (t) is the jth Bezier curve in μ dimension, and the unscaled piecewise Bezier curve t∈[0,1] is represented by g μj (t), let γ=(tt j-1 ) / s j ; Then the objective function of the j-th Bezier curve in μ dimension can be expressed as the following quadratic form:

[0043]

[0044] Where P j Represents the Bezier curve g μj (t) is the set of control points, Q is the Hessian matrix of the curve;

[0045] S3.4. During the Bezier curve optimization process, the safety and dynamic constraints, the first and last state constraints, and the continuity constraints of the trajectory are set respectively;

[0046] Safety and dynamic constraints: Based on the Convex Hull and Hodograph characteristics of the Bezier curve, ensure that the trajectory, velocity curve, and acceleration curve are strictly constrained within the convex hull formed by the control points, that is:

[0047]

[0048] In the formula, and They represent the upper and lower bounds of the k-th order derivative of the j-th curve respectively;

[0049] First and last state constraints: The starting and ending states of trajectory optimization are constrained to the center point of the rear axle of the vehicle and the planned target position respectively, that is:

[0050]

[0051] In the formula, and They represent the k-order derivatives of the rear axle center point of the ego vehicle and the planned target position respectively;

[0052] Continuity constraint: To ensure the smoothness of the trajectory, the derivatives of each Bezier curve at the connection point should be continuous, that is:

[0053]

[0054] In the expressions of trajectory safety and dynamic constraints, initial and final state constraints, and continuity constraints, the values ​​of k include: 0, 1, 2, and 3;

[0055] Specifically, in the safety and dynamic constraint expressions of the trajectory, k takes the values ​​of 0, 1, and 2, where when k = 0, the trajectory position is constrained, when k = 1, the velocity curve is constrained, and when k = 2, the acceleration curve is constrained;

[0056] Specifically, in the trajectory continuity constraint expression, k takes the values ​​of 0, 1, and 2;

[0057] Specifically, in the trajectory continuity constraint expression, k is 3, which means that the jerk is constrained;

[0058] S3.5. By introducing the linear inequalities, equality constraints and quadratic objective functions in S3.4, the trajectory optimization problem can be transformed into a quadratic programming problem, and the OOQP solver is used to solve the quadratic programming problem to obtain the optimized trajectory.

[0059] Beneficial effects of the present invention

[0060] The present invention considers both time and space dimensions in the trajectory planning process, so that the vehicle can achieve combined lateral and longitudinal motion, which is more in line with human driving habits.

[0061] The trajectory generated by the present invention has higher flexibility and can effectively enhance the ability to interact with dynamic obstacles. Secondly, the present invention constructs a space-time traversable area in three-dimensional space-time, namely a three-dimensional space-time corridor, which provides clear constraints for the subsequent trajectory optimization algorithm, ensures the avoidance of collision between the trajectory and obstacles, and makes the trajectory planned in an unstructured environment with high uncertainty and complexity safer.

[0062] The present invention uses piecewise Bezier curves to optimize the trajectory in the drivable area of ​​the vehicle in the space-time domain. During the optimization process, the vehicle's safety and dynamic constraints, first and final state constraints, and continuity constraints are fully considered to ensure that a safe, continuous and smooth trajectory is generated under multiple constraint conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 This is a flow chart of the steps of unmanned vehicle trajectory planning in an unstructured environment based on a space-time corridor according to the present invention;

[0064] Figure 2 This is a flow chart of the unmanned vehicle trajectory planning method in an unstructured environment based on a space-time corridor according to the present invention;

[0065] Figure 3 It is a schematic diagram of the vehicle's two-degree-of-freedom kinematic model;

[0066] Figure 4 Schematic diagram of a single layer of motion primitives for initial trajectory search;

[0067] Figure 5 Schematic diagram of the motion primitive tree for initial trajectory search;

[0068] Figure 6 It is a spatiotemporal grid map and a schematic diagram of the drivable area of ​​the vehicle in the spatiotemporal domain;

[0069] Figure 7 It is a schematic diagram of a piecewise Bezier curve;

[0070] Figure 8 A schematic diagram of a scenario in which the present invention and the TEB algorithm are compared in a simulation scenario;

[0071] Fig. 9 A comparison diagram of the longitudinal speed and azimuth angle planned by the present invention and the TEB algorithm at a certain moment;

[0072] Fig.10 A comparison chart of the driving trajectories of the present invention and the TEB algorithm in a simulation scenario;

[0073] Fig.11 A schematic diagram of a scene for comparative experiments between the present invention and the TEB algorithm in a scene with multiple dynamic obstacles;

[0074] Fig.12 This is a comparison chart of the average driving time of the present invention and the TEB algorithm in a multi-dynamic obstacle experiment. DETAILED DESCRIPTION

[0075] In order to better illustrate the purpose, technical solutions and advantages of the present invention, the present invention will be further described below in conjunction with specific embodiments.

[0076] like Figure 1 and Figure 2 As shown in the figure, a trajectory planning method for an unmanned vehicle in an unstructured environment uses a laser radar to perceive the surrounding environment, uses an IMU to provide positioning information for the vehicle, and then obtains the final trajectory through initial trajectory search, construction of the drivable area of ​​the vehicle in the spatiotemporal domain, and trajectory optimization. The steps are as follows:

[0077] S1. Construct a motion primitive tree based on the vehicle kinematic model in a Cartesian coordinate system, search the motion primitive tree, and obtain the initial trajectory. The steps are as follows:

[0078] S1.1, if Figure 3 As shown, the vehicle differential equation can be obtained based on the vehicle's two-degree-of-freedom kinematic model, and the expression is as follows:

[0079]

[0080] In the formula, x and y are the coordinates of the center point of the rear axle of the vehicle, respectively, where x is the horizontal coordinate of the center point and y is the vertical coordinate of the center point; v is the longitudinal speed of the vehicle; l is the wheelbase of the vehicle; is the vehicle body direction angle; δ is the front wheel turning angle; a is the vehicle acceleration; They represent the derivatives of the coordinates of the center point of the rear axle of the vehicle, the direction angle of the vehicle body, and the longitudinal speed of the vehicle respectively;

[0081] S1.2, the vehicle acceleration a and the front wheel steering angle δ are used as the longitudinal control variable and the lateral control variable of the vehicle, respectively, where a∈[a min ,a max],δ∈[-δ min ,δ max ], in this embodiment, a min Take -0.5, a max Take 0.5, -δ min Take -28, δ max Take 28; discretize the longitudinal control amount and lateral control amount of the vehicle into a∈{a 1 ,a 2 ,...,a n},δ∈{δ 1 ,δ 2 ,...,δ n}, the value of n is positively correlated with the trajectory, and the forward simulation time length is τ. In this embodiment, τ = 0.2s, so as to obtain Figure 4 The single-layer motion primitive shown; during the node expansion process, with a fixed time interval Δt and a minimum expansion step size d min Perform step-by-step node expansion, the expression is as follows:

[0082]

[0083] definition is the vehicle state space, and the state at a given time T is X T , the corresponding control quantity is a T and δ T , then the state X at the next moment T+1 It can be expressed as:

[0084]

[0085] Where d is the distance traveled by the vehicle during the forward simulation, R is the turning radius of the vehicle, and x T ,y T is the coordinate of the rear axle center point of the vehicle at a given time T, is the direction angle of the vehicle at a given time T, v T is the longitudinal velocity of the vehicle at a given time T, a T is the acceleration of the vehicle at time T, t T is the time of the vehicle at a given time T, is the direction angle of the vehicle at the T+1th moment;

[0086] By discretizing the control quantity {a 1 ,a 2 ,...,a n} and {δ 1 ,δ 2 ,...,δ n} to form a multi-layer motion primitive, namely, a motion primitive tree, such as Figure 5As shown; then, an initial trajectory search is performed on the motion primitive tree to obtain the initial trajectory;

[0087] S2, discretize the initial trajectory into state points and project them into the xyt configuration space, and construct the drivable area of ​​the vehicle in the space-time domain based on the state points;

[0088] Status point X 1 ,X 2 ,...,X n Positively correlated with the trajectory; in this embodiment, the state point is taken at intervals of 0.2s on the trajectory;

[0089] The build steps include:

[0090] S2.1. Construct a spatiotemporal grid map to describe the distribution information of obstacles in space and time in the future. The construction process is generated by the C++ programming language through the grid_map open source program.

[0091] S2.2, based on the space-time grid map and the initial trajectory, construct the drivable area of ​​the vehicle in the space-time domain, that is, the three-dimensional space-time corridor;

[0092] In the process of building the spatiotemporal grid map, in order to simplify the construction process, obstacles (environmental complexity) are classified into two categories: static obstacles and dynamic obstacles, and they are processed separately;

[0093] For static obstacles, since their positions are fixed, in order to improve the efficiency of the algorithm, they can be regarded as obstacles occupying the same space on the entire discretized time axis t, and there is no need to perform trajectory prediction;

[0094] For dynamic obstacles, since their positions change over time, trajectory prediction is required; that is, a two-dimensional constant speed model is used to predict the motion trajectory of the surrounding dynamic obstacles, and the predicted trajectory is projected along the discretized time axis to generate occupancy information in the spatiotemporal dimension, where the discrete time interval is Δt, which is adjusted according to the complexity of the surrounding environment;

[0095] Furthermore, the process of constructing the drivable area of ​​the vehicle in the time and space domain specifically includes the following steps:

[0096] S2.2.1. Generate seed points: To ensure that the seed points are collision-free, first discretize them into state points X based on the collision-free nature of the initial trajectory. 1 ,X 2 ,...,X n ; Project these state points into the xyt configuration space as seed points for corridor generation;

[0097] S2.2.2, Cube expansion: This step is the core link for constructing the drivable area of ​​the vehicle in the space-time domain; first, traverse each seed point and generate an initial cube between two adjacent seed points. Specifically, two adjacent seed points are used as vertices of the cube, and the initial solution space is formed by connecting these two seed points; secondly, on the basis of the initial cube, expansion is performed in five directions, namely, the positive and negative directions of the x-axis and the y-axis and the positive direction of the t-axis; the expansion is stopped when the expansion boundary of the cube intersects the boundaries of the static obstacle and the dynamic obstacle; in order to improve the computational efficiency, during the expansion process, the seed points already contained in the expansion area can be merged into the current expansion area; after completing the expansion of each cube, the next nearest cube that is not in the expansion area is then expanded, and so on, to construct the following Figure 6 The three-dimensional space-time grid map and the drivable area of ​​the vehicle in the space-time domain are shown;

[0098] The drivable area of ​​the ego-vehicle in the space-time domain defines a convex free space suitable for subsequent trajectory optimization, which provides clear constraints for the optimization algorithm, ensures that the trajectory avoids collisions with obstacles during the optimization process, and provides the necessary space-time guarantee for trajectory optimization;

[0099] S3. Use piecewise Bezier curves to optimize the initial trajectory in the drivable area of ​​the vehicle in the time and space domain to obtain the final drivable trajectory; the steps are as follows:

[0100] S3.1. Generate an L-order Bezier curve f(t) consisting of L+1 control points within the drivable area of ​​the vehicle in the space-time domain. The expression for generating the Bezier curve is as follows:

[0101]

[0102] Where t represents time, t∈[0,1]; p i is the i-th control point of the Bezier curve; is the Bernstein polynomial (Bernsteinbasis);

[0103] In this embodiment, L=5, L+1∈n;

[0104] The Bezier curve has two properties: Convex Hull and Hodograph, which can ensure that the optimized trajectory strictly follows the constraints, thereby improving the smoothness and safety of the trajectory;

[0105] Convex Hull property: This property ensures that the Bezier curve f(t) is always constrained in the convex hull formed by the control points [p0, p1, ... pL]. It is applicable to the curve constraint problem in convex free space. Specifically, ignoring the uncertainty of dynamic obstacle motion, by constraining the control points in the convex free space defined by the drivable area of ​​the vehicle in the space-time domain, it can ensure that the generated curve always satisfies the collision-free constraint, thereby ensuring the safety of the trajectory.

[0106] Hodograph property: This property helps to effectively constrain the higher-order derivatives of the Bezier curve; specifically, the derivative of the L-order Bezier curve is still a line with a control point p i '=L·(p i+1 -p i )’s L-1 order Bezier curve; combined with the Convex Hull property, the speed and acceleration dynamics indicators can be further restricted within the convex hull range, ensuring the continuity of the speed and acceleration curves, thereby achieving precise control of the vehicle’s dynamic performance;

[0107] S3.2. Divide the drivable area of ​​the vehicle in the time and space domain according to the complexity of the environment to improve the trajectory optimization effect;

[0108] Specifically, when the drivable area of ​​the ego vehicle in the space-time domain encounters an obstacle during the expansion phase, the expansion stops. Therefore, the drivable area of ​​the ego vehicle in the space-time domain is a segmented structure.

[0109] Each expanded cube constitutes an independent drivable area of ​​the ego vehicle in the space-time domain. The trajectory is segmented in the space-time domain according to the segmented structure of the drivable area of ​​the ego vehicle in the space-time domain. This process ensures that each segment of the drivable area of ​​the ego vehicle in the space-time domain is described by an independent piecewise Bezier curve. In this embodiment, Figure 7 As shown in the figure, the three-segment division results are shown; in this way, the trajectory can be optimized separately in the drivable area of ​​each local vehicle in the time and space domain, which improves the flexibility of trajectory optimization;

[0110] If the time allocated to each segmented trajectory described in S3.2 is different, it is necessary to define a corresponding scaling factor s according to the time allocated to each segmented trajectory; if the time allocated to each segmented trajectory described in S3.2 is the same, then the scaling factor s=1 is defined; thus, the m-segment piecewise Bezier curve in the one-dimensional space μ∈{x,y} can be expressed as:

[0111]

[0112] In the formula, represents the hth control point in the mth trajectory, h∈L; t0 ,t 1 ,...,t m Indicates the start and end time of each trajectory, t 0 ,t 1 ,...,t m ∈t;

[0113] S3.3. Optimize the initial trajectory with minimizing jerk as the optimization goal to avoid sudden acceleration and deceleration of the vehicle and improve the smoothness of the trajectory; define the optimization objective function of the j-th Bezier curve, j∈m; the expression is as follows:

[0114]

[0115] In the formula, f x (t) and f y (t) represents the Bezier curve on the x and y axes respectively, t j-1 and t j represents the start and end time of the jth trajectory, t j-1 and t j ∈t;

[0116] Define f μj (t) is the jth Bezier curve in μ dimension, and the unscaled piecewise Bezier curve t∈[0,1] is represented by g μj (t), let γ=(tt j-1 ) / s j ; Then the objective function of the j-th Bezier curve in μ dimension can be expressed as the following quadratic form:

[0117]

[0118] Where P j Represents the Bezier curve g μj (t) is the set of control points, Q is the Hessian matrix of the curve;

[0119] S3.4. During the Bezier curve optimization process, the safety and dynamic constraints, the first and last state constraints, and the continuity constraints of the trajectory are set respectively;

[0120] Safety and dynamic constraints: Based on the Convex Hull and Hodograph characteristics of the Bezier curve, ensure that the trajectory, velocity curve, and acceleration curve are strictly constrained within the convex hull formed by the control points, that is:

[0121]

[0122] In the formula, and They represent the upper and lower bounds of the k-th order derivative of the j-th curve respectively;

[0123] First and last state constraints: The starting and ending states of trajectory optimization are constrained to the center point of the rear axle of the vehicle and the planned target position respectively, that is:

[0124]

[0125] In the formula, and They represent the k-order derivatives of the rear axle center point of the ego vehicle and the planned target position respectively;

[0126] Continuity constraint: To ensure the smoothness of the trajectory, the derivatives of each Bezier curve at the connection point should be continuous, that is:

[0127]

[0128] In the expressions of trajectory safety and dynamic constraints, initial and final state constraints, and continuity constraints, the values ​​of k include: 0, 1, 2, and 3;

[0129] Specifically, in the safety and dynamic constraint expressions of the trajectory, k takes the values ​​of 0, 1, and 2, where when k = 0, the trajectory position is constrained, when k = 1, the velocity curve is constrained, and when k = 2, the acceleration curve is constrained;

[0130] Specifically, in the trajectory continuity constraint expression, k takes the values ​​of 0, 1, and 2;

[0131] Specifically, in the trajectory continuity constraint expression, k is 3, which means that the jerk is constrained;

[0132] S3.5. By introducing the linear inequalities, equality constraints and quadratic objective functions in S3.4, the trajectory optimization problem can be transformed into a quadratic programming problem, and the OOQP solver is used to solve the quadratic programming problem to obtain the optimized trajectory.

[0133] In order to illustrate that the method of the present invention can achieve safer obstacle avoidance driving in unstructured environments, the method of the present invention is compared with the TEB algorithm, and the following Figure 8The figure shows a dynamic simulation environment with a length of 14 meters and a width of 10 meters. Cylinders, spheres and cubes are set as static obstacles in the environment, and pedestrians are added as dynamic obstacles, moving at a constant speed of 1.5m / s. Pedestrian D1 moves back and forth along a straight line between (4m, 0m) and (4m, -4m), and pedestrian D2 moves back and forth along a straight line between (8m, 4m) and (8m, -3m). The starting point of the trajectory planning is set to (0m, 0m) and the end point is (7.5m, -5m).

[0134] The experimental parameters are shown in Table 1:

[0135] Table 1 Experimental parameters

[0136]

[0137] like Fig. 9 The figure shows the comparison of the longitudinal speed and azimuth planning results of the algorithm of the present invention and the TEB algorithm at a certain moment. It can be seen that the driving time planned by the TEB algorithm is significantly longer than that of the algorithm of the present invention, and the speed and azimuth planning by the algorithm of the present invention change more smoothly.

[0138] Fig.10 The driving trajectories of the algorithm of the present invention and the TEB algorithm are shown respectively. The algorithm of the present invention successfully avoids all obstacles and reaches the target point smoothly. However, due to the high safety risk of the planned trajectory of the TEB algorithm, the vehicle collides with the dynamic obstacle D1 during driving.

[0139] Unstructured environments have the characteristics of no clear rules, irregular obstacle distribution and strong dynamics, and the movement of dynamic obstacles is highly uncertain. Therefore, in order to more comprehensively evaluate the performance of the algorithm of the present invention and the TEB algorithm, a multi-dynamic obstacle experimental scene with a size of 10m×10m is set up, such as Fig.11 As shown in the figure; in the experiment, the number of dynamic obstacles gradually increased from 1 to 7, two at a time, and the starting and ending positions of the dynamic obstacles were randomly generated each time a new plan was made, and they moved back and forth between the starting and ending positions along a straight line at a speed of 1.5m / s; the starting point of the plan was (0m, 0m), and the end point was (10m, 0m); during the experiment, the average driving time of the two algorithms when the planning was successful and no collision occurred was counted respectively, and each algorithm collected 30 valid data.

[0140] like Fig.12As shown in the figure, the comparison results of the average driving time between the algorithm of the present invention and the TEB algorithm in the scenario with multiple dynamic obstacles are as follows: when the number of dynamic obstacles is small, the average driving time of the TEB algorithm is slightly shorter than that of the algorithm of the present invention, which is mainly due to the fact that the TEB algorithm optimizes the trajectory through the elastic time band, making the trajectory closer to the obstacle, thereby shortening the driving time. However, when the number of dynamic obstacles increases to 3, the driving time of the algorithm of the present invention begins to be shorter than that of the TEB algorithm; as the number of dynamic obstacles further increases to 5 and 7, the average driving time of the algorithm of the present invention is significantly lower than that of the TEB algorithm, and the gap between the two gradually widens. The fundamental reason for this difference is that with the increase in the number of dynamic obstacles, the TEB algorithm is prone to fall into a local suboptimal solution during the trajectory adjustment process due to the lack of a clear spatiotemporal constraint area, resulting in a significant increase in driving time. In addition, the average driving time of the algorithm of the present invention under different numbers of obstacles changes little, showing good stability and adaptability. In contrast, the average driving time of the TEB algorithm increases significantly with the increase in the number of obstacles, reflecting its limitation of weak adaptability in complex scenarios.

[0141] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the essence and scope of the technical solution of the present invention.

Claims

1. A method for unmanned vehicle trajectory planning in an unstructured environment, characterized in that: The steps include: S1. Construct a motion primitive tree based on the vehicle kinematic model in a Cartesian coordinate system, search the motion primitive tree, and obtain an initial trajectory; S2, discretize the initial trajectory into state points and project them into the xyt configuration space, and construct the drivable area of ​​the vehicle in the space-time domain based on the state points; S3. Using a piecewise Bezier curve in the drivable area of ​​the vehicle in the time and space domain to optimize the initial trajectory to obtain the final drivable trajectory.

2. According to the unmanned vehicle trajectory planning method in an unstructured environment according to claim 1, it is characterized in that: The steps of constructing a motion primitive tree based on the vehicle kinematic model in a Cartesian coordinate system, searching the motion primitive tree, and obtaining an initial trajectory are as follows: S1.

1. According to the two-degree-of-freedom kinematic model of the vehicle, the vehicle differential equation can be obtained, which is expressed as follows: In the formula, x and y are the coordinates of the center point of the rear axle of the vehicle, respectively, where x is the horizontal coordinate of the center point and y is the vertical coordinate of the center point; v is the longitudinal speed of the vehicle; l is the wheelbase of the vehicle; is the vehicle body direction angle; δ is the front wheel turning angle; a is the vehicle acceleration; They represent the derivatives of the coordinates of the center point of the rear axle of the vehicle, the direction angle of the vehicle body, and the longitudinal speed of the vehicle respectively; S1.2, the vehicle acceleration a and the front wheel steering angle δ are used as the longitudinal control variable and the lateral control variable of the vehicle, respectively, where a∈[a min ,a max ],δ∈[-δ min ,δ max ]; Discretize the longitudinal control amount and lateral control amount of the vehicle into a∈{a1,a2,...,a n },δ∈{δ1,δ2,...,δ n }, the value of n is positively correlated with the trajectory, and the forward simulation time length is τ; in the node expansion process, with a fixed time interval Δt and a minimum expansion step length d min Perform step-by-step node expansion, the expression is as follows: definition is the vehicle state space, and the state at a given time T is X T , the corresponding control quantity is a T and δ T , then the state X at the next moment T+1 It can be expressed as: Where d is the distance traveled by the vehicle during the forward simulation, R is the turning radius of the vehicle, and x T ,y T is the coordinate of the rear axle center point of the vehicle at a given time T, is the direction angle of the vehicle at a given time T, v T is the longitudinal velocity of the vehicle at a given time T, a T is the acceleration of the vehicle at time T, t T is the time of the vehicle at a given time T, is the direction angle of the vehicle at the T+1th moment.

3. The method for unmanned vehicle trajectory planning in an unstructured environment according to claim 1, characterized in that: The steps of discretizing the initial trajectory into state points and projecting them into the xyt configuration space, and constructing the drivable area of ​​the vehicle in the space-time domain based on the state points are as follows: S2.

1. Construct a space-time grid map to describe the distribution information of obstacles in space and time in the future; S2.

2. Based on the spatiotemporal grid map and the initial trajectory, construct the drivable area of ​​the vehicle in the spatiotemporal domain.

4. The method for unmanned vehicle trajectory planning in an unstructured environment according to claim 1, characterized in that: The steps of optimizing the initial trajectory using the piecewise Bezier curve in the drivable area of ​​the vehicle in the time and space domain to obtain the final drivable trajectory are as follows: S3.

1. Generate an L-order Bezier curve f(t) consisting of L+1 control points within the drivable area of ​​the vehicle in the space-time domain. The expression for generating the Bezier curve is as follows: Where t represents time, t∈[0,1]; p i is the i-th control point of the Bezier curve; is the Bernstein polynomial; S3.

2. Divide the drivable area of ​​the vehicle in the time and space domain according to the complexity of the environment to improve the trajectory optimization effect; The environmental complexity includes: static obstacles and dynamic obstacles; S3.3, optimizing the initial trajectory with minimizing jerk as the optimization objective; S3.

4. During the Bezier curve optimization process, the safety and dynamic constraints, the first and last state constraints, and the continuity constraints of the trajectory are set respectively; S3.

5. By introducing the linear inequalities, equality constraints and quadratic objective functions in S3.4, the trajectory optimization problem can be transformed into a quadratic programming problem, and the OOQP solver is used to solve the quadratic programming problem to obtain the optimized trajectory.

Images