Trajectory tracking method and device, equipment, storage medium and program product

Abstract

The embodiment of the invention provides a trajectory tracking method and device, equipment, a storage medium and a program product. The trajectory tracking method comprises the steps of determining a reference trajectory of a vehicle and a trajectory tracking error at the current moment, determining a prediction time domain of a vehicle state according to the trajectory tracking error and the curvature of the reference trajectory, and according to the prediction time domain, optimizing the vehicle state by taking a minimum vehicle state error and a variable quantity of a speed control quantity as an optimization target. According to the reference track, the vehicle state at the current moment and the speed control quantity at the previous moment, the speed control quantity at the current moment is determined; according to the method, the adaptability of the prediction time domain to different working conditions in the trajectory tracking process is improved, so that the control performance of the system under the dynamic working condition is improved.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving control technology, and particularly to a trajectory tracking method, device, equipment, storage medium and program product. Background Art

[0002] The trajectory tracking method is a dynamic control technology that enables a moving vehicle (such as a vehicle or a drone) to accurately track a preset reference trajectory through real-time control decisions. Among them, Model Predictive Control (MPC), as an advanced control algorithm, is widely used in algorithm development fields such as industrial process control and intelligent driving due to its advantages of predicting the future system behavior, online optimization solving, and handling complex constraint conditions.

[0003] Currently, the trajectory tracking method based on the MPC algorithm usually constructs a nonlinear prediction model based on the dynamic equations of the vehicle in the longitudinal, lateral, and yaw directions, and linearizes it based on the current state in each control cycle. Then, by constructing a constrained optimization problem aiming to minimize a specific performance index, the optimal control sequence is solved in real time within the prediction horizon, only the first control quantity of the control sequence is executed, and the optimal control problem within the prediction horizon is re-solved based on the real-time system state in the next cycle.

[0004] However, the prediction horizon, as a key parameter in the above method, has poor adaptability to different working conditions during the trajectory tracking process, which in turn leads to a decline in the control performance of the system under dynamic working conditions. Summary of the Invention

[0005] Embodiments of the present application provide a trajectory tracking method, device, equipment, storage medium and program product to solve the problem that the prediction horizon in the existing trajectory tracking method has poor adaptability to different working conditions, resulting in a decline in the control performance of the algorithm under dynamic working conditions.

[0006] In a first aspect, embodiments of the present application provide a trajectory tracking method, including:

[0007] Determine the reference trajectory of the vehicle and the trajectory tracking error at the current moment;

[0008] Determine the prediction horizon of the vehicle state according to the trajectory tracking error and the curvature of the reference trajectory;

[0009] According to the prediction horizon, with the goal of minimizing the change in the vehicle state error and the speed control quantity, determine the speed control quantity at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment.

[0010] Optionally, determining the prediction time domain according to the trajectory tracking error and the curvature of the reference trajectory includes:

[0011] Determining at least one error fuzzy set corresponding to the trajectory tracking error and the corresponding first membership degree, and determining at least one curvature fuzzy set corresponding to the curvature of the reference trajectory and the corresponding second membership degree;

[0012] According to the preset fuzzy rules, each of the error fuzzy sets and the corresponding first membership degree, and each of the curvature fuzzy sets and the corresponding second membership degree, determining at least one time domain fuzzy set and the corresponding third membership degree;

[0013] Determining the prediction time domain according to each of the time domain fuzzy sets and the corresponding third membership degree.

[0014] Optionally, determining the trajectory tracking error of the vehicle at the current moment includes:

[0015] Obtaining the vehicle state at the current moment and the reference vehicle state corresponding to the reference trajectory at the current moment;

[0016] Determining the trajectory tracking error according to the vehicle state at the current moment and the reference vehicle state.

[0017] Optionally, the vehicle state at the current moment includes the first lateral position, the first longitudinal position, and the first yaw angle, and the reference vehicle state includes the second lateral position, the second longitudinal position, and the second yaw angle;

[0018] The determining the trajectory tracking error according to the vehicle state at the current moment and the reference vehicle state includes:

[0019] Determining the square of the difference between the first lateral position and the second lateral position, and the sum value of the square of the difference between the first longitudinal position and the second longitudinal position;

[0020] Determining the absolute value of the difference between the first yaw angle and the second yaw angle;

[0021] Determining the sum of the absolute value of the square root of the sum value and the absolute value of the difference as the trajectory tracking error.

[0022] Optionally, according to the prediction time domain, with the optimization objective of minimizing the change amount of the vehicle state error and the speed control quantity, determining the speed control quantity at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment includes:

[0023] Taking the minimization of the vehicle state error and the variation of the speed control quantity as the optimization objective, an objective function is constructed, and the vehicle state error at the end of the prediction horizon is zero, and the variation of the speed control quantity and the speed control quantity are within the set range, which are used as the constraint conditions of the objective function;

[0024] Based on the objective function, the constraint conditions, the relationship model between the vehicle state error and the variation of the speed control quantity, the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment, the speed control quantity at the current moment is determined.

[0025] Optionally, the step of constructing an objective function with the minimization of the vehicle state error and the variation of the speed control quantity as the optimization objective includes:

[0026] Determine the first weight corresponding to the vehicle state error and the second weight corresponding to the variation of the speed control quantity;

[0027] Based on the first weight and the second weight, the weighted sum function of the vehicle state error and the variation of the speed control quantity is determined as the objective function.

[0028] In a second aspect, an embodiment of the present application provides a trajectory tracking device, including:

[0029] A determination module, configured to determine the reference trajectory of the vehicle and the trajectory tracking error at the current moment;

