An unmanned vehicle trajectory planning method for narrow large-curvature roads

Abstract

This invention relates to a trajectory planning method for unmanned vehicles (UAVs) suitable for narrow, high-curvature road scenarios. It constructs a motion primitive tree based on the vehicle's kinematics model in a Cartesian coordinate system and searches for motion primitives with low collision constraints and minimal costs. These primitives are then sequentially connected to generate an initial trajectory containing spatiotemporal information. The initial trajectory is then smoothed and optimized to minimize energy and time consumption, resulting in a first optimized trajectory. Obstacle safety distance constraints are introduced onto this trajectory, and continuous collision detection is performed in conjunction with the vehicle's swept volume for further optimization, yielding the final drivable trajectory. This invention addresses narrow, high-curvature, unstructured roads. By simultaneously satisfying vehicle kinematics and body shape constraints and integrating spatiotemporal dimensions to achieve joint lateral and longitudinal motion, it obtains the globally optimal trajectory through a complete solution space and combines swept volume and signed distance field (SDF) gradient optimization to achieve global obstacle avoidance and safe driving.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, specifically relating to an autonomous vehicle trajectory planning method for narrow, highly curved roads. Background Technology

[0002] Trajectory planning for autonomous vehicles is one of the key technologies for achieving high-level autonomous driving. Its core task is to generate an optimal trajectory that satisfies vehicle kinematic constraints and safety requirements while also possessing smoothness. Existing research has achieved relatively systematic results in structured environments and has formed mature planning strategies. However, unstructured environments often have complex characteristics such as irregular boundaries, narrow spaces, and large road curvatures, which significantly increase the difficulty and challenges of trajectory planning.

[0003] Currently, common trajectory planning methods in unstructured environments often employ collision detection by enclosing the vehicle shape as multiple circles or using oriented bounding boxes (OBBs). However, these methods approximate vehicle geometry in a coarse manner, easily leading to decreased trajectory traversability or inaccurate collision detection results. Other studies model the configuration space using simple geometric shapes and construct safe corridors based on this set. While these methods impose safety constraints on the trajectory, they sacrifice the integrity of the trajectory solution space to some extent. It should be noted that all the above methods perform collision detection based on the discrete state of the trajectory; therefore, potential collision risks may be missed during the time gaps between adjacent sampling moments. Furthermore, since their detection results only provide collision determination information and cannot provide gradient information to effectively guide trajectory optimization, they are difficult to directly apply to the trajectory optimization stage.

[0004] Therefore, this invention proposes a trajectory planning method for unmanned vehicles on narrow, highly curved roads. Summary of the Invention

[0005] This invention proposes a trajectory planning method for unmanned vehicles on narrow, highly curved roads to address the shortcomings of the aforementioned background technology.

[0006] To achieve the above-mentioned technical effects, the present invention is implemented through the following technical solution: a method for unmanned vehicle trajectory planning for narrow, high-curvature roads, characterized by comprising the following steps:

[0007] S1. Construct a motion primitive tree based on the vehicle kinematics model in the Cartesian coordinate system, and search the motion primitive tree to obtain motion primitives that meet the collision-free constraint and have low cost. Connect the motion primitives in sequence to generate an initial trajectory with spatiotemporal three-dimensional information.

[0008] S2. With the goal of minimizing energy consumption and time consumption, the initial trajectory is smoothed to obtain the first optimized trajectory.

[0009] S3. Based on the first optimized trajectory, an obstacle safety distance constraint is introduced to further optimize the trajectory. The vehicle body sweep volume during the movement is calculated, and continuous collision detection is performed based on the sweep volume to obtain the final drivable trajectory.

[0010] Furthermore, the specific steps of S1 are as follows:

[0011] S1.1. Based on the two-degree-of-freedom kinematic model of the vehicle, the differential equation of the vehicle can be obtained, and the expression is as follows:

[0012]

[0013] In the formula, , These are the coordinates of the center point of the rear axle of the vehicle, where x is the x-coordinate of the center point and y is the y-coordinate of the center point; The longitudinal speed of the vehicle; This refers to the vehicle's wheelbase. The vehicle's steering angle; The steering angle of the front wheels; To accelerate the vehicle; These represent the derivatives of the vehicle's rear axle center point coordinates, vehicle body steering angle, and vehicle longitudinal velocity, respectively.

