Trajectory tracking control method and device

Abstract

The embodiment of the invention provides a trajectory tracking control method and device, and the method comprises the steps: obtaining a to-be-tracked target trajectory of a target vehicle, and enabling the target trajectory to comprise a target path and a target speed; based on the actuator response delay of the target vehicle and the state quantity and the control quantity of the target vehicle, determining a transverse control compensation quantity, the transverse control compensation quantity being used for tracking the target path; on the basis of the motion amount error of the target vehicle, longitudinal motion amount compensation gain is determined, target acceleration after longitudinal compensation is determined under the longitudinal motion amount compensation gain, and the target acceleration after longitudinal compensation is used for tracking the target speed; and outputting the transverse control compensation amount and the target acceleration after longitudinal compensation to an actuator of the target vehicle, so that the target vehicle tracks the target trajectory. The response delay of the actuator is compensated transversely and longitudinally effectively, the precision and robustness of tracking the target trajectory under the response delay of the actuator are improved, and the debugging and calibration workload of control delay compensation of different vehicles is reduced.

Description

TECHNICAL FIELD

[0001] Embodiments of the present specification relate to the technical field of automatic driving, and in particular to a trajectory tracking control method and device. BACKGROUND

[0002] With the rapid development of computer technology and artificial intelligence technology, automatic driving technology has also developed rapidly. An automatic driving vehicle can safely and efficiently complete a preset driving task without human intervention, thereby reducing traffic accidents and economic losses caused by driver misoperation.

[0003] An automatic driving semitrailer is composed of a tractor and a trailer, and the two are connected through a hinge. The automatic driving semitrailer tracks an expected trajectory issued under a planning task. Not only the tractor part needs to track the expected trajectory, but also the trailer part needs to follow the expected trajectory.

[0004] In the prior art, the semitrailer needs corresponding driving, braking, and steering actuators to achieve the above tracking. According to the mechanical structure characteristics of the semitrailer, the response delay of the actuators is large, and the semitrailer may have a significant lag when tracking the expected trajectory, resulting in a large delay in trajectory tracking. Therefore, there is an urgent need for a trajectory tracking control scheme that can accurately track the expected trajectory. SUMMARY

[0005] Therefore, the embodiments of the present specification provide a trajectory tracking control method. One or more embodiments of the present specification also relate to a trajectory tracking control device, a computing device, a computer-readable storage medium, and a computer program product to solve the technical defects in the prior art.

[0006] According to a first aspect of the embodiments of the present specification, a trajectory tracking control method is provided, including: obtaining a target trajectory to be tracked by a target vehicle, wherein the target trajectory includes a target path and a target speed; determining a lateral control compensation amount based on an actuator response delay of the target vehicle, a state quantity of the target vehicle, and a control quantity, wherein the lateral control compensation amount is used to track the target path; determining a longitudinal motion quantity compensation gain based on a motion quantity error of the target vehicle, and determining a target acceleration after longitudinal compensation under the longitudinal motion quantity compensation gain, wherein the target acceleration after longitudinal compensation is used to track the target speed; outputting the lateral control compensation amount and the target acceleration after longitudinal compensation to an actuator of the target vehicle, so that the target vehicle tracks the target trajectory.

[0007] According to a second aspect of the embodiments of the present specification, a trajectory tracking control device is provided, comprising: an acquisition module configured to acquire a target trajectory to be tracked by a target vehicle, wherein the target trajectory comprises a target path and a target speed; a first determination module configured to determine a lateral control compensation amount based on an actuator response delay of the target vehicle, a state quantity of the target vehicle, and a control quantity, wherein the lateral control compensation amount is used to track the target path; a second determination module configured to determine a longitudinal motion quantity compensation gain based on a motion quantity error of the target vehicle, and determine a longitudinal compensated target acceleration under the longitudinal motion quantity compensation gain, wherein the longitudinal motion quantity compensation gain refers to a coefficient used to compensate for the motion quantity error, and the longitudinal compensated target acceleration is used to track the target speed; an output module configured to output the lateral control compensation amount and the longitudinal compensated target acceleration to an actuator of the target vehicle, so that the target vehicle tracks the target trajectory.

[0008] According to a third aspect of the embodiments of the present specification, a computing device is provided, comprising: a memory and a processor; the memory is configured to store computer executable instructions, and the processor is configured to execute the computer executable instructions, and the computer executable instructions, when executed by the processor, implement the steps of the trajectory tracking control method described above.

[0009] According to a fourth aspect of the embodiments of the present specification, a computer readable storage medium is provided, which stores computer executable instructions, and the instructions, when executed by a processor, implement the steps of the trajectory tracking control method described above.

[0010] According to a fifth aspect of the embodiments of the present specification, a computer program product is provided, comprising computer programs / instructions, and the computer programs / instructions, when executed by a processor, implement the steps of the trajectory tracking control method described above.

[0011] One embodiment of the present specification provides a trajectory tracking control method, obtaining a target trajectory to be tracked by a target vehicle, wherein the target trajectory comprises a target path and a target speed; determining a lateral control compensation amount based on an actuator response delay of the target vehicle, a state quantity of the target vehicle and a control quantity, wherein the lateral control compensation amount is used to track the target path; determining a longitudinal motion quantity compensation gain based on a motion quantity error of the target vehicle, and determining a target acceleration after longitudinal compensation under the longitudinal motion quantity compensation gain, wherein the longitudinal motion quantity compensation gain refers to a coefficient used to compensate the motion quantity error, and the target acceleration after longitudinal compensation is used to track the target speed; and outputting the lateral control compensation amount and the target acceleration after longitudinal compensation to an actuator of the target vehicle, so that the target vehicle tracks the target trajectory.

[0012] One embodiment of the present specification can comprehensively consider the actuator response delay of the target vehicle, the state quantity of the target vehicle and the control quantity to obtain the lateral control compensation amount, track the target path through lateral delay compensation; and can comprehensively consider the motion quantity error of the target vehicle to determine the longitudinal motion quantity compensation gain, obtain the target acceleration after longitudinal compensation, and track the target speed through longitudinal delay compensation; control the target vehicle to track the target trajectory based on the lateral control compensation amount and the target acceleration after longitudinal compensation, and comprehensively control the target vehicle to track the target trajectory through lateral and longitudinal delay compensation. In this way, the response delay of the actuator is effectively compensated in the lateral and longitudinal directions, the accuracy and robustness of tracking the target trajectory under the response delay of the actuator are improved, and the debugging and calibration workload of different vehicle control delay compensation is reduced. BRIEF DESCRIPTION OF DRAWINGS

[0013] Figure 1 is a flowchart of a trajectory tracking control method provided by one embodiment of the present specification; Figure 2 is a flowchart of a method for building a no-delay tracking error model provided by one embodiment of the present specification; Figure 3 is a flowchart of a lateral prediction delay compensation strategy outputting a lateral control compensation amount provided by one embodiment of the present specification; Figure 4 is a flowchart of a motion quantity prediction provided by one embodiment of the present specification; Figure 5 is a processing process schematic diagram of a trajectory tracking control method provided by one embodiment of the present specification; Figure 6 is a structure schematic diagram of a trajectory tracking control device provided by one embodiment of the present specification; Figure 7 is a structure block diagram of a computing device provided by one embodiment of the present specification. DETAILED DESCRIPTION

[0014] In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present description. However, the present description can be practiced without the specific details, and it is understood that the present description is not limited to the specifics of the description. Accordingly, those who work in the art will recognize that the description is well suited for application to other embodiments, and the scope of the description is accordingly broad.

[0015] The terminology used in this description of one or more embodiments shall include descriptions of specific embodiments only and is not intended to limit the scope of the description one or more embodiments. As used in this description one or more embodiments and the accompanying claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will also be understood that the term "and / or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.

[0016] It will be understood that, although the terms first, second, etc. can be used herein to describe various information, these terms are not intended to denote a temporal or chronological order. Rather, these terms are used solely to distinguish one from another only. For example, a first item could be termed a second item, and, similarly, a second item could be termed a first item without departing from the scope of the description one or more embodiments. As used herein, the term "if' can be construed to mean "when" or "upon" or "in response to determining" terms that indicate a logical relationship between an event and a consequence that typically follows the event, depending on the context in which the term is used.