[0030] A processing module, configured to determine the prediction horizon of the vehicle state according to the trajectory tracking error and the curvature of the reference trajectory;

[0031] The processing module is further configured to, according to the prediction horizon, take the minimization of the vehicle state error and the variation of the speed control quantity as the optimization objective, and determine the speed control quantity at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment.

[0032] Optionally, the processing module is further configured to determine at least one error fuzzy set corresponding to the trajectory tracking error and the corresponding first membership degree, and determine at least one curvature fuzzy set corresponding to the curvature of the reference trajectory and the corresponding second membership degree;

[0033] The processing module is further configured to determine at least one time-domain fuzzy set and the corresponding third membership degree according to the preset fuzzy rules, each of the error fuzzy sets and the corresponding first membership degree, and each of the curvature fuzzy sets and the corresponding second membership degree;

[0034] The processing module is further configured to determine the prediction horizon according to each of the time-domain fuzzy sets and the corresponding third membership degree.

[0035] Optionally, the determining module is further configured to obtain the vehicle state at the current moment and the reference vehicle state corresponding to the reference trajectory at the current moment;

[0036] The determining module is further configured to determine the trajectory tracking error according to the vehicle state at the current moment and the reference vehicle state.

[0037] Optionally, the vehicle state at the current moment includes a first lateral position, a first longitudinal position, and a first yaw angle, and the reference vehicle state includes a second lateral position, a second longitudinal position, and a second yaw angle;

[0038] The determining module is further configured to determine the square of the difference between the first lateral position and the second lateral position, and the sum value of the squares of the differences between the first longitudinal position and the second longitudinal position;

[0039] The determining module is further configured to determine the absolute value of the difference between the first yaw angle and the second yaw angle;

[0040] The determining module is further configured to determine the sum of the absolute value of the square root of the sum value and the absolute value of the difference as the trajectory tracking error.

[0041] Optionally, the processing module is further configured to construct an objective function with minimizing the vehicle state error and the change amount of the speed control quantity as the optimization objective, and use the vehicle state error at the end of the prediction time domain being zero, the change amount of the speed control quantity, and the speed control quantity all within the set range as the constraint conditions of the objective function;

[0042] The processing module is further configured to determine the speed control quantity at the current moment based on the objective function, the constraint conditions, the relationship model between the vehicle state error and the change amount of the speed control quantity, the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment.

[0043] Optionally, the processing module is further configured to determine a first weight corresponding to the vehicle state error and a second weight corresponding to the change amount of the speed control quantity;

[0044] The processing module is further configured to determine the weighted sum function of the vehicle state error and the change amount of the speed control quantity as the objective function based on the first weight and the second weight.

[0045] In a third aspect, an embodiment of the present application provides an electronic device, including: a memory, a processor;

[0046] The memory stores computer execution instructions;

[0047] The processor executes the computer-executable instructions stored in the memory, so that the processor executes the trajectory tracking method as described in the first aspect above and / or various possible implementation manners of the first aspect.

[0048] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, in which computer-executable instructions are stored, and when the computer-executable instructions are executed by a processor, they are used to implement the trajectory tracking method as described in the first aspect above and / or various possible implementation manners of the first aspect.

[0049] In a fifth aspect, an embodiment of the present application provides a computer program product, including a computer program, and when the computer program is executed by a processor, it implements the trajectory tracking method as described in the first aspect above and / or various possible implementation manners of the first aspect.

[0050] The trajectory tracking method provided by the embodiment of the present application determines the reference trajectory of the vehicle and the trajectory tracking error at the current moment, determines the prediction time domain of the vehicle state according to the trajectory tracking error and the curvature of the reference trajectory, and according to the prediction time domain, takes minimizing the change amount of the vehicle state error and the speed control amount as the optimization objective, and determines the speed control amount at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control amount at the previous moment; this method improves the adaptability of the prediction time domain to different working conditions during the trajectory tracking process, thereby improving the control performance of the system under dynamic working conditions. Description of the Drawings

[0051] The drawings here are incorporated into the specification and constitute a part of this specification, showing the embodiments consistent with the present application, and are used together with the specification to explain the principles of the present application.

[0052] Figure 1 It is a schematic flowchart of the trajectory tracking method provided by the embodiment of the present application;

[0053] Figure 2 It is a schematic diagram of the principle of the trajectory tracking algorithm provided by the embodiment of the present application;

[0054] Figure 3 It is a schematic diagram of the principle of the fuzzy control algorithm provided by the embodiment of the present application;

[0055] Figure 4 It is a schematic structural diagram of the trajectory tracking device provided by the present application;

[0056] Figure 5 It is a schematic structural diagram of the electronic device provided by the present application.

[0057] Through the above-mentioned drawings, specific embodiments of the present application have been shown, and will be described in more detail hereinafter. These drawings and the written description are not intended to limit the scope of the concept of the present application in any way, but to illustrate the concept of the present application to those skilled in the art by referring to specific embodiments. Detailed Description of the Invention

[0058] To make the objectives, technical solutions, and advantages of the present application clearer, the technical solutions in the present application will be clearly and completely described below with reference to the drawings in the present application. Apparently, the described embodiments are some, but not all, of the embodiments of the present application. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application without creative efforts shall fall within the scope of protection of the present application.

[0059] In the description of the present invention and the claims, as well as in the above drawings, the terms "first", "second", "third", "fourth", etc. (if any) are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein.

[0060] In the embodiments of the present application, words such as "exemplary" or "for example" are used to represent examples, illustrations, or explanations. Any embodiment or design solution described as "exemplary" or "for example" in the present application should not be construed as being more preferred or having more advantages than other embodiments or design solutions. Rather, the use of words such as "exemplary" or "for example" is intended to present related concepts in a specific manner.

