Vehicle trajectory tracking control method and device, unmanned vehicle and storage medium

Abstract

The application discloses a vehicle trajectory tracking control method and device, an unmanned vehicle and a storage medium. The method comprises the following steps: acquiring a reference trajectory of a target vehicle in a prediction time domain; determining at least two target functions corresponding to at least two steps of the target vehicle based on state quantities, lateral control quantities, longitudinal control quantities and curvatures of the reference trajectory in the at least two steps, wherein the target function comprises a state target subterm, a lateral control target subterm and a longitudinal control target subterm, the state target subterm is associated with the curvature of the corresponding step, and at least one of the lateral control target subterm and the longitudinal control target subterm is associated with the curvature of the corresponding step; taking the at least two target functions as a target function group of a first step of the target vehicle in the prediction time domain, and solving the lateral control quantity increment and the longitudinal control quantity increment of the target vehicle in the first step under the constraint of the state quantity, so as to control the trajectory tracking of the target vehicle. In this way, the future predictability of trajectory tracking to the reference trajectory is improved.

Description

Technical Field

[0001] This application belongs to the field of autonomous driving technology, specifically relating to a vehicle trajectory tracking and control method and device, an unmanned vehicle, and a storage medium. Background Technology

[0002] In recent years, autonomous driving technology has developed rapidly. Its goal is usually to control the vehicle to travel autonomously along the road, to reach the destination as quickly as possible while ensuring the safety of the vehicle itself, and to ensure that it does not pose a direct or indirect threat to the safety of other road users.

[0003] Trajectory tracking control is a crucial component of autonomous driving systems. Its main function is to enable autonomous vehicle navigation by tracking a pre-planned reference trajectory. Improving the predictability of trajectory tracking control to the reference trajectory is a significant research topic.

[0004] The information disclosed in this background section is intended only to enhance the understanding of the overall background of this application and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention

[0005] The purpose of this application is to provide a vehicle trajectory tracking control method that addresses the problem of how to improve the predictability of autonomous driving trajectory tracking control to a reference trajectory.

[0006] To achieve the above objectives, this application provides a vehicle trajectory tracking control method, the method comprising:

[0007] Obtain the reference trajectory of the target vehicle in the prediction time domain, wherein the reference trajectory includes the state variables of the target vehicle at each step in the prediction time domain;

[0008] Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, at least two objective functions corresponding to the target vehicle and the at least two step lengths are determined. The objective functions include state objective sub-items, lateral control objective sub-items, and longitudinal control objective sub-items. The state objective sub-items are associated with the curvature of the corresponding step length, and at least one of the lateral control objective sub-items and the longitudinal control objective sub-items is associated with the curvature of the corresponding step length.

[0009] The at least two objective functions are used as the objective function set for the target vehicle in the first step of the prediction time domain. Under state constraints, the lateral control increment and longitudinal control increment of the target vehicle in the first step are solved to achieve trajectory tracking control of the target vehicle.

[0010] In one embodiment, based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, at least two objective functions corresponding to the target vehicle and the at least two step lengths are determined, specifically including:

[0011] A first state target sub-item is determined based on a first state quantity of the reference trajectory, and a second state target sub-item is determined based on a second state quantity of the reference trajectory, wherein the first state target sub-item is negatively correlated with the curvature of the corresponding step length, and the second state target sub-item is positively correlated with the curvature of the corresponding step length;

[0012] Based on the first state objective sub-item, the second state objective sub-item, the lateral control objective sub-item, and the longitudinal control objective sub-item, the objective function with the corresponding step size is determined.

[0013] In one embodiment, the state quantities include the target vehicle's horizontal and vertical coordinate positions, as well as its yaw angle;

[0014] Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, determine at least two objective functions corresponding to the target vehicle and the at least two step lengths, specifically including:

[0015] Based on the reference trajectory at the x-coordinate position of the corresponding step length, determine the x-coordinate position target sub-item;

[0016] Based on the reference trajectory at the ordinate position of the corresponding step length, determine the ordinate position target sub-item;

[0017] Based on the yaw angle of the reference trajectory at the corresponding step length, determine the yaw angle target sub-item;

[0018] Based on the target sub-items of the horizontal coordinate position, vertical coordinate position, yaw angle, lateral control, and longitudinal control for the corresponding step length, the objective function for the corresponding step length is determined. The target sub-items of the horizontal coordinate position and vertical coordinate position are negatively correlated with the curvature of the corresponding step length, and the target sub-item of the yaw angle is positively correlated with the curvature of the corresponding step length.

[0019] In one embodiment, the lateral control target sub-item is positively correlated with the curvature of the corresponding step size, and the longitudinal control target sub-item is positively correlated with the target vehicle speed of the corresponding step size; or,

[0020] The lateral control target sub-item is positively correlated with the curvature of the corresponding step length, and the longitudinal control target sub-item is negatively correlated with the curvature of the corresponding step length.

[0021] In one embodiment, the lateral control quantity includes the front wheel steering angle of the target vehicle, and the longitudinal control quantity includes the vehicle speed of the target vehicle;

[0022] Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, determine at least two objective functions corresponding to the target vehicle and the at least two step lengths, specifically including:

[0023] Based on the reference trajectory at the corresponding step length, determine the target sub-item of the front wheel steering angle;

[0024] Based on the vehicle speed at the corresponding step length of the reference trajectory, determine the vehicle speed target sub-item;

[0025] Based on the front wheel steering angle objective sub-item, vehicle speed objective sub-item, and state objective sub-item with corresponding step lengths, determine the objective function with corresponding step lengths.

[0026] In one embodiment, the target sub-item of the front wheel steering angle is positively correlated with the square of the corresponding step curvature.

[0027] In one embodiment, the at least two step sizes include the first step size in the prediction time domain;

