A dynamic compensation algorithm based on vehicle detection data

Abstract

The invention provides a dynamic compensation algorithm based on vehicle-mounted detection data, which belongs to the field of data processing and solves the problem of low efficiency of vehicle-mounted dynamic positioning compensation. The algorithm is specifically as follows: step S1: acquiring original data; step S2: analyzing the motion state of a vehicle in the original data and constructing a vehicle motion state change equation; step S3: calculating a compensation coefficient of the vehicle, and defining a vehicle compensation coefficient equation in combination with the vehicle motion state change equation; step S4: acquiring the motion state of the vehicle in real time and dynamically compensating the positioning of the vehicle. The invention constructs a digital model according to the position, speed and angle changes of the vehicle within a certain period of time by analyzing the motion state of the vehicle, thereby improving the efficiency of dynamic positioning compensation of the vehicle.

Description

Technical Field

[0001] The invention discloses a dynamic compensation algorithm based on vehicle-mounted detection data, and relates to the field of data processing. Background Art

[0002] The existing dynamic compensation algorithms for vehicle positioning have the following deficiencies:

[0003] High algorithm complexity: Existing dynamic compensation algorithms usually involve real-time processing of sensor data and the interaction of CPU algorithms, which increases the computational complexity of the system. Especially in the real-time compensation process, a large amount of computing resources are required for data processing and algorithm optimization, thereby increasing hardware and software requirements.

[0004] High hardware requirements: Existing dynamic compensation algorithms need to rely on path sensor data, and these sensors themselves also need to have high accuracy and stability; for vehicles or applications that cannot be equipped with displacement sensors, some negative dynamic compensation effects will occur.

[0005] Data consistency issues: Dynamic compensation relies on the fusion of multi-source data, and there may be time delays, different sampling frequencies, and signal noise between data sources. These factors may lead to data consistency issues, thus affecting the compensation effect.

[0006] Poor resilience to environmental changes: Existing dynamic compensation algorithms are easily affected by the environment. For example, in heavy rain or snow, the performance of the sensor may drop sharply, causing deviations in the results of the dynamic compensation algorithm. In addition, dynamic compensation algorithms are usually derived based on known models and have weak adaptability. Summary of the invention

[0007] In view of the deficiencies in the prior art, the present invention aims to provide a dynamic compensation algorithm based on vehicle-mounted detection data, aiming to solve the problem of low efficiency of vehicle-mounted dynamic positioning compensation.

[0008] In order to achieve the above object, the present invention is implemented by the following technical solution: A dynamic compensation algorithm based on vehicle-mounted detection data includes:

[0009] Step S1: Obtain the driving time of the car in the target area; during the driving time, obtain the speed per second, acceleration, heading angle, steering angle, laser radar positioning and satellite navigation positioning of the car to obtain raw data;

[0010] Step S2: Taking the laser radar positioning, speed and heading angle of the car as state variables, and taking the acceleration and steering angle of the car as control variables, respectively construct the car's motion state transfer Jacobian matrix and the control state transfer Jacobian matrix, and combine the original data to analyze the position change of the car during the driving time, and obtain the Kalman gain of the car's motion state change; according to the Kalman gain, construct the car's motion state change equation;

[0011] Step S3: Obtain the number of vehicles in the target area, obtain the movement data of each vehicle in the target area, and calculate the compensation coefficient of the vehicle in combination with the vehicle movement state change equation; define the vehicle compensation coefficient equation according to the compensation coefficient and the vehicle movement state change equation;

[0012] Step S4: Acquire the position, speed, acceleration, heading angle and steering angle of the car in real time, and dynamically compensate the positioning of the car in combination with the compensation coefficient equation.

[0013] Furthermore, the specific steps of step S2 are as follows:

[0014] Step S21: record the travel time as tr;

[0015] The speed of the car per second during the driving time is recorded as: v （1） 、v （2） ~v （tr） ;

[0016] The acceleration per second is recorded as: a （1） 、a （2） ~a （tr） ;

[0017] The heading angle per second is recorded as: θ （1） ,θ （2） ~θ （tr） ;

[0018] The steering angle per second is recorded as: δ （1） ,δ （2） ~δ （tr） ;

[0019] Step S22: During the driving time, the laser radar positioning position of the car per second is recorded as: (x （1） ,y （1） )、(x （2） ,y （2） )~(x （tr） ,y （tr） );

[0020] The car's satellite navigation position per second is recorded as: (j （1） , w （1） )、(j （2） , w（2） )~(j （tr） , w （tr） );

[0021] Step S23: Taking the laser radar positioning as the standard positioning, calculate the position deviation of the satellite navigation positioning relative to the laser radar positioning, and obtain: (Δx （1） , Δy （1） ), (Δx （2） , Δy （2） )~(Δx （tr） , Δy （tr） );