[0017] Furthermore, it should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in the description one or more embodiments are all information and data authorized by the user or authorized by all parties, and the collection, use and processing of the relevant data need to comply with the relevant laws, regulations and standards of the country and region, and provide corresponding operation portal for the user to choose authorization or refusal.

[0018] First, the terms involved in the description one or more embodiments are explained.

[0019] PID controller: a feedback control mechanism widely used in industrial control systems, which adjusts the system output through the combination of proportional (P), integral (I), and derivative (D) components to stabilize it at the desired setpoint. Proportional control (P) is used to respond immediately to current errors, integral control (I) is used to eliminate steady-state errors, and derivative control (D) is used to predict error trends and suppress overshoot.

[0020] MPC (Model Predictive Control): a control strategy widely used in industrial process control, robotics, autonomous driving, energy management, and other fields. Unlike PID control, MPC optimizes based on system models, enabling optimal control while meeting constraint conditions. The basic principle of MPC can be summarized as predicting the behavior of the system in the future using mathematical models, solving an optimization problem at each control period to calculate the optimal control sequence (but only the first step is executed), and updating the optimization problem by measuring the system state and updating the optimization problem at the next control time, forming a closed-loop control. Key features of MPC include optimizing control inputs for multiple time steps in the future (prediction horizon), explicitly considering input, output, and state constraints (such as actuator saturation, safety limits, etc.), and solving the optimal solution of the objective function (such as tracking error, control amount change, etc.).

[0021] The trajectory tracking control method provided by the embodiments of the present specification can be applied to the automatic driving scene of a semitrailer, and provides a semitrailer trajectory tracking control scheme considering actuator delay. The automatic driving semitrailer is composed of a tractor and a trailer, and the two are connected by a hinge. The automatic driving semitrailer tracks the expected trajectory issued by the planning task, not only the tractor part tracks the expected trajectory, but also the trailer part needs to follow the expected trajectory. The semitrailer needs corresponding drive, brake, and steering actuators to achieve the above trajectory tracking. According to the mechanical structure characteristics of the semitrailer, the response delay of the actuator is large, and the semitrailer tracking the expected trajectory will have a significant lag. At the same time, for the difference between different driving distances, driving environments, and the degree of mechanical wear of the semitrailer actuators, there will be differences. Therefore, it is necessary to identify the response delay of the actuators under different use conditions, compensate for the response delay of the actuators, and achieve precise tracking of the trajectory of the semitrailer.

[0022] In an implementation, in terms of control compensation for actuator delay, a simpler feedback gain compensation control output can be used to adjust the compensation amount by using a feedback gain coefficient, and the robustness of the actuator output delay problem is poor. However, the feedback gain coefficient cannot achieve good control compensation effect for different sizes of correction error; and for vehicles with different actuator delay conditions, the adaptability of the control compensation method is poor, and a large amount of debugging and calibration work is required.

[0023] In addition, when considering the response delay of the actuator, the response delay of the steering actuator under lateral path tracking is usually considered, but due to the mechanical structure characteristics of the drive and brake actuators of the semitrailer, the output control delay (i.e. the delay under the target speed of longitudinal tracking) is more significant than the steering actuator (the delay under lateral path tracking), and more correction error of control compensation is needed to optimize the tracking effect.

[0024] Therefore, one embodiment of the present specification provides a trajectory tracking control method, mainly including two parts of a lateral prediction delay compensation strategy and a longitudinal prediction delay compensation strategy, considering the response delay problem of longitudinal speed tracking, the target trajectory tracking control between the planning module and the actuator module is planned through the lateral and longitudinal prediction delay compensation strategies, different delay compensation strategies are used in the lateral and longitudinal tracking control, and the control compensation amount is calculated by using the MPC controller and the double-loop PID controller respectively, the adaptability of the control compensation effect of the vehicle with different delay degrees is increased, the workload of calibration and debugging is reduced, and the output control compensation amount can ensure the correction delay response effect, solving the actuator delay problem of the semitrailer in lateral and longitudinal tracking control. In addition, the response delay of the drive, brake and steering actuators, as well as the influence of the cumulative mileage, driving environment and actuator working wear, can be considered comprehensively, and the trajectory tracking precision and robustness under high delay response are improved for vehicles with different actuator delay conditions.

[0025] In the present specification, a trajectory tracking control method is provided, and the present specification also relates to a trajectory tracking control device, a computing device, a computer readable storage medium, and a computer program product, which are described in detail one by one in the following embodiments.

[0026] Referring to Figure 1 , Figure 1 A flowchart of a trajectory tracking control method according to one embodiment of the present specification is shown, which specifically includes the following steps 102-108.

[0027] Step 102: obtaining a target trajectory to be tracked by a target vehicle, wherein the target trajectory includes a target path and a target speed.

[0028] Specifically, the target vehicle refers to the vehicle that needs to be controlled at present to realize the task of tracking the target trajectory. The target vehicle can be a trailer or a semitrailer, which is a common freight vehicle widely used in the field of logistics transportation and consists of a tractor and a trailer connected by a hinge. An autonomous driving semitrailer needs to track the target trajectory issued by the planning task, and not only the tractor part needs to track the target trajectory, but also the trailer part needs to follow the target trajectory.

[0029] In actual implementation, the target trajectory to be tracked can refer to the trajectory that the planning module issues for the target vehicle to track, that is, the driving path planned by the planning module for the autonomous driving of the target vehicle. The target trajectory includes a target path and a target speed. The target path refers to the specific route followed by the target vehicle during driving, which clearly defines the route to be traveled by the target vehicle. It can be a straight line, a curve, a polyline, etc., and is the specific trajectory line that the target vehicle passes through from the starting point to the ending point in space. The target speed refers to the speed of the target vehicle along the target path, which specifies the speed of the target vehicle on the target path. It reflects the distance moved by the target vehicle per unit time and is usually measured by the unit of speed (such as meters / second, kilometers / hour, etc.). The target trajectory comprehensively describes the motion to be tracked by the target vehicle through the target path and the target speed.

[0030] It should be noted that in an autonomous driving or intelligent vehicle system, the target trajectory to be tracked by the target vehicle can be obtained, which can be issued by the planning module. Subsequently, the actuators of the target vehicle can be controlled to track the target trajectory. The planning module is an important component in the autonomous driving or intelligent vehicle system. It calculates a suitable driving trajectory based on the current position of the vehicle, surrounding environment information (such as road conditions, traffic signs, positions of other vehicles and pedestrians, etc.), and driving targets (such as destinations), and sends it to the subsequent control module. After obtaining the target trajectory, the autonomous driving or intelligent vehicle system controls the actuators of the target vehicle. The actuators are components on the vehicle that can implement specific actions, such as controllers, steering actuators, engines, etc. By precisely controlling the actions of these actuators, the vehicle can travel according to the planned target trajectory, achieving tracking of the target trajectory.

[0031] Step 104: determining a lateral control compensation amount based on the actuator response delay of the target vehicle, the state quantity of the target vehicle, and the control quantity, wherein the lateral control compensation amount is used to track the target path.

[0032] Specifically, the actuators (such as driving, braking, and steering actuators, etc.) of the target vehicle do not respond immediately after receiving the control instructions, but there is a certain time delay, which may be caused by mechanical structure, electronic signal transmission, etc.

[0033] The state quantity refers to a physical quantity reflecting the current driving state of the target vehicle. Common state quantities can include the position, speed, acceleration, heading angle, etc. of the vehicle, which can be obtained in real time through sensors (such as GPS, inertial measurement unit, etc.). The control quantity refers to the control signal that the control system needs to output in order to make the target vehicle drive along the desired path, such as steering angle, throttle opening, brake pressure, etc.

[0034] The lateral control compensation quantity refers to the compensation control quantity calculated to compensate for the influence of factors such as actuator response delay, so that the vehicle can more accurately track the target path. It is mainly used to adjust the lateral motion of the target vehicle, such as correcting the lateral deviation and heading deviation of the vehicle.

[0035] It should be noted that the target vehicle needs corresponding driving, braking, and steering actuators to realize trajectory tracking. According to the mechanical structure characteristics of the semitrailer, the response delay of its actuators is large, and the semitrailer will have a significant lag when tracking the desired trajectory. In actual implementation, based on the actuator response delay, state quantity, and control quantity of the target vehicle, a lateral prediction delay compensation strategy can be used to calculate the lateral control compensation quantity, so that the lateral motion of the target vehicle can be more accurately controlled based on the lateral control compensation quantity, thereby making the vehicle better track the target path.

