An unmanned semi-trailer vehicle real-time trajectory planning method based on EDT constraint

Abstract

This invention discloses a real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints, belonging to the field of multi-vehicle autonomous driving technology. The method includes: acquiring surrounding environmental information through an onboard LiDAR; obtaining vehicle positioning information through an IMU and vehicle odometer; performing Euclidean distance transformation on obstacles on a grid map to obtain the environmental distance field; constructing a set of motion primitives based on the semi-trailer's kinematic model and generating an initial trajectory using graph search; constructing a model predictive control objective function based on the initial trajectory, including trajectory tracking, control energy, smoothness, and EDT obstacle avoidance costs; introducing semi-trailer kinematic constraints, control constraints, and EDT safety distance constraints; and solving for the real-time optimal trajectory, enabling safe and smooth driving of unmanned semi-trailer vehicles in unstructured or semi-structured environments. This invention can plan smoother, real-time solvable trajectories while ensuring a safe distance, improving obstacle avoidance performance and driving comfort in complex environments.

Description

Technical Field

[0001] This invention relates to the field of unmanned driving technology for multi-vehicles, specifically to a real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints. Background Technology

[0002] Local trajectory planning methods commonly employ passenger vehicle assumptions based on bicycle models, neglecting the articulation constraints of multi-unit vehicles in tractor-semi-trailer combinations. This leads to a higher risk of towed collisions in narrow roads or reversing situations. Traditional cost maps only provide obstacle occupancy information and typically use collision detection or inflated grids for obstacle avoidance. These methods cannot quantitatively reflect the safety threshold between vehicles and obstacles. Furthermore, while common local planning methods such as TEB and DWA can solve problems in real time, they cannot simultaneously adapt to structured and complex unstructured environments. They tend to overlook the impact of obstacles on semi-trailers and are insufficient in suppressing the turning and swaying of multi-unit vehicles. Summary of the Invention

[0003] To address the aforementioned problems, this invention provides a real-time trajectory planning method for unmanned semi-trailer vehicles based on Euclidean distance transformation (EDT) constraints.

[0004] To achieve the above objectives, the technical solution adopted by this invention is as follows: a real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints. This method uses LiDAR to perceive the surrounding environment, and an IMU to provide positioning information for the vehicle. After initial trajectory generation based on motion primitives and graph search, and finally trajectory optimization based on MPC and EDT obstacle avoidance constraints, a smooth and safe optimized trajectory is obtained. The specific steps are as follows: S1. Based on the single articulated model of tractor-semi-trailer in Cartesian coordinate system, construct continuous-time kinematic equations; construct a set of motion primitives based on the kinematic equations and connect the set of motion primitives into a state diagram; use Dijkstra graph search algorithm to search and obtain the initial trajectory that satisfies the vehicle kinematic constraints and is collision-free, and discretize it into state points. S1.1 Based on the single articulated model of tractor-semi-trailer, define the state vector and motion vector, and construct the continuous-time kinematic equations; Based on the single-articulated tractor-semi-trailer model, the expression for the state vector is: in, , These are the x and y coordinates of the center of the rear axle of the tractor in a Cartesian coordinate system. The heading angle of the tractor unit. The angle between the semi-trailer and the tractor unit, i.e., the articulation angle. The speed of the tractor unit is given; the control vector is: in, To accelerate the vehicle; Let be the steering angle of the front wheel of the tractor; the continuous-time kinematic equation is: in, This refers to the wheelbase of the tractor unit. This refers to the wheelbase of the semi-trailer. , , , , They represent The derivative with respect to time.

[0005] S1.2, Set the acceleration of the vehicle model and front wheel steering angle set Discretized, they are used as longitudinal and lateral control variables of the vehicle to form a discrete control pair. ; S1.3, with a fixed prediction time length With discrete time step Forward integration is performed on the kinematic model of the semi-trailer vehicle, and for each set of discrete control pairs... Generate a motion primitive starting from the state of the parent node and construct a set of motion primitives; S1.4. Using the current position of the vehicle as the root node, connect the set of motion primitives into a multi-layer state diagram. Using the Dijkstra graph search algorithm, under the premise that the outlines of the tractor and the semi-trailer do not overlap with the obstacle grid, the initial trajectory from the starting point to the target point is obtained.

