Trajectory optimization method, device, equipment and storage medium

Abstract

The application discloses a trajectory optimization method, device and equipment and a storage medium. The trajectory optimization method comprises the following steps: obtaining an optimization item of a target optimization trajectory based on first coordinates of a plurality of target optimization trajectory points on the target optimization trajectory, wherein the optimization item comprises at least one of a smoothing item, a trajectory length item and an offset item of the target optimization trajectory; constructing a cost function with the plurality of target optimization trajectory points as optimization targets based on the optimization item and the first coordinates of the plurality of target optimization trajectory points; and determining second coordinates of each target optimization trajectory point as optimized trajectory point coordinates of each target optimization trajectory point when the cost function meets preset requirements. In this way, the smoothness and stability of the target optimization trajectory can be improved, thereby improving the riding comfort.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent driving, in particular to a trajectory optimization method, device, equipment and storage medium. BACKGROUND

[0002] Trajectory planning is an important link in the process of intelligent driving. However, the trajectory planned by the vehicle trajectory planner may have oscillation, instability and the like. If the vehicle is directly controlled to travel according to the trajectory planned by the vehicle trajectory planner, the vehicle may have phenomena such as sudden acceleration, sudden deceleration, and violent steering, so that the vehicle travel stability is poor, and thus the ride comfort is poor.

[0003] Therefore, how to optimize the planned trajectory becomes a technical problem to be solved. SUMMARY

[0004] The technical problem solved by the present application is to provide a trajectory optimization method, device, equipment and computer readable storage medium, which can improve the stability of the optimized planned trajectory, thereby improving the ride comfort.

[0005] To solve the above technical problem, one technical solution adopted by the present application is to provide a trajectory optimization method, which comprises: obtaining an optimization term of a target optimization trajectory based on first coordinates of a plurality of target optimization trajectory points on the target optimization trajectory, wherein the optimization term comprises at least one of a smoothing term, a trajectory length term and an offset term of the target optimization trajectory; constructing a cost function with the plurality of target optimization trajectory points as optimization targets based on the optimization term and the first coordinates of the plurality of target optimization trajectory points; and determining second coordinates of each target optimization trajectory point as optimized trajectory point coordinates of each target optimization trajectory point when the cost function meets a preset requirement.

[0006] In the method, the constructing of the cost function with the plurality of target optimization trajectory points as optimization targets based on the optimization term and the first coordinates of the plurality of target optimization trajectory points comprises: determining first coefficient matrices and second coefficient matrices corresponding to each optimization term, wherein the first coefficient matrix corresponding to each optimization term is composed of coefficients of quadratic terms in the optimization term, and the second coefficient matrix corresponding to each optimization term is composed of coefficients of linear terms in the optimization term; obtaining a target optimization matrix composed of the first coordinates of the plurality of target optimization trajectory points; and constructing the cost function based on the target optimization matrix and the first coefficient matrices and the second coefficient matrices corresponding to each optimization term.

[0007] The cost function is constructed based on the target optimization matrix and the first coefficient matrix and the second coefficient matrix corresponding to each optimization term, including: performing weighted processing on the first coefficient matrix corresponding to each optimization term to obtain a third coefficient matrix, and performing weighted processing on the second coefficient matrix corresponding to each optimization term to obtain a fourth coefficient matrix; and constructing the cost function based on the third coefficient matrix, the fourth coefficient matrix and the target optimization matrix.

[0008] The cost function is constructed based on the third coefficient matrix, the fourth coefficient matrix and the target optimization matrix, including: obtaining a first product term composed of a product of a transposed matrix corresponding to the target optimization matrix, the third coefficient matrix and the target optimization matrix, and obtaining a second product term composed of a product of a transposed matrix of the fourth coefficient matrix and the target optimization matrix; and taking a sum of the first product term and the second product term as the cost function.

[0009] The obtaining step of the smoothness term includes: for each target optimization trajectory point, obtaining a sub-smoothness term corresponding to the target optimization trajectory point based on the first coordinate of the target optimization trajectory point and the first coordinates of two target optimization trajectory points adjacent to the target optimization trajectory point on the target optimization trajectory; and taking a cumulative sum of the sub-smoothness terms corresponding to each target optimization trajectory point as the smoothness term.

[0010] The obtaining step of the trajectory length term includes: for each target optimization trajectory point, obtaining a sub-length term corresponding to the target optimization trajectory point based on the first coordinate of the target optimization trajectory point and the first coordinate of a target optimization trajectory point adjacent to the target optimization trajectory point on the target optimization trajectory; and taking a cumulative sum of the sub-length terms corresponding to each target optimization trajectory point as the trajectory length term.

[0011] The obtaining step of the offset term includes: for each target optimization trajectory point, obtaining a sub-offset term corresponding to the target optimization trajectory point based on the first coordinate of the target optimization trajectory point and the coordinate of a target planning trajectory point corresponding to the target optimization trajectory point on the original planning trajectory; and taking a cumulative sum of the sub-offset terms corresponding to each target optimization trajectory point as the offset term.

[0012] The preset requirement includes that the cost function is a minimum value.

[0013] The preset requirements also include: multiple target optimized trajectory points satisfying constraints, which include at least one of the following: the first coordinate of the trajectory starting point among the multiple target optimized trajectory points is the same as the coordinate of the trajectory splicing point; the absolute value of the difference in the lateral coordinates between each target optimized trajectory point and its corresponding original planned trajectory point is less than or equal to a first difference threshold, and the absolute value of the difference in the longitudinal coordinates between each target optimized trajectory point and its corresponding original planned trajectory point is less than or equal to a second difference threshold; the vehicle speed corresponding to each target optimized trajectory point is greater than or equal to a first vehicle speed threshold and less than or equal to a second vehicle speed threshold, wherein the first vehicle speed threshold is less than the second vehicle speed threshold; and the acceleration corresponding to each target optimized trajectory point is greater than or equal to a first acceleration threshold and less than or equal to a second acceleration threshold, wherein the first acceleration threshold is less than the second acceleration threshold.

[0014] In the case where the first coordinate of the trajectory starting point among multiple target optimization trajectory points is the same as the coordinate of the trajectory splicing point, the method further includes: determining the trajectory splicing point based on the previous segment of the original planned trajectory of the current segment; and using the trajectory splicing point as the trajectory starting point of the current segment of the original planned trajectory.

[0015] Specifically, determining the trajectory splicing point based on the previous segment of the current segment's original planned trajectory includes: in response to the absence of the previous segment's original planned trajectory, or in response to the distance between the vehicle's position at the start of planning the current segment's original planned trajectory and the target original planned trajectory point on the previous segment's original planned trajectory being greater than a distance threshold, determining the vehicle's position as the trajectory splicing point, where the target original planned trajectory point is the trajectory point on the previous segment's original planned trajectory corresponding to the completion of the current segment's original planned trajectory planning; and in response to the distance between the vehicle's position and the target original planned trajectory point being less than or equal to a distance threshold, determining the target original planned trajectory point as the trajectory splicing point.