[0036] In an optional implementation of the present embodiment, the lateral control compensation quantity is determined based on the actuator response delay of the target vehicle, the state quantity of the target vehicle, and the control quantity of the target vehicle, including: Generating an instruction response delay and an output stability delay based on the actuator response delay of the target vehicle, and obtaining an actuator delay model by concatenating the instruction response delay and the output stability delay; Building a lateral tracking error model based on the control quantity of the target vehicle and the state quantity of the target vehicle; Adding the actuator delay model to the lateral tracking error model to obtain an augmented no-delay tracking error model; According to the current state quantity of the target vehicle and the expected state quantity of the preview point on the target path, calculating a state quantity error, taking the state quantity error as the input of the no-delay tracking error model, and solving the no-delay tracking error model to obtain the lateral control compensation quantity.

[0037] It should be noted that the upstream planning module can issue a target trajectory, and the target path is tracked through a lateral prediction delay compensation strategy. The lateral prediction delay compensation strategy can include a tracking error model, an actuator delay model, a delay-free tracking error model, and an MPC controller.

[0038] In actual implementation, the actuator response delay refers to the time taken by the vehicle actuator to actually start acting and achieve the expected effect after receiving the control instruction. This delay process can be described as a structure in series of instruction response delay and output stabilization delay, and an actuator delay model is built. The instruction response delay is a response delay part in the actuator response delay, and the instruction response delay represents a fixed time delay from the issuance of the control instruction to the start of the actuator response. The output stabilization delay is a first-order inertia delay part in the actuator response delay, and the output stabilization delay represents the dynamic process of the actuator from the start of the response to the stable output. The response speed has a gradual change process, and is not completed instantaneously, that is, the delay from the start of the response to the stable output.

[0039] The lateral tracking error refers to the deviation of the actual trajectory of the target vehicle from the target path in the lateral direction (perpendicular to the driving direction). Based on the control quantity of the target vehicle and the state quantity of the target vehicle, a lateral tracking error model can be constructed. This model is used to describe the relationship between the degree of deviation of the vehicle from the target path in the lateral direction and the control quantity and the state quantity.

[0040] The actuator delay model is added to the lateral tracking error model, the delay control quantity is taken as an augmented state quantity, and the tracking error model is augmented and converted into a delay-free tracking error model. The tracking error model originally containing the delay factor is converted into a delay-free tracking error model.

[0041] Subsequently, the state quantity error can be calculated according to the current state quantity of the target vehicle and the expected state quantity of the preview point on the target path. The expected state quantity error is zero. The state quantity error can be taken as the input of the delay-free tracking error model through the MPC (model predictive control) controller of the lateral prediction compensation strategy. The MPC controller will predict the state of the vehicle in the future period of time according to the current state quantity error, and solve the delay-free tracking error model through an optimization algorithm to obtain a control quantity that can reduce the lateral tracking error of the target vehicle and approach the target path. This control quantity is a lateral control compensation quantity. By applying this compensation quantity, the lateral tracking performance of the vehicle can be improved, and the vehicle can better track the target path.

[0042] The preview point on the target path is a trajectory point pre-configured on the target path, and the expected state quantity is an expected state quantity of the target vehicle at the trajectory point configured by the planning module.

[0043] Figure 2is a flowchart of building a non-delay tracking error model provided by an embodiment of the present specification, referring to Figure 2 The actuator delay model includes an instruction response delay and an output stabilization delay generated by an actuator response delay of the target vehicle. The actuator delay model is added to the lateral tracking error model, the state quantity is augmented, and a non-delay tracking error model is obtained.

[0044] In the embodiments of the present specification, the actuator response delay of the target vehicle is described as a structure in series of the instruction response delay and the output stabilization delay, the actuator delay model is built, the lateral tracking error model is added to the actuator delay model, the tracking error model is augmented and converted into a non-delay tracking error model, then the state quantity error is calculated based on the current state quantity and the preview point expected state quantity, the lateral control compensation quantity is solved, the target path is tracked through the lateral prediction delay compensation strategy, the influence of the actuator delay on the lateral tracking target path is reduced, and the accuracy of tracking the target path under the actuator response delay is improved.

[0045] In an optional embodiment of the present embodiment, the instruction response delay and the output stabilization delay are generated based on the actuator response delay of the target vehicle, the instruction response delay and the output stabilization delay are connected in series to obtain the actuator delay model, including: The instruction response delay is generated based on the vehicle steering angle at the current time and the steering delay time, and the output stabilization delay is generated based on the steering delay time and the value of the steering angle command issued by the controller after the response delay time; The instruction response delay and the output stabilization delay are connected in series to obtain an initialization delay model; The steering angle sequence after the instruction response delay is discretized is determined based on the response delay time and the discrete time step; The initialization delay model is discretized based on the discretized steering angle sequence to obtain the actuator delay model.

[0046] In actual implementation, in the vehicle system, there is a certain time delay from the issuance of the steering instruction to the actual generation of the steering effect, which is the steering delay time. Based on the current vehicle steering angle and the steering delay time, the instruction response delay can be constructed. The change of the output stable delay output does not immediately follow the change of the input, but presents a gradually changing characteristic, and the output stable delay can be generated based on the steering delay time and the value of the steering angle command issued by the controller after the response delay time. That is, after the response delay time, the response of the steering angle command will also be affected by a first-order inertia system, so that the steering effect will not reach the ideal state instantaneously, but will have a transition process. The instruction response delay and the output stable delay generated in the foregoing are connected in series, that is, the two parts act on the input signal in turn, first through the response delay and then through the first-order inertia delay, that is, the initialization delay model is obtained, which comprehensively considers the response delay and the first-order inertia delay characteristics.

[0047] In an actual control system, continuous systems need to be discretized for computer processing and control. The response delay time is a continuous time quantity, that is, a continuous time, and the discrete time step is a fixed time interval set in the discretization process. According to the response delay time and the discrete time step, the steering angle sequence after the discretization of the instruction response delay can be determined, and the initialization delay model is discretized based on the discretized steering angle sequence determined in the foregoing. Discretization processing refers to converting the continuous initialization delay model into a discrete difference equation form, so that the control system can be analyzed and controlled at discrete time points. After discretization processing, the final actuator delay model can be obtained, which can be used to simulate and analyze the delay characteristics of the vehicle actuator in actual work, and provide a basis for the control and optimization of the vehicle.

[0048] Specifically, the response delay of the steering system actuator can be described as a structure in which the instruction response delay and the output stable delay are connected in series, and an initialization delay model is obtained. The relationship expression of the initialization delay model is as follows formula (1): (1) Wherein, is the steering angle change rate of the tractor; is the steering delay time; is the current vehicle steering angle; t represents the current time under continuous time; is the response delay time in the delay model; is the value of the steering angle command issued by the controller after the response delay time.

[0049] It should be noted that the above steering delay time can be obtained based on the calculation of the driving mileage accumulation, the driving environment, the actuator working wear condition, etc., or obtained based on the experience configuration, considering the different actuator delay conditions, realizing the control compensation of vehicles with different delay degrees, reducing the workload of calibration debugging, and improving the trajectory tracking accuracy and robustness under higher delay response. In addition, the current vehicle steering angle, the response delay time in the delay model, and the value of the steering angle command after the response delay time issued by the controller can be obtained by the sensors on the target vehicle.

[0050] The instruction response delay in the calculation delay response occupies a step in the discrete system, and the steering angle sequence after the instruction response delay is discretized is calculated through the following formula (2): (2) Among them, is the response delay time in the delay model, that is, the pure lag time, is the discrete time step, is the discretized steering angle sequence of the output.

[0051] In order to apply the steering actuator response delay system to the digital discrete system, it can be discretized in the forward Euler way, or the initialization delay model can be discretized to obtain the actuator delay model, which is a discretization expression as shown in the following formula (3): (3) Among them, k represents the time after discretization; represents the steering angle control amount of the towing vehicle at the k+1 time after discretization; represents the steering angle control amount of the towing vehicle at the k time; represents the value of the steering angle command after the response delay time after discretization.