[0006] S2. Project the initial trajectory onto the grid map, and after establishing the EDT distance field and the safe distance threshold, define the obstacle cost function; construct the drivable area of ​​the vehicle in the grid map based on the state points and the EDT distance field and the safe distance threshold. S2.1 Construct a grid map, using obstacle grids in the grid map environment as zero-distance source points. Calculate the Euclidean distance from each non-obstacle grid to the center of the nearest obstacle grid, forming a continuous distance field distribution, and establish the EDT distance field. The safe distance threshold for semi-trailer vehicle trajectory planning.

[0007] The EDT distance field is obtained by calculating the Euclidean distance from each non-obstacle grid cell in the grid map to the center of the nearest obstacle grid cell; S2.2. Define the obstacle cost function based on the EDT distance field and the safe distance threshold; In the trajectory optimization of this invention, the obstacle cost function based on EDT is defined as follows: in, To predict the time, As a result of obstacles, To match the predicted time The location of the tractor reference point on the grid map. For safe distance threshold, For obstacle avoidance weighting coefficients, EDT distance; When considering the rear profile of a semi-trailer, a reference point for the rear of the semi-trailer is needed. Further enhance tail obstacle avoidance features: In the formula, This is the tail obstacle avoidance weight coefficient.

[0008] S2.3 Based on the initial trajectory and EDT distance field, construct the drivable area of ​​the semi-trailer vehicle within the safe distance threshold.

[0009] S3. Based on the initial trajectory and obstacle cost function, construct the MPC optimization objective function, trajectory optimization problem and three constraints. Use the IPOPT solver of CasADi to solve the trajectory optimization problem and obtain the real-time optimal drivable trajectory, thus completing the real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints.

[0010] S3.1 Based on the initial trajectory and obstacle cost function, construct the MPC optimization objective function and trajectory optimization problem; Construct the MPC optimization objective function based on the initial trajectory. The expression is as follows: in This provides the vehicle's real-time status. For vehicle reference trajectory, As a result of obstacles, As a cost of smoothness, This is a weighted matrix of terminal costs. This represents the squared error between the vehicle's current position and the reference trajectory. This indicates the error between the terminal state and the reference trajectory.

[0011] When the rear profile of the semi-trailer needs to be considered, the expression for the MPC optimization objective function is as follows: in, For rear obstacle avoidance; MPC optimization is a commonly used optimization method for trajectory planning. It predicts future states and control inputs, and performs optimal control based on the current state. Its trajectory optimization problem can be expressed as: in To control the input.

[0012] The smoothness cost can be expressed as: in, and These are the predicted times. The turning angle and acceleration of the tractor on the path, and the sum of their squares, can reflect the smoothness of the path.

[0013] S3.2. Based on the kinematic equations and the EDT distance field, construct three constraint conditions; In addition to trajectory optimization, obstacle cost optimization, and smoothness cost optimization during semi-trailer planning, it is also necessary to satisfy the kinematic constraints, control and state boundary constraints, and EDT-based safe distance constraints of the semi-trailer, the expressions of which are as follows: (1) Kinematic constraints: in This indicates the vehicle's status at the next moment. This is the kinematic equation for a semi-trailer vehicle over continuous time, meaning that the vehicle's motion over continuous time must satisfy the kinematics of a semi-trailer vehicle.

[0014] (2) Control and state boundary constraints: in, , , , These represent the maximum turning angle, maximum acceleration, maximum speed, and maximum articulation angle of the tractor unit, respectively.

[0015] (3) Safety distance constraints based on EDT: S3.3. Use CasADi's IPOPT solver or other real-time SQP solvers to solve online, update in each control cycle, take the first control quantity and send it to the bottom controller, then roll the prediction window forward and repeat the solution to obtain the real-time optimal drivable trajectory.

[0016] Compared with existing technologies, this invention provides a real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints, which has the following advantages: (1) The present invention explicitly describes the articulation relationship between the traction and the semi-trailer through a semi-trailer kinematic model, effectively suppressing the semi-trailer swaying and tail sweeping, and improving the drivability and safety of long vehicle combinations in narrow environments.

[0017] (2) The present invention introduces the EDT distance field, which transforms discrete grid obstacles into a continuous and differentiable distance function. In MPC, it is used simultaneously in the form of cost and constraint, so that the trajectory stays as far away from the obstacle as possible while maintaining a safe distance, thereby improving obstacle avoidance robustness.

