Layered direct trajectory planning method for unmanned special vehicle in complex non-structural environment

Abstract

The invention discloses a layered direct trajectory planning method for an unmanned special vehicle in a complex non-structural environment. The method comprises the following steps: 1, acquiring perception information such as obstacles and road boundaries in the non-structural environment and a vehicle starting point state; 2, aiming at anti-rollover and efficient driving requirements in a virtual space, planning and modeling a long sight distance trajectory into an off-line constraint optimal control problem based on pseudo-spectral method numerical solution, and obtaining a global reference trajectory; 3, aiming at tracking error elimination and dynamic obstacle avoidance requirements in a real space, planning and modeling a short-sight-distance trajectory into an online optimal control problem based on model prediction control, and obtaining a local actual trajectory; and 4, inputting the local actual track into a bottom layer control system, and obtaining an end point state as a subsequent planning starting point state. On the basis of meeting common track planning requirements such as obstacle avoidance, the special vehicle can be prevented from rolling over under aggressive working conditions, and meanwhile the passing efficiency is improved.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving of special vehicles, and specifically relates to a hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments. Background Art

[0002] Special vehicles originated from military applications in the 20th century and have gradually expanded to civil special fields such as disaster relief and polar scientific research. With the upgrading of the demand for unmanned operations, their application scenarios have covered complex unstructured environments such as forest fire prevention and high-voltage line inspection. The current mature application of autonomous driving technology in urban road scenarios is in sharp contrast to complex unstructured scenarios: the former relies on structured road features to achieve stable navigation, while the latter faces multiple challenges such as the sudden emergence of dynamic obstacles, variable ground adhesion, and unstable vehicle body postures. In typical operating environments such as wilderness and ruins without clear path markings, the intelligent system not only needs to cope with mechanical constraints brought by terrain undulations, but also needs to coordinate the decision-making conflict between maintaining motion stability and avoiding sudden risks. This composite requirement of ensuring the passing safety of the vehicle on terrains such as gravel beaches and steep slopes while also responding in real time to threats such as falling rocks and moving obstacles constitutes the key technical barrier restricting the in-depth application of unmanned special vehicles.

[0003] With the in-depth application of unmanned driving technology in the field of special vehicles, trajectory planning in complex unstructured environments faces technical challenges of multi-modal constraint coupling. Traditional trajectory planning methods mostly adopt a single-layer planning architecture and are difficult to effectively coordinate the contradiction between the global path optimality and the real-time local obstacle avoidance. Especially in special operation scenarios that include dynamic obstacles, unstructured terrains, and roll stability requirements, the existing technologies have three significant defects: First, conventional dynamic models are difficult to accurately predict and characterize the stability state of special vehicles during aggressive maneuvers, resulting in a rollover risk in the trajectory planning results; Second, the trajectory prediction of dynamic obstacles mostly uses the model extrapolation method and cannot adapt to the stochastic characteristics of complex interaction scenarios; Third, the static obstacle avoidance strategy lacks an intelligent decision-making mechanism that combines bypassing and surrounding, resulting in a significant reduction in passing efficiency.

[0004] In summary, the existing technologies have the technical problem that they cannot accurately plan the global and local trajectories that take into account efficiency, stability, and local obstacle avoidance safety for the trajectory planning problem of unmanned special vehicles in complex unstructured environments. Summary of the Invention

[0005] The present invention is proposed to solve the above-mentioned deficiencies of the existing technologies, and provides a hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments, aiming to achieve the collaborative optimization of global path optimality and local dynamic obstacle avoidance, so as to effectively improve the motion stability, task execution reliability, and complex dynamic environment adaptability of unmanned special vehicles in complex unstructured environments.

[0006] To achieve the above object of the invention, the following technical solutions are adopted:

[0007] A hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments according to the present invention is characterized in that it is carried out according to the following steps:

[0008] Step 1: The perception layer of the unmanned special vehicle obtains the trajectory planning task requirements issued by the decision-making layer of the unmanned special vehicle, so as to online collect non-structured environment perception information, and identify road boundary, static obstacle height and radius parameters, and dynamic obstacle trajectory data;