[0061] Currently, the trajectory tracking method based on the MPC algorithm generally first constructs a nonlinear prediction model according to the dynamic equations of the vehicle in the longitudinal, lateral, and yaw directions. In each control cycle, the model will perform local linearization processing according to the current system state. Then, by defining an objective of minimizing the trajectory tracking error, the optimal control sequence is calculated in real time within the set prediction horizon. In actual operation, only the first control instruction in the sequence is implemented, and then the optimal control problem is solved again in the next cycle based on the latest system state, repeating this process to achieve closed-loop control.

[0062] However, in the process of trajectory tracking using the above method, the adaptability of the prediction horizon to different working conditions is poor, which in turn leads to a reduction in the tracking accuracy and a slowdown in the response speed of the system under dynamic working conditions.

[0063] Regarding the above problems, the root cause is that the prediction horizon is usually set to a fixed value and remains unchanged throughout the control process. When the system enters different working conditions, using a fixed prediction horizon results in poor control performance of the system and large tracking errors.

[0064] In view of this, the present application proposes a trajectory tracking method. First, based on the vehicle state at the current moment and the reference trajectory of the vehicle, the trajectory tracking error at the current moment is determined. Then, based on the trajectory tracking error and the curvature of the reference trajectory at the current moment, the prediction horizon of the vehicle state is determined. In this way, the prediction horizon is adaptively adjusted according to different working conditions. After obtaining the updated prediction horizon, according to this prediction horizon, with the goal of minimizing the change in the vehicle state error and the speed control quantity, based on the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment, the speed control quantity at the current moment is obtained through optimal solution. In this way, by improving the adaptability of the prediction horizon to different working conditions during the trajectory tracking process, the system formulates a more accurate and effective control strategy based on the dynamically adjusted prediction horizon to reduce errors and improve the response speed, thereby improving the control performance of the system under dynamic working conditions.

[0065] The following uses specific embodiments to elaborate in detail on the technical solution of the present application and how the technical solution of the present application solves the above technical problems. These several specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The following will describe the embodiments of the present application in conjunction with the drawings.

[0066] Figure 1 It is a schematic flowchart of the trajectory tracking method provided by the embodiment of the present application. Taking the vehicle controller as the execution subject as an example, as Figure 1 shown, the trajectory tracking method provided by this embodiment includes the following steps:

[0067] S101. Determine the reference trajectory of the vehicle and the trajectory tracking error at the current moment.

[0068] In the embodiment of the present application, the reference trajectory of the vehicle may refer to the target path that the vehicle needs to track. The trajectory tracking error may refer to the deviation between the actual state of the vehicle and the reference trajectory. The trajectory tracking error may include, for example, lateral error, longitudinal error, and yaw angle error.

[0069] Exemplarily, the reference trajectory of the vehicle may be a driving path from the current point to the target point generated by the path planning module based on the current position of the vehicle, the destination, and the environmental information using the path planning algorithm. The reference trajectory may include the position information of a series of path points that the vehicle needs to pass through, the speed information and curvature information of each path point and other motion state information, as well as the expected arrival time corresponding to each path point.

[0070] After obtaining the reference trajectory of the vehicle, based on the actual state of the vehicle at present and the desired state of the reference trajectory at the current moment, each error component of the vehicle in the lateral, longitudinal, and yaw angle directions can be calculated, and through error integration, the trajectory tracking error at the current moment can be obtained.

[0071] S102. Determine the prediction time domain of the vehicle state according to the trajectory tracking error and the curvature of the reference trajectory.

[0072] Among them, the prediction time domain may refer to the time range in which the controller predicts the future system state during the trajectory tracking process. For example, in autonomous driving, the vehicle needs to predict the trajectory in the next few seconds to calculate the optimal control input.

[0073] It can be understood that the trajectory tracking error can be used to indicate the deviation between the current position of the vehicle and the reference trajectory. If the trajectory tracking error is large, it means that the vehicle is currently far from the target trajectory, and a longer prediction time domain may be required to plan how to gradually return to the reference trajectory; if the trajectory tracking error is small, it means that the vehicle is already close to the reference trajectory, and a shorter prediction time domain can be selected to focus on fine adjustment.

[0074] The curvature of the reference trajectory can be used to indicate the degree of bending of the reference trajectory. When the curvature is large, the vehicle needs more complex steering operations and may require a longer prediction time domain to plan in advance how to smoothly follow the curve; while when the curvature is small, the control of the vehicle is relatively simple, and a shorter prediction time domain can be used.

[0075] Therefore, in the case where the curvature of the reference trajectory is small and the trajectory tracking error is not large, the prediction time domain can be appropriately reduced to reduce the computational amount; in the case where the curvature of the reference trajectory is large and the tracking error is large, the prediction time domain can be appropriately increased to ensure that the system can normally track the reference trajectory.

[0076] In actual application scenarios, the environment and vehicle state are dynamically changing. A fixed prediction time domain may not be able to adapt to all situations. For example, in the scenario of a curve and the vehicle deviating from the reference trajectory, if a fixed short time domain is still used, it can only cover the entry stage of the curve and does not include the exit section, resulting in understeering and thus increasing the tracking error; while in the scenario of a simple straight section and the vehicle approaching the reference trajectory, if a fixed long time domain is still used, there will be redundant calculations, and the computational complexity of the trajectory tracking algorithm will increase significantly with the increase of the prediction time domain, resulting in an excessive computational burden.