[0018] (3) The present invention uses motion primitives + graph search to obtain the initial trajectory, and then performs local continuous optimization through MPC, taking into account both global rationality and local smoothness, reducing the dependence of nonlinear optimization on the initial value, realizing real-time solution, and the obtained trajectory is continuous and differentiable. The velocity, acceleration and smoothness are constrained, which is conducive to improving the driving safety and mechanical life of unmanned semi-trailer vehicles in scenarios such as ultra-large parts transportation, loading and unloading stations, and port areas. It can also be applied to the field of intelligent vehicle equipment manufacturing to improve the performance of trajectory planning and control modules of unmanned semi-trailer vehicles. Attached Figure Description

[0019] Figure 1 This is a flowchart of the real-time trajectory planning steps for unmanned semi-trailer vehicles based on EDT constraints as described in this invention. Figure 2 This is a flowchart of the real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints as described in this invention. Figure 3 This is a schematic diagram of the kinematics model of a semi-trailer vehicle. Figure 4 A schematic diagram of the motion primitive tree for initial trajectory search; Figure 5 A rasterized environment map based on EDT distance field; Figure 6 This is a schematic diagram of a semi-trailer vehicle and obstacle scenario in a structured simulation environment. Figure 7 This is a schematic diagram of a semi-trailer vehicle and obstacle scenario in an unstructured simulation environment. Figure 8 This is a comparison diagram of the driving trajectories of the present invention and the TEB algorithm in a structured simulation environment. Figure 9 This is a comparison diagram of the driving trajectories of the present invention and the TEB algorithm in an unstructured simulation environment. Figure 10 This is a comparison chart of the longitudinal velocity and azimuth angle of the tractor in a structured simulation environment using the present invention and the TEB algorithm. Figure 10 (a) is a comparison diagram of the longitudinal speed of the tractor in a structured simulation scenario. Figure 10(b) is a comparison diagram of the azimuth angle of the tractor in a structured simulation scenario; Figure 11 The image shows a comparison of the front wheel angles of the tractor and the semi-trailer before and after path optimization using the algorithm of this invention in a structured simulation environment. Figure 11 (a) is a comparison diagram of the front wheel angle of the tractor in a structured simulation scenario. Figure 11 (b) is a comparison diagram of the turning angles of the tractor and semi-trailer in a structured simulation scenario; Figure 12 This is a comparison chart of the longitudinal velocity and azimuth angle of the tractor in an unstructured simulation environment, using the present invention and the TEB algorithm. Figure 12 (a) is a comparison chart of the longitudinal speed of the tractor in an unstructured simulation scenario. Figure 12 (b) is a comparison diagram of the azimuth angle of the tractor in an unstructured simulation scenario; Figure 13 This is a comparison chart of the front wheel angles of the tractor and the semi-trailer before and after path optimization using the algorithm of this invention in an unstructured simulation environment. Figure 13 (a) is a comparison diagram of the front wheel angle of the tractor in an unstructured simulation scenario. Figure 13 (b) is a comparison of the turning angles of the tractor-trailer in an unstructured simulation scenario; Figure 14 A semi-trailer intelligent driving test platform for conducting real-vehicle tests; Figure 15 This describes the actual vehicle test process and results, including... Figure 15 (A) is the scenario at time t1, the starting point of the actual vehicle test. Figure 15 (B) represents the trajectory planning result at the starting time t1. Figure 15 (C) represents the scenario at time t2, the midpoint of the actual vehicle test. Figure 15 (D) represents the trajectory planning result at the intermediate time t2. Detailed Implementation

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