[0009] Step 2: The planning layer of the unmanned special vehicle obtains the initial conditions for trajectory planning, including: state variables and control variables of the unmanned special vehicle at the starting point, and transforms the rollover prevention requirement in the trajectory planning task requirement into an explicit inequality constraint for the state variables and control variables in the virtual space, and transforms the efficient driving requirement in the trajectory planning task requirement into a cost function, so as to construct an offline constrained optimal control model for long-horizon trajectory planning, and after numerically solving using the pseudospectral method, obtain the optimal global reference trajectory;

[0010] Step 3: In the real space, a tracking error cost function is established using the feedback correction characteristic of model predictive control to minimize the tracking error of the optimal global reference trajectory;

[0011] Based on the known dynamic obstacle trajectory data obtained by the perception layer, the LSTM neural network is used to predict the future dynamic obstacle trajectory, obtain the future trajectory data of the dynamic obstacle, and use it to construct the obstacle avoidance constraint condition;

[0012] After constructing and solving an online constrained optimal control model for short-horizon trajectory planning based on model predictive control from the tracking error cost function and the obstacle avoidance constraint condition, obtain the optimal local actual trajectory;

[0013] Step 4: Input the optimal local actual trajectory into the bottom control system of the unmanned special vehicle, and obtain the desired quantized roll stability state and end state variables to complete the response to the trajectory planning task requirements.

[0014] The hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments according to the present invention is also characterized in that the step 2 includes:

[0015] Step 2.1: Establish a four-degree-of-freedom dynamic model of the unmanned special vehicle in the longitudinal, lateral, yaw and roll directions, and ignore the longitudinal force component therein. After transforming the four-degree-of-freedom dynamic model into a closed ordinary differential equation system and solving it, obtain the explicit inequality constraint conditions of the state variables and control variables under the rollover prevention requirement;

[0016] Step 2.2: Establish a three-degree-of-freedom kinematic model for the unmanned special vehicle in the longitudinal, lateral, and yaw directions, which is used to derive the kinematic constraint conditions and the extreme value constraint conditions of the state variables and control variables;

[0017] Step 2.3: Distinguish between bypassing and straddling static obstacles based on the height and radius parameters of the static obstacles and the minimum ground clearance and wheelbase of the unmanned special vehicle, so as to establish the combined obstacle avoidance constraint conditions for straddling and bypassing;

[0018] Step 2.4: Establish constraints for the state variables and control variables at the end point according to the high-efficiency driving requirements, which together with the state variables and control variables at the starting point form the two-point boundary constraint conditions;

[0019] Step 2.5: Establish a cost function for the longitudinal passing distance in the frenet coordinate system according to the high-efficiency driving requirements, and jointly construct an optimal control model with the constraint conditions in Steps 2.1 - 2.4;

[0020] Step 2.6: Establish a control space according to the control variables, design a trajectory planner for sampling the control space, and combine the constraint conditions in Steps 2.1 - 2.4 to obtain an initial trajectory using the trajectory planner;

[0021] Step 2.7: Based on the initial trajectory, use the pseudospectral method to solve the offline constrained optimal control model of the long-range trajectory planning for iterative optimization, and obtain the optimal global reference trajectory.

[0022] Furthermore, the third step includes:

[0023] Step 3.1: Combine the four-degree-of-freedom dynamics model and the three-degree-of-freedom kinematic model of the unmanned special vehicle in Step 2 into a unified system model, and then perform linearization and discretization processing on the system model to obtain the state space equation and the output prediction equation;

[0024] Step 3.2: Establish a tracking error cost function for the desired state variables and the actual state variables according to the requirement of minimizing the tracking error in the short-range trajectory planning;

[0025] Step 3.3: Establish obstacle avoidance constraint conditions using the future trajectory data of the dynamic obstacle according to the dynamic obstacle avoidance requirement in the short-range trajectory planning;

[0026] Step 3.4: Construct the online constrained optimal control model for the short-range trajectory planning based on model predictive control from the tracking error cost function in Step 3.2 and the obstacle avoidance constraint conditions in Step 3.3, and perform rolling optimization using a nonlinear optimal solver to obtain the optimal local actual trajectory.