[0028] Solving for the lateral and longitudinal control increments of the target vehicle at the first step under state constraints specifically includes:

[0029] Under the state constraints of the objective function corresponding to the first step size, the lateral control increment and longitudinal control increment of the target vehicle at the first step size are solved, wherein the state constraints include a relaxation factor.

[0030] In one embodiment, the objective function includes a relaxation sub-term determined based on the relaxation factor.

[0031] In one embodiment, the at least two step sizes include the first ten step sizes in the prediction time domain.

[0032] This application also provides an unmanned vehicle, including:

[0033] At least one processor; and

[0034] A memory that stores instructions that, when executed by the at least one processor, cause the at least one processor to perform the vehicle trajectory tracking control method as described above.

[0035] This application also provides a machine-readable storage medium storing executable instructions that, when executed, cause the machine to perform the vehicle trajectory tracking control method as described above.

[0036] Compared with the prior art, the vehicle trajectory tracking control method according to this application constructs at least two objective functions by combining the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step sizes. These two objective functions are then used as the objective function set for the first step size in the prediction time domain of the target vehicle, thereby solving for a better tracking control increment and improving the predictability of the future of the reference trajectory.

[0037] In another aspect, during the construction of the objective function, the objective sub-item was constructed from two dimensions: the state variables and the control variables of the reference trajectory. Furthermore, the control variable dimension considered both lateral and longitudinal coupled control, which improved the stability of the trajectory tracking control.

[0038] On another front, the curvature factor is taken into account during the construction of the objective function, which can further improve the predictability of the future curvature of the reference trajectory.

[0039] On the other hand, although the optimization is performed with the objective function set as the objective when solving the control increment of the first step, by constraining the state of the objective function corresponding to the first step, it can be expected that the trajectory will converge to the target position range in the first step, thus improving the efficiency of trajectory tracking control. Attached Figure Description

[0040] Figure 1 This is an application scenario diagram of an embodiment of the road vehicle trajectory tracking and control method according to this application;

[0041] Figure 2 This is a flowchart of a vehicle trajectory tracking control method according to an embodiment of this application;

[0042] Figure 3 This is a force analysis diagram of the target vehicle in the global coordinate system and the vehicle body coordinate system in a vehicle trajectory tracking control method according to an embodiment of this application;

[0043] Figure 4 This is a simulation diagram of the tracking trajectory in a vehicle trajectory tracking control method according to an embodiment of this application, which performs tracking control on a reference trajectory;

[0044] Figure 5 A block diagram of a vehicle trajectory tracking control device according to an embodiment of this application;

[0045] Figure 6 This is a hardware structure diagram of an unmanned vehicle according to an embodiment of this application. Detailed Implementation

[0046] The present application will now be described in detail with reference to the embodiments shown in the accompanying drawings. However, these embodiments do not limit the present application, and any structural, methodological, or functional modifications made by those skilled in the art based on these embodiments are included within the protection scope of the present application.

[0047] The terms “first,” “second,” “third,” “fourth,” etc. (if present) in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a particular order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of this application described herein can be implemented, for example, in orders other than those illustrated or described herein. Furthermore, the terms “comprising” and “corresponding to,” and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0048] Before introducing the embodiments of this application, the basic technologies and some technical terms involved in the embodiments of this application will be explained illustratively:

[0049] Autonomous driving refers to the ability to guide and make decisions regarding vehicle operation without requiring a driver to perform physical driving maneuvers, thus enabling the vehicle to drive safely. Autonomous driving technology typically includes high-precision mapping, environmental perception, behavioral decision-making, path planning, and motion control.

[0050] Autonomous driving systems: Systems that enable different levels of autonomous driving functions in vehicles, such as driver assistance systems (L2), high-speed autonomous driving systems requiring human supervision (L3), and highly / fully autonomous driving systems (L4 / L5).

[0051] Intelligent Traffic Systems (ITS), also known as Intelligent Transportation Systems, effectively integrate advanced science and technology (information technology, computer technology, data communication technology, sensor technology, electronic control technology, automatic control theory, operations research, and artificial intelligence, etc.) into transportation, service control, and vehicle manufacturing. This strengthens the connection between vehicles, roads, and users, thereby forming a comprehensive transportation system that ensures safety, improves efficiency, enhances the environment, and saves energy.

[0052] The Intelligent Vehicle Infrastructure Cooperative Systems (IVICS), also known as the vehicle-road collaborative system for short, is a development direction of intelligent transportation systems. The vehicle-road collaborative system adopts advanced wireless communication and new-generation Internet technologies to comprehensively implement dynamic real-time information interaction between vehicles and vehicles, and between vehicles and roads. Based on the collection and fusion of dynamic traffic information in the whole space-time, it conducts active safety control of vehicles and collaborative management of roads, fully realizing the effective collaboration of people, vehicles and roads, ensuring traffic safety, improving traffic efficiency, and thus forming a safe, efficient and environmentally friendly road traffic system.

[0053] Global Coordinate: The global coordinate is the coordinate system where three-dimensional space objects are located, and the vertex coordinates of the model are expressed based on this coordinate system. For example, the global coordinate can be the WGS (World Geodetic System) 84 coordinate system, the GCJ-02 coordinate system (the coordinate system of the geographic information system formulated by the China National Administration of Surveying, Mapping and Geoinformation), etc.

[0054] Vehicle Body Coordinate System (Forward-Left-Up, FLU): A coordinate system used to describe the relationship between objects around the vehicle and the vehicle. Usually, the center of the rear axle of the vehicle can be used as the origin, the direction along the vehicle's driving direction is the x-axis direction (longitudinal), the direction pointed by the left hand facing the front of the vehicle is the y-axis direction (lateral), and the direction perpendicular to the ground and pointing to the roof of the vehicle is the z-axis direction.