[0021] Please see Figures 1-15 A real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints includes the following steps: like Figure 1 and Figure 2As shown, a real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints is proposed. This method acquires surrounding environmental information through an onboard LiDAR and obtains vehicle positioning information through an IMU and vehicle odometer. Euclidean distance transformation is performed on obstacles on a grid map to obtain the environmental distance field. A set of motion primitives is constructed based on the semi-trailer's kinematic model, and an initial trajectory is generated using graph search. Based on the initial trajectory, a model predicts the control objective function, including trajectory tracking, control energy, smoothness, and EDT obstacle avoidance costs. Kinematic constraints, control constraints, and EDT safety distance constraints are introduced to solve for the real-time optimal trajectory. The steps are as follows: S1. Based on the single articulated model of the tractor-semi-trailer in the Cartesian coordinate system, construct the continuous-time kinematic equations of the semi-trailer; construct a set of motion primitives based on the kinematic equations and connect the set of motion primitives into a state diagram; use the Dijkstra graph search algorithm to search and obtain the initial trajectory that satisfies the kinematic constraints of the semi-trailer and is collision-free, and discretize it into state points. S1.1 Based on the single articulated model of tractor-semi-trailer, define the state vector and motion vector, and construct the continuous-time kinematic equations; Based on the single-articulated tractor-semi-trailer model, the expression for the state vector is: in, , These are the x and y coordinates of the center of the rear axle of the tractor in a Cartesian coordinate system. The heading angle of the tractor unit. The angle between the semi-trailer and the tractor unit, i.e., the articulation angle. The speed of the tractor; The control vector is: in, To accelerate the vehicle, The turning angle of the front wheels of the tractor; This invention assumes the vehicle is traveling at a low speed and neglects tire sideslip effect, thus obtaining the vehicle's continuous-time kinematic equations as follows: in, This refers to the wheelbase of the tractor unit. This refers to the wheelbase of the semi-trailer. , , , , They represent The derivative with respect to time.

[0022] S1.2, Set the acceleration of the vehicle model and front wheel steering angle set Discretized, they are used as longitudinal and lateral control variables of the vehicle to form a discrete control pair. ; S1.3, with a fixed prediction time length With discrete time step Forward integration is performed on the kinematic model of the semi-trailer vehicle, and for each set of discrete control pairs... Generate a motion primitive starting from the state of the parent node and construct a set of motion primitives; S1.4, Schematic diagram of the motion primitive tree for initial trajectory search as follows: Figure 4 As shown, with the current position of the vehicle as the root node, the set of motion primitives is connected into a multi-layer state diagram. Using the A* or Dijkstra graph search algorithm, the initial trajectory from the starting point to the target point is obtained under the premise that the outline of the tractor and the semi-trailer do not overlap with the obstacle grid, and then discretized into state points. In this embodiment, the graph search algorithm uses Dijkstra's algorithm, and the maximum search time is set to 300. .

[0023] S2. Project the initial trajectory onto the grid map, and after establishing the EDT distance field and the safe distance threshold, define the obstacle cost function; construct the drivable area of ​​the vehicle in the grid map based on the state points and the EDT distance field and the safe distance threshold. S2.1, such as Figure 5 As shown, a grid map is constructed, using obstacle grids in the environment as zero-distance source points. The Euclidean distance from each non-obstacle grid to the center of the nearest obstacle grid is calculated, thus forming a continuous distance field distribution throughout the planning space. This establishes the EDT distance field and the safe distance for semi-trailer vehicle trajectory planning. The color intensity in the EDT distance field represents the Euclidean distance value from each location in the environment to the nearest obstacle, with color intensity indicating decreasing distance. Cubes represent obstacles in the environment, and dashed lines represent the planned path of the semi-trailer vehicle. During the path planning process, guided by the EDT distance field, the path automatically avoids high-risk areas with small distance values ​​and generates feasible paths towards the target location along areas with larger safety margins. In this embodiment, the distance threshold for automatic avoidance is set to 0.5m.

[0024] S2.2, the aforementioned EDT range field To occupy all non-obstacle grid cells in the grid map, the Euclidean distance to the center of the nearest obstacle grid cell is calculated. In trajectory optimization, the obstacle cost based on EDT is defined as: in, To predict the time, As a result of obstacles, To match the predicted time The location of the tractor reference point on the grid map. For safe distance threshold, For obstacle avoidance weighting coefficients, EDT distance; When considering the rear profile of a semi-trailer, a reference point for the rear of the semi-trailer is needed. Further enhance tail obstacle avoidance features: In the formula, This is the tail obstacle avoidance weight coefficient.

[0025] S2.3 Based on the initial trajectory of the grid map and the constructed EDT distance field, construct the drivable area of ​​the semi-trailer vehicle within the safe distance threshold.

[0026] S3. Based on the initial trajectory and obstacle cost function, construct the MPC optimization objective function, trajectory optimization problem and three constraints. Use the IPOPT solver of CasADi to solve the trajectory optimization problem and obtain the real-time optimal drivable trajectory, thus completing the real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints.