[0016] To solve the above-mentioned technical problems, another technical solution adopted in this application is: providing a trajectory optimization device, the device comprising: an acquisition module, configured to obtain optimization terms of the target optimization trajectory based on the first coordinates of multiple target optimization trajectory points on the target optimization trajectory, wherein the optimization terms include at least one of a smoothing term, a trajectory length term, and an offset term of the target optimization trajectory; a construction module, configured to construct a cost function with the multiple target optimization trajectory points as optimization targets based on the optimization terms and the first coordinates of the multiple target optimization trajectory points; and a determination module, configured to determine the second coordinates of each target optimization trajectory point when the cost function meets preset requirements as the optimized trajectory point coordinates of each target optimization trajectory point.

[0017] To solve the above-mentioned technical problems, another technical solution adopted in this application is: to provide a processing device, including a memory and a processor coupled to each other, wherein the memory stores program instructions; and the processor is used to execute the program instructions stored in the memory to implement the above-mentioned trajectory optimization method.

[0018] To solve the above-mentioned technical problems, another technical solution adopted in this application is to provide a computer-readable storage medium for storing program instructions that can be executed to implement the above-mentioned trajectory optimization method.

[0019] The above scheme obtains the optimization terms of the target optimized trajectory based on the first coordinates of multiple target optimized trajectory points on the target optimized trajectory; based on the optimization terms and the first coordinates of the multiple target optimized trajectory points, a cost function is constructed with the multiple target optimized trajectory points as the optimization targets; the second coordinates of each target optimized trajectory point when the cost function meets the preset requirements are determined as the optimized trajectory point coordinates of each target optimized trajectory point. Since the cost function is constructed based on the optimization terms and the first coordinates of the multiple target optimized trajectory points, and the optimization terms include at least one of the smoothing term, trajectory length term, and offset term of the target optimized trajectory, during the iterative optimization of the cost function with the multiple target optimized trajectory points as the optimization targets, at least one of the smoothing term, trajectory length term, and offset term of the target optimized trajectory can be continuously optimized until the cost function meets the preset requirements, thereby improving the stability of the target optimized trajectory and thus improving the ride comfort. Attached Figure Description

[0020] Figure 1 This is a flowchart illustrating an embodiment of the trajectory optimization method provided in this application;

[0021] Figure 2 This is a schematic diagram illustrating the principle of coordinate transformation provided in this application;

[0022] Figure 3 This is a flowchart illustrating an embodiment of the cost function construction method provided in this application;

[0023] Figure 4 This is a schematic diagram of the framework of an embodiment of the trajectory optimization device provided in this application;

[0024] Figure 5 This is a schematic diagram of the framework of an embodiment of the processing device provided in this application;

[0025] Figure 6 This is a schematic diagram of a framework of an embodiment of the computer-readable storage medium provided in this application. Detailed Implementation

[0026] To make the purpose, technical solution and effects of this application clearer and more explicit, the following describes this application in further detail with reference to the accompanying drawings and embodiments.

[0027] It should be noted that if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, features defined with "first" or "second" may explicitly or implicitly include at least one of those features. Furthermore, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0028] Please see Figure 1 , Figure 1 This is a flowchart illustrating an embodiment of the trajectory optimization method provided in this application. It should be noted that if substantially the same result is obtained, the method of this invention is not necessarily identical. Figure 1 The illustrated process sequence is limited. For example... Figure 1 As shown, the method includes the following steps:

[0029] S11: Based on the first coordinates of multiple target optimization trajectory points on the target optimization trajectory, obtain the optimization terms of the target optimization trajectory.

[0030] In this embodiment, the initial target optimized trajectory is either an arbitrarily selected trajectory or the original planned trajectory. The original planned trajectory is the unoptimized trajectory planned by the vehicle trajectory planner. In one example, the original planned trajectory and the target optimized trajectory are discrete trajectories; for example, the original planned trajectory includes multiple discrete original planned trajectory points, and the target optimized trajectory includes multiple target optimized trajectory points. In another example, the original planned trajectory and the target optimized trajectory are continuous trajectories; for example, multiple original planned trajectory points can be obtained by sampling on a continuous original planned trajectory, and multiple target optimized trajectory points can be obtained by sampling on a continuous target optimized trajectory.

[0031] In one implementation, the first coordinates of the multiple target optimized trajectory points are the coordinates of the multiple target optimized trajectory points in a reference coordinate system, and the coordinates of the multiple original planned trajectory points are the coordinates of the multiple original planned trajectory points in the reference coordinate system. The reference coordinate system is established based on the lane in which the vehicle is located. In one example, the center line of the lane in which the vehicle is located is used as the reference path, the vertical axis (s-axis) of the reference coordinate system is the reference path, the positive direction of the vertical axis is the forward direction of the vehicle, and the horizontal axis (d-axis) of the reference coordinate system is perpendicular to the reference path.

[0032] Optionally, in this embodiment, when the original planned trajectory and the target optimized trajectory output by the vehicle trajectory planner are trajectories in the world coordinate system, it is necessary to perform coordinate transformation on the original planned trajectory points on the original planned trajectory and the target optimized trajectory points on the target optimized trajectory. In one example, the coordinate transformation process includes: for each original planned trajectory point or each target optimized trajectory point, traversing the points on the reference path to determine the first reference point and the second reference point corresponding to each original planned trajectory point or each target optimized trajectory point, wherein the first reference point is the point on the reference path with the closest Euclidean distance to each original planned trajectory point or each target optimized trajectory point, and the second reference point is the point on the reference path adjacent to the first reference point; based on the Euclidean distance between each original planned trajectory point and the first reference point corresponding to each original planned trajectory point, the Euclidean distance between each original planned trajectory point and the second reference point corresponding to each original planned trajectory point, and the Euclidean distance between the first reference point corresponding to each original planned trajectory point and the second reference point corresponding to each original planned trajectory point, determining the coordinates of each original planned trajectory point in the reference coordinate system; based on the Euclidean distance between each target optimized trajectory point and the first reference point corresponding to each target optimized trajectory point, the Euclidean distance between each target optimized trajectory point and the second reference point corresponding to each target optimized trajectory point, and the Euclidean distance between the first reference point corresponding to each target optimized trajectory point and the second reference point corresponding to each target optimized trajectory point, determining the coordinates of each target optimized trajectory corresponding to each target optimized trajectory in the reference coordinate system.

