Course angle compensation determination method and device, terminal and storage medium

Abstract

The invention discloses a course angle compensation determination method and device, a terminal and a storage medium. The method comprises the following steps: acquiring a vehicle speed and a transverse position error; establishing a vehicle kinematic model based on the transverse position error and the vehicle speed; calculating an interference estimation value based on the vehicle kinematics model and Kalman filtering; and correcting the vehicle kinematics model through the interference estimation value to obtain course angle compensation. According to the method, Kalman filtering is used for replacing integration, interference (namely an interference value estimation value) at each moment is identified, so that the situation that course angle compensation lags behind an error due to integration is avoided, the influence of interference is effectively overcome in time, and the accuracy and reliability of course angle compensation are improved.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and more specifically, to a method, device, terminal, and storage medium for determining heading angle compensation. Background Art

[0002] With the advancement of science and technology, advanced computer technology, information technology, automatic control technology, and artificial intelligence technology are gradually being applied to autonomous driving technology. Vehicle motion control is one of the key technologies for autonomous driving and is also a fundamental issue and a prerequisite for the research of intelligent vehicles. Vehicle motion control is typically based on vehicle dynamics or vehicle kinematics models, and feedback control laws are derived based on various control theories. However, vehicle dynamics or vehicle kinematics models are idealized and typically linear models, unlike actual vehicles, which are nonlinear. Therefore, to improve vehicle motion control capabilities, observation of actual vehicle models is necessary.

[0003] In the actual process, there are some unknown quantities (called interference) in the actual vehicle model, and the model with interference is observed through the Lumberg observer.

[0004] However, the interference in the above observation method is obtained by integrating the rate of change, and the integration has a lag, which will cause the heading angle compensation to lag behind the error, resulting in the control being unable to overcome the influence of the interference in a timely and effective manner. Summary of the Invention

[0005] The main purpose of the present application is to provide a method, device, terminal and storage medium for determining heading angle compensation, so as to solve the problem in the related art that the heading angle compensation lags behind the error due to integration.

[0006] To achieve the above objectives, in a first aspect, the present application provides a method for determining heading angle compensation, comprising:

[0007] Get vehicle speed and lateral position error;

[0008] Based on the lateral position error and vehicle speed, a vehicle kinematic model is established;

[0009] Calculate disturbance estimates based on the vehicle kinematic model and Kalman filter;

[0010] The vehicle kinematic model is modified using the disturbance estimation value to obtain heading angle compensation.

[0011] In one possible implementation, a vehicle kinematic model is established based on the lateral position error and the vehicle speed, including:

[0012] Get the heading angle error;

[0013] Obtaining a lateral velocity based on the lateral position error;

[0014] A vehicle kinematic model is established based on lateral velocity, vehicle speed, and heading angle error.

[0015] In one possible implementation, the interference estimation value is calculated based on the vehicle kinematic model and Kalman filtering, including:

[0016] Determine the continuous-time system model based on the vehicle kinematic model;

[0017] Discretize the continuous-time system model to obtain a discrete model;

[0018] The discrete model is processed by Kalman filtering to calculate the interference estimation value.

[0019] In one possible implementation, determining a continuous-time system model based on a vehicle kinematic model includes:

[0020] Get model interference, coefficient matrix and identity matrix;

[0021] A continuous-time system model is established based on the vehicle kinematic model, model disturbance, coefficient matrix and identity matrix.

[0022] In one possible implementation, Kalman filtering is performed on the discrete model to calculate the interference estimate, including:

[0023] Based on the continuous-time system model and control input, calculate the predicted estimated state and prediction error covariance matrix at the preset time;

[0024] Determine the target estimated state quantity and target error covariance matrix at a preset time based on the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error;

[0025] Returning to the step of calculating the predicted estimated state quantity and the prediction error covariance matrix at the preset time based on the continuous-time system model and the control input, until the predicted estimated state quantity and the prediction error covariance matrix at each time within the preset time are obtained, wherein the preset time is a time within the preset time;

[0026] The predicted estimated state quantity at each moment within the preset time is used as the interference estimation value.