[0027] Further, step four includes:

[0028] Step 4.1: Input the optimal local actual trajectory into the underlying control system for execution;

[0029] Step 4.2: Substitute the optimal local actual trajectory, the state variables and control variables at the starting point into the closed ordinary differential equations to obtain the expected quantified roll stability state, which is used as the stability reference for the underlying control system;

[0030] Step 4.3: Obtain the end state variables and control variables as the initial conditions for subsequent trajectory planning.

[0031] An electronic device according to the present invention includes a memory and a processor, characterized in that the memory is used to store a program for supporting the processor to execute the hierarchical direct trajectory planning method, and the processor is configured to execute the program stored in the memory.

[0032] A computer-readable storage medium according to the present invention, characterized in that a computer program is stored on the computer-readable storage medium, and when the computer program is run by a processor, it executes the steps of the hierarchical direct trajectory planning method.

[0033] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0034] 1. Through the hierarchical planning architecture, the present invention realizes the closed-loop coupling of long-range global trajectory optimization and short-range local obstacle avoidance for the first time, breaks through the bottleneck of the mutual restriction between strategic decision-making and tactical adjustment in traditional single-layer planning, and takes into account the path optimality and response real-time performance in complex terrains.

[0035] 2. The present invention innovatively integrates the explicit conversion of dynamic constraints and the cross-round obstacle avoidance decision-making mechanism, incorporates vehicle rollover stability guarantee, dynamic obstacle prediction accuracy, and static obstacle handling strategies into a unified optimization framework, and significantly improves the comprehensive performance index of trajectory planning in unstructured environments. Description of the Drawings

[0036] Figure 1 It is a four-degree-of-freedom dynamics model diagram of the present invention;

[0037] Figure 2 It is a three-degree-of-freedom kinematics model diagram of the present invention;

[0038] Figure 3 It is a schematic diagram of the body contour fitting of the present invention;

[0039] Figure 4 It is an algorithm flowchart for the planner to solve the initial solution of the present invention. Detailed Embodiments

[0040] In this embodiment, a hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments is carried out according to the following steps:

[0041] Step 1: The perception layer of the unmanned special vehicle obtains the trajectory planning task requirements issued by the decision-making layer of the unmanned special vehicle, so as to collect non-structured environment perception information online and identify road boundary, static obstacle height and radius parameters, and dynamic obstacle trajectory data;

[0042] Step 2: The planning layer of the unmanned special vehicle obtains the initial conditions for trajectory planning, including: the state variables and control variables of the unmanned special vehicle at the starting point, and transforms the anti-rollover requirement in the trajectory planning task requirement into an explicit inequality constraint for the state variables and control variables in the virtual space, and transforms the efficient driving requirement in the trajectory planning task requirement into a cost function, so as to construct an offline constrained optimal control model for long-range trajectory planning, and after numerically solving using the pseudospectral method, obtain the optimal global reference trajectory;

[0043] Step 2.1: Establish a four-degree-of-freedom dynamic model of the unmanned special vehicle for longitudinal, lateral, yaw and roll, and ignore the longitudinal force component therein. After transforming the four-degree-of-freedom dynamic model into a closed ordinary differential equation system and solving it, obtain the explicit inequality constraint conditions of the state variables and control variables under the anti-rollover requirement;

[0044] In specific implementation, the coordinate axis center of the above-mentioned unmanned special vehicle is selected at the chassis centroid, and the schematic diagram of the dynamic model is as Figure 1 shown. The following simplifications are made in the modeling process:

[0045] 1. Ignore the vertical and pitch motions of the vehicle;

[0046] 2. Ignore the influence of unsprung mass;

[0047] 3. Ignore the Ackermann steering characteristics of the whole vehicle;

[0048] According to Newton's second law, the differential equation system of the four-degree-of-freedom dynamic model can be obtained as Equation (1):

[0049] (1)