[0055] The vehicle trajectory tracking control method of the embodiments of this application can be applied to intelligent transportation systems, such as scenarios like driving navigation, logistics transportation, food delivery, online car-hailing, etc. Taking an autonomous vehicle as an example, during the trajectory tracking process, there is always an expected reference trajectory, which can be obtained according to a planning algorithm or specified in advance. The solution of the embodiments of this application calculates the control quantity increment from two dimensions of longitudinal control and lateral control by establishing the kinematic model of the autonomous vehicle, and issues it at each control step of the autonomous vehicle, so as to enable the autonomous vehicle to continuously and stably track the reference trajectory. According to different usage scenarios, the vehicle trajectory tracking control method provided by the embodiments of this application can be applied to the autonomous driving systems of autonomous vehicles, including levels L2, L3, L4 and above.

[0056] See Figure 1, taking an application scenario of the vehicle trajectory tracking control method provided in the embodiments of the present application as an example. The vehicle can be manually driven by a user or can be automatically driven with the help of the vehicle's intelligent driving system. During either manual driving or automatic driving, the terminal can collect scenario information based on sensors, lidar, cameras, millimeter-wave radars, navigation systems, positioning systems, high-precision maps, etc., and provide some decision-making basis information for vehicle trajectory tracking control. Among them, the terminal can be the vehicle driven by the user, or the intelligent in-vehicle device / module on the vehicle, or the desktop computer, notebook computer, smartphone, and tablet computer configured on the vehicle during the user's driving of the vehicle, as well as the portable wearable device carried by the user, etc.

[0057] Refer Figure 2 , an embodiment of the vehicle trajectory tracking control method of the present application is introduced. In this embodiment, the method includes:

[0058] S11. Obtain the reference trajectory of the target vehicle in the prediction time domain.

[0059] The reference trajectory can refer to the trajectory that the target vehicle has pre-planned to travel. The reference trajectory can be obtained according to a planning algorithm or can be specified in advance. Taking trajectory planning as an example, a smooth trajectory can be calculated through the given initial state of the vehicle (including starting position, speed, and acceleration), target state (including target position, speed, and acceleration), obstacle position, and dynamic and comfort constraints, so that the vehicle can reach the target state along this trajectory. Trajectory planning usually includes two parts: path planning and speed planning. Path planning is responsible for calculating a smooth path from the starting position to the target position, and speed planning calculates the speed of each path point based on this path, thereby forming a speed curve.

[0060] In this embodiment, the reference trajectory includes the state quantities of each step of the target vehicle in the prediction time domain. The prediction time domain is a reasonable duration set in advance, such as 3 seconds, 5 seconds, 8 seconds. Taking 5 seconds as an example, if the number of planned steps is set to 20, then each step length is 0.25 seconds. Here, the step length can be the duration between the sampled points. At each sampling point, the sampled state quantity can be used as the state quantity of the target vehicle in the next step. Here, the state quantity of the target vehicle includes, for example, the abscissa position, ordinate position, etc., and the control quantity includes, for example, speed, acceleration, front wheel steering angle, etc.

[0061] In specific application scenarios, trajectory tracking control of a target vehicle can be achieved by controlling the vehicle's kinematic system. To achieve fast and stable tracking of the reference trajectory, a vehicle kinematic model needs to be determined. This kinematic model can be deployed in the target vehicle's control layer and used as the model for the optimal controller. Different forms of kinematic models can be constructed depending on the expected selection of state variables and control variables. However, overall, the kinematic model should include the target vehicle's state variables, lateral control variables, and longitudinal control variables. The following will demonstrate the modeling process of a kinematic model provided in an embodiment of this application.

[0062] Coordination Figure 3 Assuming the target vehicle travels on a flat, structured road, we ignore the vertical, lateral, and pitch motions caused by factors such as road unevenness, slope, and high curvature. Instead, we primarily consider the vehicle's lateral, longitudinal, and yaw degrees of freedom. Furthermore, we assume that the turning angles and angular velocities of the left and right wheels remain equal at any given time. Therefore, we can combine the left and right wheels into a single wheel, transforming the four-wheeled vehicle model into a two-wheeled bicycle model. Treating the vehicle as a rigid body, we can then construct the Ackermann steering model by observing the front and rear wheels rotating around the instantaneous steering center.

[0063] Figure 3 This includes the XOY global coordinate system and the xoy vehicle body coordinate system. Here, R represents the instantaneous turning radius of the target vehicle's rear wheels. Let be the front wheel steering angle of the target vehicle, and l be the distance between the front and rear axles of the target vehicle. The angle between the x-axis representing the target vehicle's direction of travel and the x-axis in the global coordinate system.

[0064] Because rigid body vehicles rotate around the instantaneous center of rotation The rotation and orientation geometric model yields:

[0065] (1)

[0066] According to the laws of rigid body motion, the relationship between the yaw rate of the target vehicle, the rear wheel speed of the target vehicle, and the instantaneous turning radius of the target vehicle is as follows:

[0067] (2)

[0068] In this embodiment of the application, the speed of the rear wheels of the target vehicle is also referred to as "vehicle speed".

[0069] Combining equations (1) and (2), the yaw rate of the vehicle can be obtained as:

[0070] (3)

[0071] Assuming the target vehicle's rear axle has no slippage, the kinematic constraints of the rear axle are:

[0072] (4)

[0073] in, , The coordinates of the rear axle center of the target vehicle The derivatives with respect to time are respectively, and x and y are also referred to as the "horizontal coordinate position" and "vertical coordinate position" of the target vehicle in this embodiment of the application.

[0074] Therefore, equations (3) and (4) can be rearranged as follows:

[0075] (5)