[0077] Therefore, dynamically adjusting the prediction time domain according to the characteristics of the current vehicle state and the reference trajectory can make the system more flexible, adapt to different working conditions, and thus reduce unnecessary computational overhead on the premise of ensuring the control effect.

[0078] S103. According to the prediction time domain, with the goal of minimizing the vehicle state error and the change in the speed control amount, determine the current speed control amount based on the reference trajectory, the vehicle state at the current moment, and the speed control amount at the previous moment.

[0079] In the embodiments of the present application, the vehicle state error may refer to the deviation between the predicted vehicle state and the expected state of the reference trajectory at each moment within the prediction time domain. The change in the speed control amount may refer to the deviation between the speed control amounts at adjacent moments within the control time domain. Here, the control time domain refers to the number of steps of the future speed control amount directly calculated in the optimization goal, and the prediction time domain refers to the number of steps of state error prediction. The control time domain is less than or equal to the prediction time domain.

[0080] It can be understood that the vehicle state error may include error components such as position error, speed error, and heading error between the predicted state of the vehicle and the expected state on the reference trajectory at each moment. In the function corresponding to the optimization goal, the vehicle state error is used to evaluate in advance the deviation of the vehicle's future state within the prediction time domain. By minimizing the vehicle state error, it can be ensured that the vehicle follows the planned path as accurately as possible. The change in the speed control amount, as the control input increment of the system, refers to the change in the current control amount relative to the previous moment in each control cycle. By minimizing the control input increment, the drastic change of the control command can be avoided, thereby improving the smoothness and comfort of the system.

[0081] Therefore, the above two minimization goals can be combined and optimized in one function, aiming to find the best control strategy that can minimize both the vehicle state error and keep the control input smooth.

[0082] Then, based on the defined optimization goal, combined with the reference trajectory, the current state, and the control amount at the previous moment, determine the specific control command for the current cycle, that is, the speed control amount, by solving the optimization problem. For example, take the first control input increment in a set of optimal control sequences obtained by optimization as the control input increment at the current moment, and add it to the control input at the previous moment to generate the final control command.

[0083] Figure 2 It is a schematic diagram of the principle of the trajectory tracking algorithm provided by the embodiments of the present application. As Figure 2 shown, the prediction time domain is , the vehicle state error can be the deviation between the reference trajectory -1 and the predicted output -2 at moment, moment, moment , moment. The control time domain is , and the change in the speed control amount can be the predicted control amount -4 at The change in the control quantity between moments and the change in the control quantity between moments, and the change in the control quantity between moments and the change in the control quantity between moments. , the change in the control quantity between moments and the change in the control quantity between moments. If the current moment is , the speed control quantity of the vehicle at moment can be obtained by subtracting 5 from the executed control quantity. According to the measurement output -3, the vehicle state at moment is obtained, and combined with the reference trajectory -1 in the time domain, the speed control quantity at moment is obtained by solving with the optimization objective of minimizing the vehicle state error and the change in the speed control quantity.

[0084] The trajectory tracking method provided by the embodiments of the present application determines the reference trajectory of the vehicle and the trajectory tracking error at the current moment, determines the prediction time domain of the vehicle state according to the trajectory tracking error and the curvature of the reference trajectory, and determines the speed control quantity at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment, with the optimization objective of minimizing the vehicle state error and the change in the speed control quantity; this method improves the adaptability of the prediction time domain to different working conditions during the trajectory tracking process, thereby improving the control performance of the system under dynamic working conditions.

[0085] Optionally, determining the prediction time domain according to the trajectory tracking error and the curvature of the reference trajectory includes:

[0086] Determining at least one error fuzzy set corresponding to the trajectory tracking error and the corresponding first membership degree, and determining at least one curvature fuzzy set corresponding to the curvature of the reference trajectory and the corresponding second membership degree;

[0087] Determining at least one time domain fuzzy set and the corresponding third membership degree according to the preset fuzzy rules, each error fuzzy set and the corresponding first membership degree, and each curvature fuzzy set and the corresponding second membership degree;

[0088] Determining the prediction time domain according to each time domain fuzzy set and the corresponding third membership degree.

[0089] In the embodiments of the present application, the trajectory tracking error can correspond to multiple error fuzzy sets. The trajectory tracking error is set with a corresponding universe of discourse and membership function. First, the actual error value of the trajectory tracking error at the current moment can be mapped into the corresponding universe of discourse to obtain the standardized trajectory tracking error value. Then, the membership degrees of the error belonging to each fuzzy set, that is, the first membership degrees, can be calculated through the membership function. Similarly, the curvature of the reference trajectory can also correspond to multiple curvature fuzzy sets. The curvature of the reference trajectory is set with a corresponding universe of discourse and membership function, and the membership degrees of each curvature fuzzy set, that is, the second membership degrees, can be calculated through the membership function.

[0090] Here, the trajectory tracking error, curvature, and prediction horizon can all have multiple fuzzy sets within their respective universes of discourse, thus achieving more refined control. Moreover, the prediction horizon can also be set with a corresponding universe of discourse, for example, it can be to avoid the stability of the system being affected by overly large or overly small prediction horizon values.

[0091] Among them, the preset fuzzy rules are control rules preset based on the error fuzzy set and the curvature fuzzy set. The fuzzy rules are used to associate the relationship between the inputs (error fuzzy set and curvature fuzzy set) and the output (time-domain fuzzy set).

[0092] By traversing the preset fuzzy rules, for all possible combinations of the error fuzzy set - curvature fuzzy set, the activation strength of each rule is calculated to perform fuzzy inference, obtaining the third membership degree corresponding to each time-domain fuzzy set, that is, the fuzzy output. By defuzzifying and rounding the fuzzy output, the value of the prediction horizon is finally obtained.