[0014] S1.2, Accelerate the vehicle and front wheel cornering These are respectively used as the longitudinal and lateral control variables of the vehicle. , Discretize the longitudinal and lateral control variables of the vehicle into , The value of n is positively correlated with the trajectory, and the forward simulation time is... During node expansion, at fixed time intervals... and minimum expansion step size To perform step-by-step node expansion, the expression is as follows:

[0015]

[0016] definition Let the vehicle state space be defined, and let a given time be defined. The state is The corresponding control quantity is and Then the state at the next moment It can be represented as:

[0017]

[0018] In the formula, This represents the distance the vehicle travels during the forward simulation process. This represents the vehicle's turning radius. , For the vehicle at a given time The coordinates of the rear axle center point For the vehicle at a given time The direction angle, For the vehicle at a given time longitudinal velocity, For the vehicle in the acceleration at any moment For the vehicle at a given time Time, For the vehicle in the The direction angle at any given moment.

[0019] Furthermore, the specific steps of S2 are as follows:

[0020] S2.1. The initial trajectory is parametrically represented using the Minimum Control (MINCO) method:

[0021]

[0022] In the formula, Represents a fifth-degree polynomial curve. The time interval representing optimization, since the trajectory is... The complete trajectory is formed by connecting the state points sequentially, therefore it has a total of A segment of a quintic polynomial curve; where each segment of the quintic polynomial curve is represented as:

[0023]

[0024] coefficient By trajectory control points and time Joint decision, Indicates the first Index of coefficients in a polynomial segment;

[0025] S2.2. With the optimization objective of minimizing the energy and time consumption of the trajectory, and constraining the trajectory within the initial range of the initial trajectory, the optimization problem can be defined as:

[0026]

[0027] In the formula, , and These represent the weights of energy consumption, time consumption, and trajectory position deviation, respectively. , and These represent penalties for energy, time, and trajectory position deviation, respectively.

[0028] in, Its gradient derivation is as follows:

[0029]

[0030] In the formula, It is with time The relevant weight matrix;

[0031] Its gradient derivation is as follows:

[0032]

[0033] Its gradient derivation is as follows:

[0034]

[0035] In the formula, This represents a trajectory point in the initial trajectory.

[0036] Furthermore, the specific steps of S3 are as follows:

[0037] S3.1 The second trajectory optimization mainly focuses on optimizing the safe distance between the trajectory and obstacles and the swept volume area. The optimization problem can be defined as:

[0038]

[0039] In the formula, and These represent the safe distance between the trajectory and the obstacle, and the sweep volume weight, respectively. and Each represents a penalty item, where, Its gradient is calculated using the signed distance field (SDF);

[0040] S3.2. Using the vehicle's geometric center as the origin of the coordinate system, the vehicle's forward direction is set as... The positive half-shaft, the left side in the direction of vehicle travel, is used as... Establish a coordinate system using the positive half-axis, then any point inside the vehicle... The distance to the vehicle boundary can be expressed as:

[0041]

[0042] In the formula, and Let length and width represent the vehicle length and width, respectively. Then, the SDF function at any point inside the vehicle can be defined as:

[0043]

[0044] The gradient function of SDF can be derived from the above equation. for:

[0045]

[0046] In the formula, and These represent the signed coordinates of the points in the SDF file.

[0047] S3.3, When the vehicle is located in the first When there are multiple trajectory control points, obstacles located in the global coordinate system This can be expressed by the above formula as:

[0048]

[0049] In the formula, This indicates the vehicle's trajectory from the first control point to the... Rotation matrix of trajectory control points Represents the translation matrix;

[0050] S3.4, Define penalty items for:

[0051]

[0052] In the formula, Indicates the index value of the obstacle. Indicates the number of obstacles, where, Defined as:

[0053]

[0054] In the formula, This represents the safe distance threshold between the trajectory and the obstacle. The gradient is:

[0055]

[0056] S3.5. To improve computational efficiency, this invention performs A-dimensionality reduction on the three-dimensional swept volume, only... , Calculate its two-dimensional projection in the plane, therefore a penalty term is applied. It can be defined as:

[0057]