[0052] It should be noted that the steering angle control amount sequence in the delay time can be obtained by iterative calculation of the actuator delay model. Specifically, according to the steering angle control amount before the delay time, the steering angle control amount after one step of response delay and the gain of first-order inertia is calculated, and the steering angle control amount after one step is calculated. The steering angle control amount sequence in the whole delay time is calculated step by step.

[0053] In the embodiments of the present specification, the response delay of the steering system actuator can be described as a structure in series of an instruction response delay and an output stabilization delay, an initialization delay model is obtained, the steering angle sequence occupied by the instruction response delay in the discrete system is calculated, the initialization delay model is discretized based on the discretized steering angle sequence, an actuator delay model is obtained, the response delay of the steering system actuator is considered in the lateral prediction delay compensation strategy, and the actuator delay model is built to provide a basis for subsequent lateral tracking target path, and the response delay of the steering system actuator is compensated.

[0054] In an optional embodiment of the present embodiment, a lateral tracking error model is built based on the control quantity of the target vehicle and the state quantity of the target vehicle, including: determining the linear discretized state matrix and the control matrix; building the lateral tracking error model based on the state matrix and the state quantity at the current time, the control matrix and the control quantity at the current time, and the linear discretized tail term matrix, wherein the control quantity at the current time is determined based on the actuator delay model.

[0055] In actual implementation, in the control system, the mathematical model of the continuous time system is usually described by a differential equation, but when numerical calculation is performed in the computer, the continuous time model needs to be converted into a discrete time model, and this process is discretization. Linear discretization is a discretization process for a linear system.

[0056] In the state space model, the state matrix describes the evolution law of the system state over time, and the control matrix describes the influence of the control input on the system state. Determining the linear discretized state matrix and the control matrix is to obtain the relationship matrix between the system state and the control input under the discrete time. The tail term matrix of linear discretization contains some additional terms generated in the discretization process, which reflects the approximation error and other information after the discretization of the continuous system.

[0057] It should be noted that the discretized state matrix and the control matrix can be determined first, and then the lateral tracking error model is constructed using these matrices, the state quantity and the control quantity at the current time, and the tail term matrix, so as to more accurately compensate for the delay error in the process of tracking the target path.

[0058] Specifically, the tracking error model of the target vehicle can be linearized using the first-order Taylor expansion, and after forward Euler discretization, it is expressed in the form of a state space equation, that is, the built lateral tracking error model is expressed as the following formula (4): (4) wherein, is the state quantity at k+1 time; , These are the state variables and control variables at time k, respectively; , These are the state matrix and control matrix after linear discretization of the lateral tracking error equation, respectively. These are the Jacobian matrices in the first-order Taylor expansion; This is the tail term matrix after the linear discretization of the lateral tracking error equation.

[0059] In one optional implementation of this embodiment, the target vehicle includes a tractor and a trailer; the state variables of the target vehicle in the lateral tracking error model include the lateral error of the tractor, the derivative of the lateral error, the heading angle error of the tractor, the derivative of the heading angle error, the articulation angle between the tractor and the trailer, and the derivative of the articulation angle; the control variables of the target vehicle in the lateral tracking error model include the steering angle of the tractor's front wheels. By comprehensively considering multiple state variables, the compensation effect of lateral tracking of the target path is improved, ensuring the accuracy and robustness of trajectory tracking. In other words, Let k be the state quantity at time k, i.e. ,in, This indicates the lateral error of the tractor unit. This represents the derivative of the lateral error. This indicates the heading angle error of the tractor unit. This represents the derivative of the heading angle error. Indicates the articulation angle between the tractor and the trailer. This represents the derivative of the hinge angle.

[0060] in addition, Let k be the control variable at time k. This control variable can be the steering angle of the front wheels of the tractor. This control variable can be determined based on the actuator delay model, i.e. = .

[0061] In the embodiments of this specification, a lateral tracking error model can be built based on the state matrix and the state variables at the current time, the control matrix and the control variables at the current time, and the tail term matrix after linear discretization. This allows the tracking error model to be subsequently augmented and converted into a delay-free tracking error model to compensate for the actuator delay of the target vehicle. By implementing a lateral prediction delay compensation strategy, the target path can be tracked, reducing the impact of actuator delay on the lateral tracking target path.

[0062] In one optional implementation of this embodiment, the actuator delay model is added to the lateral tracking error model to augment and obtain a delay-free tracking error model, including: The state-space equation for the next time step is determined based on the lateral tracking error model. The control vectors in the delay period are added to the state space equation at the next time point, the state space equation at the next time point is augmented, the augmented state space equation without delay is obtained, and the augmented state space equation without delay is taken as the tracking error model without delay, wherein the control vectors in the delay period are determined based on the actuator delay model.

[0063] It should be noted that in the lateral delay compensation strategy, the steering angle control quantity sequence in the delay time can be added to the tracking error model to be augmented to a tracking error model without delay, and the steering angle control quantity sequence in the delay time can be determined based on the actuator delay model.

[0064] In actual implementation, the control quantity of the lateral tracking error model is the front wheel steering angle of the towing vehicle. The augmented state quantity in the tracking error model without delay is the steering angle control quantity sequence of the towing vehicle in the delay time period.

[0065] Specifically, the steering angle control quantity at the k+1 time point can be taken as a new state quantity and added to the state space equation at the next time point determined based on the lateral tracking error model to obtain a new state space equation, as shown in the following formula (5): (5) Further, the control quantities in the delay time period can be added to the state space equation , , that is, the steering angle control quantity sequence determined based on the actuator delay model , , …, , the steering angle control quantity sequence is added to the state vector , and the entire state space equation is augmented, and the expression of the tracking error model without delay is shown in the following formula (6): (6) Further, the tracking error model without delay shown in the above formula (6) can be expressed in the form of a new state space equation as shown in the following formula (7): (7) wherein, is the augmented state vector without delay at the k+1 time point; , , is the augmented state vector without delay, the control vector and the tail term at the k time point.

[0066] In the embodiments of the present specification, the augmented state space equation is used by using the tracking error model and the actuator delay model to compensate for the response error of the actuator when tracking the target path in the lateral direction, so as to ensure the accuracy and robustness of the trajectory tracking.

[0067] In the lateral delay compensation strategy in the embodiments of the present specification, the state vector of the lateral tracking error model is augmented, and the control sequence in the delay time is taken as part of the augmented state vector. Of course, in actual implementation, the lateral delay compensation strategy can also not augment the state vector, but directly solve the subsequent MPC of the lateral tracking error model to output the lateral control compensation amount, and the present specification is not limited thereto.

[0068] In an optional embodiment of the present embodiment, the state quantity error is taken as the input of the non-delay tracking error model, the non-delay tracking error model is solved to obtain the lateral control compensation amount, which comprises: The state quantity error is taken as the input of the non-delay tracking error model to construct a quadratic objective function. The optimal target control quantity sequence of the quadratic objective function is solved. The first control quantity in the target control quantity sequence is taken as the lateral control compensation amount.

[0069] It should be noted that in the lateral delay compensation strategy, the state quantity error between the current state quantity of the target vehicle and the expected state quantity of the preview point on the target path can be taken as the input of the non-delay tracking error model to construct a quadratic objective function, and the OSQP (Operator Splitting Quadratic Program) library is used to solve the objective function to obtain the optimal target control quantity sequence, and the first control quantity is taken as the lateral control compensation amount output to track the target trajectory. The OSQP is a high-efficiency numerical optimization solver for solving convex quadratic programming (QP) and quadratic cone programming (SOCP), which adopts a first-order operator splitting method (such as the ADMM algorithm), is suitable for medium-large-scale optimization problems, and performs well in embedded systems and real-time applications.

[0070] In actual implementation, the state quantity error in the lateral delay compensation strategy can include the tractor lateral error, the lateral error derivative, the tractor heading angle error, the heading angle error derivative, the hinged angle error, and the hinged angle derivative error, which can be taken as the input of the non-delay tracking error model to construct the objective function of the MPC controller, and the optimal target control quantity sequence is obtained by solving the objective function in the form of a standard quadratic form.

[0071] Specifically, the prediction time domain of the MPC controller is , and the state quantity error in the time domain is represented by the following formula (8): (8) wherein, , , , , .