[0033] Please see Figure 2 , Figure 2 This is a schematic diagram illustrating the principle of coordinate transformation provided in this application, as shown below. Figure 2 As shown, point P is either a point on the original planned trajectory or a point on the target optimized trajectory. The coordinates of point P in the world coordinate system are (x, y). r Point P is the first reference point on the reference path corresponding to point P. r+1 Point P is the second reference point on the reference path corresponding to point P. Point P' is the projection of point P onto the reference path.

[0034] In one example, the longitudinal coordinate of point P in the reference coordinate system can be calculated using the following formula:

[0035] s = s r +ds=s r +b*cosθ (1)

[0036] In formula (1), s represents the longitudinal coordinate of point P in the reference coordinate system. r P represents r The vertical coordinate of point P in the reference coordinate system, ds represents P r The Euclidean distance between point P and point P', where b represents the distance between point P and point P'. rThe Euclidean distance between points, θ represents PP r With P r P r+1 The angle between them. cosθ can be calculated using the following formula:

[0037]

[0038] In formula (2), a represents the distance between point P and P r+1 The Euclidean distance between points P and P' is represented by b. r The Euclidean distance between points, where c represents P r Point and P r+1 The Euclidean distance between points.

[0039] In one example, the lateral coordinate of point P in the reference coordinate system can be calculated using the following formula:

[0040] l=dl=b*sinθ (3)

[0041] In formula (3), l represents the horizontal coordinate of point P in the reference coordinate system, and b represents the coordinate of point P relative to P. r The Euclidean distance between points, θ represents PP r With P r P r+1 The angle between them. sinθ can be calculated using the following formula:

[0042]

[0043] In formula (4), cosθ is calculated using the aforementioned formula (2).

[0044] In this embodiment, the optimization terms of the target optimized trajectory include at least one of a smoothing term, a trajectory length term, and an offset term. In one embodiment, the optimization terms of the target optimized trajectory include at least an offset term. For example, the optimization terms of the target optimized trajectory include a smoothing term, a trajectory length term, and an offset term; or, the optimization terms of the target optimized trajectory include a smoothing term and an offset term; or, the optimization terms of the target optimized trajectory include a trajectory length term and an offset term. The smoothing term represents the smoothness of the target optimized trajectory, the trajectory length term represents the trajectory length of the target optimized trajectory, and the offset term represents the deviation between the target optimized trajectory and the original planned trajectory. The smoothing term is used to improve the smoothness of the target optimized trajectory, the trajectory length term is used to make the optimized target optimized trajectory as short as possible, and the offset term is used to prevent the optimized target optimized trajectory from deviating too much from the original planned trajectory.

[0045] In one embodiment, the step of obtaining the smoothing term of the target optimization trajectory includes: for each target optimization trajectory point, based on the first coordinate of the target optimization trajectory point and the first coordinates of two target optimization trajectory points adjacent to the target optimization trajectory point, obtaining the sub-smoothing term corresponding to the target optimization trajectory point; and summing the sub-smoothing terms corresponding to each target optimization trajectory point as the smoothing term of the target optimization trajectory. In a specific application, the smoothing term of the target optimization trajectory can be calculated using the following formula:

[0046]

[0047] In formula (5), cost1 represents the smoothing term of the target optimization trajectory, (s i , l i (s) represents the first coordinate of any point on the target optimization trajectory. i-1 , l i-1 ) and (s i+1 , l i+1 ) represent points on the target optimization trajectory and points on the target optimization trajectory (s) respectively. i , l i The first coordinates of two adjacent target trajectory points, [(s i-1 +s i+1 -2*s i ) 2 +(l i-1 +l i+1 -2*l i ) 2 ] represents the target optimized trajectory point (s) i , l i The corresponding sub-smoothing term is ), where n represents the number of target optimization trajectory points.

[0048] In one embodiment, the step of obtaining the trajectory length term of the target optimized trajectory includes: for each target optimized trajectory point, based on the first coordinate of the target optimized trajectory point and the first coordinates of the target optimized trajectory points adjacent to the target optimized trajectory point, obtaining the sub-length term corresponding to the target optimized trajectory point; and summing the sub-length terms corresponding to each target optimized trajectory point as the trajectory length term of the target optimized trajectory. In a specific application, the trajectory length term of the target optimized trajectory can be calculated using the following formula:

[0049]

[0050] In formula (6), cost2 represents the trajectory length term of the target optimized trajectory, (s i , l i (s) represents the first coordinate of any point on the target optimization trajectory. i+1 , li+1 ) represents the point (s) on the target optimization trajectory that is adjacent to the target optimization trajectory. i , l i The first coordinates of adjacent target optimized trajectory points, [(s i -s i+1 ) 2 +(l i -l i+1 ) 2 ] represents the target optimized trajectory point (s) i , l i The corresponding sub-length term is ), where n represents the number of target optimization trajectory points.

[0051] In one embodiment, the step of obtaining the offset term of the target optimized trajectory includes: for each target optimized trajectory point, based on the first coordinate of the target optimized trajectory point and the coordinates of the original planned trajectory point corresponding to the target optimized trajectory point on the original planned trajectory, obtaining the sub-offset term corresponding to the target optimized trajectory point; and summing the sub-offset terms corresponding to each target optimized trajectory point as the offset term of the target optimized trajectory. In a specific application, the offset term of the target optimized trajectory can be calculated using the following formula:

[0052]

[0053] In formula (7), cost3 represents the offset term of the target optimized trajectory, (s i , l i () represents the first coordinate of any point on the target optimization trajectory. The point (s) on the original planned trajectory and the target optimized trajectory. i , l i The coordinates of the original planned trajectory points corresponding to ) Represents the target optimized trajectory point (s) i , l i The corresponding sub-offset term is ), where n represents the number of target optimized trajectory points.

[0054] S12: Based on the optimization terms and the first coordinates of multiple target optimization trajectory points, construct a cost function with multiple target optimization trajectory points as the optimization objective.

[0055] Please see Figure 3 , Figure 3 This is a flowchart illustrating an embodiment of the cost function construction method provided in this application. Figure 3 As shown, the method includes the following steps:

[0056] S301: Determine the first coefficient matrix and the second coefficient matrix corresponding to each optimization term.

[0057] The first coefficient matrix corresponding to each optimization term is composed of the coefficients of the quadratic terms in each optimization term, and the second coefficient matrix corresponding to each optimization term is composed of the coefficients of the linear terms in each optimization term. Specifically, by expanding the cumulative expression corresponding to each optimization term, the coefficients of the quadratic and linear terms corresponding to each optimization term are obtained.