[0022] Among them, (Δx （1） , Δy （1） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the first second, Δx （1） The calculation formula is: Δx （1） =x （1） -j （1） , Δy （1） The calculation formula is: Δy （1） =y （1） -w （1） ;

[0023] (Δx （2） , Δy （2） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the second second, Δx （2） The calculation formula is: Δx （2） =x （2） -j （2） , Δy （2） The calculation formula is: Δy （2） =y （2） -w （2） ;

[0024] Similarly, (Δx （tr） , Δy （tr） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the trth second, Δx （tr） The calculation formula is: Δx （tr） =x （tr） -j （tr） , Δy （tr） The calculation formula is: Δy （tr） =y （tr） -w （tr） ;

[0025] Step S24: Obtain the wheelbase of the car, recorded as L; construct the Jacobian matrix of the motion state transfer of the car at the (k+1)th second, recorded as matrix Fd （k+1） ; Construct the Jacobian matrix of the control state transfer, denoted as matrix Fc（k+1） .

[0026] Furthermore, the subsequent steps of step S24 are as follows:

[0027] Step S25: is the initial state matrix, denoted as matrix B （EO） ; The acceleration of the car at 0 seconds is a （0） is zero; calculate the Kalman gain of the vehicle's motion state from 0 to 1 second, recorded as; Kk （0-1） ;

[0028] Step S26: Construct the motion state matrix of the car at the second second, denoted as matrix B （2） ; Matrix B （2） The mathematical expression is: ;

[0029] Calculate the Kalman gain of the vehicle's motion state from the 1st to the 2nd second, denoted as; Kk （1-2） ;

[0030] Step S27: Similarly, construct the motion state matrix of the car at the (tr-1)th second, recorded as matrix B （tr-1） ; Matrix B （tr-1） The mathematical expression is:

[0031] ;

[0032] Among them, (x （tr-1） ,y （tr-1） ) The laser radar position of the car at (tr-1) seconds; v （tr-1） and θ （tr-1） , respectively represent the speed and heading angle of the car at the (tr-1) second; a （tr-1） and δ （tr-1） , respectively represent the acceleration and steering angle of the car at the (tr-1)th second;

[0033] Construct the motion state matrix of the car at the trth second, denoted as matrix B （tr） ; Matrix B （tr） The mathematical expression is: ;

[0034] Calculate the Kalman gain of the vehicle's motion state from (tr-1) to tr seconds, denoted as; Kk （（tr-1）-tr） ;

[0035] Step S28: Summarize the Kalman gain of the vehicle motion state from the 0th second to the 1st second, the 1st second to the 2nd second, and finally the (tr-1)th second to the trth second: Kk （0-1） , Kk（1-2） ~Kk （（tr-1）-tr） ;

[0036] According to Kk （0-1） ~Kk （（tr-1）-tr） The determined vehicle motion state change equation is recorded as equation ME, and enters step S3.

[0037] Furthermore, the specific steps of step S24 are as follows:

[0038] Step S241: Record the position of the car at the kth second as (x （k） ,y （k） ), speed is denoted as v （k） , the heading angle is denoted as θ （k） , acceleration is denoted as a （k） , the steering angle is denoted as δ （k） ;

[0039] The position of the car at (k+1) seconds is recorded as (x （k+1） ,y （k+1） ), speed is denoted as v （k+1） , the heading angle is denoted as θ （k+1） ;

[0040] Step S242: construct the motion function of the car at the (k+1)th second, recorded as f(k+1); the mathematical expression of f(k+1) is:

[0041] ;

[0042] With (x （k） ,y （k） , v （k） ,θ （k） ) is the state variable, and the motion state matrix of the car is intercepted in the function f(k+1), denoted as Fd （k+1） ;Fd （k+1） The mathematical expression is:

[0043] ;

[0044] Calculate the matrix Fd （k+1） The partial derivative of the relative state variable is used to obtain the Jacobian matrix Fk of the car's motion state transfer at the (k+1)th second （k+1） ; Fk （k+1） The mathematical expression is:

[0045] ;

[0046] Step S243: （k） , δ （k） ) is the control variable, and the control state matrix of the car is intercepted in the function f(k+1), which is recorded as Fi（k+1） Fi （k+1） The mathematical expression is:

[0047] ;

[0048] Calculate the matrix Fi （k+1） The partial derivative of the relative control variable is used to obtain the control state transfer Jacobian matrix Fc of the vehicle at the (k+1)th second （k+1） ; Fc （k+1） The mathematical expression is:

[0049] ; Among them, sec() represents the secant function.

[0050] Furthermore, the specific steps of step S25 are as follows:

[0051] Step S251: Construct the motion state matrix of the car at the first second, denoted as matrix B （1） ; Matrix B （1） The mathematical expression is: ;

[0052] Calculate the matrix B （EO） Becomes matrix B （1） The state transfer matrix of （EO-1） ; Matrix B （EO-1） The calculation formula is:

[0053] ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix;

[0054] Step S252: Matrix B （EO） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the first second, denoted as matrix Fk （1） ; Fk （1） The mathematical expression is:

[0055] ;

[0056] A （0） and δ （0） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the control state of the car at the first second, denoted as matrix Fc （1） ; Fc （1） The mathematical expression is:

[0057] ;

[0058] Step S253: Simultaneous matrix B （EO） and matrix B（1） , we get the motion state matrix of the car from the 0th to the 1st second, denoted as P （EO-1） ;P （EO-1） The mathematical expression is:

[0059] ;

[0060] Calculate the error matrix of the car changing from the 0th second motion state to the 1st second motion state, denoted as Pk （EO-1） ;Pk （EO-1） The calculation formula is:

[0061] ; Where, represents matrix multiplication, and T represents the transpose of the matrix;

[0062] Step S254: According to (Δx （1） , Δy （1） ) Construct the position error matrix of the car at the first second, denoted as Q （1） ;Q （1） The mathematical expression is:

[0063] ;

[0064] Calculate the Kalman gain of the car from the 0th second motion state to the 1st second motion state, denoted as Kk （0-1） ;Kk （0-1） The calculation formula is:

[0065] .

[0066] Furthermore, the specific steps of step S26 are as follows:

[0067] Step S261: Calculate matrix B （1） Becomes matrix B （2） The state transfer matrix of （1-2） ; Matrix B （1-2） The calculation formula is:

[0068] ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix;

[0069] Step S262: Matrix B （1） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the second second, recorded as matrix Fk （2） ; Fk （2） The mathematical expression is:

[0070] ;

[0071] A（1） and δ （1） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the vehicle's control state at the second second, denoted as matrix Fc （2） ; Fc （2） The mathematical expression is:

[0072] ;

[0073] Step S263: Simultaneous matrix B （1） and matrix B （2） , we get the motion state matrix of the car from the 1st to the 2nd second, denoted as P （1-2） ;P （1-2） The mathematical expression is:

[0074] ;

[0075] Calculate the error matrix of the car changing from the 1st second motion state to the 2nd second motion state, recorded as Pk （1-2） ;Pk （1-2） The calculation formula is:

[0076] ; Where, represents matrix multiplication, and T represents the transpose of the matrix;

[0077] Step S264: According to (Δx （2） , Δy （2） ) Construct the position error matrix of the car at the first second, denoted as Q （2） ;Q （2） The mathematical expression is:

[0078] ;

[0079] Calculate the Kalman gain of the car from the 1st second motion state to the 2nd second motion state, denoted as Kk （1-2） ;Kk （1-2） The calculation formula is:

[0080] .

[0081] Furthermore, the specific steps of step S27 are as follows:

[0082] Step S271: Calculate matrix B （tr-1） Becomes matrix B （tr） The state transfer matrix of （（tr-1）-tr） ; Matrix B （（tr-1）-tr） The calculation formula is:

[0083] ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix;

[0084] Step S272: Matrix B （tr-1） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the trth second, recorded as matrix Fk （tr） ; Fk （tr） The mathematical expression is:

[0085] ;

[0086] A （tr-1） and δ （tr-1） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the vehicle's control state at the second second, denoted as matrix Fc （tr） ; Fc （tr） The mathematical expression is:

[0087] ;

[0088] Step S273: Simultaneous matrix B （tr-1） and matrix B （tr） , we get the motion state matrix of the car from (tr-1) to tr seconds, denoted as P （（tr-1）-tr） ;P （（tr-1）-tr） The mathematical expression is:

[0089] ;

[0090] Calculate the error matrix of the car from the (tr-1) second motion state to the tr second motion state, denoted as Pk （（tr-1）-tr） ;Pk （（tr-1）-tr） The calculation formula is:

[0091] ; Where, represents matrix multiplication, and T represents the transpose of the matrix;

[0092] Step S274: According to (Δx （tr） , Δy （tr） ) Construct the position error matrix of the car at the first second, denoted as Q （tr） ;Q （tr） The mathematical expression is:

[0093] ;

[0094] Calculate the Kalman gain of the car from the (tr-1) second motion state to the tr second motion state, denoted as Kk （（tr-1）-tr） ;Kk （（tr-1）-tr） The calculation formula is:

[0095] .

[0096] Furthermore, the specific steps of step S3 are as follows:

[0097] Step S31: Obtain the number of cars in the target area, denoted as ca;

[0098] Obtain the position, speed, acceleration, heading angle and steering angle of all cars in the target area within the tr time as motion data;

[0099] The speeds of the first to the cath cars in the motion data are recorded as: v （1,1） ~v （ca,tr） ;

[0100] Acceleration is denoted by: a （1,1） ~a （ca,tr） ;

[0101] The heading angle is denoted as: θ （1,1） ~θ （ca,tr） ;

[0102] The steering angle is recorded as: δ （1,1） ~δ （ca,tr） ;

[0103] Step S32: Record the laser radar positions corresponding to the 1st to the cth vehicles as:

[0104] (x （1,1） ,y （1,1） )~(x （ca,tr） ,y （ca,tr） );

[0105] Substituting into (x （1,1） ,y （1,1） )~(x （ca,tr） ,y （ca,tr） )、v （1,1） ~v （ca,tr） ,θ （1,1） ~θ （ca,tr） 、a （1,1） ~a （ca,tr） and δ （1,1） ~δ （ca,tr） Substituting into equation ME, the estimated motion state of cars 1 to ca from the 2nd to trth seconds is calculated, and we get:

[0106] ~ ;

[0107] Step S33: Define the relation B1:

[0108] ;

[0109] Among them, × represents matrix multiplication, I represents a one-matrix; β represents the error coefficient;

[0110] (x （i,j） ,y （i,j） ) represents the position of the i-th vehicle at the j-th second;: v （i,j） and θ （i,j） , respectively represent the speed and heading angle of the i-th vehicle at the j-th second; the value range of i is: 1~ca, the value range of j is: 2~tr;

[0111] represents the estimated motion state matrix of the i-th car at the j-th second;

[0112] Step S34: Substitute steps S31 to S32 into relational expression B1, and count the number of items that do not satisfy relational expression B1, recorded as nw;

[0113] The error coefficient of the calculation equation ME is recorded as nc; the calculation formula of nc is:

[0114] ;

[0115] The estimated motion state matrix that does not satisfy the relation B1 is recorded as a type A matrix;

[0116] The actual vehicle motion state matrix corresponding to the type A matrix is ​​recorded as the type B matrix;

[0117] Calculate the compensation coefficient of the vehicle according to the A-type matrix and the B-type matrix;

[0118] Step S35: extracting the compensation coefficient cx of the lateral position of the vehicle, the compensation coefficient cy of the longitudinal position, the compensation coefficient cv of the speed, and the compensation coefficient cθ of the heading angle;

[0119] The vehicle motion state change equation ME is redefined according to the compensation coefficient, and the vehicle compensation coefficient equation is obtained, which is recorded as MOV; the mathematical expression of MOV is:

[0120] .

[0121] Furthermore, the specific steps of step S34 are as follows:

[0122] Step S341: record the 1st to nwth A-type matrices as:

[0123] ~ ;

[0124] The 1st to nwth B-type matrices are denoted as:

[0125] ~ ;

[0126] Step S342: Integrate the 1st to nwth A-type matrices to obtain a matrix Ba; the mathematical expression of Ba is:

[0127] ;

[0128] Integrate the 1st to nwth B-type matrices to obtain the matrix Bb; the mathematical expression of Bb is:

[0129] ;

[0130] Calculate the difference matrix between matrix Bb and matrix Ba, denoted as Bc;

[0131] Step S343: Calculate the covariance matrix of the matrix Bc, denoted as Cc; the calculation formula is:

[0132] ; Where × represents matrix multiplication and T represents matrix transpose;

[0133] Zc represents the centralization matrix of matrix Bc. The calculation formula of matrix Zc is:

[0134] ; Wherein, I represents a matrix;

[0135] Step S344: Split the matrix Cc by columns to obtain the matrix D （1） , matrix D （2） , matrix D （3） And the matrix D （4） ;

[0136] Using Intel MKL library functions, define and calculate weight values ​​λ1, λ2, λ3, λ4, and λ5 and offsets μ1, μ2, μ3, μ4, and μ3 so that the matrix D （1） ~Matrix D （4） The following mathematical relationship is satisfied:

[0137] ;

[0138] Step S345: Calculate the influence coefficient of the lateral position of the vehicle on the positioning, denoted as bx, bx=λ1 / μ1;

[0139] The influence coefficient of the longitudinal position of the vehicle on positioning is calculated and recorded as by, by = λ2 / μ2;

[0140] The influence coefficient of vehicle speed on positioning is calculated and recorded as bv, bv = λ3 / μ3;

[0141] The influence coefficient of the vehicle heading angle on positioning is calculated and recorded as bθ, bθ=λ4 / μ4;

[0142] Step S346: Calculate the compensation coefficient of the lateral position of the vehicle and record it as cx, cx=bx / 4;

[0143] The compensation coefficient for calculating the longitudinal position of the vehicle is denoted as cy, cy = by / 4;

[0144] The compensation coefficient for calculating the vehicle speed is recorded as cv, cv = bv / 4;

[0145] The compensation coefficient for calculating the heading angle of the vehicle is recorded as cθ, cθ=bθ / 4.

[0146] Furthermore, the specific steps of step S28 are as follows:

[0147] Step S281: Calculate Kk （0-1） ~Kk （（tr-1）-tr） The average value of is denoted as aKk;

[0148] The position of the car at the i-th second is recorded as (x （i） ,y （i） ), speed is denoted as v （i） , the heading angle is denoted as θ （i） , acceleration is denoted as a （i） , the steering angle is denoted as δ （i） ;

[0149] The position of the car at the (i+1)th second is recorded as (x （i+1） ,y （i+1） ), speed is denoted as v （i+1） , the heading angle is denoted as θ （i+1） ;

[0150] Step S282: （i） ,y （i） , v （i） ,θ （i） ) is the state variable, with (a （i） , δ （i） ) is the control variable, constructing the motion state of the car at the (i+1)th second (x （i+1） ,y （i+1） , v （i+1） ,θ （i+1） ) to obtain equation MO;

[0151] The mathematical expression of MO is:

[0152] ;

[0153] Among them, × represents matrix multiplication, T represents the transpose of the matrix, CC represents the (1×4) error matrix, and the initial parameters in the matrix CC are 0;

[0154] Step S283: Set v （1） ~v（tr） 、a （1） ~a （tr） ,θ （1） ~θ （tr） , δ （1） ~δ （tr） and (x （1） ,y （1） )~(x （tr） ,y （tr） ) is substituted into equation MO, and MATLAB is used to fit the values ​​of the parameters in the matrix CC to obtain the determined error matrix, which is recorded as QC;

[0155] Substituting matrix CC in equation MO with matrix QC, we get the definite equation of the vehicle motion state change, recorded as equation ME; the mathematical expression of ME is:

[0156] .

[0157] Compared with the prior art, the present invention has the following beneficial effects:

[0158] Improve positioning accuracy: The present invention can correct sensor artifacts in real time based on the vehicle's position, speed, travel direction and other motion data to improve positioning accuracy. For example, IMU sensors are prone to deviations when measuring acceleration and angular velocity, resulting in reduced positioning accuracy. Through the dynamic compensation algorithm of the present invention, the system can combine data from other sensors (such as vehicle position and vehicle travel direction) to reduce the impact of IMU sensors and further improve overall positioning accuracy.

[0159] Enhanced system stability: The present invention can adapt to changes in different driving environments and improve the robustness of the system; especially in complex road conditions, the vehicle's detection data may be subject to varying degrees of interference. Through dynamic compensation, the algorithm can identify these interferences in real time and make adjustments to maintain the normal operation of the vehicle positioning system.

[0160] Improve data fusion effect: Vehicle detection systems usually rely on data fusion technology of multiple sensors, such as Kalman filtering and particle filtering. The present invention combines Kalman filtering to fuse the vehicle's position, speed, acceleration, heading angle and other motion state data, effectively optimizing the deviation of these data from vehicle positioning and improving fusion accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0161] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0162] Figure 1It is a schematic diagram of the algorithm of the present invention;

[0163] Figure 2 It is a data schematic diagram of the present invention;

[0164] Figure 3 This is a schematic diagram of data processing of the present invention. DETAILED DESCRIPTION

[0165] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and understandable, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0166] See also Figure 1 and Figure 2 , a dynamic compensation algorithm based on vehicle detection data includes:

[0167] Step S1: Obtain the driving time of the car in the target area; during the driving time, obtain the speed, acceleration, heading angle, steering angle, laser radar positioning and satellite navigation positioning (such as Beidou positioning or GPS positioning) of the car per second to obtain raw data;

[0168] It should be noted that the "target area" in the present invention refers to the city-level area where the present invention (a dynamic compensation algorithm based on vehicle detection data) is used to perform vehicle dynamic compensation;

[0169] Step S2: Taking the laser radar positioning, speed and heading angle of the car as state variables, and taking the acceleration and steering angle of the car as control variables, respectively construct the car's motion state transfer Jacobian matrix and the control state transfer Jacobian matrix, and combine the original data to analyze the position change of the car during the driving time, and obtain the Kalman gain of the car's motion state change; according to the Kalman gain, construct the car's motion state change equation;

[0170] The specific steps of step S2 are as follows:

[0171] Step S21: Please refer to Figure 3 , let the driving time (of the car in the target area) be tr;

[0172] The speed of the car per second during the driving time is recorded as: v （1） 、v （2） ~v （tr） ;

[0173] The acceleration per second is recorded as: a （1） 、a （2） ~a （tr） ;

[0174] The heading angle per second is recorded as: θ （1） ,θ （2） ~θ （tr） ;