[0076] Furthermore, set , Equation (5) can be written as:

[0077] (6)

[0078] Therefore, for each trajectory point planned in trajectory tracking, the following should also be satisfied:

[0079] (7)

[0080] Taking a Taylor expansion of equation (7) from a trajectory point i, we get:

[0081] (8)

[0082] According to the method for finding the Jacobian matrix, we can obtain:

[0083] (9)

[0084] (10)

[0085] Therefore, equation (8) can be rearranged as follows:

[0086] (11)

[0087] Discretizing the model in equation (11), we get:

[0088] (12)

[0089] in, , , The state variables representing the output at prediction time t+1 are the target vehicle's horizontal coordinate position, vertical coordinate position, and yaw angle, respectively. , , The state variables representing the current moment are the target vehicle's horizontal coordinate position, vertical coordinate position, and yaw angle. , The control variables at the current moment are vehicle speed and front wheel steering angle. This represents the time interval between adjacent sampling points.

[0090] Equation (12) can be further written as:

[0091] (13)

[0092] Here, g, as the feedforward for the left-right control problem, can be obtained from each planned trajectory point:

[0093] (14)

[0094] Thus, based on the above kinematic model, the current state point (state variable) of the target vehicle can be used to make iterative predictions for the future.

[0095] S12. Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, determine at least two objective functions corresponding to the target vehicle and the at least two step lengths.

[0096] S13. Use at least two objective functions as the objective function set for the target vehicle in the first step of the prediction time domain, and solve for the lateral control increment and longitudinal control increment of the target vehicle in the first step under state constraints, so as to perform trajectory tracking control of the target vehicle.

[0097] Based on the determined reference trajectory, the trajectory curvature and state variables of each step can be determined, and the corresponding control variables can be determined. Taking the model established by equation (12) above as an example, the vehicle speed and front wheel steering angle feedforward of the target vehicle can be calculated by the trajectory curvature of each step.

[0098] The objective function can be used to ultimately solve for the control input in predictive control. Typically, the point on the reference trajectory closest to the target vehicle is used as the reference target, and the goal is to obtain the control input that best approximates this reference target. It can be seen that this approach does not utilize more future reference trajectories for planning, particularly losing the predictability of future curve curvature of the reference trajectory, resulting in poor trajectory tracking control performance. Therefore, in the embodiments of this application, it is desirable to construct multiple objective functions by combining the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at more step sizes, and then solve for better tracking control increments (i.e., lateral and longitudinal control increments).

[0099] Specifically, the objective function in this embodiment includes a state objective sub-item, which is associated with the curvature of the corresponding step size. For example, for the horizontal coordinate position of a state variable, a state objective sub-item can be represented as:

[0100]

[0101] in, kr is the curvature of the reference trajectory for this step size. Let x be the x-coordinate position of the reference trajectory for this step length, and construct the state target sub-item of the x-coordinate position by taking the sum of squared differences.

[0102] In such a state objective sub-item, the curvature of the reference trajectory at that step length directly affects the function value of the sub-item; that is, the larger the curvature, the smaller the state objective sub-item, and vice versa. Therefore, the smaller the curvature, the more likely the state quantity objective difference in the state objective sub-item will converge. In the sense of physical quantities, that is, the smaller the curvature, the more accurate the tracking of the horizontal coordinate position at the corresponding step length is expected.

[0103] Meanwhile, unlike conventional model prediction where the rolling solution of the objective function only focuses on the first step in the prediction time domain, this embodiment determines at least two objective functions corresponding to at least two step lengths as the objective function set for the target vehicle in the first step in the prediction time domain. During the solution process, by expecting the overall objective function set to achieve the desired objective, the curvature, state variables, lateral control variables, and longitudinal control variables of the reference trajectory for the at least two step lengths can be referenced predictively. This allows the solved lateral control variable increment and longitudinal control variable increment to more predictively reflect future changes in the reference trajectory, thereby optimizing the trajectory tracking control effect.

[0104] Understandably, the above example illustrates a reference embodiment of the state objective sub-item in the objective function, using the inverse relationship between a state variable and its corresponding step size curvature. In more embodiments, the specific correlation of curvature in the state objective sub-item can be determined based on the different state variables involved in the constructed kinematic model, and this application does not impose any limitations on this.

[0105] In one embodiment, a first state target sub-item can be determined based on a first state variable of the reference trajectory, and a second state target sub-item can be determined based on a second state variable of the reference trajectory; then, an objective function for the corresponding step size can be determined based on the first state target sub-item, the second state target sub-item, the lateral control target sub-item, and the longitudinal control target sub-item. The first state target sub-item is negatively correlated with the curvature of the corresponding step size, and the second state target sub-item is positively correlated with the curvature of the corresponding step size.

[0106] For example, the first state objective sub-item is represented as:

[0107]

[0108] The second-state objective sub-item is represented as:

[0109]

[0110] in, kr is the curvature of the reference trajectory for this step size. Let the x-coordinate position of the reference trajectory for this step size be defined, and construct the state target sub-item of the x-coordinate position by taking the sum of squared differences; , Let the yaw angle be the reference trajectory for this step size, and construct the state objective sub-item of the yaw angle by taking the sum of squared differences.

[0111] The curvature of the reference trajectory at a given step length has different effects on different state objective sub-items: the greater the curvature, the smaller the first state objective sub-item and the larger the second state objective sub-item; conversely, the smaller the curvature, the larger the first state objective sub-item and the smaller the second state objective sub-item. Therefore, with a larger curvature, the objective difference of the state variables in the second state objective sub-item is more likely to converge; conversely, with a smaller curvature, the objective difference of the state variables in the first state objective sub-item is more likely to converge. Similarly, in terms of physical quantities, a larger curvature leads to more accurate tracking of the yaw angle at the corresponding step length, while a smaller curvature leads to more accurate tracking of the x-coordinate position at the corresponding step length.