[0027] S3.1 Based on the initial trajectory and obstacle cost function, construct the MPC optimization objective function and trajectory optimization problem; Construct the MPC optimization objective function based on the initial trajectory. The expression is as follows: in, This provides the vehicle's real-time status. For vehicle reference trajectory, As a result of obstacles, As a cost of smoothness, This is a weighted matrix of terminal costs. This represents the squared error between the vehicle's current position and the reference trajectory. This represents the error between the terminal state and the reference trajectory, where N is the total time step.

[0028] The smoothness cost can be expressed as: in, and These are the predicted times. The turning angle and acceleration of the tractor on the path, and the sum of their squares, can reflect the smoothness of the path.

[0029] When the rear profile of the semi-trailer needs to be considered, the expression for the MPC optimization objective function is as follows: in, For rear obstacle avoidance; MPC optimization is a commonly used optimization method for trajectory planning. It predicts future states and control inputs and performs optimal control based on the current state. The trajectory optimization problem in this invention can be expressed as: in, For controlling input.

[0030] S3.2. Based on the kinematic equations and the EDT distance field, construct three constraint conditions; The constraints include kinematic constraints, control and state boundary constraints, and safety distance constraints; In addition to trajectory optimization, obstacle cost optimization, and smoothness cost optimization during semi-trailer planning, it is also necessary to satisfy the kinematic constraints, control and state boundary constraints, and EDT-based safe distance constraints of the semi-trailer, the expressions of which are as follows: (1) Kinematic constraints: in, This indicates the vehicle's status at the next moment. This is the kinematic equation for a semi-trailer vehicle over continuous time, meaning that the vehicle's motion over continuous time must satisfy the kinematics of a semi-trailer vehicle.

[0031] (2) Control and state boundary constraints: in, , , , These represent the maximum turning angle, maximum acceleration, maximum speed, and maximum articulation angle of the tractor unit, respectively.

[0032] (3) Safety distance constraints based on EDT: S3.3. Based on the constraints and trajectory optimization problem, the trajectory optimization problem is solved online using CasADi's IPOPT solver or other real-time SQP solvers. The solution is updated in each control cycle, and the first control variable is sent to the underlying controller. Then the prediction window is rolled forward and the solution is repeated to obtain the real-time optimal drivable trajectory.

[0033] In this embodiment, the IPOPT solver of CasADi is used to solve the trajectory optimization problem online. It is updated in each control cycle, and the first control value is sent to the underlying controller. Then the prediction window is rolled forward and the solution is repeated to obtain the real-time optimal drivable trajectory.

[0034] To verify that the method of this invention can achieve safer obstacle avoidance for semi-trailer vehicles in various environments, the method of this invention was compared with the TEB algorithm. Considering that the operating environment of semi-trailer vehicles is mostly static or quasi-static, the following was designed: Figure 6 The structured static simulation environment shown was used to test the planning and obstacle avoidance capabilities of semi-trailer vehicles in a standard intersection scenario. It also included designs such as... Figure 7 The unstructured static simulation environment shown contains multiple cubes of varying heights as static obstacles.

[0035] The relevant experimental parameters are shown in Table 1: Table 1: Experimental Relevant Parameters like Figure 8 and Figure 9 As shown, the two figures represent the driving trajectories of the algorithm of this invention and the TEB algorithm in structured and unstructured environments, respectively. The figures show the search trajectory and optimized trajectory of the tractor and semi-trailer in the algorithm of this invention. The algorithm of this invention successfully avoids all obstacles and reaches the target point smoothly, while the TEB algorithm has a high safety risk due to the planned trajectory, which causes the vehicle to collide with the right edge of the static obstacle during the driving process.

[0036] Figure 10 and Figure 12 The graphs show the changes in the longitudinal speed and azimuth angle of the tractor under two simulation environments. As can be seen from the graphs, the travel time planned by the TEB algorithm is significantly longer than that of the algorithm of this invention, and the speed and azimuth angle changes planned by the algorithm of this invention are smoother. Figure 10 (a) is a comparison diagram of the longitudinal speed of the tractor in a structured simulation scenario. Figure 10 (b) is a comparison diagram of the azimuth angle of the tractor in a structured simulation scenario. Figure 12 (a) is a comparison chart of the longitudinal speed of the tractor in an unstructured simulation scenario. Figure 12 (b) is a comparison diagram of the azimuth angle of the tractor in an unstructured simulation scenario.