[0072] In actual implementation, according to the current state quantity of the target vehicle and the expected state quantity of the preview point on the target path, a state quantity error is calculated, and a quadratic objective function is constructed by taking the state quantity error as the input of the no-delay tracking error model, as shown in the following formulas (9) and (10): (9) (10) wherein, is the output value in the prediction time domain, is the input value in the prediction time domain, , is a weight coefficient matrix, , is the upper and lower bound of the control quantity constraint, , is the upper and lower bound of the control increment constraint.

[0073] In order to solve the above quadratic objective function to obtain the optimal target control quantity sequence, it is necessary to convert it into a standard quadratic form, as shown in the following formula (11): (11) wherein, , is a coefficient matrix; is the optimal solution of the objective function.

[0074] In specific implementation, the OSQP solver can be used to solve the standard quadratic objective function shown in the above formula (11) to obtain the optimal control sequence of the objective function, that is, the target control quantity sequence, and the first control quantity in the target control quantity sequence is taken as the lateral control compensation quantity output, which is used for lateral tracking of the target path.

[0075] Figure 3 is a flowchart of a lateral prediction delay compensation strategy provided by an embodiment of the present specification for outputting a lateral control compensation quantity, as shown in Figure 3, based on the state quantity error between the current state quantity of the target vehicle and the expected state quantity of the preview point on the target path, the state quantity error, the target control quantity sequence output in the last iteration, the control quantity actually output by the controller, etc. are taken as the current inputs of the no-delay tracking error model, a quadratic objective function of the MPC controller is constructed, the optimal target control quantity sequence is obtained by solving the objective function, and the first control quantity in the target control quantity sequence is output as the lateral control compensation quantity to the actuator for iteration.

[0076] In the embodiments of the present specification, the state quantity error between the current state quantity of the target vehicle and the expected state quantity of the preview point on the target path can be taken as the input of the no-delay tracking error model, a quadratic objective function is constructed, the optimal solution of the objective function is solved to obtain a target control quantity sequence, and the first control quantity in the target control quantity sequence is output as a lateral control compensation quantity to realize tracking of the target trajectory and compensate for the response delay of the actuator, thereby increasing the adaptability of the control compensation effect, reducing the workload of calibration and debugging, improving the lateral path tracking precision and robustness under a higher delay response, and optimizing the tracking effect.

[0077] Step 106: determining a longitudinal motion quantity compensation gain based on the motion quantity error of the target vehicle, and determining a longitudinal compensated target acceleration under the longitudinal motion quantity compensation gain, wherein the longitudinal motion quantity compensation gain refers to a coefficient for compensating the motion quantity error, and the longitudinal compensated target acceleration is used for tracking the target speed.

[0078] Specifically, the motion quantity error of the target vehicle refers to an error caused by the output control delay when the target vehicle tracks the target speed. The motion quantity error can include a speed error, a position error, etc. The longitudinal motion quantity compensation gain refers to a coefficient for compensating the motion quantity error. By adjusting this gain, the vehicle can better cope with the influence of the longitudinal control delay. The longitudinal compensated target acceleration is the target acceleration calculated after considering the motion quantity error and determining the compensation gain. The purpose is to enable the actual speed of the vehicle to more accurately track the target speed.

[0079] In actual implementation, the longitudinal motion quantity compensation gain can be first determined according to the motion quantity error of the target vehicle, and then the longitudinal compensated target acceleration is determined according to the compensation gain. The role of this compensated target acceleration is to enable the vehicle to better track the target speed and reduce the tracking error caused by the influence of the output control delay when tracking the target speed.

[0080] It should be noted that the upstream planning module issues a target trajectory, and the longitudinal control can track the target speed in the target trajectory. In the embodiments of the present specification, the required acceleration control quantity is calculated according to the current motion quantity of the target vehicle, and the acceleration is not directly sent to the actuator, but the motion quantity can be updated through the kinematic model, and the acceleration control quantity is calculated by using the updated motion quantity, and the future motion quantity after the delay time is obtained by iteration. By the future motion quantity and the expected motion quantity of the preview point on the target path, the predicted motion quantity error after the delay time is calculated, and the longitudinal motion quantity compensation gain is calculated based on the predicted motion quantity error through the double-loop PID controller of the longitudinal prediction compensation strategy, and the compensated acceleration control quantity is output to the acceleration and braking actuator, so as to realize the tracking of the target speed and reduce the delay effect.

[0081] In an optional embodiment of the present embodiment, the current motion quantity includes the current position coordinates and the current vehicle speed of the towing vehicle, and the predicted motion quantity includes the predicted position coordinates and the predicted vehicle speed; based on the motion quantity error of the target vehicle, the longitudinal motion quantity compensation gain is determined, and before the target acceleration after the longitudinal compensation is determined under the longitudinal motion quantity compensation gain, the method further includes: determining an initial acceleration based on the current position coordinates and the current vehicle speed of the target vehicle; inputting the current position coordinates and the current vehicle speed of the target vehicle, and the initial acceleration into the kinematic model to predict the predicted position coordinates and the predicted vehicle speed of the next step; determining the position error and the vehicle speed error of the target vehicle in the next step based on the predicted position coordinates and the predicted vehicle speed of the next step, and the expected position coordinates and the expected vehicle speed of the preview point.

[0082] Specifically, the state quantity of the vehicle in the longitudinal control tracking is the coordinate position and the vehicle speed of the towing vehicle, and the control quantity is the acceleration of the towing vehicle.

[0083] In actual implementation, the longitudinal prediction delay compensation strategy can include a motion quantity prediction module and a double-loop PID controller. Specifically, the initial acceleration can be determined according to the current vehicle speed and the current position coordinates, the current position coordinates and the current vehicle speed of the target vehicle, and the initial acceleration are input into the kinematic model to predict the predicted position coordinates and the predicted vehicle speed of the next step, and then the position error and the vehicle speed error of the target vehicle in the next step are determined. The kinematic model is a mathematical model for describing the motion state and motion law of an object, which is mainly used to determine the relationship between the position, speed, acceleration and other motion parameters of the target vehicle in space and time, without considering the force factors causing the motion of the object.

[0084] Specifically, the acceleration control amount can be calculated according to the current vehicle speed and the current position coordinate, and the predicted vehicle speed and the predicted position of the next step under the action of the acceleration can be calculated by using the kinematic model, as shown in the following formula (12). (12) wherein, , is the position and speed of the towing vehicle at the current time in the Frenet coordinate system. , , is the position, speed and initial acceleration of the towing vehicle at the last time in the Frenet coordinate system.

[0085] The Frenet coordinate system, also known as the Frenet-Serret coordinate system, is a coordinate system widely used in the fields of curve research and motion planning. In the Frenet coordinate system, a reference curve is defined based on the reference curve, and there are usually two main coordinate axes: one is the coordinate axis along the tangent direction of the reference curve (tangent axis), and the other is the coordinate axis perpendicular to the tangent direction and pointing to the inside of the curve (normal axis). Such a coordinate system is very convenient for describing the motion of an object along a curve. For example, in the field of autonomous driving, it is used to describe the position and motion state of a vehicle relative to the center line of a road; in robot path planning, the Frenet coordinate system can also be used to plan the motion trajectory of a robot along a specific curve.

[0086] Figure 4 is a flowchart of a motion amount prediction provided by an embodiment of the present specification, referring to Figure 4 , the current position coordinate and the current speed of the target vehicle are obtained, and the expected position coordinate and the expected speed of the preview point on the target path planned by the planning module are output. The initial acceleration is determined based on the current position coordinate and the current speed of the target vehicle, and the predicted position coordinate and the predicted speed of the next step are predicted based on the current position coordinate and the current speed of the target vehicle, the initial acceleration, and the kinematic model. The step length is determined based on whether the step length reaches the current control period (i.e. ), if not, the predicted position coordinate and the predicted speed of the next step are iteratively predicted based on the kinematic model; if yes, the predicted position coordinate and the predicted speed are output, and compared with the expected position coordinate and the expected speed of the preview point on the target path to determine the position error and the speed error, i.e. the motion amount error.

[0087] In the longitudinal delay compensation strategy in the embodiments of the present specification, the motion amount of the towing vehicle can include position coordinates, vehicle speed, acceleration, etc., and the motion amount is taken as the input of the kinematic model to iteratively calculate the predicted future time position and vehicle speed, so as to consider the response delay problem of longitudinal speed tracking, increase the adaptability of the control compensation effect of the longitudinal tracking target speed, reduce the workload of calibration debugging, ensure the effect of correcting the delay response, and solve the delay problem of the semi-trailer longitudinal tracking control actuator.