[0027] In one possible implementation, determining a target estimated state quantity and a target error covariance matrix at a preset time according to the predicted estimated state quantity, the prediction error covariance matrix, and the lateral position error includes:

[0028] Get the lateral position error;

[0029] Calculate the Kalman gain matrix based on the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error;

[0030] The estimated state quantity and error covariance matrix are updated based on the Kalman gain matrix to obtain the target estimated state quantity and target error covariance matrix at the preset time.

[0031] In one possible implementation, the Kalman gain matrix is ​​calculated based on the predicted estimated state quantity, the prediction error covariance matrix, and the lateral position error, including:

[0032] Get the measured noise covariance matrix;

[0033] The Kalman gain matrix is ​​calculated based on the measured noise covariance matrix, the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error.

[0034] In a second aspect, an embodiment of the present invention provides a device for determining a heading angle compensation, comprising:

[0035] An acquisition module, used to obtain vehicle speed and lateral position error;

[0036] Establishing a module for establishing a vehicle kinematic model based on lateral position error and vehicle speed;

[0037] A calculation module, used for calculating interference estimation based on a vehicle kinematic model and Kalman filtering;

[0038] The correction module is used to correct the vehicle kinematic model through the interference estimation value to obtain the heading angle compensation.

[0039] An embodiment of the present invention provides a method, device, terminal, and storage medium for determining heading angle compensation, including: first obtaining vehicle speed and lateral position error, then establishing a vehicle kinematic model based on the lateral position error and vehicle speed, and then calculating an interference estimate based on the vehicle kinematic model and Kalman filtering, thereby correcting the vehicle kinematic model using the interference estimate to obtain heading angle compensation. The present invention uses Kalman filtering instead of integration to identify interference at each moment (i.e., interference value estimate) to avoid the situation where the heading angle compensation lags behind the error due to integration, thereby timely and effectively overcoming the influence of interference and improving the accuracy and reliability of heading angle compensation. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The drawings that constitute part of this application are used to provide a further understanding of this application and make other features, objects and advantages of this application more apparent. The illustrative embodiment drawings of this application and their descriptions are used to explain this application and do not constitute an improper limitation of this application. In the drawings:

[0041] Figure 1This is a schematic structural diagram of a control system provided by an embodiment of the present invention;

[0042] Figure 2 This is a flow chart of an implementation method for determining a heading angle compensation provided by an embodiment of the present invention;

[0043] Figure 3 is a flowchart of another method for determining heading angle compensation provided by an embodiment of the present invention;

[0044] Figure 4 1 is a structural diagram of a device for determining heading angle compensation provided by an embodiment of the present invention;

[0045] Figure 5 is a schematic diagram of a terminal provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0046] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0047] The terms "first," "second," "third," "fourth," and so forth (if any) in the description and claims of the present invention and the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or sequential sequence. It should be understood that the terms used in this manner are interchangeable where appropriate, such that the embodiments of the present invention described herein can be practiced in sequences other than those illustrated or described herein.

[0048] It should be understood that in various embodiments of the present invention, the size of the sequence number of each process does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0049] It should be understood that in the present invention, "include" and "have" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or apparatus that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to these processes, methods, products or apparatuses.

[0050] It should be understood that in the present invention, "multiple" refers to two or more. "And / or" is only a description of the association relationship of associated objects, indicating that three relationships can exist. For example, and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the previous and next associated objects are in an "or" relationship. "Contains A, B and C", "Contains A, B, C" means that A, B, and C are all included, "Contains A, B or C" means that one of A, B, and C is included, and "Contains A, B and / or C" means that any one, any two, or any three of A, B, and C are included.

[0051] It should be understood that, in the present invention, "B corresponding to A," "B corresponding to A," "A corresponds to B," or "B corresponds to A" means that B is associated with A and B can be determined based on A. Determining B based on A does not mean determining B based solely on A; B can also be determined based on A and / or other information. A and B match when the similarity between A and B is greater than or equal to a preset threshold.