[0058] In the formula, Indicates the vehicle is on the trajectory. The heading angle at each control point Indicates the vehicle is in The gradient can be obtained from the angle between the lateral and longitudinal velocity components at each control point:

[0059]

[0060] S3.6 From the formula in S3.3, we can obtain that for any point... exist The SDF function of time relative to trajectory can be expressed as:

[0061]

[0062] Similarly, in the formula and These represent the vehicle's state from the initial moment. arrive The rotation and translation matrices at each moment;

[0063] S3.7 To calculate the sweep volume SDF function of the vehicle during its trajectory movement, it is necessary to evaluate each point on the map, considering the vehicle's movement from the initial moment. Until the end time Given all possible orientations of a point, find the minimum distance from that point to the vehicle's swept area; since for any point in space... Each has a unique moment. To obtain the minimum distance, this invention employs a line search method to solve for the optimal time. ,Will Substituting into the above formula, we can obtain the point. Minimum distance at:

[0064]

[0065] Through the above process, the minimum distance between any point on the map and the sweep volume of the vehicle can be accurately evaluated in the spatiotemporal domain, thereby achieving continuous collision detection of the trajectory in the spatiotemporal dimension.

[0066] The beneficial effects of this invention are:

[0067] This invention can simultaneously consider vehicle kinematic and shape constraints in narrow, high-curvature unstructured roads. By considering both time and space dimensions during trajectory planning, it enables vehicles to achieve combined lateral and longitudinal movements, thus better aligning with human driving habits. Furthermore, the trajectory generated by this invention possesses global optimality. Compared to trajectory planning methods that approximate the vehicle's shape as a regular shape, this method ensures the integrity of the trajectory solution space and can find the global optimal solution. In addition, this invention, through gradient optimization of the swept volume and the signed distance field (SDF), constrains the swept volume traversed by the vehicle during movement within the range of minimum gradients, ensuring overall vehicle safety and guaranteeing that no point on the vehicle body will collide with obstacles. Attached Figure Description

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

[0069] Figure 1 This is a flowchart illustrating the steps of an unmanned vehicle trajectory planning method for narrow, highly curved roads as described in this invention.

[0070] Figure 2 This is a schematic diagram of a two-degree-of-freedom kinematic model of a vehicle.

[0071] Figure 3 A schematic diagram of a single-layer motion primitive for initial trajectory search;

[0072] Figure 4 A schematic diagram of the motion primitive tree for initial trajectory search;

[0073] Figure 5 A comparison chart of trajectory planning;

[0074] Figure 6 This is a schematic diagram of the swept volume. Detailed Implementation

[0075] Example 1

[0076] like Figure 1 As shown, an autonomous vehicle trajectory planning method for narrow, high-curvature roads is proposed. This method uses LiDAR to perceive the surrounding environment, employs an IMU to provide positioning information for the vehicle, and then obtains the final trajectory through initial trajectory search, a first trajectory optimization, and a second trajectory optimization. The steps are as follows:

[0077] S1. The steps of constructing a motion primitive tree based on the vehicle kinematics model in the Cartesian coordinate system, searching the motion primitive tree to obtain motion primitives that satisfy collision-free constraints and have low cost, and connecting the motion primitives sequentially to generate an initial trajectory with spatiotemporal three-dimensional information are as follows:

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

[0079]

[0080] In the formula, , These are the coordinates of the center point of the rear axle of the vehicle, where x is the x-coordinate of the center point and y is the y-coordinate of the center point; The longitudinal speed of the vehicle; This refers to the vehicle's wheelbase. The vehicle's steering angle; The steering angle of the front wheels; To accelerate the vehicle; These represent the derivatives of the vehicle's rear axle center point coordinates, vehicle body steering angle, and vehicle longitudinal velocity, respectively.

[0081] S1.2, Accelerate the vehicle and front wheel cornering These are respectively used as the longitudinal and lateral control variables of the vehicle. , In this embodiment, a min Take -0.5, a max Take 0.5, Take -28, Take 28; discretize the longitudinal and lateral control variables of the vehicle as follows: , The value of n is positively correlated with the trajectory, and the forward simulation time is... In this embodiment, =0.2s, to obtain as Figure 4 The single-layer motion primitive shown; during node expansion, at fixed time intervals... and minimum expansion step size To perform step-by-step node expansion, the expression is as follows:

[0082]

[0083] definition Let the vehicle state space be defined, and let a given time be defined. The state is The corresponding control quantity is and Then the state at the next moment It can be represented as:

[0084]

[0085] In the formula, This represents the distance the vehicle travels during the forward simulation process. This represents the vehicle's turning radius. , For the vehicle at a given time The coordinates of the rear axle center point For the vehicle at a given time The direction angle, For the vehicle at a given time longitudinal velocity, For the vehicle in the acceleration at any moment For the vehicle at a given time Time, For the vehicle in the The direction angle at that moment;

[0086] By controlling the discretized control quantity and The expansion forms a multi-layered motion primitive, i.e., a motion primitive tree, such as... Figure 5 As shown; subsequently, an initial trajectory search is performed on the motion primitive tree to obtain the initial trajectory;

[0087] S2. Perform trajectory smoothing on the initial trajectory above, with the optimization objective of minimizing energy consumption and time consumption, and perform trajectory smoothing optimization on the initial trajectory to obtain the first optimized trajectory;

[0088] The steps for the first trajectory optimization are as follows:

[0089] S2.1. The initial trajectory is parametrically represented using the Minimum Control (MINCO) method:

[0090]

[0091] In the formula, Represents a fifth-degree polynomial curve. The time interval representing optimization, since the trajectory is... The complete trajectory is formed by connecting the state points sequentially, therefore it has a total of A segment of a quintic polynomial curve; where each segment of the quintic polynomial curve is represented as:

[0092]

[0093] coefficient By trajectory control points and time Joint decision, Indicates the first Index of coefficients in a polynomial segment;

[0094] S2.2. With the optimization objective of minimizing the energy and time consumption of the trajectory, and constraining the trajectory within the initial range of the initial trajectory, the optimization problem can be defined as:

[0095]

[0096] In the formula, , and These represent the weights of energy consumption, time consumption, and trajectory position deviation, respectively. , and These represent penalties for energy, time, and trajectory position deviation, respectively.

[0097] in, Its gradient derivation is as follows:

[0098]

[0099] In the formula, It is with time The relevant weight matrix;

[0100] Its gradient derivation is as follows:

[0101]

[0102] Its gradient derivation is as follows:

[0103]

[0104] In the formula, These represent trajectory points in the initial trajectory;

[0105] Specifically, , and All are set to 1.

[0106] S3. Based on the first optimized trajectory, an obstacle safety distance constraint is introduced to further optimize the trajectory. The vehicle body sweep volume during the movement is calculated, and continuous collision detection is performed based on the sweep volume to obtain the final drivable trajectory.

[0107] The second trajectory optimization steps are as follows:

[0108] S3.1 The second trajectory optimization mainly focuses on optimizing the safe distance between the trajectory and obstacles and the swept volume area. The optimization problem can be defined as:

[0109]

[0110] In the formula, and These represent the safe distance between the trajectory and the obstacle, and the sweep volume weight, respectively. and Each represents a penalty item, where, Its gradient is calculated using the signed distance field (SDF);

[0111] Specifically, Set to 1;

[0112] Specifically, Set to 0.01;

[0113] S3.2. Using the vehicle's geometric center as the origin of the coordinate system, the vehicle's forward direction is set as... The positive half-shaft, the left side in the direction of vehicle travel, is used as... Establish a coordinate system using the positive half-axis, then any point inside the vehicle... The distance to the vehicle boundary can be expressed as:

[0114]

[0115] In the formula, and These represent the vehicle length and width, respectively.

[0116] Specifically, in this embodiment, the vehicle length is 1.9m and the vehicle width is 1.2m;

[0117] The SDF function at any point inside the vehicle can then be defined as:

[0118]

[0119] The gradient function of SDF can be derived from the above equation. for:

[0120]

[0121] In the formula, and These represent the signed coordinates of the points in the SDF file.

[0122] S3.3, When the vehicle is located in the first When there are multiple trajectory control points, obstacles located in the global coordinate system This can be expressed by the above formula as:

[0123]

[0124] In the formula, This indicates the vehicle's trajectory from the first control point to the... Rotation matrix of trajectory control points Represents the translation matrix;

[0125] S3.4, Define penalty items for:

[0126]

[0127] In the formula, Indicates the index value of the obstacle. Indicates the number of obstacles, where, Defined as:

[0128]

[0129] In the formula, This represents the safe distance threshold between the trajectory and the obstacle. The gradient is:

[0130]

[0131] Specifically, Set to 1.5m;

[0132] S3.5 To improve computational efficiency, this invention performs dimensionality reduction processing on the three-dimensional sweep volume, only applying it to... , Calculate its two-dimensional projection in the plane, therefore a penalty term is applied. It can be defined as:

[0133]

[0134] In the formula, Indicates the vehicle is on the trajectory. The heading angle at each control point Indicates the vehicle is in The gradient can be obtained from the angle between the lateral and longitudinal velocity components at each control point:

[0135]

[0136] The result obtained after calculation using the above formula is as follows: Figure 5 The vehicle sweep volume shown;

[0137] S3.6 From the formula in S3.3, we can obtain that for any point... exist The SDF function of time relative to trajectory can be expressed as:

[0138]

[0139] Similarly, in the formula and These represent the vehicle's state from the initial moment. arrive The rotation and translation matrices at each moment;

[0140] S3.7 To calculate the sweep volume SDF function of the vehicle during its trajectory movement, it is necessary to evaluate each point on the map, considering the vehicle's movement from the initial moment. Until the end time Given all possible orientations of a point, find the minimum distance from that point to the vehicle's swept area. Since for any point in space... Each has a unique moment. To obtain the minimum distance, this invention employs a line search method to solve for the optimal time. ,Will Substituting into the above formula, we can obtain the point. Minimum distance at:

[0141]

[0142] Specifically, in this embodiment, the time step is set to 0.1s;

[0143] Specifically, in this embodiment, the maximum number of iterations for trajectory optimization is set to 1000.

[0144] Through the above process, the minimum distance between any point on the map and the sweep volume of the vehicle can be accurately evaluated in the spatiotemporal domain, thereby achieving continuous collision detection of the trajectory in the spatiotemporal dimension.

[0145] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the essence and scope of the technical solutions of the present invention.

Claims

1. A trajectory planning method for unmanned vehicles on narrow, highly curved roads, characterized in that, Includes the following steps: S1. Construct a motion primitive tree based on the vehicle kinematics model in the Cartesian coordinate system, and search the motion primitive tree to obtain motion primitives that meet the collision-free constraint and have low cost. Connect the motion primitives in sequence to generate an initial trajectory with spatiotemporal three-dimensional information. S2. With the goal of minimizing energy consumption and time consumption, the initial trajectory is smoothed to obtain the first optimized trajectory. S3. Based on the first optimized trajectory, an obstacle safety distance constraint is introduced to further optimize the trajectory. The vehicle body sweep volume during the movement is calculated, and continuous collision detection is performed based on the sweep volume to obtain the final drivable trajectory.

2. The method for unmanned vehicle trajectory planning for narrow, high-curvature roads according to claim 1, characterized in that, The specific steps of S1 are as follows: S1.

1. Based on the two-degree-of-freedom kinematic model of the vehicle, the differential equation of the vehicle can be obtained, and the expression is as follows: In the formula, , These are the coordinates of the center point of the rear axle of the vehicle, where x is the x-coordinate of the center point and y is the y-coordinate of the center point; The longitudinal speed of the vehicle; This refers to the vehicle's wheelbase. The vehicle's steering angle; The steering angle of the front wheels; To accelerate the vehicle; These represent the derivatives of the vehicle's rear axle center point coordinates, vehicle body steering angle, and vehicle longitudinal velocity, respectively. S1.2, Accelerate the vehicle and front wheel cornering These are respectively used as the longitudinal and lateral control variables of the vehicle. , Discretize the longitudinal and lateral control variables of the vehicle into , The value of n is positively correlated with the trajectory, and the forward simulation time is... During node expansion, at fixed time intervals... and minimum expansion step size To perform step-by-step node expansion, the expression is as follows: definition Let the vehicle state space be defined, and let a given time be defined. The state is The corresponding control quantity is and Then the state at the next moment It can be represented as: In the formula, This represents the distance the vehicle travels during the forward simulation process. This represents the vehicle's turning radius. , For the vehicle at a given time The coordinates of the rear axle center point For the vehicle at a given time The direction angle, For the vehicle at a given time longitudinal velocity, For the vehicle in the acceleration at any moment For the vehicle at a given time Time, For the vehicle in the The direction angle at any given moment.