[0050] In Equation (1), is the mass of the whole vehicle, v x , v y are the longitudinal and lateral speeds at the centroid respectively, represents the first derivative of the longitudinal speed, represents the first derivative of the lateral speed, ω r is the yaw angular velocity, F xi (i = 1, 2, 3, 4) are the longitudinal forces of each wheel, F yi$(i = 1, 2, 3, 4)$ is the lateral force of each wheel, $F$ zi $(i = 1, 2, 3, 4)$ is the vertical force of each wheel, $\delta$ f is the front wheel steering angle, $I$ z is the moment of inertia in the axial direction at the center of mass, $a$ and $b$ are the distances from the center of mass to the centers of the front and rear axles respectively, $I$ x is the moment of inertia in the axial direction at the roll center, $h$ is the vertical distance from the center of mass of the sprung mass to the roll center, is the roll angle of the sprung mass, represents the first derivative of the roll angle, represents the second derivative of the roll angle, 、 are the equivalent damping and stiffness of the whole vehicle respectively, $d$ is the wheelbase of the front and rear wheels.

[0051] According to vehicle dynamics theory, the decisive factors for the vehicle roll stability state are the lateral acceleration of the whole vehicle and the roll angle of the sprung mass. Therefore, the research on roll stability only involves three degrees of freedom: lateral, yaw, and roll. In addition, the longitudinal forces of the four wheels are affected by the output torque and rolling resistance couple moment of the wheels, and their change trends cannot be predicted during the trajectory planning stage. Moreover, due to the small front wheel steering angle and small lateral components of the longitudinal forces, the term $(F$ x1 + $F$ x2 )$\sin\delta$ f in the equation is ignored, and the differences in longitudinal forces between the two wheels on both sides during vehicle driving $(F$ x1 - $F$ x2 ) and $(F$ x3 - $F$ x4 ) are also ignored. In summary, the lateral and yaw motion equations can be simplified to Equation (2):

[0052] (2)

[0053] Select the Magic Formula to calculate the tire lateral force, and its formula is as Equation (3):

[0054] (3)

[0055] In Equation (3), $X$ is the input variable; $Y$ is the output variable, which are the sideslip angle $\beta$ and the lateral force $F$ yi here respectively; $B$, $C$, $D$, $E$, $S$ h , $S$ v are the model parameters, represents the input variable, which is the sideslip angle of the front and rear wheels here, represents the output variable, which is the tire lateral force here. The calculation formula for the sideslip angle of the front and rear wheels is Equation (4):

[0056] (4)

[0057] The quantitative characterization of roll stability uses the lateral load transfer ratio LTR shown in Equation (5), which is defined as the ratio of the difference between the left and right wheels to the sum, and takes values between ±1. ±1 indicates that the wheels leave the ground, and 0 indicates no roll:

[0058] (5)

[0059] According to the above dynamic model, F z1 +F z3 -F z2 -F z4 can be derived. Since the vertical motion of the whole vehicle is not considered above, the value of F z1 +F z3 +F z2 +F z4 is the vehicle weight mg. The following assumptions are made here:

[0060] 1. The first and second derivatives of the roll angle ;

[0061] 2. The absolute value of the roll angle is extremely small, that is, , ;

[0062] 3. The absolute value of the sideslip angle of the center of mass is extremely small, that is, ;

[0063] 4. The first derivative of the sideslip angle of the center of mass .

[0064] In summary, the simplified LTR expression can be obtained as Equation (6):

[0065] (6)

[0066] In Equation (6), represents the gravitational acceleration;

[0067] Since the pitch motion of the vehicle body is not considered above, the expression of the vertical load Fzi (i = 1, 2, 3, 4) of the four wheels is as Equation (7):

[0068] (7)

[0069] Considering that model simplification and a series of approximations will introduce errors to the estimation of LTR, the roll stability constraint is defined as Equation (8):

[0070] (8)

[0071] In Equation (8), represents the maximum value of the lateral load transfer ratio.

[0072] Step 2.2: Establish a three-degree-of-freedom kinematic model for the unmanned special vehicle in the longitudinal, lateral, and yaw directions, which is used to derive the kinematic constraint conditions and the extreme value constraint conditions of the state variables and control variables;