[0112] In such embodiments, curvature is used to apply different "directions" of influence to different state target sub-items, thereby enabling more accurate tracking of the state variables of interest, whether the curvature is greater or smaller. In the example shown, a larger curvature focuses more on the yaw angle, while a smaller curvature focuses more on the horizontal coordinate position.

[0113] In this embodiment, the kinematic model established by equation (12) includes three state variables: the vehicle's horizontal and vertical coordinate positions, and the yaw angle. The following will use these three state variables as examples to further illustrate the method for determining the objective function set.

[0114] Overall, firstly, based on the three state variables, the corresponding state objective sub-items are determined: the horizontal coordinate position objective sub-item is determined based on the horizontal coordinate position of the reference trajectory at the corresponding step length; the vertical coordinate position objective sub-item is determined based on the vertical coordinate position of the reference trajectory at the corresponding step length; and the yaw angle objective sub-item is determined based on the yaw angle of the reference trajectory at the corresponding step length. Secondly, based on these three state objective sub-items, the objective function is determined: based on the horizontal coordinate position objective sub-item, vertical coordinate position objective sub-item, yaw angle objective sub-item, lateral control objective sub-item, and longitudinal control objective sub-item, the objective function for the corresponding step length is determined. Finally, the objective functions for at least two selected step lengths are used as the objective function set for the first step length in the prediction time domain.

[0115] Depending on the specific application scenario or user preferences, different step lengths in the prediction time domain can be selected to determine the corresponding objective function. For example, starting from the first step length, five or ten consecutive step lengths can be selected to form an objective function group; or, starting from the first step length, one step length can be selected at intervals of one step length, for a total of ten step lengths to form an objective function group; or, for example, the curvature of each step length on the reference trajectory can be determined first, and step lengths with curvature greater than a set value can be selected to form an objective function group. This application does not impose any restrictions on these methods.

[0116] In each objective function, the x-coordinate position and y-coordinate position sub-terms are negatively correlated with the curvature of the corresponding step size, while the yaw angle sub-term is positively correlated with the curvature of the corresponding step size. The x-coordinate and yaw angle positions define the location of the target vehicle's rear axle center; that is, the smaller the curvature, the more desirable it is for the target vehicle's rear axle center position to converge. From a physical perspective, a smaller curvature makes it more likely for the target vehicle to track the x-coordinate and yaw angle positions, allowing for relative tolerance of yaw angle deviations. Conversely, a larger curvature makes yaw angle convergence more desirable; from a physical perspective, a larger curvature prioritizes the target vehicle's stability, thus requiring less deviation in yaw angle tracking compared to tracking the x-coordinate and yaw angle positions.

[0117] For example, the objective function can be expressed as:

[0118] (14)

[0119] In some embodiments, , For curvature, , and At least one of them is related to curvature. and These are the corresponding weighting coefficients.

[0120] In some embodiments, , For curvature, , and At least one of them is related to curvature. and These are the corresponding weighting coefficients.

[0121] In some embodiments, , For curvature, , and At least one of them is related to curvature. and These are the corresponding weighting coefficients.

[0122] As can be seen from the three examples above, the relationship between the objective sub-items of each state and the curvature of the objective function can be adaptively adjusted as needed.

[0123] In another aspect, the objective function in this embodiment further includes a lateral control objective sub-item and a longitudinal control objective sub-item, and at least one of the lateral control objective sub-item and the longitudinal control objective sub-item is associated with curvature. Exemplarily: ① The lateral control objective sub-item is associated with curvature, and the longitudinal control objective sub-item is independent of curvature; ② The lateral control objective sub-item is independent of curvature, and the longitudinal control objective sub-item is associated with curvature; ③ Both the lateral control objective sub-item and the longitudinal control objective sub-item are associated with curvature.

[0124] For example, the lateral control target sub-item is represented as:

[0125]

[0126] The vertical control objectives are represented as follows:

[0127]

[0128] in, , , Let the front wheel steering angle be the reference trajectory for this step length, and construct the lateral control target sub-item of the front wheel steering angle by taking the sum of squared differences. The vehicle speed is the reference trajectory for this step length, and the longitudinal control target sub-item of the vehicle speed is constructed by taking the sum of squared differences.

[0129] Similarly, the curvature of the reference trajectory at that step length will have different effects on the lateral and longitudinal control target items. Specifically, a larger curvature results in a larger lateral control target item and a smaller longitudinal control target item, while a smaller curvature results in a smaller lateral control target item and a larger longitudinal control target item. Therefore, a larger curvature leads to a higher expectation of convergence for the lateral control quantity target difference within the lateral control target item, while a smaller curvature leads to a higher expectation of convergence for the total control quantity target difference within the longitudinal control target item. In physical terms, a larger curvature results in a higher expectation of more precise tracking of the front wheel steering angle at the corresponding step length, while a smaller curvature results in a higher expectation of more precise tracking of the vehicle speed at the corresponding step length.

[0130] For example, the longitudinal control target sub-item can also be expressed as:

[0131]

[0132] in, .

[0133] In this way, in the dimension of control target items, curvature will only affect the lateral control target items and will not be related to the longitudinal control target items. The higher the vehicle speed at the corresponding step size, the more accurate the tracking of vehicle speed is desired, because higher speeds will result in greater adverse effects on trajectory tracking due to tracking errors.