[0058] In one embodiment, the first coefficient matrix corresponding to each optimization term is a square matrix. The left half of the first coefficient matrix corresponding to each optimization term consists of the coefficients of the quadratic terms of the vertical axis variables, and the right half consists of the coefficients of the quadratic terms of the horizontal axis variables. The second coefficient matrix corresponding to each optimization term is a column vector. The upper half of the second coefficient matrix corresponding to each optimization term consists of the coefficients of the linear terms of the vertical axis variables, and the lower half consists of the coefficients of the linear terms of the horizontal axis variables. For example, the number of linear terms corresponding to each optimization term is 2^n, where n represents the number of target optimization trajectory points.

[0059] For example, the first coefficient matrix and the second coefficient matrix corresponding to the smoothing term of the target optimized trajectory are respectively:

[0060]

[0061] For example, the first coefficient matrix and the second coefficient matrix corresponding to the trajectory length term of the target optimized trajectory are as follows:

[0062]

[0063] For example, the first coefficient matrix and the second coefficient matrix corresponding to the offset term of the target optimized trajectory are respectively:

[0064]

[0065] S302: Obtain the target optimization matrix composed of the first coordinates of multiple target optimization trajectory points.

[0066] In one embodiment, the target optimization matrix is ​​a column vector consisting of the first coordinates of multiple target optimization trajectory points. For example, the target optimization matrix is:

[0067]

[0068] S303: Construct the cost function based on the objective optimization matrix and the first and second coefficient matrices corresponding to each optimization term.

[0069] In this embodiment, constructing the cost function may include the following sub-steps:

[0070] Sub-step one involves weighting the first coefficient matrix corresponding to each optimization term to obtain the third coefficient matrix, and weighting the second coefficient matrix corresponding to each optimization term to obtain the fourth coefficient matrix.

[0071] In a specific application, the first coefficient matrix corresponding to each optimization term is weighted using the following formula:

[0072] Q = w1*Q1 + w2*Q2 + w3*Q3 (8)

[0073] In a specific application, the second coefficient matrix corresponding to each optimization term is weighted using the following formula:

[0074] P = w1*P1 + w2*P2 + w3*P3 (9)

[0075] In formula (8), Q1, Q2 and Q3 represent the first coefficient matrix corresponding to the smoothing term, the first coefficient matrix corresponding to the trajectory length term and the first coefficient matrix corresponding to the offset term of the target optimized trajectory in step S301, respectively. In formula (9), P1, P2 and P3 represent the second coefficient matrix corresponding to the smoothing term, the second coefficient matrix corresponding to the trajectory length term and the second coefficient matrix corresponding to the offset term of the target optimized trajectory in step S301, respectively.

[0076] It should be noted that since Q1 and P1 are obtained based on the same expression (formula (5) mentioned above), Q2 and P2 are obtained based on the same expression (formula (6) mentioned above), and Q3 and P3 are obtained based on the same expression (formula (7) mentioned above), the weighting coefficient w1 of Q1 in formula (8) is the same as the weighting coefficient w1 of P1 in formula (9), the weighting coefficient w2 of Q2 in formula (8) is the same as the weighting coefficient w2 of P2 in formula (9), and the weighting coefficient w3 of Q3 in formula (8) is the same as the weighting coefficient w3 of P3 in formula (9). For example, the specific values ​​of w1, w2, and w3 can be arbitrarily set according to the actual emphasis on the smoothing term, trajectory length term, and offset term.

[0077] Sub-step two involves constructing a cost function based on the third coefficient matrix, the fourth coefficient matrix, and the objective optimization matrix.

[0078] In one embodiment, sub-step two includes: obtaining a first product term consisting of the product of the transpose matrix corresponding to the target optimization matrix, the third coefficient matrix, and the target optimization matrix; and obtaining a second product term consisting of the product of the transpose matrix of the fourth coefficient matrix and the target optimization matrix; and using the sum of the first product term and the second product term as the cost function.

[0079] In a specific application, the cost function can be expressed by the following formula:

[0080]

[0081] In formula (10), J represents the cost function, x represents the objective optimization matrix, and x T Let Q be the transpose of the objective optimization matrix, Q be the third coefficient matrix, and P be the fourth coefficient matrix. T This represents the transpose of the fourth coefficient matrix.

[0082] S13: Determine the second coordinates of each target's optimized trajectory point when the cost function meets the preset requirements as the optimized trajectory point coordinates of each target's optimized trajectory point.

[0083] In this embodiment, the optimization process of the target trajectory is a step-by-step iterative optimization process. In one implementation, gradient descent is used to iteratively optimize the target trajectory. Specifically, for each round of iterative optimization, the partial derivative of the offset between each target trajectory point and its corresponding original planned trajectory point (the aforementioned sub-offset terms corresponding to each target trajectory point) is calculated to obtain the gradient corresponding to each target trajectory point. Based on the first coordinates of each target trajectory point and the gradients corresponding to each target trajectory point, the new first coordinates corresponding to each target trajectory point in the next round of iterative optimization can be obtained. Substituting the first coordinates corresponding to each target trajectory point in each round of iterative optimization into the cost function calculation expression, the value of the cost function in each round of iterative optimization can be obtained. When the value of the cost function meets the preset requirements, the iterative optimization ends. The first coordinates of each target trajectory point when the cost function meets the preset requirements are designated as the second coordinates of each target trajectory point. At this time, the second coordinates of each target trajectory point are the optimized trajectory point coordinates of each target trajectory point. Based on the optimized trajectory point coordinates of each target trajectory point, the final optimized target trajectory can be determined.

[0084] In one embodiment, the preset requirement includes minimizing the cost function. That is, the second coordinates of each target optimized trajectory point when the cost function is minimized are determined as the optimized trajectory point coordinates of each target optimized trajectory point.

[0085] In another embodiment, the preset requirement may further include: multiple target optimization trajectory points satisfying constraint conditions. That is, the second coordinates of each target optimization trajectory point when the multiple target optimization trajectory points satisfy the constraint conditions and the cost function is at its minimum value are determined as the optimized trajectory point coordinates of each target optimization trajectory point. In this embodiment, the constraint conditions include at least one of the following:

[0086] Constraint 1 (Track splicing constraint): The first coordinate of the starting point of the trajectory among multiple target optimization trajectory points is the same as the coordinate of the trajectory splicing point.

[0087] In a specific application, the equation expression corresponding to constraint one includes:

[0088] s'(0)=P stitch (s), l'(0)=P stitch (l). Where s'(0) and l'(0) represent the longitudinal and lateral coordinates of the starting point of the trajectory in the reference coordinate system among multiple target optimization trajectory points, respectively, P stitch (s) and P stitch (l) represent the longitudinal and transverse coordinates of the trajectory splicing point in the reference coordinate system, respectively.

[0089] The constraint matrix corresponding to constraint condition one includes:

[0090]