[0093] Figure 3 is a schematic diagram of the principle of the fuzzy control algorithm provided by the embodiments of the present application. Next, in combination with Figure 3 the process of determining the prediction horizon based on the trajectory tracking error and the curvature of the reference trajectory will be introduced.

[0094] As Figure 3 shown, the trajectory tracking error is , the reference trajectory has a curvature of , is the scaling factor of the trajectory tracking error, is the scaling factor of the curvature of the reference trajectory. By using and respectively, the actual input values of the trajectory tracking error and the curvature of the reference trajectory can be mapped into their respective fuzzy universes of discourse and defuzzified, respectively obtaining the current trajectory tracking error The corresponding at least one error fuzzy set A and the first membership degree of each error fuzzy set , and the current curvature At least one corresponding curvature fuzzy set B, the second membership degree of each curvature fuzzy set .

[0095] For the above error fuzzy set and the corresponding first membership, curvature fuzzy set and the corresponding second membership, a preset fuzzy rule table is used to perform fuzzy reasoning to obtain a fuzzy output, namely, at least one time domain fuzzy set T and the corresponding third membership. , and use appropriate clarification algorithms, such as the centroid method, to clarify the fuzzy output and obtain the predicted time domain , and then input it into the trajectory tracking algorithm to calculate the speed control amount of the vehicle at the current moment.

[0096] Among them, the preset fuzzy rule table is shown in Table 1 below:

[0097] Table 1 Preset fuzzy rules table

[0098]

[0099] As shown in Table 1, the trajectory tracking error The corresponding error fuzzy set A can be At least one of the reference trajectory curvature The corresponding curvature fuzzy set B can also be At least one of the prediction time domain The corresponding time domain fuzzy set T is at least one of .

[0100] As can be seen from Table 1, the trajectory tracking error Smaller (L) and curvature When L is small, a smaller prediction time domain is required to reduce the amount of calculation; in the case of trajectory tracking error Large (H) and curvature When H is larger, a larger prediction time domain (H) is required to ensure that the system can accurately track the reference trajectory.

[0101] The trajectory tracking method provided in the embodiment of the present application performs fuzzy processing on the predicted time domain by comprehensively considering the curvature and tracking error of the reference trajectory, thereby achieving fine-grained adjustment of the predicted time domain under different working conditions, reducing the computational burden while ensuring the tracking effect, and improving the tracking accuracy and response speed of the system under dynamic working conditions.

[0102] In some embodiments, determining the trajectory tracking error of the vehicle at a current moment includes:

[0103] Obtain the vehicle state at the current moment and the reference vehicle state corresponding to the reference trajectory at the current moment;

[0104] Determine the trajectory tracking error according to the vehicle state and the reference vehicle state at the current moment.

[0105] Among them, the vehicle state at the current moment refers to the kinematic and dynamic characteristic information of the vehicle at the current moment, including information such as position, speed, and angle. The reference vehicle state refers to the state that the vehicle expects to reach when moving along the reference trajectory at the same moment. Its function is to provide the state benchmark of the vehicle at each moment, enabling the controller to calculate the error and optimize future control inputs.

[0106] The vehicle state at the current moment can be measured by sensors on the vehicle, and the reference vehicle state can be extracted from the reference points on the reference trajectory that best match the current state of the vehicle.

[0107] Continue to refer to Figure 2 and Figure 3 , at time k, there is a deviation between the actual state of the vehicle (measurement output - 3) and the expected state corresponding to reference trajectory - 1. Therefore, by measuring the output of the vehicle with sensors, the actual state of the vehicle is obtained as , and the reference trajectory The corresponding reference vehicle state on is used as to determine the trajectory tracking error at the current moment .

[0108] In this way, by determining the trajectory tracking error of the vehicle at the current moment as the input for real - time adjustment of the prediction horizon, the controller can predict the system behavior and possible changes in a longer or shorter future time period based on the real - time tracking error, thereby formulating a more accurate and effective control strategy to reduce the error.

[0109] Exemplarily, a specific implementation method for determining the trajectory tracking error according to the vehicle state and the reference vehicle state at the current moment is given here. The vehicle state at the current moment includes the first lateral position, the first longitudinal position, and the first yaw angle. The reference vehicle state includes the second lateral position, the second longitudinal position, and the second yaw angle. Specifically:

[0110] Determine the sum of the square of the difference between the first lateral position and the second lateral position, and the square of the difference between the first longitudinal position and the second longitudinal position;

[0111] Determine the absolute value of the difference between the first yaw angle and the second yaw angle;

[0112] Determine the sum of the absolute value of the square root of the sum value and the absolute value of the difference as the trajectory tracking error.

[0113] For example, the vehicle state at the current moment can be expressed as , and the reference vehicle state can be expressed as .

[0114] Furthermore, the trajectory tracking error of the vehicle can be calculated using the following formula (1):

[0115]

[0116] where is the trajectory tracking error of the vehicle, , , are the first lateral position, the first longitudinal position, and the first yaw angle of the vehicle at the current moment, respectively, , , are the second lateral position, the second longitudinal position, and the second yaw angle corresponding to the vehicle on the reference trajectory at the current moment, respectively.

[0117] Since the trajectory tracking error directly quantifies the degree to which the vehicle deviates from the reference trajectory and provides real-time feedback to the controller. Thus, by reasonably designing the calculation formula of the trajectory tracking error, an accurate deviation signal can be provided for the fuzzy controller, thereby improving the adaptability of the prediction horizon to different working conditions.