[0134] In this embodiment, the kinematic model established by equation (12) is used as an example. The lateral control quantity is the front wheel steering angle of the target vehicle, and the longitudinal control quantity is the vehicle speed of the target vehicle. The front wheel steering angle target item can be determined first based on the front wheel steering angle of the reference trajectory at the corresponding step length, and the vehicle speed target item can be determined based on the vehicle speed of the reference trajectory at the corresponding step length. Then, based on the front wheel steering angle target item, vehicle speed target item, and state target item at the corresponding step length, the objective function at the corresponding step length can be determined. The form of the objective function can be referred to equation (14) shown above, and will not be repeated here.

[0135] Continuing with the objective function shown in equation (14) as an example, we can reorganize it into the objective function form for each step size:

[0136] (15)

[0137] in,

[0138]

[0139]

[0140]

[0141]

[0142] During the solution process, each predicted compensation q matrix (involving curvature, horizontal coordinate position, vertical coordinate position, and yaw angle) and r matrix (involving curvature, front wheel steering angle, and vehicle speed) is filled with sampling point information of the reference trajectory issued in the planning layer. Then, under the constraints of state variables, the control sequence is obtained by optimization. The lateral control quantity and longitudinal control quantity obtained from the first predicted compensation (converted into lateral control quantity increment and longitudinal control quantity increment) are issued, and the control quantities optimized later are discarded. After the vehicle moves, the optimization solution is performed again.

[0143] Among these, satisfying vehicle kinematic constraints plays a crucial role in ensuring vehicle driving safety and stability. In this embodiment, the vehicle's kinematic constraints may include state constraints on the target vehicle's lateral position, ordinate position, and yaw angle.

[0144] In this embodiment, the control increment for the first step is optimized using a set of objective functions (corresponding to multiple step lengths). However, this application further proposes that the trajectory should converge to the target position range within the first step. Therefore, the lateral and longitudinal control increments of the target vehicle in the first step can be solved under the state constraints of the objective function corresponding to the first step.

[0145] In such an embodiment, a situation may arise where the solution cannot be found. Accordingly, a relaxation factor can be added to the state constraints to avoid this problem. Exemplarily, the state constraints can be expressed as:

[0146] (16)

[0147] in It is a relaxation factor.

[0148] After introducing a relaxation factor into the state constraints, to prevent the upper and lower bounds of the constraints from being expanded beyond expectations by the relaxation factor and thus losing the restrictive effect of the constraint inequalities, this embodiment further introduces a relaxation sub-term based on the relaxation factor into the objective function to "penalize" the relaxation factor. The new objective function can be expressed as:

[0149] (17)

[0150] in, relaxation factor The 2-norm form,

[0151]

[0152] The diagonal elements of the E matrix are the weights of the relaxation factors, which can be all 1.

[0153] Correspondingly, Equation (17) is rearranged into the form of the objective function for each step size:

[0154] + (18)

[0155] Cooperating parameter Figure 4 , which demonstrates a simulation schematic diagram of the tracking trajectory generated by using the trajectory tracking control method of the present application to track and control a reference trajectory in the global coordinate system. It can be seen that the tracking trajectory converges well relative to the reference trajectory, showing a better trajectory tracking control effect.

[0156] Parameter Figure 5 , which introduces an embodiment of the vehicle trajectory tracking control device of the present application. In this embodiment, the vehicle trajectory tracking control device includes an acquisition module 21, a determination module 22, and a control module 23.

[0157] The acquisition module 21 is configured to acquire a reference trajectory of a target vehicle in a prediction time domain, where the reference trajectory includes state quantities of the target vehicle at each step size in the prediction time domain; the determination module 22 is configured to respectively determine at least two objective functions corresponding to the target vehicle and at least two step sizes based on the state quantities, lateral control quantities, longitudinal control quantities, and curvatures of the reference trajectory at at least two step sizes, where the objective function includes a state objective sub-term, a lateral control objective sub-term, and a longitudinal control objective sub-term, the state objective sub-term is associated with the curvature of the corresponding step size, and at least one of the lateral control objective sub-term and the longitudinal control objective sub-term is associated with the curvature of the corresponding step size; the control module 23 is configured to use the at least two objective functions as a group of objective functions of the target vehicle at the first step size in the prediction time domain, and solve for the increments of the lateral control quantity and the longitudinal control quantity of the target vehicle at the first step size under state quantity constraints to perform trajectory tracking control on the target vehicle.

[0158] In one embodiment, the determination module 22 is specifically configured to determine a first state objective sub-term based on the first state quantity of the reference trajectory, and determine a second state objective sub-term based on the second state quantity of the reference trajectory, where the first state objective sub-term is negatively correlated with the curvature of the corresponding step size, and the second state objective sub-term is positively correlated with the curvature of the corresponding step size; determine the objective function of the corresponding step size based on the first state objective sub-term, the second state objective sub-term, the lateral control objective sub-term, and the longitudinal control objective sub-term.

[0159] In one embodiment, the state variables include the horizontal and vertical coordinate positions of the target vehicle, and the yaw angle; the determining module 22 is specifically used to: determine the horizontal coordinate position target sub-item based on the horizontal coordinate position of the reference trajectory at the corresponding step length; determine the vertical coordinate position target sub-item based on the vertical coordinate position of the reference trajectory at the corresponding step length; determine the yaw angle target sub-item based on the yaw angle of the reference trajectory at the corresponding step length; and determine the objective function for the corresponding step length based on the horizontal coordinate position target sub-item, vertical coordinate position target sub-item, yaw angle target sub-item, lateral control target sub-item, and longitudinal control target sub-item for the corresponding step length, wherein the horizontal coordinate position target sub-item and vertical coordinate position target sub-item are negatively correlated with the curvature of the corresponding step length, and the yaw angle target sub-item is positively correlated with the curvature of the corresponding step length.

[0160] In one embodiment, the lateral control target sub-item is positively correlated with the curvature of the corresponding step length, and the longitudinal control target sub-item is positively correlated with the target vehicle speed of the corresponding step length.