[0091] In this embodiment, the vehicle trajectory planner plans multiple segments of original planned trajectories according to a planning period. For example, the planning period is represented by dt. The vehicle trajectory planner starts planning the previous segment of the current original planned trajectory at system time T, completes the planning and output of the previous segment at time T+dt, and starts planning the current segment of the original planned trajectory at time T+dt, completes the planning and output of the current segment of the original planned trajectory at time T+2dt. This results in multiple segments of original planned trajectories during vehicle movement. It should be noted that the starting point of the previous segment of the original planned trajectory is the vehicle position at the time the planning of the previous segment began, i.e., the vehicle position at time T; the starting point of the current segment of the original planned trajectory is the vehicle position at the time the planning of the current segment began, i.e., the vehicle position at time T+dt. When the current segment of the original planned trajectory is not yet completed, the vehicle is controlled to move according to the previous segment of the original planned trajectory. When the current segment of the original planned trajectory is completed, the vehicle is controlled to move according to the current segment of the original planned trajectory.

[0092] However, when switching the current segment of the original planned trajectory to control vehicle movement at time T+2dt, if the starting point of the current segment of the original planned trajectory (the vehicle's position at time T+dt) deviates from the starting point of the previous segment at time T+2dt, that is, if the starting point of the current segment of the original planned trajectory is not on the previous segment, the current segment of the original planned trajectory is discontinuous with the previous segment. This abrupt change in position will affect the vehicle control performance. Therefore, it is necessary to perform trajectory splicing processing on the multiple segments of the original planned trajectory planned by the vehicle trajectory planner to make them continuous, thereby improving the vehicle control performance.

[0093] In this embodiment, the trajectory splicing process includes: determining the trajectory splicing point based on the previous segment of the original planned trajectory of the current segment; and using the trajectory splicing point as the trajectory starting point of the current segment of the original planned trajectory.

[0094] In one example, in response to the absence of a previous segment of the original planned trajectory, the vehicle's position at the start of planning the current segment of the original planned trajectory is determined as the trajectory splicing point. For instance, the vehicle trajectory planner starts planning the current segment of the original planned trajectory at time T+dt. When the vehicle trajectory planner fails to plan the previous segment of the original planned trajectory, the vehicle's position at time T+dt is determined as the trajectory splicing point.

[0095] In another example, in response to the vehicle's position at the start of planning the current segment of the original trajectory and the distance between the target original trajectory point on the previous segment being greater than a distance threshold, the vehicle's position at the start of planning the current segment of the original trajectory is determined as the trajectory splicing point. The target original trajectory point is the trajectory point on the previous segment of the original trajectory corresponding to the completion of planning the current segment of the original trajectory. The distance threshold can be set according to actual needs. When the distance between the vehicle's position at the start of planning the current segment of the original trajectory and the target original trajectory point is greater than the distance threshold, it indicates that the distance between the vehicle's position at the start of planning the current segment of the original trajectory and the target original trajectory point is large. For example, the vehicle trajectory planner starts planning the previous segment of the original trajectory at time T, completes the planning of the previous segment of the original trajectory at time T+dt and starts planning the current segment of the original trajectory, and completes the planning of the current segment of the original trajectory at time T+2dt. The target original trajectory point is the trajectory point on the previous segment of the original trajectory corresponding to time T+2dt. When the distance between the vehicle position at time T+dt when the current segment's original planned trajectory is started and the target original planned trajectory point at time T+2dt is greater than a distance threshold, the vehicle position at time T+dt is determined as the trajectory splicing point.

[0096] In another example, in response to the distance between the vehicle's position at the start of planning the current segment of the original trajectory and the target original trajectory point on the previous segment being less than or equal to a distance threshold, the target original trajectory point on the previous segment is determined as the trajectory splicing point. When the distance between the vehicle's position at the start of planning the current segment of the original trajectory and the target original trajectory point is less than or equal to the distance threshold, it indicates that the distance difference between the vehicle's position at the start of planning the current segment of the original trajectory and the target original trajectory point is small. For example, the vehicle trajectory planner starts planning the previous segment of the original trajectory at time T, completes the planning of the previous segment of the original trajectory at time T+dt and starts planning the current segment of the original trajectory, and completes the planning of the current segment of the original trajectory at time T+2dt. The target original trajectory point is the trajectory point on the previous segment of the original trajectory corresponding to time T+2dt. When the distance between the vehicle's position at the start of planning the current segment of the original trajectory at time T+dt and the target original trajectory point at time T+2dt is less than or equal to the distance threshold, the target original trajectory point at time T+2dt is determined as the trajectory splicing point.

[0097] In this embodiment, based on the starting point of the previous original planned trajectory and the vehicle's movement within the planning cycle of the trajectory planner, the vehicle's position when planning the current original planned trajectory is started is predicted. Specifically, given the vehicle's starting point of the previous original planned trajectory, assuming the vehicle performs a preset straight-line motion with a constant curvature within the planning cycle of the vehicle trajectory planner, the vehicle's trajectory following the preset straight-line motion within the planning cycle can be predicted, thus predicting the vehicle's position when planning the current original planned trajectory is started. For example, the preset straight-line motion can be uniformly accelerated, uniformly decelerated, or uniformly accelerated, etc., and this embodiment does not specifically limit this.

[0098] In a specific application, the linear motion is assumed to be uniformly accelerated linear motion. The vehicle's position at the start of planning the original trajectory for the current segment can be determined using the following formula:

[0099]

[0100] In formula (11), (x1, y1) represents the vehicle position when the current segment of the original planned trajectory is started; (x0, y0) represents the starting point position of the previous segment of the original planned trajectory; v0, a0, θ0 and k0 represent the vehicle speed, acceleration, yaw angle and motion curvature corresponding to the starting point position of the previous segment of the original planned trajectory, respectively, and v0, a0, θ0 and k0 can be obtained by the corresponding sensing modules; dt represents the planning period of the vehicle trajectory planner.

[0101] Optionally, in addition to predicting the vehicle's position when it begins planning the current segment of the original planned trajectory, the motion curvature, acceleration, yaw angle, and vehicle speed of the vehicle can also be predicted. In a specific application, the linear motion is assumed to be uniformly accelerated linear motion. Then, the motion curvature of the vehicle when it begins planning the current segment of the original planned trajectory is the same as the motion curvature of the vehicle at the starting point of the previous segment of the original planned trajectory. The acceleration of the vehicle when it begins planning the current segment of the original planned trajectory is the same as the acceleration of the vehicle at the starting point of the previous segment of the original planned trajectory. The yaw angle and vehicle speed of the vehicle when it begins planning the current segment of the original planned trajectory can be calculated using formulas (12) and (13), respectively. Formulas (12) and (13) are expressed as follows:

[0102] θ1=θ0+dt*(v0+0.5*dt*a0)*k0 (12)

[0103] v1=v0+dt*a0 (13)

[0104] Constraint 2 (Offset Constraint): To further reduce the deviation between the optimized target trajectory and the original trajectory, the absolute value of the lateral coordinate difference between each optimized target trajectory point and its corresponding original trajectory point is less than or equal to a first difference threshold, and the absolute value of the longitudinal coordinate difference between each optimized target trajectory point and its corresponding original trajectory point is less than or equal to a second difference threshold. For example, the first and second difference thresholds can be determined according to actual needs.

[0105] In a specific application, the inequality expression corresponding to constraint condition two includes:

[0106] s(i) ref -s limit ≤s(i)≤s(i) ref +s limit , l(i) ref -l limit ≤l(i)≤l(i) ref +l limit Where i = 0,

[0107] 1, 2, ..., n-1. l(i) represents the horizontal coordinate of the target optimized trajectory point in the reference coordinate system. ref The l represents the longitudinal coordinate of the original planned trajectory point corresponding to the target optimized trajectory point in the reference coordinate system. limit This represents the first difference threshold. s(i) represents the longitudinal coordinate of the target optimized trajectory point in the reference coordinate system. ref s represents the longitudinal coordinate of the original planned trajectory point corresponding to the target optimized trajectory point in the reference coordinate system.limit This represents the second difference threshold.

[0108] The constraint matrix corresponding to constraint condition two includes:

[0109]

[0110] Constraint 3 (Speed ​​Constraint): Considering real-world driving scenarios, to further improve the feasibility and safety of the target optimized trajectory, the vehicle speed corresponding to each target optimized trajectory point is greater than or equal to a first speed threshold and less than or equal to a second speed threshold, where the first speed threshold is less than the second speed threshold. The first and second speed thresholds can be set according to actual needs. For example, the first speed threshold can be 0, and the second speed threshold can be the vehicle's maximum safe speed or the maximum speed the vehicle can reach.

[0111] In a specific application, the inequality expression corresponding to constraint three is as follows:

[0112] Among them, v min v represents the first vehicle speed threshold. max denoted by the second vehicle speed threshold, and T represents the time interval between two adjacent target optimized trajectory points.

[0113] The constraint matrix corresponding to constraint condition three includes:

[0114]

[0115] Constraint 4 (Acceleration Constraint): Considering real-world driving scenarios, to further improve the feasibility and safety of the target optimized trajectory, the acceleration corresponding to each target optimized trajectory point is greater than or equal to a first acceleration threshold and less than or equal to a second acceleration threshold, where the first acceleration threshold is less than the second acceleration threshold. The first and second acceleration thresholds can be set according to actual needs. For example, the first and second acceleration thresholds can be the vehicle's maximum safe acceleration, or the first and second acceleration thresholds can be the maximum acceleration the vehicle can achieve. The first acceleration threshold is negative, and the second acceleration threshold is positive.

[0116] In a specific application, the inequality expression corresponding to constraint four is as follows:

[0117] Among them, a dec Indicates the first acceleration threshold, a acc This represents the second acceleration threshold, and T represents the time interval between two adjacent target optimized trajectory points.

[0118] The constraint matrix representation corresponding to constraint condition four includes:

[0119]

[0120] Alternatively, in this embodiment, the constraint condition can also be expressed as: lb≤Ax≤ub. Where x represents the aforementioned objective optimization matrix.

[0121] In this embodiment, optimization terms for the target optimized trajectory are obtained based on the first coordinates of multiple target optimized trajectory points on the target optimized trajectory. Based on the optimization terms and the first coordinates of the multiple target optimized trajectory points, a cost function is constructed with the multiple target optimized trajectory points as the optimization targets. The second coordinates of each target optimized trajectory point when the cost function meets preset requirements are determined as the optimized trajectory point coordinates of each target optimized trajectory point. Since the cost function is constructed using optimization terms and the first coordinates of multiple target optimized trajectory points, and the optimization terms include at least one of a smoothing term, a trajectory length term, and an offset term for the target optimized trajectory, during the iterative optimization of the cost function with multiple target optimized trajectory points as the optimization targets, at least one of the smoothing term, trajectory length term, and offset term for the target optimized trajectory can be continuously optimized until the cost function meets preset requirements, thereby improving the stability of the target optimized trajectory and thus improving ride comfort.

[0122] Please see Figure 4 , Figure 4 This is a schematic diagram of a framework of an embodiment of the trajectory optimization device provided in this application. In this embodiment, the trajectory optimization device 40 includes: an acquisition module 41, a construction module 42, and a determination module 43. The acquisition module 41 is used to obtain optimization terms of the target optimized trajectory based on the first coordinates of multiple target optimized trajectory points on the target optimized trajectory, wherein the optimization terms include at least one of a smoothing term, a trajectory length term, and an offset term of the target optimized trajectory. The construction module 42 is used to construct a cost function with the multiple target optimized trajectory points as optimization targets based on the optimization terms and the first coordinates of the multiple target optimized trajectory points. The determination module 43 is used to determine the second coordinates of each target optimized trajectory point when the cost function meets preset requirements as the optimized trajectory point coordinates of each target optimized trajectory point.

[0123] Optionally, the construction module 42 is used to determine the first coefficient matrix and the second coefficient matrix corresponding to each optimization term, wherein the first coefficient matrix corresponding to each optimization term is composed of the coefficients of the quadratic terms in each optimization term, and the second coefficient matrix corresponding to each optimization term is composed of the coefficients of the linear terms in each optimization term; to obtain the target optimization matrix composed of the first coordinates of multiple target optimization trajectory points; and to construct the cost function based on the target optimization matrix and the first and second coefficient matrices corresponding to each optimization term.

[0124] Optionally, the construction module 42 is used to perform weighted processing on the first coefficient matrix corresponding to each optimization term to obtain the third coefficient matrix, and to perform weighted processing on the second coefficient matrix corresponding to each optimization term to obtain the fourth coefficient matrix; and to construct the cost function based on the third coefficient matrix, the fourth coefficient matrix and the target optimization matrix.

[0125] Optionally, the construction module 42 is used to obtain a first product term consisting of the transpose of the target optimization matrix, the third coefficient matrix, and the product of the target optimization matrix, and to obtain a second product term consisting of the transpose of the fourth coefficient matrix and the product of the target optimization matrix; the sum of the first product term and the second product term is used as the cost function.

[0126] Optionally, for each target optimized trajectory point, the acquisition module 41 is used to obtain the sub-smoothing term corresponding to the target optimized trajectory point based on the first coordinate of the target optimized trajectory point and the first coordinates of two target optimized trajectory points adjacent to the target optimized trajectory point before and after the target optimized trajectory point; and the sum of the sub-smoothing terms corresponding to each target optimized trajectory point is used as the smoothing term.

[0127] Optionally, for each target optimized trajectory point, the acquisition module 41 obtains the sub-length term corresponding to the target optimized trajectory point based on the first coordinate of the target optimized trajectory point and the first coordinate of the target optimized trajectory point adjacent to the target optimized trajectory point on the target optimized trajectory; and the sum of the sub-length terms corresponding to each target optimized trajectory point is used as the trajectory length term.

[0128] Optionally, for each target optimized trajectory point, the acquisition module 41 obtains the sub-offset term corresponding to the target optimized trajectory point based on the first coordinate of the target optimized trajectory point and the coordinates of the original planned trajectory point corresponding to the target optimized trajectory point on the original planned trajectory; and the sum of the sub-offset terms corresponding to each target optimized trajectory point is used as the offset term.

[0129] Optionally, the preset requirement includes the cost function being at its minimum.

[0130] Optionally, the preset requirements also include: multiple target optimized trajectory points satisfying constraint conditions, the constraint conditions including at least one of the following: the first coordinate of the trajectory starting point among the multiple target optimized trajectory points is the same as the coordinate of the trajectory splicing point; the absolute value of the difference in the lateral coordinates between each target optimized trajectory point and its corresponding original planned trajectory point is less than or equal to a first difference threshold, and the absolute value of the difference in the longitudinal coordinates between each target optimized trajectory point and its corresponding original planned trajectory point is less than or equal to a second difference threshold; the vehicle speed corresponding to each target optimized trajectory point is greater than or equal to a first vehicle speed threshold and less than or equal to a second vehicle speed threshold, the first vehicle speed threshold being less than the second vehicle speed threshold; the acceleration corresponding to each target optimized trajectory point is greater than or equal to a first acceleration threshold and less than or equal to a second acceleration threshold, the first acceleration threshold being less than the second acceleration threshold.

[0131] Optionally, if the first coordinate of the trajectory starting point among multiple target optimization trajectory points is the same as the coordinate of the trajectory splicing point, the trajectory optimization device 40 further includes a trajectory splicing module 44. The trajectory splicing module 44 is used to determine the trajectory splicing point based on the previous segment of the original planned trajectory of the current segment; and to use the trajectory splicing point as the trajectory starting point of the current segment of the original planned trajectory.

[0132] Optionally, the trajectory stitching module 44 is used to determine the vehicle position as the trajectory stitching point in response to the absence of the previous original planned trajectory, or in response to the distance between the vehicle position when the current original planned trajectory is started and the target original planned trajectory point on the previous original planned trajectory being greater than a distance threshold. The target original planned trajectory point is the trajectory point on the previous original planned trajectory corresponding to the completion of the current original planned trajectory planning. In response to the distance between the vehicle position and the target original planned trajectory point being less than or equal to the distance threshold, the target original planned trajectory point is determined as the trajectory stitching point.

[0133] It should be noted that the apparatus of this embodiment can perform the steps in the above method. For detailed descriptions of the relevant content, please refer to the method section above, which will not be repeated here.

[0134] Please see Figure 5 , Figure 5 This is a schematic diagram of a framework of an embodiment of the processing device provided in this application. In this embodiment, the processing device 50 includes a memory 51 and a processor 52.

[0135] Processor 52 can also be referred to as CPU (Central Processing Unit). Processor 52 may be an integrated circuit chip with signal processing capabilities. Processor 52 can also be a general-purpose processor, digital signal processor (DSP), application-specific integrated circuit (ASIC), field-programmable gate array (FPGA), or other programmable logic device, discrete gate or transistor logic device, or discrete hardware component. A general-purpose processor can be a microprocessor, or processor 52 can be any conventional processor 52, etc.

[0136] The memory 51 in the processing device 50 is used to store the program instructions required for the processor 52 to run.

[0137] The processor 52 is used to execute program instructions to implement the trajectory optimization method in this application.

[0138] Please see Figure 6 , Figure 6 This is a schematic diagram of a framework of an embodiment of the computer-readable storage medium provided in this application. The computer-readable storage medium 60 of this embodiment stores program instructions 61, which, when executed, implement the trajectory optimization method provided in this application. The program instructions 61 can be formed into a program file and stored in the aforementioned computer-readable storage medium 60 in the form of a software product, so that a computer device (which may be a personal computer, server, or network device, etc.) can execute all or part of the steps of the methods of various embodiments of this application. The aforementioned computer-readable storage medium 60 includes various media capable of storing program code, such as a USB flash drive, portable hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk, or terminal devices such as computers, servers, mobile phones, and tablets.

[0139] The above scheme obtains the optimization terms of the target optimized trajectory based on the first coordinates of multiple target optimized trajectory points on the target optimized trajectory; based on the optimization terms and the first coordinates of the multiple target optimized trajectory points, a cost function is constructed with the multiple target optimized trajectory points as the optimization targets; the second coordinates of each target optimized trajectory point when the cost function meets the preset requirements are determined as the optimized trajectory point coordinates of each target optimized trajectory point. Since the cost function is constructed based on the optimization terms and the first coordinates of the multiple target optimized trajectory points, and the optimization terms include at least one of the smoothing term, trajectory length term, and offset term of the target optimized trajectory, during the iterative optimization of the cost function with the multiple target optimized trajectory points as the optimization targets, at least one of the smoothing term, trajectory length term, and offset term of the target optimized trajectory can be continuously optimized until the cost function meets the preset requirements, thereby improving the stability of the target optimized trajectory and thus improving the ride comfort.

[0140] In some embodiments, the functions or modules of the apparatus provided in this disclosure can be used to perform the methods described in the above method embodiments. The specific implementation can be referred to the description of the above method embodiments, and for the sake of brevity, it will not be repeated here.

[0141] The description of the various embodiments above tends to emphasize the differences between the various embodiments. The similarities or similarities between them can be referred to, and for the sake of brevity, they will not be repeated here.

[0142] In the several embodiments provided in this application, it should be understood that the disclosed methods, apparatuses, and systems can be implemented in other ways. For example, the apparatus implementations described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection of apparatuses or units may be electrical, mechanical, or other forms.

[0143] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0144] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0145] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods of various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0146] The above description is merely an embodiment of this application and does not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A trajectory optimization method, characterized by, The method comprises: obtaining an optimization item of the target optimization trajectory based on first coordinates of a plurality of target optimization trajectory points on the target optimization trajectory, wherein the optimization item comprises at least one of a smoothing item, a trajectory length item and an offset item of the target optimization trajectory; constructing a cost function with the plurality of target optimization trajectory points as optimization targets based on the optimization item and the first coordinates of the plurality of target optimization trajectory points; determining second coordinates of each of the target optimization trajectory points as optimized trajectory point coordinates of each of the target optimization trajectory points when the cost function meets a preset requirement, wherein the preset requirement comprises that the plurality of target optimization trajectory points meet a constraint condition, and the constraint condition comprises that first coordinates of a trajectory starting point in the plurality of target optimization trajectory points are the same as coordinates of a trajectory splicing point; in response to a missing of a previous segment of an original planning trajectory, or in response to a distance between a vehicle position and a target original planning trajectory point on the previous segment of the original planning trajectory being greater than a distance threshold when planning a current segment of the original planning trajectory, determining the vehicle position as the trajectory splicing point, and the target original planning trajectory point being a trajectory point on the previous segment of the original planning trajectory corresponding to a planning completion time of the current segment of the original planning trajectory; in response to the distance between the vehicle position and the target original planning trajectory point being less than or equal to the distance threshold, determining the target original planning trajectory point as the trajectory splicing point; taking the trajectory splicing point as a trajectory starting point of the current segment of the original planning trajectory.