[0175] The steering angle per second is recorded as: δ （1） , δ （2） ~δ （tr） ;

[0176] Among them, v （1） 、a （1） ,θ （1） and δ （1） , respectively represent the speed, acceleration, heading angle and steering angle of the car at the first second; v （2） 、a （2） ,θ （2） and δ （2） , respectively represent the speed, acceleration, heading angle and steering angle of the car at the second second; and so on, v （tr） 、a （tr） ,θ （tr） and δ （tr） , respectively represent the speed, acceleration, heading angle and steering angle of the car at the trth second;

[0177] Step S22: During the driving time, the laser radar positioning position of the car per second is recorded as: (x （1） ,y （1） )、(x （2） ,y （2） )~(x （tr） ,y （tr） );

[0178] The car's satellite navigation position per second is recorded as: (j （1） , w （1） )、(j （2） , w （2） )~(j （tr） , w （tr） );

[0179] 1 Among them, (x （1） ,y （1） ) and (j （1） , w （1） ), respectively represent the laser radar position and satellite navigation positioning position of the car at the first second; (x （2） ,y （2） ) and (j （2） , w （2） ), respectively represent the laser radar position and satellite navigation positioning position of the car at the second second; and so on, (x （tr） ,y （tr） ) and (j （tr） , w （tr） ), respectively represent the lidar position and satellite navigation positioning position of the car at the trth second;

[0180] Step S23: Taking the laser radar positioning as the standard positioning, calculate the position deviation of the satellite navigation positioning relative to the laser radar positioning, and obtain: (Δx （1） , Δy （1） ), (Δx （2） , Δy （2） )~(Δx （tr） , Δy （tr） );