3. The method for unmanned vehicle trajectory planning for narrow, high-curvature roads according to claim 1, characterized in that, The specific steps of S2 are as follows: S2.

1. The initial trajectory is parametrically represented using the Minimum Control (MINCO) method: In the formula, Represents a fifth-degree polynomial curve. The time interval representing optimization, since the trajectory is... The complete trajectory is formed by connecting the state points sequentially, therefore it has a total of A segment of a quintic polynomial curve; where each segment of the quintic polynomial curve is represented as: coefficient By trajectory control points and time Joint decision, Indicates the first Index of coefficients in a polynomial segment; S2.

2. With the optimization objective of minimizing the energy and time consumption of the trajectory, and constraining the trajectory within the initial range of the initial trajectory, the optimization problem can be defined as: In the formula, , and These represent the weights of energy consumption, time consumption, and trajectory position deviation, respectively. , and These represent penalties for energy, time, and trajectory position deviation, respectively. in, Its gradient derivation is as follows: In the formula, It is with time The relevant weight matrix; Its gradient derivation is as follows: Its gradient derivation is as follows: In the formula, This represents a trajectory point in the initial trajectory.

4. The method for unmanned vehicle trajectory planning for narrow, high-curvature roads according to claim 1, characterized in that, The specific steps of S3 are as follows: S3.1 The second trajectory optimization mainly focuses on optimizing the safe distance between the trajectory and obstacles and the swept volume area. The optimization problem can be defined as: In the formula, and These represent the safe distance between the trajectory and the obstacle, and the sweep volume weight, respectively. and Each represents a penalty item, where, Its gradient is calculated using the signed distance field (SDF); S3.

2. Using the vehicle's geometric center as the origin of the coordinate system, the vehicle's forward direction is set as... The positive half-shaft, the left side in the direction of vehicle travel, is used as... Establish a coordinate system using the positive half-axis, then any point inside the vehicle... The distance to the vehicle boundary can be expressed as: In the formula, and Let length and width represent the vehicle length and width, respectively. Then, the SDF function at any point inside the vehicle can be defined as: The gradient function of SDF can be derived from the above equation. for: In the formula, and These represent the signed coordinates of the points in the SDF file. S3.3, When the vehicle is located in the first When there are multiple trajectory control points, obstacles located in the global coordinate system This can be expressed by the above formula as: In the formula, This indicates the vehicle's trajectory from the first control point to the... Rotation matrix of trajectory control points Represents the translation matrix; S3.4, Define penalty items for: In the formula, Indicates the index value of the obstacle. Indicates the number of obstacles, where, Defined as: In the formula, This represents the safe distance threshold between the trajectory and the obstacle. The gradient is: S3.

5. To improve computational efficiency, this invention performs A-dimensionality reduction on the three-dimensional swept volume, only... , Calculate its two-dimensional projection in the plane, therefore a penalty term is applied. It can be defined as: In the formula, Indicates the vehicle is on the trajectory. The heading angle at each control point Indicates the vehicle is in The gradient can be obtained from the angle between the lateral and longitudinal velocity components at each control point: S3.6 From the formula in S3.3, we can obtain that for any point... exist The SDF function of time relative to trajectory can be expressed as: Similarly, in the formula and These represent the vehicle's state from the initial moment. arrive The rotation and translation matrices at each moment; S3.7 To calculate the sweep volume SDF function of the vehicle during its trajectory movement, it is necessary to evaluate each point on the map, considering the vehicle's movement from the initial moment. Until the end time Given all possible orientations of a point, find the minimum distance from that point to the vehicle's swept area; since for any point in space... Each has a unique moment. To obtain the minimum distance, this invention employs a line search method to solve for the optimal time. ,Will Substituting into the above formula, we can obtain the point. Minimum distance at: Through the above process, the minimum distance between any point on the map and the sweep volume of the vehicle can be accurately evaluated in the spatiotemporal domain, thereby achieving continuous collision detection of the trajectory in the spatiotemporal dimension.

Images