[0161] In one embodiment, the lateral control target sub-item is positively correlated with the curvature of the corresponding step length, and the longitudinal control target sub-item is negatively correlated with the curvature of the corresponding step length.

[0162] In one embodiment, the lateral control quantity includes the front wheel steering angle of the target vehicle, and the longitudinal control quantity includes the vehicle speed of the target vehicle; the determining module 22 is specifically used to determine a front wheel steering angle target sub-item based on the front wheel steering angle of the reference trajectory at a corresponding step length; determine a vehicle speed target sub-item based on the vehicle speed of the reference trajectory at a corresponding step length; and determine a target function at a corresponding step length based on the front wheel steering angle target sub-item, vehicle speed target sub-item, and state target sub-item at a corresponding step length.

[0163] In one embodiment, the target sub-item of the front wheel steering angle is positively correlated with the square of the corresponding step curvature.

[0164] In one embodiment, the at least two step sizes include the first step size in the prediction time domain; the control module 23 is specifically used to solve the lateral control increment and longitudinal control increment of the target vehicle in the first step size under the state quantity constraints of the objective function corresponding to the first step size, wherein the state quantity constraints include a relaxation factor.

[0165] In one embodiment, the objective function includes a relaxation sub-term determined based on the relaxation factor.

[0166] In one embodiment, the at least two step sizes include the first ten step sizes in the prediction time domain.

[0167] As per the above reference Figures 1 to 4This specification describes a vehicle trajectory tracking control method according to embodiments thereof. The details mentioned in the above description of the method embodiments also apply to the vehicle trajectory tracking control device according to embodiments thereof. The above-described vehicle trajectory tracking control device can be implemented in hardware, software, or a combination of hardware and software.

[0168] Figure 6 A hardware structure diagram of an unmanned vehicle according to an embodiment of this specification is shown. Figure 6 As shown, the unmanned vehicle 30 may include at least one processor 31, a memory 32 (e.g., non-volatile memory), a RAM 33, and a communication interface 34, and the at least one processor 31, memory 32, RAM 33, and communication interface 34 are connected together via an internal bus 35. At least one processor 31 executes at least one computer-readable instruction stored or encoded in the memory 32.

[0169] It should be understood that the computer-executable instructions stored in memory 32, when executed, cause at least one processor 31 to perform the above-described combinations in the various embodiments of this specification. Figures 1 to 4 The description includes various operations and functions.

[0170] In the embodiments of this specification, the unmanned vehicle 30 can be configured with a functional terminal to carry the above-mentioned hardware structure. The terminal may include, but is not limited to: personal computer, server computer, workstation, desktop computer, laptop computer, notebook computer, mobile electronic device, smartphone, tablet computer, cellular phone, personal digital assistant (PDA), handheld device, messaging device, wearable electronic device, consumer electronic device, etc.

[0171] According to one embodiment, a program product, such as a machine-readable medium, is provided. The machine-readable medium may have instructions (i.e., the elements implemented in software as described above), which, when executed by a machine, cause the machine to perform the above-described combinations of the various embodiments of this specification. Figures 1 to 5 The various operations and functions described. Specifically, a system or apparatus equipped with a readable storage medium storing software program code that implements the functions of any of the embodiments described above, and enabling the computer or processor of the system or apparatus to read and execute the instructions stored in the readable storage medium.

[0172] In this case, the program code read from the readable medium itself can perform the functions of any of the above embodiments, and therefore the machine-readable code and the readable storage medium storing the machine-readable code constitute a part of this specification.

[0173] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD-RW), magnetic tapes, non-volatile memory cards, and ROMs. Alternatively, program code can be downloaded from a server computer or the cloud via a communication network.

[0174] Those skilled in the art will understand that the various embodiments disclosed above can be modified and varied without departing from the spirit of the invention. Therefore, the scope of protection of this specification should be defined by the appended claims.

[0175] It should be noted that not all steps and units in the above process and system structure diagrams are mandatory; some steps or units can be omitted according to actual needs. The execution order of each step is not fixed and can be determined as needed. The device structure described in the above embodiments can be a physical structure or a logical structure. That is, some units may be implemented by the same physical client, or some units may be implemented by multiple physical clients, or they may be jointly implemented by certain components in multiple independent devices.

[0176] In the above embodiments, the hardware units or modules can be implemented mechanically or electrically. For example, a hardware unit, module, or processor may include permanent dedicated circuitry or logic (such as a dedicated processor, FPGA, or ASIC) to perform the corresponding operation. The hardware unit or processor may also include programmable logic or circuitry (such as a general-purpose processor or other programmable processor), which can be temporarily configured by software to perform the corresponding operation. The specific implementation method (mechanical, dedicated permanent circuitry, or temporarily configured circuitry) can be determined based on cost and time considerations.

[0177] The specific embodiments described above with reference to the accompanying drawings are exemplary embodiments, but do not represent all embodiments that can be implemented or fall within the scope of the claims. The term "exemplary" as used throughout this specification means "serving as an example, instance, or illustration" and does not imply that it is "preferred" or "advantageous" compared to other embodiments. Specific details are included to provide an understanding of the described techniques. However, these techniques can be practiced without these specific details. In some instances, well-known structures and apparatuses are shown in block diagram form to avoid obscuring the concepts of the described embodiments.

[0178] The foregoing description of this disclosure is provided to enable any person skilled in the art to implement or use this disclosure. Various modifications to this disclosure will be apparent to those skilled in the art, and the general principles applicable herein can be applied to other variations without departing from the scope of this disclosure. Therefore, this disclosure is not limited to the examples and designs described herein, but is consistent with the widest scope of the principles and novel features disclosed herein.

Claims