2. The method of claim 1, wherein, The constructing of the cost function with the plurality of target optimization trajectory points as optimization targets based on the optimization item and the first coordinates of the plurality of target optimization trajectory points comprises: determining first coefficient matrices and second coefficient matrices corresponding to each of the optimization items, wherein the first coefficient matrix corresponding to each of the optimization items is composed of coefficients of quadratic terms in each of the optimization items, and the second coefficient matrix corresponding to each of the optimization items is composed of coefficients of linear terms in each of the optimization items; obtaining a target optimization matrix composed of the first coordinates of the plurality of target optimization trajectory points; constructing the cost function based on the target optimization matrix and the first coefficient matrices and the second coefficient matrices corresponding to each of the optimization items.

3. The method of claim 2, wherein, The constructing of the cost function based on the target optimization matrix and the first coefficient matrices and the second coefficient matrices corresponding to each of the optimization items comprises: performing weighted processing on the first coefficient matrices corresponding to each of the optimization items to obtain third coefficient matrices, and performing weighted processing on the second coefficient matrices corresponding to each of the optimization items to obtain fourth coefficient matrices; constructing the cost function based on the third coefficient matrices, the fourth coefficient matrices and the target optimization matrix.

4. The method of claim 3, wherein, The constructing of the cost function based on the third coefficient matrices, the fourth coefficient matrices and the target optimization matrix comprises: obtaining a first product term composed of a product of a transpose matrix corresponding to the target optimization matrix, the third coefficient matrix and the target optimization matrix, and obtaining a second product term composed of a product of a transpose matrix of the fourth coefficient matrix and the target optimization matrix; taking a sum of the first product term and the second product term as the cost function.