[0037] Figure 11 and Figure 13 The curves showing the changes in the front wheel angle of the tractor and the steering angle of the semi-trailer before and after optimization under two simulation environments are presented. Figure 13 (a) is a comparison diagram of the front wheel angle of the tractor in an unstructured simulation scenario. Figure 13 (b) is a comparison of the turning angles of the tractor-trailer in an unstructured simulation scenario; Figure 11 (a) is a comparison diagram of the front wheel angle of the tractor in a structured simulation scenario. Figure 11 (b) is a comparison of the turning angles of the tractor and semi-trailer in the structured simulation scenario. As can be seen from the figure, the changes in the front wheel turning angle and trailer turning angle in the trajectory after MPC optimization and EDT constraint optimization are smaller and the curve changes are smoother.

[0038] To verify the accuracy and feasibility of the proposed algorithm in real-world engineering scenarios, this paper deploys the algorithm on... Figure 14 The unmanned semi-trailer intelligent driving test platform shown is used for subsequent real-vehicle verification tests in various test scenarios. This test platform integrates multi-source sensors such as a 16-line LiDAR and IMU, providing key measurement information for environmental perception and vehicle positioning required by the algorithm. The host industrial control computer runs Ubuntu 18.04 and ROS Melodic systems, establishing real-time communication with the drive-by-wire chassis via a CAN bus to issue control commands and obtain vehicle status feedback, thus providing underlying support for trajectory tracking control and vehicle closed-loop verification. The vehicle and algorithm parameters used in the real-vehicle tests are shown in Table 2. Table 2: Key Relevant Parameters from Real Vehicle Tests like Figure 15 As shown, the proposed hierarchical planning framework of "motion primitive search and EDT distance field constrained MPC optimization" was verified by a real vehicle test on a real vehicle platform. Figure 15 (A) gives the start time. t Scene 1, Figure 15 (B) After the semi-trailer has traveled a certain distance... t The scene at time 2, Figure 15 (C) and Figure 15 (D) are respectively t 1 moment and t The planning results of the semi-trailer truck under the algorithm presented in this paper are shown at time 2. As can be seen from the figure, the planning results of the semi-trailer truck ensure that the vehicle maintains a reasonable safe distance when turning. At the initial time, a safe and drivable route can be freely planned, including the routes of the tractor and trailer, ensuring the stable driving of the semi-trailer truck. After the vehicle has traveled for a period of time... t At time 2, the sensors in the semi-trailer will identify new obstacles and adjust the planned route in real time according to the changes in the surrounding obstacle scene to meet the requirements of trajectory planning.

[0039] comprehensive t 1 and tIt can be concluded from step 2 that the proposed method has online adaptability to real-world scenarios—it can reconstruct the safe distance boundary and output a safe and trackable trajectory in a timely manner when environmental geometric constraints change, while taking into account the overall safety and feasibility of the tractor and trailer, thus verifying the real-time obstacle avoidance and stable planning capabilities of the proposed algorithm in engineering scenarios.

[0040] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints, characterized in that, Includes the following steps: S1. Based on the single articulation model of the tractor-semi-trailer in the Cartesian coordinate system, construct the continuous-time kinematic equations; construct a set of motion primitives based on the equations and connect the set of motion primitives into a state diagram; use the Dijkstra graph search algorithm to search and obtain the initial trajectory that satisfies the requirement that the contours of the tractor and semi-trailer do not overlap with the obstacle grid and discretize it into state points. The maximum search time for the graph search algorithm is a preset value; S2. Project the initial trajectory onto the grid map, and after establishing the EDT distance field and the safe distance threshold, define the obstacle cost function; construct the drivable area of ​​the vehicle in the grid map based on the state points and the EDT distance field and the safe distance threshold. S3. Based on the initial trajectory and obstacle cost function, construct the MPC optimization objective function, trajectory optimization problem and three constraints. Use the IPOPT solver of CasADi to solve the trajectory optimization problem and obtain the real-time optimal drivable trajectory, thus completing the real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints.

2. The real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints according to claim 1, characterized in that, S1 specifically includes the following steps: S1.1 Based on the single articulated model of tractor-semi-trailer, define the state vector and motion vector, and construct the continuous-time kinematic equations; Based on the single-articulated tractor-semi-trailer model, the expression for the state vector is: in, , These are the x and y coordinates of the center of the rear axle of the tractor in a Cartesian coordinate system. The heading angle of the tractor unit. The angle between the semi-trailer and the tractor unit, i.e., the articulation angle. The speed of the tractor; The expression for the control vector is: in, To accelerate the vehicle; The turning angle of the front wheels of the tractor; The expression for the continuous-time kinematic equations is: in, This refers to the wheelbase of the tractor unit. This refers to the wheelbase of the semi-trailer. , , , , They represent The derivative with respect to time; S1.2 Discretize the acceleration set and front wheel steering angle set of the vehicle model, and use them as the longitudinal control quantity and lateral control quantity of the vehicle to form a discrete control pair; S1.