[0073] In specific implementation, the center point of the rear axle is selected as the reference point for the kinematic model. As Figure 2 shown, it can be described by the differential equation group matrix shown in Equation (9), that is, the kinematic constraint conditions:

[0074] (9)

[0075] In Equation (9), respectively represent the derivatives of the two-dimensional reference point coordinates at time t, represents the first derivative of the vehicle heading angle at time t. represents the front wheel steering angular velocity at time t, represents the first derivative of the longitudinal velocity at time t. During the planning process, each state variable and control variable should not exceed the maximum nominal value allowed by the vehicle itself, that is:

[0076] (10)

[0077] In Equation (10), a max and a min are the maximum driving acceleration and maximum braking deceleration that the drive system and braking system can provide respectively. δ f is the maximum front wheel steering angle. represents the maximum front wheel steering angle, represents the maximum front wheel steering angular velocity, represents the maximum longitudinal velocity.

[0078] Step 2.3: According to the height and radius parameters of the static obstacle and the minimum ground clearance and wheelbase of the unmanned special vehicle, distinguish between bypassing the static obstacle and straddling the static obstacle, so as to establish the cross-bypass obstacle avoidance constraint conditions;

[0079] In specific implementation, since the cross-section of the vehicle is approximately rectangular, it will cause the optimization solver to face non-differentiable problems when dealing with obstacle avoidance constraints. Therefore, a common spatio-temporal corridor based on multi-circle fitting of the vehicle body contour is constructed to deal with bypassing obstacles. As Figure 3 shown, the obstacle avoidance constraint conditions for bypassing obstacles are as Equation (11):

[0080] (11)