[0088] In an optional embodiment of the present embodiment, the motion amount error includes vehicle speed error and position error; based on the motion amount error of the target vehicle, a longitudinal motion amount compensation gain is determined, and a longitudinal compensated target acceleration is determined under the longitudinal motion amount compensation gain, including: Based on the vehicle speed error and the position error in the motion amount error, a position compensation gain and a speed compensation gain are determined; Based on the position compensation gain and the speed compensation gain, a longitudinal compensated target acceleration is determined.

[0089] In actual implementation, the motion amount error includes vehicle speed error and position error, and a double-loop PID controller is used to respectively perform PID control on the vehicle speed error and the position error to calculate the position compensation gain and the speed compensation gain. The double-loop PID controller is a controller combining two PID (proportional-integral-derivative) control loops. The PID controller calculates the motion amount error and processes the motion amount error according to three parameters of proportion (P), integral (I), and derivative (D) to output the position compensation gain and the speed compensation gain. The proportion term is used for rapid response to error, the integral term is used for eliminating steady-state error, and the derivative term is used for predicting the trend of error change to make adjustments in advance.

[0090] It should be noted that in the longitudinal delay compensation strategy, the current position coordinates, the current vehicle speed, and the initial acceleration of the towing vehicle can be taken as the input of the kinematic model to calculate the predicted position coordinates and the predicted vehicle speed of the next step, and the predicted position coordinates and the predicted vehicle speed of the future time are iterated in turn. The position error and the vehicle speed error between the predicted position coordinates and the predicted vehicle speed and the expected position coordinates and the expected vehicle speed of the preview point are taken as the input of the double-loop PID, and the acceleration control amount of the position compensation gain and the acceleration control amount of the speed compensation gain output by the double-loop PID are obtained, so as to obtain the longitudinal compensated target acceleration.

[0091] Specifically, the predicted vehicle speed and the predicted position coordinates after the response delay time can be recursively calculated, and the vehicle speed error and the position error between the predicted vehicle speed and the predicted position coordinates and the expected vehicle speed and the expected position coordinates of the preview point are used for PID control to calculate the position compensation gain and the speed compensation gain, as shown in the following formula (13): (13) wherein, is the current time acceleration control amount, that is, the target acceleration after longitudinal compensation; is the current time motion amount error (including vehicle speed error, position error); is the motion amount error of the last time; are the proportional coefficient, the differential coefficient and the integral coefficient of the controller, respectively.

[0092] In the embodiments of the present specification, the state quantity prediction module and the double-loop PID controller are used to output the acceleration control compensation amount, and the longitudinal prediction compensation strategy is used to track the target speed under different actuator delay conditions. The actuator output delay is effectively compensated, the accuracy and robustness of tracking the expected trajectory are improved, and the debugging and calibration workload of different vehicle control delay compensation is reduced.

[0093] Of course, in actual implementation, the double-loop PID controller used in the longitudinal delay compensation strategy can also be replaced by other controllers, such as LQR controller, MPC controller and sliding film controller. According to the debugging effect and calibration workload in longitudinal speed tracking, the corresponding controller adjustment can be performed, and the embodiments of the present specification are not limited in this regard.

[0094] It should be noted that different delay compensation strategies are used in the lateral and longitudinal tracking control, and the MPC controller and the double-loop PID controller are used to calculate the control compensation amount, which increases the adaptability of the control compensation effect of vehicles with different delay degrees, reduces the workload of calibration and debugging, realizes the semi-trailer trajectory tracking control considering the response delay of the actuator, and improves the trajectory tracking accuracy and robustness under high delay response.

[0095] Step 108: outputting the lateral control compensation amount and the longitudinal compensated target acceleration to the actuator of the target vehicle, so that the target vehicle tracks the target trajectory.

[0096] In actual implementation, the lateral control compensation amount can realize delay compensation when tracking the target path in the lateral direction, and the longitudinal compensated target acceleration can realize delay compensation when tracking the target speed in the longitudinal direction. Outputting the lateral control compensation amount and the longitudinal compensated target acceleration to the actuator of the target vehicle can make the target vehicle track the target trajectory.

[0097] Specifically, the lateral control compensation amount can solve the problem of delay in tracking the target path in the lateral direction. By introducing this compensation amount, the adjustment of the lateral control of the target vehicle can be compensated for, thereby reducing the delay in lateral tracking and enabling the vehicle to travel more accurately along the target path. In longitudinal tracking of the target speed, there is also a response delay. The target acceleration after longitudinal compensation is calculated based on the original target acceleration, taking into account the system delay factor. By using this compensated target acceleration to control the vehicle, the delay in longitudinal speed tracking can be reduced, and the actual speed of the vehicle can be closer to the target speed.

[0098] In actual implementation, the lateral control compensation amount and the target acceleration after longitudinal compensation can be output to the actuators (such as steering systems, throttle and brake systems, etc.) of the target vehicle. The actuators perform corresponding operations (such as adjusting the steering wheel angle, controlling the throttle opening or brake force) according to these instructions, so that the target vehicle can more accurately track the target trajectory and target speed, and the safety and comfort of the vehicle can be improved. Specifically, the lateral control compensation amount and the target acceleration after longitudinal compensation can be output to different actuators of the target vehicle, such as the lateral control compensation amount can be output to the steering actuator, and the target acceleration after longitudinal compensation can be output to the power actuator of the target vehicle, to realize delay compensation in the lateral and longitudinal directions and control the target vehicle to track the target trajectory. Of course, in actual implementation, the lateral control compensation amount and the target acceleration after longitudinal compensation can also be output to the controller of the target vehicle, which controls the steering actuator and the power actuator of the target vehicle to realize delay compensation in the lateral and longitudinal directions and control the target vehicle to track the target trajectory. The embodiments of the present specification do not limit this.

[0099] One embodiment of the present specification provides a trajectory tracking control method, which can comprehensively consider the response delay of the actuators of the target vehicle, the state quantity and control quantity of the target vehicle, obtain a lateral control compensation amount, and realize tracking of the target path through lateral delay compensation. In addition, the longitudinal motion quantity compensation gain can be determined by comprehensively considering the motion quantity error of the target vehicle, and the target acceleration after longitudinal compensation can be obtained, and tracking of the target speed can be realized through longitudinal delay compensation. The target vehicle is controlled to track the target trajectory based on the lateral control compensation amount and the target acceleration after longitudinal compensation, and the target vehicle is controlled to track the target trajectory through comprehensive lateral and longitudinal delay compensation. In this way, the response delay of the actuators is effectively compensated in the lateral and longitudinal directions, the accuracy and robustness of tracking the target trajectory under the response delay of the actuators are improved, and the debugging and calibration workload of different vehicle control delay compensation is reduced.

[0100] The following will be described in conjunction with the accompanying drawings Figure 5 The application of the trajectory tracking control method provided by the present specification in the trajectory tracking control scene of a semitrailer will be described in further detail. Among them, Figure 5A schematic diagram of a processing procedure of a trajectory tracking control method provided by an embodiment of the present specification is shown as follows. Figure 5 As shown, the planning module in the autonomous driving or intelligent vehicle system can output a target trajectory to be tracked by the semitrailer, the target trajectory including a target path and a target speed. A lateral prediction compensation strategy is used to track the target path, the lateral prediction compensation strategy including a lateral tracking error model, an actuator delay model, adding the actuator delay model to the lateral tracking error model, augmenting to obtain a no-delay tracking error model, building a target function of an MPC controller based on the no-delay tracking error model, solving the target function to obtain an optimal target control quantity sequence, and outputting the first control quantity in the target control quantity sequence as a lateral control compensation quantity. A longitudinal prediction compensation strategy is used to track the target speed, the longitudinal prediction compensation strategy including a kinematics model, a motion quantity prediction module, and a double-loop PID controller, the double-loop PID controller being capable of calculating a position compensation gain and a speed compensation gain and outputting a target acceleration after longitudinal compensation. The lateral control compensation quantity and the target acceleration after longitudinal compensation are output to the actuator of the semitrailer, so that the semitrailer can track the target trajectory to travel.