[0181] Among them, (Δx （1） , Δy （1） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the first second, Δx （1） The calculation formula is: Δx （1） =x （1） -j （1） , Δy （1） The calculation formula is: Δy （1） =y （1） -w （1） ;

[0182] (Δx （2） , Δy （2） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the second second, Δx （2） The calculation formula is: Δx （2） =x （2） -j （2） , Δy （2） The calculation formula is: Δy （2） =y （2） -w （2） ;

[0183] Similarly, (Δx （tr） , Δy （tr） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the trth second, Δx （tr） The calculation formula is: Δx （tr） =x （tr） -j （tr） , Δy （tr） The calculation formula is: Δy （tr） =y （tr） -w （tr） ;

[0184] Step S24: Obtain the wheelbase of the car, recorded as L; construct the Jacobian matrix of the motion state transfer of the car at the (k+1)th second, recorded as matrix Fd （k+1） ; Construct the Jacobian matrix of the control state transfer, denoted as matrix Fc （k+1） ;

[0185] Step S241: Record the position of the car at the kth second as (x （k） ,y（k） ), speed is denoted as v （k） , the heading angle is denoted as θ （k） , acceleration is denoted as a （k） , the steering angle is denoted as δ （k） ;

[0186] The position of the car at (k+1) seconds is recorded as (x （k+1） ,y （k+1） ), speed is denoted as v （k+1） , the heading angle is denoted as θ （k+1） ;

[0187] Step S242: construct the motion function of the car at the (k+1)th second, recorded as f(k+1); the mathematical expression of f(k+1) is:

[0188] ;

[0189] With (x （k） ,y （k） , v （k） ,θ （k） ) is the state variable, and the motion state matrix of the car is intercepted in the function f(k+1), denoted as Fd （k+1） ;Fd （k+1） The mathematical expression is:

[0190] ;

[0191] Calculate the matrix Fd （k+1） The partial derivative of the relative state variable is used to obtain the Jacobian matrix Fk of the car's motion state transfer at the (k+1)th second （k+1） ; Fk （k+1） The mathematical expression is:

[0192] ;

[0193] Step S243: （k） , δ （k） ) is the control variable, and the control state matrix of the car is intercepted in the function f(k+1), which is recorded as Fi （k+1） Fi （k+1） The mathematical expression is:

[0194] ;

[0195] Calculate the matrix Fi （k+1） The partial derivative of the relative control variable is used to obtain the control state transfer Jacobian matrix Fc of the vehicle at the (k+1)th second （k+1） ; Fc （k+1） The mathematical expression is:

[0196] ; Among them, sec() represents the secant function;

[0197] Step S25: is the initial state matrix, denoted as matrix B （EO） ; The acceleration of the car at 0 seconds is a （0） is zero; calculate the Kalman gain of the vehicle's motion state from 0 to 1 second, recorded as; Kk （0-1） ;

[0198] Step S251: Construct the motion state matrix of the car at the first second, denoted as matrix B （1） ; Matrix B （1） The mathematical expression is: ;

[0199] Calculate the matrix B （EO） Becomes matrix B （1） The state transfer matrix of （EO-1） ; Matrix B （EO-1） The calculation formula is:

[0200] ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix;

[0201] Step S252: Matrix B （EO） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the first second, denoted as matrix Fk （1） ; Fk （1） The mathematical expression is:

[0202] ;

[0203] A （0） and δ （0） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the control state of the car at the first second, denoted as matrix Fc （1） ; Fc （1） The mathematical expression is:

[0204] ;

[0205] Step S253: Simultaneous matrix B （EO） and matrix B （1） , we get the motion state matrix of the car from the 0th to the 1st second, denoted as P （EO-1） ;P （EO-1） The mathematical expression is:

[0206] ;

[0207] Calculate the error matrix of the car changing from the 0th second motion state to the 1st second motion state, denoted as Pk （EO-1） ;Pk （EO-1） The calculation formula is:

[0208] ; Where, represents matrix multiplication, and T represents the transpose of the matrix;

[0209] Step S254: According to (Δx （1） , Δy （1） ) Construct the position error matrix of the car at the first second, denoted as Q （1） ;Q （1） The mathematical expression is:

[0210] ;

[0211] Calculate the Kalman gain of the car from the 0th second motion state to the 1st second motion state, denoted as Kk （0-1） ;Kk （0-1） The calculation formula is:

[0212] ;

[0213] Step S26: Construct the motion state matrix of the car at the second second, denoted as matrix B （2） ; Matrix B （2） The mathematical expression is: ;

[0214] Calculate the Kalman gain of the vehicle's motion state from the 1st to the 2nd second, denoted as; Kk （1-2） ;

[0215] Step S261: Calculate matrix B （1） Becomes matrix B （2） The state transfer matrix of （1-2） ; Matrix B （1-2） The calculation formula is:

[0216] ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix;

[0217] Step S262: Matrix B （1） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the second second, recorded as matrix Fk （2） ; Fk （2） The mathematical expression is:

[0218] ;

[0219] A（1） and δ （1） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the vehicle's control state at the second second, denoted as matrix Fc （2） ; Fc （2） The mathematical expression is:

[0220] ;

[0221] Step S263: Simultaneous matrix B （1） and matrix B （2） , we get the motion state matrix of the car from the 1st to the 2nd second, denoted as P （1-2） ;P （1-2） The mathematical expression is:

[0222] ;

[0223] Calculate the error matrix of the car changing from the 1st second motion state to the 2nd second motion state, recorded as Pk （1-2） ;Pk （1-2） The calculation formula is:

[0224] ; Where, represents matrix multiplication, and T represents the transpose of the matrix;

[0225] Step S264: According to (Δx （2） , Δy （2） ) Construct the position error matrix of the car at the first second, denoted as Q （2） ;Q （2） The mathematical expression is:

[0226] ;

[0227] Calculate the Kalman gain of the car from the 1st second motion state to the 2nd second motion state, denoted as Kk （1-2） ;Kk （1-2） The calculation formula is:

[0228] ;

[0229] Step S27: Similarly, construct the motion state matrix of the car at the (tr-1)th second, recorded as matrix B （tr-1） ; Matrix B （tr-1） The mathematical expression is:

[0230] ;

[0231] Among them, (x （tr-1） ,y （tr-1） ) The LiDAR position of the car at (tr-1) seconds; v （tr-1） and θ（tr-1） , respectively represent the speed and heading angle of the car at the (tr-1) second; a （tr-1） and δ （tr-1） , respectively represent the acceleration and steering angle of the car at the (tr-1)th second;

[0232] Construct the motion state matrix of the car at the trth second, denoted as matrix B （tr） ; Matrix B （tr） The mathematical expression is: ;

[0233] Calculate the Kalman gain of the vehicle's motion state from (tr-1) to tr seconds, denoted as; Kk （（tr-1）-tr） ;

[0234] Step S271: Calculate matrix B （tr-1） Becomes matrix B （tr） The state transfer matrix of （（tr-1）-tr） ; Matrix B （（tr-1）-tr） The calculation formula is:

[0235] ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix;

[0236] Step S272: Matrix B （tr-1） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the trth second, recorded as matrix Fk （tr） ; Fk （tr） The mathematical expression is:

[0237] ;

[0238] A （tr-1） and δ （tr-1） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the vehicle's control state at the second second, denoted as matrix Fc （tr） ; Fc （tr） The mathematical expression is:

[0239] ;

[0240] Step S273: Simultaneous matrix B （tr-1） and matrix B （tr） , we get the motion state matrix of the car from (tr-1) to tr seconds, denoted as P （（tr-1）-tr） ;P （（tr-1）-tr） The mathematical expression is:

[0241] ;

[0242] Calculate the error matrix of the car from the (tr-1) second motion state to the tr second motion state, denoted as Pk （（tr-1）-tr） ;Pk （（tr-1）-tr） The calculation formula is:

[0243] ; Where, represents matrix multiplication, and T represents the transpose of the matrix;

[0244] Step S274: According to (Δx （tr） , Δy （tr） ) Construct the position error matrix of the car at the first second, denoted as Q （tr） ;Q （tr） The mathematical expression is:

[0245] ;

[0246] Calculate the Kalman gain of the car from the (tr-1) second motion state to the tr second motion state, denoted as Kk （（tr-1）-tr） ;Kk （（tr-1）-tr） The calculation formula is:

[0247] ;

[0248] Step S28: Summarize the Kalman gain of the vehicle motion state from the 0th second to the 1st second, the 1st second to the 2nd second, and finally the (tr-1)th second to the trth second: Kk （0-1） , Kk （1-2） ~Kk （（tr-1）-tr） ;

[0249] Calculate Kk （0-1） ~Kk （（tr-1）-tr） The average value of is denoted as aKk;

[0250] The position of the car at the i-th second is recorded as (x （i） ,y （i） ), speed is denoted as v （i） , the heading angle is denoted as θ （i） , acceleration is denoted as a （i） , the steering angle is denoted as δ （i） ;

[0251] The position of the car at the (i+1)th second is recorded as (x （i+1） ,y （i+1） ), speed is denoted as v （i+1） , the heading angle is denoted as θ （i+1） ;

[0252] With (x （i） ,y （i） , v （i） ,θ （i）) is the state variable, with (a （i） , δ （i） ) is the control variable, constructing the motion state of the car at the (i+1)th second (x （i+1） ,y （i+1） , v （i+1） ,θ （i+1） ) to obtain equation MO;

[0253] The mathematical expression of MO is:

[0254] ;

[0255] Among them, × represents matrix multiplication, T represents the transpose of the matrix, CC represents the (1×4) error matrix, and the initial parameters in the matrix CC are 0;

[0256] v （1） ~v （tr） 、a （1） ~a （tr） ,θ （1） ~θ （tr） , δ （1） ~δ （tr） and (x （1） ,y （1） )~(x （tr） ,y （tr） ) is substituted into equation MO, and MATLAB is used to fit the values ​​of the parameters in the matrix CC to obtain the determined error matrix, which is recorded as QC;

[0257] Substituting matrix CC in equation MO with matrix QC, we get the definite equation of the vehicle motion state change, recorded as equation ME; the mathematical expression of ME is:

[0258] .

[0259] Step S3: Obtain the number of vehicles in the target area, obtain the movement data of each vehicle in the target area, and calculate the compensation coefficient of the vehicle in combination with the vehicle movement state change equation; define the vehicle compensation coefficient equation according to the compensation coefficient and the vehicle movement state change equation;

[0260] The specific steps of step S3 are as follows:

[0261] Step S31: Obtain the number of cars in the target area, denoted as ca;

[0262] Obtain the position, speed, acceleration, heading angle and steering angle of all cars in the target area within the tr time (and in the target area) as motion data;

[0263] The speeds of the first to the cath cars in the motion data are recorded as: v （1,1）~v （ca,tr） ;

[0264] Acceleration is denoted by: a （1,1） ~a （ca,tr） ;

[0265] The heading angle is denoted as: θ （1,1） ~θ （ca,tr） ;

[0266] The steering angle is recorded as: δ （1,1） ~δ （ca,tr） ;

[0267] Step S32: Record the laser radar positions (or satellite navigation positions) corresponding to the 1st to the cth vehicles as:

[0268] (x （1,1） ,y （1,1） )~(x （ca,tr） ,y （ca,tr） );

[0269] Substituting into (x （1,1） ,y （1,1） )~(x （ca,tr） ,y （ca,tr） )、v （1,1） ~v （ca,tr） ,θ （1,1） ~θ （ca,tr） 、a （1,1） ~a （ca,tr） and δ （1,1） ~δ （ca,tr） Substituting into equation ME, the estimated motion state of cars 1 to ca from the 2nd to trth seconds is calculated, and we get:

[0270] ~ ;

[0271] Step S33: Define the relation B1:

[0272] ;

[0273] Where × represents matrix multiplication, I represents one matrix; β represents the (acceptable) error coefficient; (β is generally taken as 0.1; users or relevant technical personnel can adjust the value of β according to actual needs)

[0274] (x （i,j） ,y （i,j） ) represents the position of the i-th vehicle at the j-th second;: v （i,j） and θ （i,j） , respectively represent the speed and heading angle of the i-th vehicle at the j-th second; the value range of i is: 1~ca, the value range of j is: 2~tr;

[0275] represents the estimated motion state matrix of the i-th car at the j-th second;

[0276] (Right now exist Output in Equation ME)

[0277] Step S34: Substitute steps S31 to S32 into relational expression B1, and count the number of items that do not satisfy relational expression B1, recorded as nw;

[0278] The error coefficient of the calculation equation ME is recorded as nc; the calculation formula of nc is:

[0279] ;

[0280] The estimated motion state matrix that does not satisfy the relation B1 is recorded as a type A matrix;

[0281] The actual vehicle motion state matrix corresponding to the type A matrix is ​​recorded as the type B matrix;

[0282] Calculate the compensation coefficient of the vehicle according to the A-type matrix and the B-type matrix;

[0283] Step S341: record the first, second, and up to the nwth A-type matrices as:

[0284] , ~ ;

[0285] ( )

[0286] The first, second, and up to the nwth B-type matrices are denoted as:

[0287] , ~ ;

[0288] ( )

[0289] Step S342: Integrate the 1st to nwth A-type matrices to obtain a matrix Ba; the mathematical expression of Ba is:

[0290] ; (nw×4)

[0291] Integrate the 1st to nwth B-type matrices to obtain the matrix Bb; the mathematical expression of Bb is:

[0292] ; (nw×4)

[0293] Calculate the difference matrix between matrix Bb and matrix Ba, recorded as Bc; (i.e. Bc = Bb - Ba)

[0294] Step S343: Calculate the covariance matrix of the matrix Bc, denoted as Cc; the calculation formula is:

[0295] ; (4×4) where × represents matrix multiplication and T represents the transpose of the matrix;

[0296] Zc represents the centralization matrix of matrix Bc. The calculation formula of matrix Zc is:

[0297] ; Wherein, I represents a matrix;

[0298] Step S344: Split the matrix Cc by columns to obtain the matrix D （1） , matrix D （2） , matrix D （3） And the matrix D （4） ;

[0299] Using Intel MKL library functions, define and calculate weight values ​​λ1, λ2, λ3, λ4, and λ5 and offsets μ1, μ2, μ3, μ4, and μ3 so that the matrix D （1） ~Matrix D （4） The following mathematical relationship is satisfied:

[0300] ;

[0301] Step S345: Calculate the influence coefficient of the lateral position of the vehicle on the positioning, denoted as bx, bx=λ1 / μ1;

[0302] The influence coefficient of the longitudinal position of the vehicle on positioning is calculated and recorded as by, by = λ2 / μ2;

[0303] The influence coefficient of vehicle speed on positioning is calculated and recorded as bv, bv = λ3 / μ3;

[0304] The influence coefficient of the vehicle heading angle on positioning is calculated and recorded as bθ, bθ=λ4 / μ4;

[0305] Step S346: Calculate the compensation coefficient of the lateral position of the vehicle and record it as cx, cx=bx / 4;

[0306] The compensation coefficient for calculating the longitudinal position of the vehicle is denoted as cy, cy = by / 4;

[0307] The compensation coefficient for calculating the vehicle speed is recorded as cv, cv = bv / 4;

[0308] The compensation coefficient for calculating the heading angle of the vehicle is recorded as cθ, cθ = bθ / 4;

[0309] Step S35: extracting the compensation coefficient cx of the lateral position of the vehicle, the compensation coefficient cy of the longitudinal position, the compensation coefficient cv of the speed, and the compensation coefficient cθ of the heading angle;

[0310] The vehicle motion state change equation ME is redefined according to the compensation coefficient, and the vehicle compensation coefficient equation is obtained, which is recorded as MOV; the mathematical expression of MOV is:

[0311] .

[0312] Step S4: Acquire the position, speed, acceleration, heading angle and steering angle of the car in real time, and dynamically compensate the positioning of the car in combination with the compensation coefficient equation.

[0313] The above formulas are all dimensionless and numerical calculations. The formula is a formula for the most recent real situation obtained by collecting a large amount of data and performing software simulation. The preset parameters in the formula are set by technical personnel in this field according to actual conditions. For example, if there are weight coefficients and proportional coefficients, their set sizes are to quantify each parameter to obtain a specific value, which is convenient for subsequent comparison. Regarding the size of the weight coefficient and the proportional coefficient, it is sufficient as long as it does not affect the proportional relationship between the parameter and the quantized value.

[0314] Finally, it should be noted that the above-described embodiments are only specific implementations of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The protection scope of the present invention is not limited thereto. Although the present invention is described in detail with reference to the above-described embodiments, ordinary technicians in the field should understand that any technician familiar with the technical field can still modify the technical solutions recorded in the above-described embodiments within the technical scope disclosed by the present invention, or can easily think of changes, or make equivalent replacements for some of the technical features therein; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A dynamic compensation algorithm based on vehicle-mounted detection data, characterized in that: The algorithm includes: Step S1: Obtain the driving time of the car in the target area; during the driving time, obtain the speed, acceleration, heading angle, steering angle, laser radar positioning and satellite navigation positioning of the car per second to obtain raw data; Step S2: Taking the laser radar positioning, speed and heading angle of the car as state variables, and taking the acceleration and steering angle of the car as control variables, respectively construct the car's motion state transfer Jacobian matrix and the control state transfer Jacobian matrix, and combine the original data to analyze the position change of the car during the driving time, and obtain the Kalman gain of the car's motion state change; according to the Kalman gain, construct the car's motion state change equation; Step S3: Obtain the number of cars in the target area, obtain the movement data of each car in the target area, and calculate the compensation coefficient of the car in combination with the car movement state change equation; define the car compensation coefficient equation according to the compensation coefficient and the car movement state change equation; Step S4: Acquire the position, speed, acceleration, heading angle and steering angle of the car in real time, and dynamically compensate the positioning of the car in combination with the compensation coefficient equation.

2. The dynamic compensation algorithm based on vehicle detection data according to claim 1, characterized in that: The specific steps of step S2 are as follows: Step S21: record the travel time as tr; The speed of the car per second during the driving time is recorded as: v （1） 、v （2） ~v （tr） ; The acceleration per second is recorded as: a （1） 、a （2） ~a （tr） ; The heading angle per second is recorded as: θ （1） ,θ （2） ~θ （tr） ; The steering angle per second is recorded as: δ （1） ,δ （2） ~δ （tr） ; Step S22: During the driving time, the laser radar positioning position of the car per second is recorded as: (x （1） ,y （1） )、(x （2） ,y （2） )~(x （tr） ,y （tr） ); The car's satellite navigation position per second is recorded as: (j （1） , w （1） )、(j （2） , w （2） )~(j （tr） , w （tr） ); Step S23: Taking the laser radar positioning as the standard positioning, calculate the position deviation of the satellite navigation positioning relative to the laser radar positioning, and obtain: (Δx （1） , Δy （1） ), (Δx （2） , Δy （2） )~(Δx （tr） , Δy （tr） ); Among them, (Δx （1） , Δy （1） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the first second, Δx （1） The calculation formula is: Δx （1） =x （1） -j （1） , Δy （1） The calculation formula is: Δy （1） =y （1） -w （1） ; (Δx （2） , Δy （2） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the second second, Δx （2） The calculation formula is: Δx （2） =x （2） -j （2） , Δy （2） The calculation formula is: Δy （2） =y （2） -w （2） ; Similarly, (Δx （tr） , Δy （tr） ) represents the position deviation of the car satellite navigation positioning relative to the laser radar positioning at the trth second, Δx （tr） The calculation formula is: Δx （tr） =x （tr） -j （tr） , Δy （tr） The calculation formula is: Δy （tr） =y （tr） -w （tr） ; Step S24: Obtain the wheelbase of the car, recorded as L; construct the Jacobian matrix of the motion state transfer of the car at the (k+1)th second, recorded as matrix Fd （k+1） ; Construct the Jacobian matrix of the control state transfer, denoted as matrix Fc （k+1） .

3. The dynamic compensation algorithm based on vehicle detection data according to claim 2 is characterized in that: The subsequent steps of step S24 are as follows: Step S25: is the initial state matrix, denoted as matrix B （EO） ; The acceleration of the car at 0 seconds is a （0） is zero; calculate the Kalman gain of the vehicle's motion state from 0 to 1 second, recorded as; Kk （0-1） ; Step S26: Construct the motion state matrix of the car at the second second, denoted as matrix B （2） ; Matrix B （2） The mathematical expression is: ； Calculate the Kalman gain of the vehicle's motion state from the 1st to the 2nd second, denoted as; Kk （1-2） ; Step S27: Similarly, construct the motion state matrix of the car at the (tr-1)th second, recorded as matrix B （tr-1） ; Matrix B （tr-1） The mathematical expression is: ； Among them, (x （tr-1） ,y （tr-1） ) The laser radar position of the car at (tr-1) seconds; v （tr-1） and θ （tr-1） , respectively represent the speed and heading angle of the car at the (tr-1) second; Construct the motion state matrix of the car at the trth second, denoted as matrix B （tr） ; Matrix B （tr） The mathematical expression is: ； Calculate the Kalman gain of the vehicle's motion state from (tr-1) to tr seconds, denoted as; Kk （（tr-1）-tr） ; Step S28: Summarize the Kalman gain of the vehicle motion state from the 0th second to the 1st second, the 1st second to the 2nd second, and finally the (tr-1)th second to the trth second: Kk （0-1） , Kk （1-2） ~Kk （（tr-1）-tr） ; According to Kk （0-1） ~Kk （（tr-1）-tr） The determined vehicle motion state change equation is recorded as equation ME, and enters step S3.

4. The dynamic compensation algorithm based on vehicle detection data according to claim 2 is characterized in that: The specific steps of step S24 are as follows: Step S241: Record the position of the car at the kth second as (x （k） ,y （k） ), speed is denoted as v （k） , the heading angle is denoted as θ （k） , acceleration is denoted as a （k） , the steering angle is denoted as δ （k） ; The position of the car at (k+1) seconds is recorded as (x （k+1） ,y （k+1） ), speed is denoted as v （k+1） , the heading angle is denoted as θ （k+1） ; Step S242: construct the motion function of the car at the (k+1)th second, recorded as f(k+1); the mathematical expression of f(k+1) is: ； With (x （k） ,y （k） , v （k） ,θ （k） ) is the state variable, and the motion state matrix of the car is intercepted in the function f(k+1), denoted as Fd （k+1） ;Fd （k+1） The mathematical expression is: ； Calculate the matrix Fd （k+1） The partial derivative of the relative state variable is used to obtain the Jacobian matrix Fk of the car's motion state transfer at the (k+1)th second （k+1） ; Fk （k+1） The mathematical expression is: ； Step S243: （k） , δ （k） ) is the control variable, and the control state matrix of the car is intercepted in the function f(k+1), which is recorded as Fi （k+1） Fi （k+1） The mathematical expression is: ； Calculate the matrix Fi （k+1） The partial derivative of the relative control variable is used to obtain the control state transfer Jacobian matrix Fc of the vehicle at the (k+1)th second （k+1） ; Fc （k+1） The mathematical expression is: ; Among them, sec() represents the secant function.

5. The dynamic compensation algorithm based on vehicle detection data according to claim 3 is characterized in that: The specific steps of step S25 are as follows: Step S251: Construct the motion state matrix of the car at the first second, denoted as matrix B （1） ; Matrix B （1） The mathematical expression is: ； Calculate the matrix B （EO） Becomes matrix B （1） The state transfer matrix of （EO-1） ; Matrix B （EO-1） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Step S252: Matrix B （EO） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the first second, denoted as matrix Fk （1） ; Fk （1） The mathematical expression is: ； A （0） and δ （0） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the control state of the car at the first second, denoted as matrix Fc （1） ; Fc （1） The mathematical expression is: ； Step S253: Simultaneous matrix B （EO） and matrix B （1） , we get the motion state matrix of the car from the 0th to the 1st second, denoted as P （EO-1） ;P （EO-1） The mathematical expression is: ； Calculate the error matrix of the car changing from the 0th second motion state to the 1st second motion state, denoted as Pk （EO-1） ;Pk （EO-1） The calculation formula is: ; Where, represents matrix multiplication, and T represents the transpose of the matrix; Step S254: According to (Δx （1） , Δy （1） ) Construct the position error matrix of the car at the first second, denoted as Q （1） ;Q （1） The mathematical expression is: ； Calculate the Kalman gain of the car from the 0th second motion state to the 1st second motion state, denoted as Kk （0-1） ;Kk （0-1） The calculation formula is: 。 6. The dynamic compensation algorithm based on vehicle detection data according to claim 3 is characterized in that: The specific steps of step S26 are as follows: Step S261: Calculate matrix B （1） Becomes matrix B （2） The state transfer matrix of （1-2） ; Matrix B （1-2） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Step S262: Matrix B （1） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the second second, denoted as matrix Fk （2） ; Fk （2） The mathematical expression is: ； A （1） and δ （1） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the vehicle's control state at the second second, denoted as matrix Fc （2） ; Fc （2） The mathematical expression is: ； Step S263: Simultaneous matrix B （1） and matrix B （2） , we get the motion state matrix of the car from the 1st to the 2nd second, denoted as P （1-2） ;P （1-2） The mathematical expression is: ； Calculate the error matrix of the car changing from the 1st second motion state to the 2nd second motion state, recorded as Pk （1-2） ;Pk （1-2） The calculation formula is: ; Where, represents matrix multiplication, and T represents the transpose of the matrix; Step S264: According to (Δx （2） , Δy （2） ) Construct the position error matrix of the car at the first second, denoted as Q （2） ;Q （2） The mathematical expression is: ； Calculate the Kalman gain of the car from the 1st second motion state to the 2nd second motion state, denoted as Kk （1-2） ;Kk （1-2） The calculation formula is: 。 7. The dynamic compensation algorithm based on vehicle-mounted detection data according to claim 3 is characterized in that: The specific steps of step S27 are as follows: Step S271: Calculate matrix B （tr-1） Becomes matrix B （tr） The state transfer matrix of （（tr-1）-tr） ; Matrix B （（tr-1）-tr） The calculation formula is: ; Where × represents matrix multiplication, T represents the transpose of the matrix, and -1 represents the inverse of the matrix; Step S272: Matrix B （tr-1） Substitute the parameters in the matrix Fd （k+1） From the mathematical expression of , we get the Jacobian matrix of the car's motion state at the trth second, recorded as matrix Fk （tr） ; Fk （tr） The mathematical expression is: ； A （tr-1） and δ （tr-1） Substitute Fc （k+1） From the mathematical expression of , we get the Jacobian matrix of the vehicle's control state at the second second, denoted as matrix Fc （tr） ; Fc （tr） The mathematical expression is: ； a （tr-1） and δ （tr-1） , respectively represent the acceleration and steering angle of the car at the (tr-1)th second; Step S273: Simultaneous matrix B （tr-1） and matrix B （tr） , we get the motion state matrix of the car from (tr-1) to tr seconds, denoted as P （（tr-1）-tr） ;P （（tr-1）-tr） The mathematical expression is: ； Calculate the error matrix of the car from the (tr-1) second motion state to the tr second motion state, denoted as Pk （（tr-1）-tr） ;Pk （（tr-1）-tr） The calculation formula is: ; Where, represents matrix multiplication, and T represents the transpose of the matrix; Step S274: According to (Δx （tr） , Δy （tr） ) Construct the position error matrix of the car at the first second, denoted as Q （tr） ;Q （tr） The mathematical expression is: ； Calculate the Kalman gain of the car from the (tr-1) second motion state to the tr second motion state, denoted as Kk （（tr-1）-tr） ;Kk （（tr-1）-tr） The calculation formula is: 。 8. The dynamic compensation algorithm based on vehicle-mounted detection data according to claim 3 is characterized in that: The specific steps of step S3 are as follows: Step S31: Obtain the number of cars in the target area, denoted as ca; Obtain the position, speed, acceleration, heading angle and steering angle of all cars in the target area within the tr time as motion data; The speeds of the first to the cath cars in the motion data are recorded as: v （1,1） ~v （ca,tr） ; Acceleration is denoted by: a （1,1） ~a （ca,tr） ; The heading angle is denoted as: θ （1,1） ~θ （ca,tr） ; The steering angle is denoted as: δ （1,1） ~δ （ca,tr） ; Step S32: Record the laser radar positions corresponding to the 1st to the cth vehicles as: (x) （1,1） ,and （1,1） (x) （ca,tr） ,and （ca,tr） ); Substituting into (x （1,1） ,y （1,1） )~(x （ca,tr） ,y （ca,tr） )、v （1,1） ~v （ca,tr） ,θ （1,1） ~θ （ca,tr） 、a （1,1） ~a （ca,tr） and δ （1,1） ~δ （ca,tr） Substituting into equation ME, the estimated motion state of cars 1 to ca from the 2nd to trth seconds is calculated, and we get: ～ ； Step S33: Define the relation B1: ； Among them, × represents matrix multiplication, I represents a one-matrix; β represents the error coefficient; (x （i,j） ,y （i,j） ) represents the position of the i-th vehicle at the j-th second; v （i,j） and θ （i,j） , respectively represent the speed and heading angle of the i-th vehicle at the j-th second; the value range of i is: 1~ca, the value range of j is: 2~tr; represents the estimated motion state matrix of the i-th car at the j-th second; Step S34: Substitute steps S31 to S32 into relational expression B1, and count the number of items that do not satisfy relational expression B1, recorded as nw; The error coefficient of the calculation equation ME is recorded as nc; the calculation formula of nc is: ； The estimated motion state matrix that does not satisfy the relation B1 is recorded as a type A matrix; The actual vehicle motion state matrix corresponding to the type A matrix is ​​recorded as the type B matrix; Calculate the compensation coefficient of the car according to the A-type matrix and the B-type matrix; Step S35: extracting the compensation coefficient cx of the lateral position of the vehicle, the compensation coefficient cy of the longitudinal position, the compensation coefficient cv of the speed, and the compensation coefficient cθ of the heading angle; The vehicle motion state change equation ME is redefined according to the compensation coefficient, and the vehicle compensation coefficient equation is obtained, which is recorded as MOV; the mathematical expression of MOV is: ； aKk represents the average value of Kk(0-1)~Kk((tr-1)-tr), and QC represents the determination error matrix.

9. The dynamic compensation algorithm based on vehicle-mounted detection data according to claim 8, characterized in that: The specific steps of step S34 are as follows: Step S341: record the 1st to nwth A-type matrices as: ～ ； The 1st to nwth B-type matrices are denoted as: ～ ； Step S342: Integrate the 1st to nwth A-type matrices to obtain a matrix Ba; the mathematical expression of Ba is: ； Integrate the 1st to nwth B-type matrices to obtain the matrix Bb; the mathematical expression of Bb is: ； Calculate the difference matrix between matrix Bb and matrix Ba, denoted as Bc; Step S343: Calculate the covariance matrix of the matrix Bc, denoted as Cc; the calculation formula is: ; Where × represents matrix multiplication and T represents matrix transpose; Zc represents the centralization matrix of matrix Bc. The calculation formula of matrix Zc is: ; Wherein, I represents a matrix; Step S344: Split the matrix Cc by columns to obtain the matrix D （1） , matrix D （2） , matrix D （3） And the matrix D （4） ; Using Intel MKL library functions, define and calculate weight values ​​λ1, λ2, λ3, λ4, and λ5 and offsets μ1, μ2, μ3, μ4, and μ5 so that the matrix D （1） ~Matrix D （4） The following mathematical relationship is satisfied: ； Step S345: Calculate the influence coefficient of the lateral position of the vehicle on the positioning, denoted as bx, bx=λ1 / μ1; The influence coefficient of the longitudinal position of the vehicle on positioning is calculated and recorded as by, by = λ2 / μ2; The influence coefficient of vehicle speed on positioning is calculated and recorded as bv, bv = λ3 / μ3; The influence coefficient of the vehicle heading angle on positioning is calculated and recorded as bθ, bθ=λ4 / μ4; Step S346: Calculate the compensation coefficient of the lateral position of the vehicle and record it as cx, cx=bx / 4; The compensation coefficient for calculating the longitudinal position of the vehicle is denoted as cy, cy = by / 4; The compensation coefficient for calculating the vehicle speed is recorded as cv, cv = bv / 4; The compensation coefficient for calculating the heading angle of the vehicle is recorded as cθ, cθ=bθ / 4.

10. The dynamic compensation algorithm based on vehicle detection data according to claim 3, characterized in that: The specific steps of step S28 are as follows: Step S281: Calculate Kk （0-1） ~Kk （（tr-1）-tr） The average value of is denoted as aKk; The position of the car at the i-th second is recorded as (x （i） ,y （i） ), speed is denoted as v （i） , the heading angle is denoted as θ （i） , acceleration is denoted as a （i） , the steering angle is denoted as δ （i） ; The position of the car at the (i+1)th second is recorded as (x （i+1） ,y （i+1） ), speed is denoted as v （i+1） , the heading angle is denoted as θ （i+1） ; Step S282: （i） ,y （i） , v （i） ,θ （i） ) is the state variable, with (a （i） , δ （i） ) is the control variable, constructing the motion state of the car at the (i+1)th second (x （i+1） ,y （i+1） , v （i+1） ,θ （i+1） ) to obtain equation MO; The mathematical expression of MO is: ； Among them, × represents matrix multiplication, T represents the transpose of the matrix, CC represents the (1×4) error matrix, and the initial parameters in the matrix CC are 0; Step S283: Set v （1） ~v （tr） 、a （1） ~a （tr） ,θ （1） ~θ （tr） ,δ （1） ~δ （tr） and (x （1） ,y （1） )~(x （tr） ,y （tr） ) is substituted into equation MO, and MATLAB is used to fit the values ​​of the parameters in the matrix CC to obtain the determined error matrix, which is recorded as QC; Substituting matrix CC in equation MO with matrix QC, we get the definite equation of the vehicle motion state change, recorded as equation ME; the mathematical expression of ME is: 。

Images