[0052] Depending on the context, "if" as used herein may be interpreted as "when" or "when" or "in response to determining" or "in response to detecting."

[0053] The following specific embodiments are used to describe the technical solution of the present invention in detail. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.

[0054] In order to make the purpose, technical solutions and advantages of the present invention more clear, specific embodiments will be described below with reference to the accompanying drawings.

[0055] like Figure 1 As shown, the present application provides a control system, including: a planning module, a positioning module, a control module, a CANBUS transmission module and a steering controller, wherein the control module includes feedback control and an observer.

[0056] The planning module inputs the planned trajectory information including reference position, reference heading angle, reference curvature, reference speed, etc. to the control module, and the positioning module inputs the vehicle status information including position, heading angle, turning angle, speed, acceleration, etc. to the control module.

[0057] The observer calculates the heading angle compensation and the heading angle error based on the received vehicle state information, and the feedback control calculates the steering wheel angle based on the lateral error and the corrected heading angle error (where the corrected heading angle error is equal to the sum of the heading angle compensation and the heading angle error).

[0058] The control module forwards the steering wheel angle to the steering controller via the CANBUS (Controller Area Network-BUS) transmission module, so that the steering controller controls the steering wheel to rotate according to the steering wheel angle.

[0059] like Figure 2 As shown, the present application provides a method for determining heading angle compensation, which is applied to Figure 1 The observer shown includes:

[0060] Step S201: Obtaining lateral position error and vehicle speed;

[0061] Step S202: establishing a vehicle kinematic model based on the lateral position error and vehicle speed;

[0062] The lateral position error is obtained by subtracting the lateral position of the vehicle's rear axle center from the trajectory.

[0063] To establish a vehicle kinematic model based on the lateral position error and vehicle speed, it is necessary to first obtain the heading angle error, then obtain the lateral velocity based on the lateral position error, and then establish the vehicle kinematic model based on the lateral velocity, vehicle speed, and heading angle error.

[0064] Specifically, a vehicle kinematic model considering interference is established. The vehicle kinematic model is combined with the small angle assumption to obtain the following formula (1):

[0065]

[0066] Among them, θ is the heading angle error, v is the vehicle speed (i.e. longitudinal speed), is the lateral velocity.

[0067] Step S203: Calculate an interference estimation value based on the vehicle kinematic model and Kalman filtering.

[0068] To calculate the interference value estimate based on the vehicle kinematic model and Kalman filtering, it is necessary to first determine the continuous-time system model based on the vehicle kinematic model, then discretize the continuous-time system model to obtain a discrete model, and then perform Kalman filtering on the discrete model to calculate the interference value estimate.

[0069] Among them, based on the vehicle kinematic model, the continuous-time system model is determined, including: obtaining model interference, coefficient matrix and unit matrix, and establishing the continuous-time system model based on the vehicle kinematic model, model interference, coefficient matrix and unit matrix.

[0070] Specifically, is the state quantity x(t), θ is the control quantity u(t), and y(t) is the state observation quantity. Then, the continuous-time system model can be expressed by the following formula (2):

[0071]

[0072] Among them, z(t) is the model interference, the coefficient matrix includes: A=O (zero matrix), B=v, v is the vehicle speed (i.e., the vehicle longitudinal speed), E=I, C=I, I represents the unit matrix, and t represents time.

[0073] After obtaining the continuous-time system model shown in the above formula (2), it is necessary to discretize the continuous-time system model to obtain a discrete model.

[0074] Specifically, the continuous-time system model shown in the above formula (2) is transformed to obtain the transformed continuous-time system model shown in the following formula (3):

[0075]

[0076] Among them, A d =I+AT,B d =BT,E d =ET,C d =C, T is the discrete step length, and k represents the sampling time.

[0077] Since the model contains interference z, the Kalman filter method cannot be directly applied, so the extended state method is used to transform the original model.

[0078] Assuming that the interference does not change dramatically, that is, z(k+1)≈z(k), let Then the discrete model becomes the following formula (4):

[0079]

[0080] in,