1. A vehicle trajectory tracking control method, characterized in that, The method includes: Obtain the reference trajectory of the target vehicle in the prediction time domain, wherein the reference trajectory includes the state variables of the target vehicle at each step in the prediction time domain; Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, at least two objective functions corresponding to the target vehicle and the at least two step lengths are determined. The objective functions include state objective sub-items, lateral control objective sub-items, and longitudinal control objective sub-items. The state objective sub-items are associated with the curvature of the corresponding step length, and at least one of the lateral control objective sub-items and the longitudinal control objective sub-items is associated with the curvature of the corresponding step length. The at least two objective functions are used as the objective function set for the target vehicle in the first step of the prediction time domain. Under state constraints, the lateral control increment and longitudinal control increment of the target vehicle in the first step are solved to achieve trajectory tracking control of the target vehicle.

2. The vehicle trajectory tracking control method according to claim 1, characterized in that, Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, determine at least two objective functions corresponding to the target vehicle and the at least two step lengths, specifically including: A first state target sub-item is determined based on a first state quantity of the reference trajectory, and a second state target sub-item is determined based on a second state quantity of the reference trajectory, wherein the first state target sub-item is negatively correlated with the curvature of the corresponding step length, and the second state target sub-item is positively correlated with the curvature of the corresponding step length; Based on the first state objective sub-item, the second state objective sub-item, the lateral control objective sub-item, and the longitudinal control objective sub-item, the objective function with the corresponding step size is determined.

3. The vehicle trajectory tracking control method according to claim 2, characterized in that, The state quantities include the target vehicle's horizontal and vertical coordinate positions, as well as its yaw angle; Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, determine at least two objective functions corresponding to the target vehicle and the at least two step lengths, specifically including: Based on the reference trajectory at the x-coordinate position of the corresponding step length, determine the x-coordinate position target sub-item; Based on the reference trajectory at the ordinate position of the corresponding step length, determine the ordinate position target sub-item; Based on the yaw angle of the reference trajectory at the corresponding step length, determine the yaw angle target sub-item; Based on the target sub-items of the horizontal coordinate position, vertical coordinate position, yaw angle, lateral control, and longitudinal control for the corresponding step length, the objective function for the corresponding step length is determined. The target sub-items of the horizontal coordinate position and vertical coordinate position are negatively correlated with the curvature of the corresponding step length, and the target sub-item of the yaw angle is positively correlated with the curvature of the corresponding step length.

4. The vehicle trajectory tracking control method according to claim 1, characterized in that, The lateral control target sub-item is positively correlated with the curvature of the corresponding step size, and the longitudinal control target sub-item is positively correlated with the target vehicle speed of the corresponding step size; or... The lateral control target sub-item is positively correlated with the curvature of the corresponding step length, and the longitudinal control target sub-item is negatively correlated with the curvature of the corresponding step length.

5. The vehicle trajectory tracking control method according to claim 4, characterized in that, The lateral control quantity includes the front wheel steering angle of the target vehicle, and the longitudinal control quantity includes the vehicle speed of the target vehicle; Based on the state variables, lateral control variables, longitudinal control variables, and curvature of the reference trajectory at at least two step lengths, determine at least two objective functions corresponding to the target vehicle and the at least two step lengths, specifically including: Based on the reference trajectory at the corresponding step length, determine the target sub-item of the front wheel steering angle; Based on the vehicle speed at the corresponding step length of the reference trajectory, determine the vehicle speed target sub-item; Based on the front wheel steering angle objective sub-item, vehicle speed objective sub-item, and state objective sub-item with corresponding step lengths, determine the objective function with corresponding step lengths.

6. The vehicle trajectory tracking control method according to claim 5, characterized in that, The target sub-item of the front wheel steering angle is positively correlated with the square of the corresponding step curvature.

7. The vehicle trajectory tracking control method according to claim 1, characterized in that, The at least two step sizes include the first step size in the prediction time domain; Solving for the lateral and longitudinal control increments of the target vehicle at the first step under state constraints specifically includes: Under the state constraints of the objective function corresponding to the first step size, the lateral control increment and longitudinal control increment of the target vehicle at the first step size are solved, wherein the state constraints include a relaxation factor.

8. The vehicle trajectory tracking control method according to claim 7, characterized in that, The objective function includes relaxation sub-terms determined based on the relaxation factor; And / or, the at least two step sizes include the first ten step sizes in the prediction time domain.

9. A vehicle trajectory tracking and control device, characterized in that, include: The acquisition module is used to acquire the reference trajectory of the target vehicle in the prediction time domain, wherein the reference trajectory includes the state variables of the target vehicle at each step in the prediction time domain. The determination module is used to determine at least two objective functions corresponding to the target vehicle and the at least two step lengths based on the state quantity, lateral control quantity, longitudinal control quantity, and curvature of the reference trajectory at at least two step lengths, respectively. The objective functions include a state objective sub-item, a lateral control objective sub-item, and a longitudinal control objective sub-item. The state objective sub-item is associated with the curvature of the corresponding step length, and at least one of the lateral control objective sub-item and the longitudinal control objective sub-item is associated with the curvature of the corresponding step length. The control module is used to take the at least two objective functions as the objective function set of the target vehicle in the first step of the prediction time domain, and solve the lateral control increment and longitudinal control increment of the target vehicle in the first step under state constraints, so as to perform trajectory tracking control of the target vehicle.

10. An unmanned vehicle, comprising: At least one processor; as well as A memory that stores instructions that, when executed by the at least one processor, cause the at least one processor to perform the vehicle trajectory tracking control method as described in any one of claims 1 to 8.

11. A machine-readable storage medium storing executable instructions that, when executed, cause the machine to perform the vehicle trajectory tracking control method as described in any one of claims 1 to 8.

Images