5. The method of claim 1, wherein, The obtaining step of the smooth term comprises: for each target optimization trajectory point, obtaining a sub-smooth term corresponding to the target optimization trajectory point based on a first coordinate of the target optimization trajectory point and first coordinates of two target optimization trajectory points adjacent to the target optimization trajectory point on the target optimization trajectory; taking a cumulative sum of the sub-smooth terms corresponding to each target optimization trajectory point as the smooth term.

6. The method of claim 1, wherein, The obtaining step of the trajectory length term comprises: for each target optimization trajectory point, obtaining a sub-length term corresponding to the target optimization trajectory point based on a first coordinate of the target optimization trajectory point and a first coordinate of a target optimization trajectory point adjacent to the target optimization trajectory point on the target optimization trajectory; taking a cumulative sum of the sub-length terms corresponding to each target optimization trajectory point as the trajectory length term.

7. The method of claim 1, wherein, The obtaining step of the offset term comprises: for each target optimization trajectory point, obtaining a sub-offset term corresponding to the target optimization trajectory point based on a first coordinate of the target optimization trajectory point and a coordinate of a corresponding original planning trajectory point on the original planning trajectory; taking a cumulative sum of the sub-offset terms corresponding to each target optimization trajectory point as the offset term.

8. The method of claim 1, wherein, The preset requirement comprises that the cost function is a minimum value.