[0081] After obtaining the discrete model shown in formula (4), it is necessary to perform Kalman filtering on the discrete model to calculate the interference estimation value.

[0082] Among them, Kalman filtering is performed on the discrete model to calculate the interference value estimate, including: calculating the predicted estimated state quantity and the prediction error covariance matrix at the preset moment based on the continuous-time system model and the control input, and then determining the target estimated state quantity and the target error covariance matrix at the preset moment based on the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error, returning to execute the step of calculating the predicted estimated state quantity and the prediction error covariance matrix at the preset moment based on the continuous-time system model and the control input, until the predicted estimated state quantity and the prediction error covariance matrix at each moment in the preset time are obtained, wherein the preset moment is the moment in the preset time, and then the predicted estimated state quantity at each moment in the preset time is used as the interference value estimate.

[0083] Among them, according to the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error, the target estimated state quantity and the target error covariance matrix at the preset moment are determined, including: obtaining the measured heading angle, and then calculating the Kalman gain matrix according to the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error, and then updating the estimated state quantity and the error covariance matrix based on the Kalman gain matrix to obtain the target estimated state quantity and the target error covariance matrix at the preset moment.

[0084] The Kalman gain matrix is ​​calculated based on the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error, including: obtaining the measured noise covariance matrix, and then calculating the Kalman gain matrix based on the measured noise covariance matrix, the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error.

[0085] The specific implementation process of performing Kalman filtering on the discrete model and calculating the interference estimation value is as follows:

[0086] Step 1: Initialization

[0087] Set the initial estimated state, The initial error covariance matrix P(0)=I.

[0088] Step 2: Prediction

[0089] Starting from the moment k=1, at each moment k, the predicted estimated state quantity at the moment k is calculated based on the continuous time system model and the control input u(k-1) and the prediction error covariance matrix P - (k), as represented by the following formulas (5) and (6), respectively:

[0090]

[0091] in, is the estimated state quantity at time k-1, is the prior estimate of the control quantity at time k.

[0092]

[0093] Among them, P - (k) is the error covariance matrix, which represents the degree of trust in the current predicted state. The smaller the value, the more trust in the current predicted state. Q is the process noise covariance matrix. The smaller the value, the easier the system converges and the higher the trust in the state predicted by the model.

[0094] Step 3: Update

[0095] Obtain the lateral position error x(k) through the sensor, calculate the Kalman gain matrix g(k), and update the estimated state and the error covariance matrix P(k) to obtain the target estimated state quantity and target error covariance matrix at time k, which can be expressed by the following formulas (7) and (8) respectively:

[0096]

[0097] Among them, g(k) is the Kalman gain matrix at time k; R is the measurement noise covariance matrix, which represents the distrust of the new measurement value. The larger the value, the lower the credibility of the new measurement value.

[0098]

[0099] in, is the estimate of the state quantity at time k.

[0100]

[0101] Step 4: Continuous time update

[0102] When time k+1 is reached, steps 2 to 4 are repeated to achieve continuous state estimation and measurement update.

[0103] In summary, through the Kalman filter calculation steps, the predicted estimated state quantity at each moment can be obtained The interference estimation value at each moment can be obtained.

[0104] Step S204: Correcting the vehicle kinematic model using the interference estimation value to obtain heading angle compensation.

[0105] After obtaining the interference estimation value at each moment, the vehicle kinematic model can be modified and expressed by the following formula (9):

[0106]

[0107] Among them, v εFor minimum speed compensation, 0.1m / s is usually taken to prevent the denominator of the above formula from being zero.

[0108] The heading angle compensation Δθ is obtained by formula (9):

[0109]

[0110] It can be seen from formula (10) that the heading angle compensation observed by the vehicle kinematic model can be obtained, thereby compensating for the heading angle inaccuracy caused by errors such as camera calibration and sensor zero position deviation.

[0111] like Figure 3 As shown, the present application provides another method for determining heading angle compensation, which is applied to Figure 1 The observer shown includes:

[0112] The method for determining the heading angle compensation of the present application mainly establishes a vehicle kinematic model based on the lateral position error and vehicle speed. Since the vehicle kinematic model carries interference, the vehicle kinematic model carrying interference needs to be filtered through an extended Kalman filter model to solve the interference value of the vehicle kinematic model (i.e., the interference estimation value), and thus the heading angle compensation is calculated through the interference value.

[0113] In this embodiment, based on the lateral position error and vehicle speed, a vehicle kinematic model is established, the interference value of the vehicle kinematic model is solved, and the heading angle compensation is calculated using the interference value. For details, see steps S201 to S204 in the previous embodiment, which will not be repeated here.

[0114] An embodiment of the present invention provides a method, device, terminal, and storage medium for determining heading angle compensation, including: first obtaining vehicle speed and lateral position error, then establishing a vehicle kinematic model based on the lateral position error and vehicle speed, and then calculating an interference estimate based on the vehicle kinematic model and Kalman filtering, thereby correcting the vehicle kinematic model using the interference estimate to obtain heading angle compensation. The present invention uses Kalman filtering instead of integration to identify interference at each moment (i.e., interference value estimate) to avoid the situation where the heading angle compensation lags behind the error due to integration, thereby timely and effectively overcoming the influence of interference and improving the accuracy and reliability of heading angle compensation.

[0115] It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0116] The following are device embodiments of the present invention. For details not fully described therein, reference may be made to the corresponding method embodiments described above.

[0117] Figure 4 A schematic diagram of the structure of a heading angle compensation determination device provided by an embodiment of the present invention is shown. For ease of description, only the parts related to the embodiment of the present invention are shown. The heading angle compensation determination device includes an acquisition module 401, an establishment module 402, a calculation module 403, and a correction module 404. The details are as follows:

[0118] An acquisition module 401 is used to acquire vehicle speed and lateral position error;

[0119] Establishing module 402 for establishing a vehicle kinematic model based on the lateral position error and the vehicle speed;

[0120] A calculation module 403 is used to calculate an interference estimation value based on a vehicle kinematic model and a Kalman filter;

[0121] The correction module 404 is used to correct the vehicle kinematic model using the interference estimation value to obtain heading angle compensation.

[0122] In a possible implementation, the establishing module 402 is further configured to obtain a heading angle error;

[0123] Obtaining a lateral velocity based on the lateral position error;

[0124] A vehicle kinematic model is established based on lateral velocity, vehicle speed, and heading angle error.

[0125] In one possible implementation, the calculation module 403 is further configured to determine a continuous-time system model based on a vehicle kinematic model;

[0126] Discretize the continuous-time system model to obtain a discrete model;

[0127] The discrete model is processed by Kalman filtering to calculate the interference estimation value.

[0128] In a possible implementation, the calculation module 403 is further configured to obtain model interference, a coefficient matrix, and an identity matrix;

[0129] A continuous-time system model is established based on the vehicle kinematic model, model disturbance, coefficient matrix and identity matrix.

[0130] In one possible implementation, the calculation module 403 is further configured to calculate the predicted estimated state quantity and the prediction error covariance matrix at a preset time based on the continuous-time system model and the control input quantity;

[0131] Determine the target estimated state quantity and target error covariance matrix at a preset time based on the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error;

[0132] Returning to the step of calculating the predicted estimated state quantity and the prediction error covariance matrix at the preset time based on the continuous-time system model and the control input, until the predicted estimated state quantity and the prediction error covariance matrix at each time within the preset time are obtained, wherein the preset time is a time within the preset time;

[0133] The predicted estimated state quantity at each moment within the preset time is used as the interference estimation value.

[0134] In a possible implementation, the calculation module 403 is further configured to obtain a lateral position error;

[0135] Calculate the Kalman gain matrix based on the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error;

[0136] The estimated state quantity and error covariance matrix are updated based on the Kalman gain matrix to obtain the target estimated state quantity and target error covariance matrix at the preset time.

[0137] In a possible implementation, the calculation module 403 is further configured to obtain a measured noise covariance matrix;

[0138] The Kalman gain matrix is ​​calculated based on the measured noise covariance matrix, the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error.