[0118] In a possible implementation, according to the prediction horizon, with the goal of minimizing the vehicle state error and the change in the speed control amount, based on the reference trajectory, the vehicle state at the current moment, and the speed control amount at the previous moment, the speed control amount at the current moment is determined, including:[[]]

[0119] With the goal of minimizing the vehicle state error and the change in the speed control amount, an objective function is constructed, and with the vehicle state error at the end of the prediction horizon being zero, the change in the speed control amount, and the speed control amount all within the set range as the constraint conditions of the objective function;

[0120] Based on the relationship model between the objective function, the constraint conditions, the vehicle state error, and the change in the speed control amount, the reference trajectory, the vehicle state at the current moment, and the speed control amount at the previous moment, the speed control amount at the current moment is determined.

[0121] First, the process of establishing the relationship model between the vehicle state error and the change in the speed control amount is introduced:

[0122] According to the kinematic principle of the vehicle, the following formula (2) can be obtained:

[0123]

[0124] Among them, is a vector composed of the lateral position, longitudinal position, and yaw angle of the vehicle in the fixed coordinate system, is the velocity vector of the vehicle in the body coordinate system, and the corresponding and are the corresponding components of the reference trajectory in the fixed coordinate system and the body coordinate system, respectively, is the rate of change of the state vector of the vehicle in the fixed coordinate system. is the transformation matrix between the fixed coordinate system and the body coordinate system, which can be expressed as the following formula (3):

[0125]

[0126] Based on the above formula (2), using Taylor expansion and performing approximate linearization calculation at the reference point, the following formula (4) can be obtained:

[0127]

[0128] Among them, is the fixed state deviation, is the body velocity deviation, is the rate of change of the error, , , the matrices A and B can be expressed as the following formula (5) and formula (6) respectively:

[0129]

[0130]

[0131] Furthermore, using the forward Euler method and discretizing the above formula (4), the following formula (7) can be obtained:

[0132]

[0133] Among them, , , is the sampling time.

[0134] To achieve incremental control, a new state variable is introduced here, which can be expressed as the following formula (8):

[0135]

[0136] Combining the above formula (7) and formula (8), the following formula (9) is obtained:

[0137]

[0138] Among them, , , is the dimension of the state quantity, is the dimension of the control quantity, , , and C is the identity matrix.

[0139] After arranging the above formula (9), the relationship model between the vehicle state error and the change amount of the speed control quantity can be expressed as the following formula (10):

[0140]

[0141] Among them, is the error state vector and can be expressed as the following formula (11):

[0142]

[0143] is the state influence matrix and can be expressed as the following formula (12):

[0144]

[0145] is the change amount of the speed control quantity and can be expressed as the following formula (13):

[0146]

[0147] is the control influence matrix and can be expressed as the following formula (14):

[0148]

[0149] Among them, is the prediction horizon, is the control horizon, and , .

[0150] So far, the relationship model between the vehicle state error and the change amount of the speed control quantity, that is, formula (10), has been obtained. Then, with the goal of minimizing the vehicle state error and the change amount of the speed control quantity, an objective function is constructed.

[0151] In some embodiments, the weights of the vehicle state error and the change amount of the speed control quantity can be adjusted by combining a weight matrix so that the controller gives priority to correcting this item. For example, the first weight corresponding to the vehicle state error and the second weight corresponding to the change amount of the speed control quantity can be determined;

[0152] Based on the first weight and the second weight, the weighted sum function of the vehicle state error and the change in the speed control amount is determined as the objective function.

[0153] Among them, the objective function can be expressed by the following formula (15):

[0154]

[0155] In the above formula (15), is the first weight corresponding to the vehicle state error, is the second weight corresponding to the change in the speed control amount. The larger the values of the first weight and the second weight, the more attention is paid to the optimization of this item.

[0156] Based on the above formula (15), the constraint conditions of the objective function can be respectively expressed as the following formulas (16) to (19):

[0157]

[0158]

[0159]

[0160]

[0161] Among them, the above formula (16) is used to constrain the speed control amount within a set range, and the above formula (17) is used to constrain the change in the speed control amount within a set range; the above formula (18) is used to constrain the state of the predicted output at the end of the prediction time domain to coincide with the state of the reference trajectory at this point; the above formula (19) is used to constrain the vehicle state error at the end of the prediction time domain to be zero.

[0162] Finally, substitute the reference trajectory, the vehicle state at the current moment, and the speed control amount at the previous moment into the objective function in the above formula (15). Under the condition of the constraint conditions in the formulas (16) to (19) and the relationship model between the vehicle state error and the change in the speed control amount shown in the formula (10), optimize and solve the formula (15) to obtain the change in the speed control amount at the current moment, and then combine it with the speed control amount at the previous moment to determine the speed control amount at the current moment.

[0163] Figure 4 is a schematic structural diagram of the trajectory tracking device provided by the present application. As Figure 4 shown, the trajectory tracking device 400 provided in this embodiment includes:

[0164] A determination module 401, configured to determine the reference trajectory of the vehicle and the trajectory tracking error at the current moment;

[0165] A processing module 402, configured to determine a prediction time domain of a vehicle state according to the trajectory tracking error and the curvature of the reference trajectory.

[0166] The processing module 402 is further configured to, according to the prediction time domain, with minimizing the vehicle state error and the variation of the speed control amount as the optimization objective, determine the speed control amount at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control amount at the previous moment.

[0167] Optionally, the processing module 402 is further configured to determine at least one error fuzzy set corresponding to the trajectory tracking error and a corresponding first membership degree, and determine at least one curvature fuzzy set corresponding to the curvature of the reference trajectory and a corresponding second membership degree.