[0101] Corresponding to the method embodiments described above, the present specification also provides trajectory tracking control device embodiments, Figure 6 A structural schematic diagram of a trajectory tracking control device provided by an embodiment of the present specification is shown as follows. Figure 6 As shown, the device includes: An acquisition module 602 configured to acquire a target trajectory to be tracked by a target vehicle, wherein the target trajectory includes a target path and a target speed; A first determination module 604 configured to determine a lateral control compensation quantity based on an actuator response delay of the target vehicle, a state quantity of the target vehicle, and a control quantity, wherein the lateral control compensation quantity is used to track the target path; A second determination module 606 configured to determine a longitudinal motion quantity compensation gain based on a motion quantity error of the target vehicle, and determine a target acceleration after longitudinal compensation under the longitudinal motion quantity compensation gain, wherein the longitudinal motion quantity compensation gain refers to a coefficient used to compensate for the motion quantity error, and the target acceleration after longitudinal compensation is used to track the target speed; An output module 608 configured to output the lateral control compensation quantity and the target acceleration after longitudinal compensation to an actuator of the target vehicle, so that the target vehicle tracks the target trajectory.

[0102] Optionally, the first determination module 604 is further configured to: generate an instruction response delay and an output stabilization delay based on the actuator response delay of the target vehicle, and obtain the actuator delay model by connecting the instruction response delay and the output stabilization delay in series; a lateral tracking error model is built based on the control quantity of the target vehicle and the state quantity of the target vehicle; an actuator delay model is added to the lateral tracking error model to obtain an augmented non-delay tracking error model; a state quantity error is calculated according to the current state quantity of the target vehicle and the expected state quantity of the preview point on the target path, the state quantity error is taken as an input of the non-delay tracking error model, and the non-delay tracking error model is solved to obtain a lateral control compensation quantity.

[0103] Optionally, the first determining module 604 is further configured to: an instruction response delay is generated based on the vehicle steering angle at the current time and the steering delay time, and an output stabilization delay is generated based on the steering delay time and the value of the steering angle command issued by the controller after the response delay time; the instruction response delay and the output stabilization delay are connected in series to obtain an initialization delay model; a steering angle sequence after discretization of the instruction response delay is determined based on the response delay time and the discrete time step; the initialization delay model is discretized based on the discretized steering angle sequence to obtain an actuator delay model.

[0104] Optionally, the first determining module 604 is further configured to: a state matrix and a control matrix after linear discretization are determined; a lateral tracking error model is built based on the state matrix, the state quantity at the current time, the control matrix, the control quantity at the current time, and the tail term matrix after linear discretization, wherein the control quantity at the current time is determined based on the actuator delay model.

[0105] Optionally, the first determining module 604 is further configured to: a state space equation at the next time is determined based on the lateral tracking error model; each control vector in a delay time period is added to the state space equation at the next time to augment the state space equation at the next time, and an augmented non-delay state space equation is obtained, which is taken as a non-delay tracking error model, wherein each control vector in the delay time period is determined based on the actuator delay model.

[0106] Optionally, the target vehicle includes a tractor and a trailer; the state quantity of the target vehicle in the lateral tracking error model includes a tractor lateral error, a lateral error derivative, a heading angle error of the tractor, a heading angle error derivative, an articulation angle between the tractor and the trailer, and an articulation angle derivative; and the control quantity of the target vehicle in the lateral tracking error model includes a front wheel steering angle of the tractor.

[0107] Optionally, the first determining module 604 is further configured to: construct a quadratic objective function with the state error as the input of the no-delay tracking error model; solve the optimal target control quantity sequence of the quadratic objective function; take the first control quantity in the target control quantity sequence as the lateral control compensation quantity.

[0108] Optionally, the current motion quantity includes a current position coordinate and a current speed of the towing vehicle, and the predicted motion quantity includes a predicted position coordinate and a predicted speed; the second determining module 606 is further configured to: determine an initial acceleration based on the current position coordinate and the current speed of the target vehicle; input the current position coordinate and the current speed of the target vehicle and the initial acceleration into the kinematic model to predict the predicted position coordinate and the predicted speed of the next step; determine the position error and the speed error of the target vehicle in the next step based on the predicted position coordinate and the predicted speed of the next step and the expected position coordinate and the expected speed of the preview point.

[0109] Optionally, the motion quantity error includes the speed error and the position error; the second determining module 606 is further configured to: determine the position compensation gain and the speed compensation gain based on the speed error and the position error in the motion quantity error; determine the target acceleration after longitudinal compensation based on the position compensation gain and the speed compensation gain.

[0110] One embodiment of the present specification provides a trajectory tracking control device, which can comprehensively consider the actuator response delay of a target vehicle, the state quantity and the control quantity of the target vehicle, obtain a lateral control compensation quantity, and realize tracking of a target path through lateral delay compensation; and can comprehensively consider the motion quantity error of the target vehicle to determine a longitudinal motion quantity compensation gain, obtain a target acceleration after longitudinal compensation, and realize tracking of a target speed through longitudinal delay compensation; control the target vehicle to track a target trajectory based on the lateral control compensation quantity and the target acceleration after longitudinal compensation, and comprehensively control the target vehicle to track the target trajectory through lateral and longitudinal delay compensation. In this way, the response delay of the actuator is effectively compensated in the lateral and longitudinal directions, the accuracy and robustness of tracking the target trajectory under the response delay of the actuator are improved, and the debugging and calibration workload of delay compensation of different vehicles is reduced.

[0111] The above is a schematic scheme of the trajectory tracking control device of the embodiment. It should be noted that the technical scheme of the trajectory tracking control device is the same as the technical scheme of the trajectory tracking control method described above, and the details of the technical scheme of the trajectory tracking control device that are not described in detail can be referred to the description of the technical scheme of the trajectory tracking control method.

[0112] Figure 7 A structural block diagram of a computing device according to one embodiment of the present specification is shown. The components of the computing device 700 include, but are not limited to, a memory 710 and a processor 720. The processor 720 is connected to the memory 710 through a bus 730, and a database 750 is used to save data.

[0113] The computing device 700 also includes an access device 740, which enables the computing device 700 to communicate via one or more networks 760. Examples of these networks include a public switched telephone network (PSTN), a local area network (LAN), a wide area network (WAN), a personal area network (PAN), or a combination of communication networks such as the Internet. The access device 740 can include one or more of any type of network interface (e.g., a network interface card (NIC)), wired or wireless, such as an IEEE 802.11 wireless local area network (WLAN) wireless interface, a Worldwide Interoperability for Microwave Access (Wi-MAX) interface, an Ethernet interface, a Universal Serial Bus (USB) interface, a cellular network interface, a Bluetooth interface, a near field communication (NFC).

[0114] In one embodiment of the present specification, the above-mentioned components of the computing device 700 and other components not shown in the Figure 7 may be connected to each other, for example, through a bus. It should be understood that Figure 7 The structural block diagram of the computing device shown is only for the purpose of example, and is not a limitation on the scope of the present specification. Those skilled in the art can add or replace other components as needed.

[0115] The computing device 700 can be any type of stationary or mobile computing device, including a mobile computer or mobile computing device (e.g., a tablet computer, a personal digital assistant, a laptop computer, a notebook computer, a netbook, etc.), a mobile phone (e.g., a smartphone), a wearable computing device (e.g., a smart watch, smart glasses, etc.), or other type of mobile device, or a stationary computing device such as a desktop computer or a personal computer (PC). The computing device 700 can also be a mobile or stationary server.

[0116] The processor 720 is configured to execute computer-executable instructions to perform the steps of the trajectory tracking control method described above.

[0117] The above is a schematic solution of the computing device of the embodiment. It should be noted that the technical solution of the computing device and the technical solution of the trajectory tracking control method described above belong to the same concept, and the details of the technical solution of the computing device that are not described in detail can be referred to the description of the technical solution of the trajectory tracking control method.

[0118] An embodiment of the present specification also provides a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, implement the steps of the trajectory tracking control method described above.

[0119] The above is a schematic solution of the computer-readable storage medium of the embodiment. It should be noted that the technical solution of the storage medium and the technical solution of the trajectory tracking control method described above belong to the same concept, and the details of the technical solution of the storage medium that are not described in detail can be referred to the description of the technical solution of the trajectory tracking control method.

[0120] An embodiment of the present specification also provides a computer program, which, when executed in a computer, causes the computer to perform the steps of the trajectory tracking control method described above.