9. The method of claim 8, wherein, The constraint condition further comprises at least one of: absolute values of lateral coordinate differences between each target optimization trajectory point and a corresponding original planning trajectory point are less than or equal to a first difference threshold value, and absolute values of longitudinal coordinate differences between each target optimization trajectory point and the corresponding original planning trajectory point are less than or equal to a second difference threshold value; corresponding vehicle speeds of each target optimization trajectory point are greater than or equal to a first vehicle speed threshold value and less than or equal to a second vehicle speed threshold value, the first vehicle speed threshold value being less than the second vehicle speed threshold value; corresponding accelerations of each target optimization trajectory point are greater than or equal to a first acceleration threshold value and less than or equal to a second acceleration threshold value, the first acceleration threshold value being less than the second acceleration threshold value.

10. A trajectory optimization apparatus characterized by comprising: The device comprises: an obtaining module configured to obtain an optimization term of a target optimization trajectory based on first coordinates of a plurality of target optimization trajectory points on the target optimization trajectory, wherein the optimization term comprises at least one of a smooth term, a trajectory length term and an offset term of the target optimization trajectory; a constructing module configured to construct a cost function with the plurality of target optimization trajectory points as optimization targets based on the optimization term and the first coordinates of the plurality of target optimization trajectory points. determining, by a determining module, a second coordinate of each of the target optimized trajectory points as an optimized trajectory point coordinate of each of the target optimized trajectory points when the cost function meets a preset requirement, wherein the preset requirement comprises that the plurality of target optimized trajectory points meet a constraint condition, and the constraint condition comprises that a first coordinate of a trajectory starting point in the plurality of target optimized trajectory points is the same as a coordinate of a trajectory splicing point; splicing, by a trajectory splicing module, a trajectory, in response to a previous segment of an original planning trajectory being missing, or in response to a distance between a vehicle position and a target original planning trajectory point on the previous segment of the original planning trajectory being greater than a distance threshold when planning a current segment of the original planning trajectory, the target original planning trajectory point being a trajectory point on the previous segment of the original planning trajectory corresponding to a time when planning of the current segment of the original planning trajectory is completed; in response to the distance between the vehicle position and the target original planning trajectory point being less than or equal to the distance threshold, determining the target original planning trajectory point as the trajectory splicing point; and taking the trajectory splicing point as a trajectory starting point of the current segment of the original planning trajectory.

11. A processing device, characterized by comprising a memory and a processor coupled to each other, the memory stores program instructions; the processor is configured to execute the program instructions stored in the memory to implement the method of any one of claims 1-9.

12. A computer-readable storage medium, characterized in that, the computer readable storage medium is configured to store program instructions that can be executed to implement the method of any one of claims 1-9. the computer readable storage medium is configured to store program instructions that can be executed to implement the method of any one of claims 1-9.

Images