[0139] An embodiment of the present invention provides a device for determining heading angle compensation, specifically configured to: first obtain vehicle speed and lateral position error; then, based on the lateral position error and vehicle speed, establish a vehicle kinematic model; and then, based on the vehicle kinematic model and a Kalman filter, calculate an interference estimate, thereby correcting the vehicle kinematic model using the interference estimate to obtain heading angle compensation. The present invention uses a Kalman filter instead of integration to identify interference at each moment (i.e., an interference estimate) to avoid the situation where the heading angle compensation lags behind the error due to integration, thereby promptly and effectively overcoming the effects of interference and improving the accuracy and reliability of heading angle compensation.

[0140] Figure 5 Schematic diagram of a terminal provided by an embodiment of the present invention. Figure 5 As shown, the terminal 5 of this embodiment includes: a processor 501, a memory 502, and a computer program 505 stored in the memory 502 and executable on the processor 501. When the processor 501 executes the computer program 505, the steps in the above-mentioned various heading angle compensation determination method embodiments are implemented, such as Figure 2 Alternatively, when the processor 501 executes the computer program 505, the functions of the modules / units in the above-mentioned embodiments of the heading angle compensation determination device are realized, such as Figure 4 Functionality of modules / units 401-404 is shown.

[0141] The present invention further provides a readable storage medium, wherein the readable storage medium stores a computer program. When the computer program is executed by a processor, the present application provides a method for determining a heading angle compensation provided in the various embodiments described above, including:

[0142] Get vehicle speed and lateral position error;

[0143] Based on the lateral position error and vehicle speed, a vehicle kinematic model is established;

[0144] Calculate disturbance estimates based on the vehicle kinematic model and Kalman filter;

[0145] The vehicle kinematic model is modified using the disturbance estimation value to obtain heading angle compensation.

[0146] In one possible implementation, a vehicle kinematic model is established based on the lateral position error and the vehicle speed, including:

[0147] Get the heading angle error;

[0148] Obtaining a lateral velocity based on the lateral position error;

[0149] A vehicle kinematic model is established based on lateral velocity, vehicle speed, and heading angle error.

[0150] In one possible implementation, the interference estimation value is calculated based on the vehicle kinematic model and Kalman filtering, including:

[0151] Determine the continuous-time system model based on the vehicle kinematic model;

[0152] Discretize the continuous-time system model to obtain a discrete model;

[0153] The discrete model is processed by Kalman filtering to calculate the interference estimation value.

[0154] In one possible implementation, determining a continuous-time system model based on a vehicle kinematic model includes:

[0155] Get model interference, coefficient matrix and identity matrix;

[0156] A continuous-time system model is established based on the vehicle kinematic model, model disturbance, coefficient matrix and identity matrix.

[0157] In one possible implementation, Kalman filtering is performed on the discrete model to calculate the interference estimate, including:

[0158] Based on the continuous-time system model and control input, calculate the predicted estimated state and prediction error covariance matrix at the preset time;

[0159] Determine the target estimated state quantity and target error covariance matrix at a preset time based on the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error;

[0160] Returning to the step of calculating the predicted estimated state quantity and the prediction error covariance matrix at the preset time based on the continuous-time system model and the control input, until the predicted estimated state quantity and the prediction error covariance matrix at each time within the preset time are obtained, wherein the preset time is a time within the preset time;

[0161] The predicted estimated state quantity at each moment within the preset time is used as the interference estimation value.

[0162] In one possible implementation, determining a target estimated state quantity and a target error covariance matrix at a preset time according to the predicted estimated state quantity, the prediction error covariance matrix, and the lateral position error includes:

[0163] Get the lateral position error;

[0164] Calculate the Kalman gain matrix based on the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error;

[0165] The estimated state quantity and error covariance matrix are updated based on the Kalman gain matrix to obtain the target estimated state quantity and target error covariance matrix at the preset time.

[0166] In one possible implementation, the Kalman gain matrix is ​​calculated based on the predicted estimated state quantity, the prediction error covariance matrix, and the lateral position error, including:

[0167] Get the measured noise covariance matrix;

[0168] The Kalman gain matrix is ​​calculated based on the measured noise covariance matrix, the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error.

[0169] Among them, the readable storage medium can be a computer storage medium or a communication medium. Communication media include any medium that facilitates the transmission of computer programs from one place to another. Computer storage media can be any available medium that can be accessed by a general-purpose or special-purpose computer. For example, a readable storage medium is coupled to a processor so that the processor can read information from the readable storage medium and write information to the readable storage medium. Of course, the readable storage medium can also be an integral part of the processor. The processor and the readable storage medium can be located in an application-specific integrated circuit (ASIC). In addition, the ASIC can be located in a user device. Of course, the processor and the readable storage medium can also exist in a communication device as discrete components. The readable storage medium can be a read-only memory (ROM), a random access memory (RAM), a CD-ROM, a magnetic tape, a floppy disk, an optical data storage device, etc.

[0170] The present invention also provides a program product, comprising execution instructions stored in a readable storage medium. At least one processor of a device can read the execution instructions from the readable storage medium, and at least one processor executes the execution instructions so that the device implements the various embodiments described above. The present application provides a method for determining heading angle compensation, comprising:

[0171] Get vehicle speed and lateral position error;

[0172] Based on the lateral position error and vehicle speed, a vehicle kinematic model is established;

[0173] Calculate disturbance estimates based on the vehicle kinematic model and Kalman filter;

[0174] The vehicle kinematic model is modified using the disturbance estimation value to obtain heading angle compensation.

[0175] In one possible implementation, a vehicle kinematic model is established based on the lateral position error and the vehicle speed, including:

[0176] Get the heading angle error;

[0177] Obtaining a lateral velocity based on the lateral position error;

[0178] A vehicle kinematic model is established based on lateral velocity, vehicle speed, and heading angle error.

[0179] In one possible implementation, the interference estimation value is calculated based on the vehicle kinematic model and Kalman filtering, including:

[0180] Determine the continuous-time system model based on the vehicle kinematic model;

[0181] Discretize the continuous-time system model to obtain a discrete model;

[0182] The discrete model is processed by Kalman filtering to calculate the interference estimation value.

[0183] In one possible implementation, determining a continuous-time system model based on a vehicle kinematic model includes:

[0184] Get model interference, coefficient matrix and identity matrix;

[0185] A continuous-time system model is established based on the vehicle kinematic model, model disturbance, coefficient matrix and identity matrix.

[0186] In one possible implementation, Kalman filtering is performed on the discrete model to calculate the interference estimate, including:

[0187] Based on the continuous-time system model and control input, calculate the predicted estimated state and prediction error covariance matrix at the preset time;

[0188] Determine the target estimated state quantity and target error covariance matrix at a preset time based on the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error;

[0189] Returning to the step of calculating the predicted estimated state quantity and the prediction error covariance matrix at the preset time based on the continuous-time system model and the control input, until the predicted estimated state quantity and the prediction error covariance matrix at each time within the preset time are obtained, wherein the preset time is a time within the preset time;

[0190] The predicted estimated state quantity at each moment within the preset time is used as the interference estimation value.

[0191] In one possible implementation, determining a target estimated state quantity and a target error covariance matrix at a preset time according to the predicted estimated state quantity, the prediction error covariance matrix, and the lateral position error includes:

[0192] Get the lateral position error;

[0193] Calculate the Kalman gain matrix based on the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error;

[0194] The estimated state quantity and error covariance matrix are updated based on the Kalman gain matrix to obtain the target estimated state quantity and target error covariance matrix at the preset time.

[0195] In one possible implementation, the Kalman gain matrix is ​​calculated based on the predicted estimated state quantity, the prediction error covariance matrix, and the lateral position error, including:

[0196] Get the measured noise covariance matrix;

[0197] The Kalman gain matrix is ​​calculated based on the measured noise covariance matrix, the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error.