[0121] The above is a schematic solution of the computer program of the embodiment. It should be noted that the technical solution of the computer program and the technical solution of the trajectory tracking control method described above belong to the same concept, and the details of the technical solution of the computer program that are not described in detail can be referred to the description of the technical solution of the trajectory tracking control method.

[0122] The above-described embodiments of the application have several aspects, no single one of which is solely responsible for the application's desirable attributes. Without limiting the scope of the application as expressed by the claims which follow, some further embodiments make these aspects even more useful. Other embodiments can result in less desirable attributes.

[0123] The computer readable medium can include any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, magnetic disk, optical disk, computer memory, Read-Only Memory (ROM), Random Access Memory (RAM), electrical carrier signal, telecommunication signal, software distribution medium, and the like. It should be noted that the computer readable medium contained in the present application can be appropriately increased or decreased according to the requirements of the patent practice, for example, according to the patent practice in some areas, the computer readable medium does not include electrical carrier signals and telecommunication signals.

[0124] It should be noted that for the foregoing method embodiments, the acts described therein can be performed in a different order from the order described, and some acts can be performed in parallel with each other. In addition, those skilled in the art should know that the embodiments described in the specification are all preferred embodiments, and the acts and modules involved are not necessarily essential to the embodiments.

[0125] In the above embodiments, the description of each embodiment is focused on, and the parts not described in detail in a certain embodiment can be referred to the relevant description of other embodiments.

[0126] The above disclosed preferred embodiments of the present application are only used to help explain the present application. The alternative embodiments do not describe all the details and do not limit the application to the specific embodiments described. Obviously, according to the content of the embodiments of the present application, many modifications and changes can be made. The present application selects and describes these embodiments in order to better explain the principles and practical application of the embodiments of the present application, so that those skilled in the art can well understand and use the present application. The present application is limited by the claims and their full scope and equivalents.

Claims

1. A trajectory tracking control method characterized by, The method comprises: acquiring a target trajectory to be tracked by a target vehicle, wherein the target trajectory comprises a target path and a target speed; determining a lateral control compensation amount based on an actuator response delay of the target vehicle, a state quantity of the target vehicle, and a control quantity of the target vehicle, wherein the lateral control compensation amount is used for tracking the target path; determining a longitudinal motion quantity compensation gain based on a motion quantity error of the target vehicle, and determining a longitudinal compensated target acceleration under the longitudinal motion quantity compensation gain, wherein the longitudinal motion quantity compensation gain is a coefficient used for compensating the motion quantity error, and the longitudinal compensated target acceleration is used for tracking the target speed; outputting the lateral control compensation amount and the longitudinal compensated target acceleration to an actuator of the target vehicle, so that the target vehicle tracks the target trajectory.

2. The trajectory tracking control method according to claim 1, characterized by, The method further comprises: generating an instruction response delay and an output stabilization delay based on the actuator response delay of the target vehicle, and obtaining an actuator delay model by connecting the instruction response delay and the output stabilization delay in series; building a lateral tracking error model based on the control quantity of the target vehicle and the state quantity of the target vehicle; adding the actuator delay model to the lateral tracking error model to obtain an undelayed tracking error model by augmentation; calculating a state quantity error according to a current state quantity of the target vehicle and an expected state quantity of a preview point on the target path, taking the state quantity error as an input of the undelayed tracking error model, and solving the undelayed tracking error model to obtain the lateral control compensation amount.

3. The trajectory tracking control method according to claim 2, characterized by, The method further comprises: generating the instruction response delay based on a current vehicle steering angle and a steering delay time, and generating the output stabilization delay based on the steering delay time and a value of the steering angle command issued by the controller after the response delay time; connecting the instruction response delay and the output stabilization delay to obtain an initialization delay model; determining a steering angle sequence after discretization of the instruction response delay based on the response delay time and a discrete time step; performing discretization processing on the initialization delay model based on the steering angle sequence after discretization to obtain the actuator delay model.

4. The trajectory tracking control method according to claim 2, characterized by, The method further comprises: determining a linearly discretized state matrix and a linearly discretized control matrix; building the lateral tracking error model based on the state matrix, a current state quantity, the control matrix, a current control quantity, and a linearly discretized tail term matrix, wherein the current control quantity is determined based on the actuator delay model.

5. The trajectory tracking control method according to claim 2, characterized by, The method further comprises: determining a state space equation of a next time based on the lateral tracking error model; augmenting the state space equation of the next time by adding each control vector in a delay time period to the state space equation of the next time, to obtain an augmented state space equation without delay, and taking the augmented state space equation without delay as the non-delay tracking error model, wherein each control vector in the delay time period is determined based on the actuator delay model.

6. The trajectory tracking control method according to claim 2, characterized by, the target vehicle comprises a tractor and a trailer; the state quantity of the target vehicle in the lateral tracking error model comprises a lateral error of the tractor, a derivative of the lateral error, a heading angle error of the tractor, a derivative of the heading angle error, an articulation angle between the tractor and the trailer, and a derivative of the articulation angle; and the control quantity of the target vehicle in the lateral tracking error model comprises a front wheel steering angle of the tractor.

7. The trajectory tracking control method according to claim 2, characterized by, the state quantity error is taken as an input of the non-delay tracking error model, and the lateral control compensation quantity is obtained by solving the non-delay tracking error model, comprising: taking the state quantity error as an input of the non-delay tracking error model to construct a quadratic objective function; solving a target control quantity sequence that is optimal for the quadratic objective function; taking a first control quantity in the target control quantity sequence as the lateral control compensation quantity.

8. The trajectory tracking control method according to any one of claims 1-7, characterized by, the current motion quantity comprises a current position coordinate and a current speed of the tractor, and the predicted motion quantity comprises a predicted position coordinate and a predicted speed; and the longitudinal motion quantity compensation gain is determined based on the motion quantity error of the target vehicle, and before the target acceleration after longitudinal compensation is determined under the longitudinal motion quantity compensation gain, further comprising: determining an initial acceleration based on the current position coordinate and the current speed of the target vehicle; inputting the current position coordinate and the current speed of the target vehicle, and the initial acceleration into a kinematic model to predict a predicted position coordinate and a predicted speed of a next step; determining a position error and a speed error of the target vehicle in the next step based on the predicted position coordinate and the predicted speed of the next step, and the expected position coordinate and the expected speed of the preview point.

9. The trajectory tracking control method according to any one of claims 1 to 7, characterized by, the motion quantity error comprises a speed error and a position error; and the longitudinal motion quantity compensation gain is determined based on the motion quantity error of the target vehicle, and the target acceleration after longitudinal compensation is determined under the longitudinal motion quantity compensation gain, comprising: determining a position compensation gain and a speed compensation gain based on the speed error and the position error in the motion quantity error; determining the target acceleration after longitudinal compensation based on the position compensation gain and the speed compensation gain.

10. A trajectory tracking control device characterized by comprising: comprising: an acquisition module configured to acquire a target trajectory to be tracked by a target vehicle, wherein the target trajectory comprises a target path and a target speed; a first determination module configured to determine a lateral control compensation quantity based on an actuator response delay of the target vehicle, a state quantity and a control quantity of the target vehicle, wherein the lateral control compensation quantity is used to track the target path; The second determining module is configured to determine a longitudinal motion amount compensation gain based on the motion amount error of the target vehicle, and determine a longitudinal compensated target acceleration under the longitudinal motion amount compensation gain, wherein the longitudinal motion amount compensation gain refers to a coefficient for compensating the motion amount error, and the longitudinal compensated target acceleration is used for tracking the target speed. The output module is configured to output the lateral control compensation amount and the longitudinal compensated target acceleration to an actuator of the target vehicle, so that the target vehicle tracks the target trajectory.

11. A computing device, comprising: The trajectory tracking control method comprises the following steps: a memory and a processor; The memory is used for storing computer executable instructions, and the processor is used for executing the computer executable instructions, and the computer executable instructions realize the steps of the trajectory tracking control method according to any one of claims 1-9 when executed by the processor.

12. A computer-readable storage medium, characterized in that, The computer executable instructions stored in the memory realize the steps of the trajectory tracking control method according to any one of claims 1-9 when executed by the processor.

13. A computer program product, characterised in that, The computer program / instructions realize the steps of the trajectory tracking control method according to any one of claims 1-9 when executed by the processor.

Images