[0168] The processing module 402 is further configured to determine at least one time domain fuzzy set and a corresponding third membership degree according to a preset fuzzy rule, each of the error fuzzy sets and the corresponding first membership degree, and each of the curvature fuzzy sets and the corresponding second membership degree.

[0169] The processing module 402 is further configured to determine the prediction time domain according to each of the time domain fuzzy sets and the corresponding third membership degree.

[0170] Optionally, the determination module 401 is further configured to obtain the vehicle state at the current moment and the reference vehicle state corresponding to the reference trajectory at the current moment.

[0171] The determination module 401 is further configured to determine the trajectory tracking error according to the vehicle state at the current moment and the reference vehicle state.

[0172] Optionally, the vehicle state at the current moment includes a first lateral position, a first longitudinal position, and a first yaw angle, and the reference vehicle state includes a second lateral position, a second longitudinal position, and a second yaw angle.

[0173] The determination module 401 is further configured to determine the sum of the square of the difference between the first lateral position and the second lateral position and the square of the difference between the first longitudinal position and the second longitudinal position.

[0174] The determination module 401 is further configured to determine the absolute value of the difference between the first yaw angle and the second yaw angle.

[0175] The determination module 401 is further configured to determine the sum of the absolute value of the square root of the sum value and the absolute value of the difference as the trajectory tracking error.

[0176] Optionally, the processing module 402 is further configured to construct an objective function with the minimization of the vehicle state error and the variation of the speed control amount as the optimization objective, and use the vehicle state error at the end of the prediction time domain being zero, the variation of the speed control amount, and the speed control amount all within the set range as the constraint conditions of the objective function;

[0177] The processing module 402 is further configured to determine the speed control amount at the current moment based on the objective function, the constraint conditions, the relationship model between the vehicle state error and the variation of the speed control amount, the reference trajectory, the vehicle state at the current moment, and the speed control amount at the previous moment.

[0178] Optionally, the processing module 402 is further configured to determine a first weight corresponding to the vehicle state error and a second weight corresponding to the variation of the speed control amount;

[0179] The processing module 402 is further configured to determine the weighted sum function of the vehicle state error and the variation of the speed control amount as the objective function based on the first weight and the second weight.

[0180] Figure 5 This is a schematic structural diagram of the electronic device provided by this application. As Figure 5 shown, this application provides an electronic device, and this electronic device 500 includes: a receiver 501, a transmitter 502, a processor 503, and a memory 504.

[0181] The receiver 501 is configured to receive instructions and data;

[0182] The transmitter 502 is configured to send instructions and data;

[0183] The memory 504 is configured to store computer execution instructions;

[0184] The processor 503 is configured to execute the computer execution instructions stored in the memory 504 to implement each step executed by the trajectory tracking method in the above embodiments. Specifically, reference can be made to the relevant descriptions in the foregoing trajectory tracking method embodiments.

[0185] Optionally, the above-mentioned memory 504 can be either independent or integrated with the processor 503.

[0186] When the memory 504 is independently provided, this trajectory tracking device further includes a bus for connecting the memory 504 and the processor 503.

[0187] This application embodiment also provides a computer-readable storage medium, and the computer-readable storage medium stores computer execution instructions, and when the computer execution instructions are executed by a processor, they are used to implement the trajectory tracking method as described in any one of the foregoing embodiments of this application.

[0188] The embodiment of the present application also provides a computer program product, which includes a computer program. When the computer program is executed by a processor, it can implement the trajectory tracking method in any of the foregoing embodiments of the present application.

[0189] In several embodiments provided by the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of modules is only a logical function division. In actual implementation, there may be other division methods. For example, multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed coupling or direct coupling or communication connection to each other can be through some interfaces. The indirect coupling or communication connection of devices or modules can be in an electrical, mechanical or other form.

[0190] The modules described as separate components may or may not be physically separated. The components displayed as modules may or may not be physical units, that is, they may be located in one place, or they may be distributed to multiple network units. Some or all of the modules can be selected according to actual needs to implement the solution of this embodiment.

[0191] In addition, each functional module in various embodiments of the present application can be integrated in a processing unit, or each module can exist physically alone, or two or more modules can be integrated in one unit. The unit formed by the above modules can be implemented in the form of hardware, or in the form of a hardware plus software functional unit.

[0192] The integrated module implemented in the form of a software functional module can be stored in a computer-readable storage medium. The above software functional module is stored in a storage medium, including several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to execute some steps of the methods described in various embodiments of the present application.

[0193] It should be understood that the above-mentioned processor may be a Central Processing Unit (CPU), or other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), etc. The general-purpose processor may be a microprocessor or any conventional processor, etc. The steps of the method disclosed in combination with the application can be directly implemented by a hardware processor, or implemented by a combination of hardware and software modules in the processor.

[0194] The memory may include high-speed random access memory (RAM), and may also include non-volatile memory (NVM), such as at least one disk memory, and may also be a USB flash drive, a mobile hard disk, a read-only memory, a magnetic disk, or an optical disc, etc.

[0195] The bus may be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience in representation, the bus in the drawings of this application is not limited to only one bus or one type of bus.

[0196] The above storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random-Access Memory (SRAM), Electrically Erasable Programmable Read Only Memory (EEPROM), Erasable Programmable Read-Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic memory, flash memory, a magnetic disk or an optical disc. The storage medium can be any available medium accessible by a general-purpose or special-purpose computer.

[0197] An exemplary storage medium is coupled to the processor, enabling the processor to read information from and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and the storage medium can be located in an Application Specific Integrated Circuits (ASIC). Of course, the processor and the storage medium can also exist as discrete components in an electronic device or a master device.