[0198] In the embodiments of the above-mentioned devices, it should be understood that the processor may be a central processing unit (CPU), or may be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), etc. The general-purpose processor may be a microprocessor or any conventional processor. The steps of the method disclosed in the present invention may be directly implemented as being executed by a hardware processor, or may be executed by a combination of hardware and software modules in the processor.

[0199] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention, and should all be included in the scope of protection of the present invention.

Claims

1. A method for determining heading angle compensation, characterized in that: include: Get vehicle speed and lateral position error; establishing a vehicle kinematic model based on the lateral position error and the vehicle speed; Calculating a disturbance estimate based on the vehicle kinematic model and Kalman filtering; The vehicle kinematic model is corrected using the interference estimation value to obtain heading angle compensation.

2. The method for determining heading angle compensation according to claim 1, wherein: The establishing of a vehicle kinematic model based on the lateral position error and the vehicle speed includes: Get the heading angle error; obtaining a lateral velocity based on the lateral position error; A vehicle kinematic model is established based on the lateral velocity, the vehicle speed, and the heading angle error.

3. The method for determining heading angle compensation according to claim 1, wherein: The calculating of the interference estimation value based on the vehicle kinematic model and the Kalman filter includes: determining a continuous-time system model based on the vehicle kinematic model; discretizing the continuous-time system model to obtain a discrete model; Perform Kalman filtering on the discrete model to calculate the interference estimation value.

4. The method for determining heading angle compensation according to claim 3, wherein: Determining a continuous-time system model based on the vehicle kinematic model includes: Get model interference, coefficient matrix and identity matrix; A continuous-time system model is established based on the vehicle kinematic model, model disturbance, coefficient matrix and identity matrix.

5. The method for determining heading angle compensation according to claim 3, wherein: The performing Kalman filtering on the discrete model to calculate the interference estimation value includes: Based on the continuous-time system model and control input, calculate the predicted estimated state and prediction error covariance matrix at the preset time; Determining a target estimated state quantity and a target error covariance matrix at a preset time according to the predicted estimated state quantity, the predicted error covariance matrix and the lateral position error; Returning to the step of calculating the predicted estimated state quantity and the prediction error covariance matrix at a preset moment based on the continuous-time system model and the control input, until the predicted estimated state quantity and the prediction error covariance matrix at each moment within the preset time are obtained, wherein the preset moment is a moment within the preset time; The predicted estimated state quantity at each moment within the preset time is used as the interference estimation value.

6. The method for determining heading angle compensation according to claim 5, wherein: The step of determining a target estimated state quantity and a target error covariance matrix at a preset time based on the predicted estimated state quantity, the predicted error covariance matrix, and the lateral position error includes: Obtaining the lateral position error; Calculating a Kalman gain matrix based on the predicted estimated state quantity, the prediction error covariance matrix, and the lateral position error; The estimated state quantity and the error covariance matrix are updated based on the Kalman gain matrix to obtain the target estimated state quantity and the target error covariance matrix at a preset moment.

7. The method for determining heading angle compensation according to claim 6, wherein: The calculating of the Kalman gain matrix according to the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error includes: Get the measured noise covariance matrix; The Kalman gain matrix is ​​calculated based on the measured noise covariance matrix, the predicted estimated state quantity, the prediction error covariance matrix and the lateral position error.

8. A device for determining heading angle compensation, characterized in that: include: An acquisition module, used to obtain vehicle speed and lateral position error; An establishment module, configured to establish a vehicle kinematic model based on the lateral position error and the vehicle speed; A calculation module, configured to calculate an interference estimation value based on the vehicle kinematic model and Kalman filtering; The correction module is used to correct the vehicle kinematic model according to the interference estimation value to obtain heading angle compensation.

9. A terminal, characterized in that: comprising a memory, and one or more processors communicatively connected to the memory; The memory stores instructions that can be executed by the one or more processors, and the instructions are executed by the one or more processors to enable the one or more processors to implement the heading angle compensation determination method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The method comprises a program or an instruction, which, when executed on a computer, implements the method for determining the heading angle compensation according to any one of claims 1 to 7.

Images