[0081] In Equation (11), [x disc,j (t), y disc,j (t)], [x obst,m , y obst,mare the coordinate values of the center of the j-th circle and the m-th detouring obstacle of the whole vehicle at time t, dis() represents the Euclidean distance between two points, and R disc and R obst,m are the radii of the circle and the m-th cross-border obstacle respectively, N is the number of time steps, and N DISC is the number of circles. represents the i-th time step, represents the (i + 1)-th time step.

[0082] Use geometric methods to solve the rectangle of the whole vehicle and N DISC disks, so as to obtain the expressions for the center coordinates and radius of the j-th circle using Equation (12):

[0083] (12)

[0084] In Equation (12), represents the initial time, represents the end time, represents the abscissa value at time t, represents the ordinate value at time t.

[0085] The center radius R disc and the number of disks N DISC The relational expression is as Equation (13):

[0086] (13)

[0087] In Equation (13), l f and l r are the distances from the front and rear axles to the envelope rectangle of the whole vehicle respectively.

[0088] The essence of the cross-border obstacle avoidance constraint is that the trajectories of all four wheels do not cross the obstacle, and the expression is as Equation (14):

[0089] (14)

[0090] In Equation (14), represents the tire width, represents the center coordinate value of the j-th tire at time t, represents the coordinate value of the n-th cross-border obstacle, represents the radius value of the n-th cross-border obstacle.

[0091] Step 2.4. According to the high-efficiency driving requirements, establish the constraints of the state variables and control variables at the end point, and jointly form the two-point boundary constraint conditions with the state variables and control variables at the starting point;

[0092] In specific implementation, since the planned goal is efficient driving, no restrictions are imposed on key positions, and only the constraint for the vehicle to maintain stable driving at this time is considered, that is, Equation (15):

[0093] (15)

[0094] Step 2.5: According to the demand for efficient driving, establish the cost function of the longitudinal passing distance in the frenet coordinate system, and jointly construct the optimal control model with the constraint conditions in Steps 2.1 - 2.4;

[0095] In specific implementation, the cost function J1 is set as the difference in the longitudinal distance between the end position at time t = t N and the start position at time t = t0 in the frenet coordinate system, that is, Equation (16):

[0096] (16)

[0097] In Equation (16), s(t N ) and s(t0) are the longitudinal coordinates of the vehicle in the frenet coordinate system at time t = t N and time t = t0, respectively.

[0098] According to the above derivation, the trajectory planning task is described using the standard optimal control framework as shown in Equation (17):

[0099] (17)

[0100] In Equation (17), X(t) is the state variable at time t, specifically , U(t) is the control variable at time t, specifically . includes the differential equation set describing the kinematic and dynamic characteristics of the vehicle at time t. represents the anti - collision constraint. represents the anti - rollover constraint. represents the upper and lower bounds of the state, represents the upper and lower bounds of the control space. represents the value of the state variable at the initial time, represents the value of the control variable at the end time, represents the value of the state variable at the end time.

[0101] Step 2.6: Establish the control space according to the control variable, design a trajectory planner for sampling the control space, and combine the constraint conditions in Steps 2.1 - 2.4 to obtain the initial trajectory using the trajectory planner;

[0102] In specific implementation, a planner for gradually sampling the control space is designed to obtain an initial solution. The algorithm is illustrated as Figure 4 shown;

[0103] Step 2.7: Based on the initial trajectory, use the pseudospectral method to solve the offline constrained optimal control model for long-horizon trajectory planning for iterative optimization, and obtain the optimal global reference trajectory.

[0104] In specific implementation, since the Radau pseudospectral method is more suitable for dealing with time endpoints and fixed boundary conditions, and the adaptive pseudospectral method has the superiority of taking into account high precision and adaptive interpolation point division, the adaptive Radau pseudospectral method is adopted to solve the optimal control problem.

[0105] Step Three: In the real space, utilize the feedback correction characteristic of model predictive control to establish a tracking error cost function to minimize the tracking error of the optimal global reference trajectory;

[0106] Based on the known dynamic obstacle trajectory data obtained by the perception layer, use the LSTM neural network to predict the future dynamic obstacle trajectory, obtain the future trajectory data of the dynamic obstacle, and use it to construct the obstacle avoidance constraint condition;

[0107] After constructing and solving the online constrained optimal control model for short-horizon trajectory planning based on model predictive control from the tracking error cost function and the obstacle avoidance constraint condition, obtain the optimal local actual trajectory;

[0108] Step 3.1: Combine the four-degree-of-freedom dynamics model and the three-degree-of-freedom kinematics model of the unmanned special vehicle in Step Two into a unified system model, and then perform linearization and discretization processing on the system model to obtain the state space equation and the output prediction equation;

[0109] In specific implementation, after linearizing the system model, the state space model shown in Equation (18) is obtained:

[0110] (18)

[0111] where, A is the state transition matrix, B is the control input matrix, C is the state output matrix, is the state variable of the unified system model at time t, is the first derivative of the state variable of the unified system model at time t, and Y(t) is to prompt the controller to better track the reference trajectory at the geometric level and monitor the roll stability state. The controller output variable is defined as Equation (19):

[0112] (19)

[0113] That is:

[0114] (20)

[0115] Explicit expressions cannot be solved for matrices A and B, so numerical methods are used for calculation. The subsequent derivation is the conventional design idea of the MPC controller and will not be elaborated here.

[0116] Step 3.2: Based on the requirement of minimizing the tracking error in the short-sighted trajectory planning, establish a tracking error cost function for the desired state variable and the actual state variable.

[0117] In the specific implementation, considering the requirements of the short-sighted trajectory planning, the MPC cost function J2 is formulated as in Equation (21):

[0118] (21)

[0119] In Equation (21), P h and C h are the prediction horizon and the control horizon respectively, U ref (t + k) is the control sequence at time t + k planned by the long-sighted planning, Y ref (t + k) is the desired state at time t + k calculated based on the nominal vehicle model and the control sequence. Q1 and Q2 are the weight coefficient matrices of the difference between the state variables and the control variables in the short-sighted and long-sighted planning results respectively. R represents the weight coefficient matrix of the terminal state variable, ε represents the relaxation variable for restricting the soft constraint, and ρ is the weight coefficient used to penalize the violation of the soft constraint.

[0120] Step 3.3: Based on the dynamic obstacle avoidance requirement in the short-sighted trajectory planning, establish an obstacle avoidance constraint condition using the future trajectory data of the dynamic obstacle.

[0121] In the specific implementation, the LSTM neural network is used to predict the future trajectory of the dynamic obstacle, and the system input is the historical trajectory:

[0122] (22)

[0123] In Equation (22), k is the k-th time step, which is the same as the time step of the above MPC. X k represents the set of input variables at the k-th time step, h h is the required length of the historical sequence, represents the position coordinates of n dynamic obstacles within a certain distance in front of the vehicle at the k-th step. To reduce the computational burden, the distance is set to 30 m in the frenet coordinate system here. n is the number of relevant obstacles. The system output is the future trajectory:

[0124] (23)

[0125] In Equation (23), hp is the prediction range, Y k represents the set of output variables at the k-th time step, represents the position coordinates of the predicted dynamic obstacle at the (k + 1)-th step. According to the predicted position, the dynamic obstacle avoidance constraint condition can be established based on the obstacle avoidance expression shown in step 2.3.

[0126] Step 3.4: Construct an online constrained optimal control model for short-sighted trajectory planning based on model predictive control from the tracking error cost function in step 3.2 and the obstacle avoidance constraint condition in step 3.3, and use a non-linear optimal solver for rolling optimization to obtain the optimal local actual trajectory.

[0127] In specific implementation, the optimal control model for short-sighted trajectory planning can be expressed by the following expression (24):

[0128] (24)

[0129] In formula (24), represents the threshold of the output variable, represents the anti-collision constraint, represents the anti-rollover constraint. Using the non-linear optimal solver Fmincon for rolling optimization of the cost function, a series of control increments in the control time domain can be obtained:

[0130] (25)

[0131] In formula (25), represents the set of control increments from the k-th to the (k + C) h th discrete points, represents the control increment at the k-th discrete point.

[0132] After obtaining the control increment sequence, the actual control input increment of the system can be calculated through the first element of the control sequence:

[0133] (26)

[0134] Similarly, according to the current state information to predict the output of the next cycle, the remaining control increments of the system can be generated in sequence.

[0135] Step Four: Input the optimal local actual trajectory into the underlying control system of the unmanned special vehicle, and obtain the desired quantized roll stability state and end state variables to complete the response to the trajectory planning task requirements.

[0136] Step 4.1: Input the optimal local actual trajectory into the underlying control system for execution;

[0137] Step 4.2: Substitute the optimal local actual trajectory and the state variables and control variables at the starting point into the closed ordinary differential equations to obtain the expected quantized roll stability state, which is used as the stability reference for the underlying control system.

[0138] Step 4.3: Obtain the end state variables and control variables as the initial conditions for subsequent trajectory planning.

[0139] In this embodiment, an electronic device includes a memory and a processor. The memory is used to store a program that supports the processor to execute the above method, and the processor is configured to execute the program stored in the memory.

[0140] In this embodiment, a computer-readable storage medium stores a computer program, and when the computer program is run by a processor, it executes the steps of the above method.

Claims

1. A hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments, characterized in that The following steps are carried out: Step 1: The perception layer of the unmanned special vehicle obtains the trajectory planning task requirements issued by the decision-making layer of the unmanned special vehicle, so as to online collect non-structural environment perception information, and identify road boundary, static obstacle height and radius parameters, and dynamic obstacle trajectory data. Step 2: The planning layer of the unmanned special vehicle obtains the initial conditions of trajectory planning, including: state variables and control variables of the unmanned special vehicle at the starting point, and transforms the rollover prevention requirement in the trajectory planning task requirement into explicit inequality constraints for state variables and control variables in the virtual space, and transforms the efficient driving requirement in the trajectory planning task requirement into a cost function, so as to construct an offline constrained optimal control model for long-range trajectory planning. After numerically solving using the pseudospectral method, the optimal global reference trajectory is obtained. Step 3: In the real space, a tracking error cost function is established using the feedback correction characteristic of model predictive control to minimize the tracking error of the optimal global reference trajectory. Based on the known dynamic obstacle trajectory data obtained by the perception layer, the LSTM neural network is used to predict the future dynamic obstacle trajectory, and the future trajectory data of the dynamic obstacle is obtained and used to construct the obstacle avoidance constraint conditions. After constructing and solving an online constrained optimal control model for short-range trajectory planning based on model predictive control from the tracking error cost function and the obstacle avoidance constraint conditions, the optimal local actual trajectory is obtained. Step 4: The optimal local actual trajectory is input into the bottom control system of the unmanned special vehicle, and the expected quantized roll stability state and end state variables are obtained to complete the response to the trajectory planning task requirements.

2. The hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments according to claim 1, wherein The said Step 2 includes: Step 2.1: Establish a four-degree-of-freedom dynamics model for the longitudinal, lateral, yaw and roll of the unmanned special vehicle, and ignore the longitudinal force component therein. After transforming the four-degree-of-freedom dynamics model into a closed ordinary differential equation group and solving it, the explicit inequality constraint conditions of state variables and control variables under the rollover prevention requirement are obtained. Step 2.2: Establish a three-degree-of-freedom kinematics model for the longitudinal, lateral and yaw of the unmanned special vehicle, which is used to deduce the kinematic constraint conditions and the extreme value constraint conditions of the state variables and control variables. Step 2.3: According to the static obstacle height, radius parameters, minimum ground clearance and wheelbase of the unmanned special vehicle, distinguish between bypassing static obstacles and striding over static obstacles, so as to establish cross-bypass combined obstacle avoidance constraint conditions. Step 2.4: According to the efficient driving requirement, establish the constraints of state variables and control variables at the end point, which together with the state variables and control variables at the starting point form two-point boundary constraint conditions. Step 2.5: According to the efficient driving requirement, establish a cost function for the longitudinal passing distance in the frenet coordinate system, and jointly construct an optimal control model with the constraint conditions in Steps 2.1 - 2.

4. Step 2.6: Establish a control space according to the control variables, design a trajectory planner for sampling the control space, and combine the constraint conditions in Steps 2.1 - 2.4 to obtain an initial trajectory using the trajectory planner. Step 2.7: Based on the initial trajectory, use the pseudospectral method to solve the offline constrained optimal control model of the long LOS trajectory planning for iterative optimization to obtain the optimal global reference trajectory.

3. The hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments according to claim 2, characterized in that The third step includes: Step 3.1: Combine the four-degree-of-freedom dynamics model and the three-degree-of-freedom kinematics model of the unmanned special vehicle in Step 2 into a unified system model, and then perform linearization and discretization processing on the system model to obtain the state space equation and the output prediction equation; Step 3.2: Establish a tracking error cost function for the desired state variable and the actual state variable according to the requirement of minimizing the tracking error in the short LOS trajectory planning; Step 3.3: Establish an obstacle avoidance constraint condition using the future trajectory data of the dynamic obstacle according to the dynamic obstacle avoidance requirement in the short LOS trajectory planning; Step 3.4: Construct the online constrained optimal control model of the short LOS trajectory planning based on model predictive control from the tracking error cost function in Step 3.2 and the obstacle avoidance constraint condition in Step 3.3, and perform rolling optimization using a nonlinear optimal solver to obtain the optimal local actual trajectory.

4. The hierarchical direct trajectory planning method for unmanned special vehicles in complex unstructured environments according to claim 3, wherein The fourth step includes: Step 4.1: Input the optimal local actual trajectory into the underlying control system for execution; Step 4.2: Substitute the optimal local actual trajectory, the state variables and control variables at the starting point into the closed ordinary differential equations to obtain the desired quantified roll stability state, which is used as the stability reference for the underlying control system; Step 4.3: Obtain the end state variables and control variables as the initial conditions for subsequent trajectory planning.

5. An electronic device, comprising a memory and a processor, characterized in that, The memory is used to store a program for supporting the processor to execute the hierarchical direct trajectory planning method according to any one of claims 1-4, and the processor is configured to execute the program stored in the memory.

6. A computer-readable storage medium, on which a computer program is stored, characterized in that, When the computer program is run by the processor, it executes the steps of the hierarchical direct trajectory planning method according to any one of claims 1-4.

Images