[0198] It should be noted that, for the foregoing method embodiments, for the sake of simple description, they are all expressed as a series of action combinations. However, those skilled in the art should know that this application is not limited by the described action sequence, because according to this application, certain steps can be performed in other sequences or simultaneously. Secondly, those skilled in the art should also know that the embodiments described in the specification are all optional embodiments, and the actions and modules involved are not necessarily essential to this application.

[0199] Furthermore, it should be noted that although the steps in the flowchart are shown in sequence according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless there is a clear description in this article, the execution of these steps has no strict order limitation, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowchart can include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same moment, but can be executed at different moments. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed alternately or in turn with at least a part of other steps or sub-steps or stages of other steps.

[0200] In the above embodiments, the descriptions of the respective embodiments have their own emphases. For parts not detailed in a certain embodiment, reference may be made to the relevant descriptions of other embodiments. The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as falling within the scope described in this specification.

[0201] Those skilled in the art will readily conceive of other embodiments of the present application after considering the specification and practicing the invention disclosed herein. The present application is intended to cover any variations, uses, or adaptations of the present application, which follow the general principles of the present application and include known common general knowledge or conventional technical means in the technical field not disclosed in the present application. The specification and embodiments are only regarded as exemplary, and the true scope and spirit of the present application are pointed out by the claims.

[0202] As described above, the above is only the specific implementation manner of the embodiments of the present application, but the protection scope of the embodiments of the present application is not limited thereto. Any changes or substitutions within the technical scope disclosed in the embodiments of the present application should be covered within the protection scope of the embodiments of the present application. Therefore, the protection scope of the embodiments of the present application should be subject to the protection scope of the claims.

Claims

1. A trajectory tracking method, characterized in that, Including: Determine the reference trajectory of the vehicle and the trajectory tracking error at the current moment; Determine the prediction time domain of the vehicle state according to the trajectory tracking error and the curvature of the reference trajectory; According to the prediction time domain, with the optimization goal of minimizing the vehicle state error and the change amount of the speed control quantity, determine the speed control quantity at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment.

2. The method according to claim 1, wherein The determining the prediction time domain according to the trajectory tracking error and the curvature of the reference trajectory includes: Determine at least one error fuzzy set corresponding to the trajectory tracking error and the corresponding first membership degree, and determine at least one curvature fuzzy set corresponding to the curvature of the reference trajectory and the corresponding second membership degree; According to the preset fuzzy rules, each of the error fuzzy sets and the corresponding first membership degree, and each of the curvature fuzzy sets and the corresponding second membership degree, determine at least one time domain fuzzy set and the corresponding third membership degree; Determine the prediction time domain according to each of the time domain fuzzy sets and the corresponding third membership degree.

3. The method according to claim 1, wherein Determining the trajectory tracking error of the vehicle at the current moment includes: Obtain the vehicle state at the current moment and the reference vehicle state corresponding to the reference trajectory at the current moment; Determine the trajectory tracking error according to the vehicle state at the current moment and the reference vehicle state.

4. The method according to claim 3, wherein The vehicle state at the current moment includes a first lateral position, a first longitudinal position, and a first yaw angle, and the reference vehicle state includes a second lateral position, a second longitudinal position, and a second yaw angle; The determining the trajectory tracking error according to the vehicle state at the current moment and the reference vehicle state includes: Determine the sum of the square of the difference between the first lateral position and the second lateral position, and the square of the difference between the first longitudinal position and the second longitudinal position; Determine the absolute value of the difference between the first yaw angle and the second yaw angle; Determine the sum of the absolute value of the square root of the sum value and the absolute value of the difference as the trajectory tracking error.

5. The method according to any one of claims 1-4, characterized in that, The determining the speed control quantity at the current moment according to the prediction time domain, with the optimization goal of minimizing the vehicle state error and the change amount of the speed control quantity, according to the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment includes: With the optimization goal of minimizing the vehicle state error and the change amount of the speed control quantity, construct an objective function, and use the vehicle state error at the end of the prediction time domain to be zero, the change amount of the speed control quantity, and the speed control quantity are all within the set range as the constraint conditions of the objective function; Based on the objective function, the constraint conditions, the relationship model between the vehicle state error and the change amount of the speed control quantity, the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment, determine the speed control quantity at the current moment.

6. The method according to claim 5, wherein The constructing an objective function with the optimization goal of minimizing the vehicle state error and the change amount of the speed control quantity includes: Determine the first weight corresponding to the vehicle state error and the second weight corresponding to the change amount of the speed control quantity; Based on the first weight and the second weight, a weighted summation function of the vehicle state error and the change amount of the speed control quantity is determined as the objective function.

7. A trajectory tracking device, characterized in that, Comprising: a determination module, configured to determine a reference trajectory of the vehicle and a trajectory tracking error at the current moment; a processing module, configured to determine a prediction time domain of the vehicle state according to the trajectory tracking error and the curvature of the reference trajectory; The processing module is further configured to, according to the prediction time domain, with minimizing the vehicle state error and the change amount of the speed control quantity as an optimization objective, determine the speed control quantity at the current moment according to the reference trajectory, the vehicle state at the current moment, and the speed control quantity at the previous moment.

8. An electronic device, characterized in that, Comprising: a memory, a processor; the memory stores computer execution instructions; the processor executes the computer execution instructions stored in the memory, so that the processor executes the trajectory tracking method according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, Computer execution instructions are stored in the computer-readable storage medium, and when the computer execution instructions are executed by a processor, they are used to implement the trajectory tracking method according to any one of claims 1-6.

10. A computer program product, characterized in that, Comprising a computer program, which when executed by a processor implements the trajectory tracking method according to any one of claims 1-6 above.

Images