3. Perform forward integration on the kinematic equations with a fixed prediction time length and discrete time step, and generate a motion primitive starting from the state of the parent node for each set of discrete control pairs to construct a set of motion primitives. S1.

4. Using the current position of the vehicle as the root node, connect the set of motion primitives into a state diagram. Using Dijkstra's graph search algorithm, under the constraint that the outlines of the tractor and the semi-trailer do not overlap with the obstacle grid, the initial trajectory from the starting point to the target point is obtained.

3. The real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints according to claim 1, characterized in that, S2 specifically includes the following steps: S2.1 Construct a grid map, using obstacle grids in the grid map environment as zero-distance source points. Calculate the Euclidean distance from all non-obstacle grids to the center of the nearest obstacle grid to form a continuous distance field distribution. Establish the EDT distance field and the safe distance threshold for semi-trailer vehicle trajectory planning. The EDT distance field is obtained by calculating the Euclidean distance from each non-obstacle grid cell in the grid map to the center of the nearest obstacle grid cell; S2.

2. Define the obstacle cost function based on the EDT distance field and the safe distance threshold; The expression for the obstacle cost function is: in, As a result of obstacles, To predict the time, To match the predicted time The location of the tractor reference point on the grid map. To preset a safe distance threshold, For obstacle avoidance weighting coefficients, EDT distance; When considering the rear profile of a semi-trailer, a reference point for the rear of the semi-trailer is needed. Further enhance tail obstacle avoidance features: In the formula, For rear obstacle avoidance, This refers to the tail obstacle avoidance weighting coefficient. S2.3 Based on the initial trajectory and EDT distance field, construct the drivable area of ​​the semi-trailer vehicle within the safe distance threshold.

4. The real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints according to claim 1, characterized in that, S3 specifically includes the following steps: S3.1 Based on the initial trajectory and obstacle cost function, construct the MPC optimization objective function and trajectory optimization problem; S3.

2. Based on the kinematic equations and the EDT distance field, construct three constraint conditions; The constraints include kinematic constraints, control and state boundary constraints, and EDT-based safe distance constraints. S3.

3. Based on the constraints and trajectory optimization problem, the trajectory optimization problem is solved using the IPOPT solver of CasADi to obtain the real-time optimal drivable trajectory.

5. The real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints according to claim 4, characterized in that, In S3.1, the expression for constructing the MPC optimization objective function is as follows: in, This provides the vehicle's real-time status. For vehicle reference trajectory, As a result of obstacles, As a cost of smoothness, This is a weighted matrix of terminal costs. This represents the squared error between the vehicle's current position and the reference trajectory. This represents the error between the terminal state and the reference trajectory, where N is the total time step. For the predicted time; The expression for the smoothness cost is: in, and These are the predicted times. The turning angle and acceleration of the tractor on the path; When considering the rear profile of the semi-trailer, the MPC optimization objective function and its expression in the trajectory optimization problem are as follows: in, This is a tail-end obstacle avoidance device.

6. The real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints according to claim 5, characterized in that, The expression for the trajectory optimization problem is as follows: in, For controlling input.

7. The real-time trajectory planning method for unmanned semi-trailer vehicles based on EDT constraints according to claim 4, characterized in that, In S3.2, the expression for the kinematic constraint is: in, This indicates the vehicle's status at the next moment. For discrete time steps, The kinematic equations for the semi-trailer vehicle over continuous time; The expression for the control and state boundary constraints is: in, In the The turning angle of the tractor's front wheels at each time step In the The acceleration of the tractor at each time step In the The speed of the tractor at each time step In the The tractor articulation angle at each time step , , , These represent the maximum turning angle, maximum acceleration, maximum speed, and maximum articulation angle of the tractor unit, respectively. The expression for the safety distance constraint based on EDT is: In the formula, EDT distance, To match the predicted time The location of the tractor reference point on the grid map. Use the rear of the semi-trailer as a reference point. This is a preset safe distance threshold.

Images