Vehicle state estimation method and device

Abstract

The invention provides a vehicle state estimation method and device, and relates to the technical field of vehicle state estimation, and the method comprises the steps: inputting a previous moment sensor value and a previous moment state quantity of a vehicle into a vehicle kinematics discrete model, and obtaining a vehicle state space matrix and a vehicle state priori estimation value through vehicle state equation prediction; generating a prior boundary matrix in a prediction process based on the vehicle state space matrix and the noise boundary matrix; the effective point cloud data residual error of the radar sensor at the current moment is fused with the corresponding observation noise according to the calculation sequence to finally obtain a vehicle state observation value in the observation process; constructing a correction matrix based on the priori boundary matrix and the vehicle state observation value, obtaining a vehicle state posteriori estimation value at the current moment by using the vehicle state priori estimation value and the observation value at the current moment and combining the correction matrix, and updating the boundary matrix at the same time; an accurate and reliable vehicle state estimation result at each moment is finally obtained in the whole vehicle state estimation process.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle state estimation, and in particular to a vehicle state estimation method and device. Background Art

[0002] Laser Inertial Odometry (LIO) is a technology that combines data from Laser Radar (LiDAR) and Inertial Measurement Unit (IMU) to achieve robot positioning and mapping by calculating the spatiotemporal relationship between laser point clouds and IMU measurements.

[0003] However, the inventors have discovered that in actual vehicle driving applications, the accuracy of vehicle status determination is not high due to the accuracy of the lidar and the actual complex driving environment, which in turn significantly affects the performance of vehicle positioning and mapping. Summary of the Invention

[0004] In view of this, an object of the present invention is to provide a vehicle state estimation method and device to alleviate the technical problem of low vehicle positioning and mapping accuracy in the prior art.

[0005] In a first aspect, the present invention provides a vehicle state estimation method, comprising:

[0006] The sensor values ​​and the vehicle's motion state at the previous moment are input into the vehicle motion discrete model, and then a prediction is performed using the vehicle state equation to determine a linear vehicle state space matrix and an estimated value of the vehicle state at the current moment. The vehicle state space matrix is ​​used to represent the changes in the vehicle state and the changes in the sensor collected values ​​during the vehicle's motion.

[0007] generating a priori boundary matrix for a current prediction process based on the vehicle state space matrix and the current noise boundary matrix;

[0008] Obtaining a vehicle state observation value with introduced observation noise based on a residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimation value at the current moment;

[0009] Based on the prior boundary matrix, the residual fusion result, the vehicle state observation value and the motion state of the vehicle at the current moment, the vehicle state estimation result of the posterior state at the current moment is estimated.

[0010] In an optional embodiment, the steps of inputting sensor values ​​and the vehicle's motion state at the previous moment into a vehicle motion discrete model, performing prediction using the vehicle state equation, and determining a linear vehicle state space matrix and a vehicle state estimate at the current moment include:

[0011] Determine the state transfer matrix of the vehicle at the previous moment based on the acceleration and angular velocity output by the sensor at the previous moment and the motion state of the vehicle at the previous moment;

[0012] Determine the motion state of the vehicle at the current moment based on the vehicle motion discrete model, the motion state of the vehicle at the previous moment, the state transfer matrix, and the time difference between the previous moment and the current moment;

[0013] Based on the motion state quantity at the current moment, the vehicle motion discrete model and the vehicle state equation at the current moment are combined to determine a vehicle state space matrix in a linear state;

[0014] The vehicle state space matrix is ​​combined with the vehicle state equation at the current moment to predict the vehicle state estimation value at the current moment.

[0015] In an optional embodiment, the step of generating a priori boundary matrix in the current prediction process based on the vehicle state space matrix and the current noise boundary matrix includes:

[0016] Multiplying the vehicle state space matrix by the current noise boundary matrix to obtain a first matrix;

[0017] First, multiply the noise upper bound matrix of the current noise boundary matrix by the current noise boundary matrix, and then perform row and diagonal matrix processing to obtain a second matrix; wherein the noise upper bound matrix is ​​a diagonal matrix of noise used in the current prediction process;

[0018] First, the noise upper bound matrix is ​​multiplied by the motion state of the vehicle at the previous moment, and then row and diagonal matrix processing is performed to obtain a third matrix;

[0019] The first matrix, the second matrix, and the third matrix are sequentially concatenated column by column to generate a priori boundary matrix in the current prediction process.

[0020] In an optional embodiment, the step of obtaining a vehicle state observation value with observation noise introduced based on a residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimate at the current moment includes:

[0021] Whenever a valid point is identified from the current radar point cloud data, the residual value corresponding to the valid point is substituted into the observation value calculation formula to generate an output matrix and a residual fusion result. This is done until all valid points in the radar point cloud data at the current moment are traversed, obtaining an output matrix corresponding to each valid point and a residual fusion result set for representing the vehicle state. The residual fusion result set includes a first residual fusion result before the current moment and a second residual fusion result at the current moment.

[0022] performing a least squares calculation on an augmented measurement equation formed by the first residual fusion result and the second residual fusion result to obtain a vehicle state estimation value with observation noise introduced;

[0023] The vehicle state estimation value with the observation noise introduced and the residual fusion result set are substituted into the observation equation, and the vehicle state observation value with the observation noise introduced is output.

[0024] In an optional embodiment, the step of estimating a vehicle state estimation result of a posterior state at a current moment based on the prior boundary matrix, the residual fusion result, the vehicle state observation value, and the vehicle's motion state at a current moment includes:

[0025] Constructing a correction matrix based on the prior boundary matrix and the residual fusion result;

[0026] The difference between the vehicle state observation value and the vehicle's current motion state is multiplied by the correction matrix, and then added to the vehicle's current motion state to estimate the vehicle state estimation result at the current moment under the action of the correction matrix.

[0027] In an optional embodiment, the method further comprises:

[0028] The posterior boundary matrix constructed based on the correction matrix and the prior boundary matrix is ​​reduced in order to serve as the current noise boundary matrix corresponding to the next moment.

[0029] In an optional embodiment, the method further comprises:

[0030] Based on the vehicle state estimation result at the current moment, vehicle positioning and mapping are performed.

[0031] In a second aspect, the present invention provides a vehicle state estimation device, comprising:

[0032] A determination module inputs the sensor values ​​and the vehicle's motion state at the previous moment into a vehicle motion discrete model, and then predicts using the vehicle state equation to determine a linear vehicle state space matrix and an estimated value of the vehicle state at the current moment; wherein the vehicle state space matrix is ​​a matrix used to represent changes in vehicle state and sensor collected values ​​during vehicle motion;

[0033] A generation module, which generates a priori boundary matrix in a current prediction process based on the vehicle state space matrix and the current noise boundary matrix;

[0034] An observation module obtains a vehicle state observation value with introduced observation noise based on a residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimate at the current moment;

[0035] The estimation module estimates the vehicle state estimation result of the posterior state at the current moment based on the prior boundary matrix, the residual fusion result, the vehicle state observation value and the motion state of the vehicle at the current moment.

[0036] In a third aspect, the present invention provides an electronic device comprising a memory and a processor, wherein the memory stores a computer program that can be run on the processor, and when the processor executes the computer program, the steps of the method described in any one of the aforementioned embodiments are implemented.

[0037] In a fourth aspect, the present invention provides a machine-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement the steps of the method described in any one of the aforementioned embodiments.

[0038] A vehicle state estimation method and device provided by an embodiment of the present invention combines a vehicle motion discrete model and a state equation, and inputs sensor and vehicle data at the previous moment, thereby obtaining a vehicle state estimation value at the current moment, as well as a vehicle state space matrix used to characterize the vehicle state and sensor collected values. Combined with the noise boundary matrix corresponding to the current moment, a priori boundary matrix in the vehicle state prediction process can be generated; based on the vehicle state estimation value at the current moment, the observation value in the radar point cloud data, i.e., the residual fusion result of each valid point, is combined to obtain a vehicle state observation value with the introduction of observation noise; finally, based on the aforementioned prior boundary matrix, the residual fusion result constructs a correction matrix, as well as the vehicle state observation value and the vehicle's motion state quantity at the current moment, to estimate the vehicle state estimation result of the posterior state at the current moment. The vehicle state estimation result obtained in this way is more accurate.

[0039] Other features and advantages of the present disclosure will be set forth in the following description, or some features and advantages may be inferred or unambiguously determined from the description, or may be learned by practicing the above-mentioned technology of the present disclosure.

[0040] In order to make the above-mentioned objectives, features and advantages of the present disclosure more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the specific embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0042] Figure 1 A flow chart of a vehicle state estimation method provided by an embodiment of the present invention;

[0043] Figure 2 A functional module diagram of a vehicle state estimation device provided by an embodiment of the present invention;

[0044] Figure 3 A schematic diagram of the hardware architecture of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

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

[0046] The inventors have found that in traditional vehicle positioning and mapping applications, the state quantity of the vehicle during operation is updated using an iterative method, which is prone to falling into local optimality under strong nonlinear conditions, thereby affecting positioning and mapping performance.

[0047] For example, the noise of radar sensors during vehicle operation often does not conform to a normal distribution. The state variables of the vehicle are updated iteratively when the vehicle is running. Under strong nonlinear conditions, the state variables are prone to divergence or fall into local optima. Consequently, in actual applications, positioning and mapping performance are susceptible to negative impacts from both the sensor itself and environmental factors, as follows:

[0048] Low-profile lidar sensors produce high noise levels, have low measurement accuracy, and are susceptible to external influences. In low-feature environments like tunnels and underground parking lots, or in complex dynamic environments like overpasses and in rainy and snowy conditions, sensor data exhibits poor stability, resulting in low vehicle positioning and mapping accuracy.

[0049] Based on this, an embodiment of the present invention provides a vehicle state estimation method and device, which utilizes boundary information of noise to replace the traditional statistical information of probability distribution of noise to achieve accurate vehicle state estimation.

[0050] To facilitate understanding of this embodiment, a vehicle state estimation method disclosed in an embodiment of the present invention is first introduced in detail. This method can be applied to a vehicle control device such as an on-board controller.

[0051] Figure 1 A flow chart of a vehicle state estimation method provided by an embodiment of the present invention.

[0052] like Figure 1 As shown, the method includes the following steps:

[0053] In step S102, the sensor values ​​at the previous moment and the vehicle's motion state at the previous moment are input into the vehicle motion discrete model, and then predicted through the vehicle state equation to determine the linear vehicle state space matrix and the vehicle state estimation value at the current moment.

[0054] Vehicle state space matrix Among them, the vehicle state space matrix is ​​a matrix used to represent the changes in vehicle state and sensor collected values ​​during vehicle movement.

[0055] Exemplarily, step S102 may also be implemented by the following steps:

[0056] Step 1.1: Determine the state transfer matrix of the vehicle at the previous moment based on the acceleration and angular velocity output by the sensor at the previous moment and the motion state of the vehicle at the previous moment.

[0057] Represents the vehicle's motion state X at the previous moment t, including: position vector pos(x, y, z), rotation matrix vector Rot (representing the change in vehicle motion at adjacent moments), rotation offset vector offet_R_L_I of the lidar and IMU (R stands for rotate, L stands for lidar, I stands for IMU, and offet stands for external parameter), translation offset vector offet_T_L_I of the lidar and IMU (T stands for translation, L stands for lidar, and I stands for IMU), vehicle velocity vector v, IMU's gyroscope bias vector bg (systematic deviation of the gyroscope measurement value in the IMU sensor), IMU's acceleration bias vector ba (systematic deviation of the accelerometer measurement value in the IMU sensor), and vehicle gravity acceleration vector g; each of the above vectors is 3*1 dimensional, and the motion state X is a 24*1 state quantity;

[0058] Here, the vehicle's motion state X at the previous moment and the acceleration and angular velocity (a t ,ω t ) as input, the following state transfer matrix is ​​obtained:

[0059]

[0060] Among them, X t is the motion state quantity at time t, v t is the vehicle speed at time t, ω t is the angular velocity at time t, u t is the input information at time t (a t ,ω t ), Rat +g t R is the rotation matrix that converts the sensor IMU acceleration to the world coordinate system. This formula transfers the vehicle acceleration provided by the IMU from the vehicle system to the world system. The state transfer matrix F represents the changes in the vehicle's own state and sensor values ​​during motion.

[0061] Step 1.2, based on the vehicle motion discrete model, the vehicle motion state quantity at the previous moment, the state transfer matrix, and the time difference between the previous moment and the current moment, determine the motion state quantity at the current moment.

[0062] On this basis, combined with the vehicle motion discrete model:

[0063] X t+1 =X t +Δt*F

[0064] Among them, Δt represents the time difference between the previous moment and the current moment. The function of the state transfer matrix F is to integrate the values ​​collected by the sensor into the state quantity X, so that the motion state quantity X at the current moment can be obtained with the help of the motion discrete model. t+1 , X t is the motion state quantity at the previous moment;

[0065] It should be noted that the aforementioned state transfer matrix F is a 24*1 vector, in which each 0 element is a 3*1 vector:

[0066]

[0067] Step 1.3: Based on the current state of motion, the current vehicle motion discrete model is linearized to determine the linear vehicle state space matrix F. x ;

[0068] It can be understood that the vehicle state error equation is:

[0069]

[0070] in, Represents the estimated value of the vehicle state at the current moment, X t+1 Represents the true value of the vehicle state at the current moment, Represents the vehicle state error between the true value and the estimated value at the current moment. The first frame of the vehicle state estimate uses the true value, and the other frames use the estimated value;

[0071] The discrete model equation of vehicle motion at the current moment is:

[0072] X t+1 =X t +Δt*f(Xt ,u t )

[0073]

[0074] The vehicle motion discrete model and the vehicle state error equation are combined, and the vehicle state error value is linearized using the first-order Taylor expansion of the binary function. Get the vehicle state space matrix F representing the linear state x , that is, the linearized Jacobian matrix of the state equation, is the error of the vehicle state quantity X at the t+1 moment (the current moment) when the noise W=0 For the partial derivative of the vehicle state X. Among them, the vehicle state space matrix F x It is a 24*24 matrix.

[0075] Step 1.4: Combine the vehicle state space matrix with the vehicle state equation at the current moment to predict the estimated value of the vehicle state at the current moment.

[0076] Here, the vehicle state space matrix F obtained in the previous step is x and the vehicle state equation X t+1 =F X *X t Combined, we get the estimated value of the vehicle state at the current moment

[0077] Step S104: Based on the vehicle state space matrix F x And the current noise boundary matrix H, generate the prior boundary matrix in the current prediction process.

[0078] The initial current noise boundary matrix H represents a special convex polyhedron in spatial geometry, with a center point representing the geometric center. In three-dimensional space, the initial position center point in the world system is the origin.

[0079] Throughout the vehicle's operation, the noise boundary matrix recursively constructs a set of matrices representing the true state variables that are compatible with the vehicle's motion model, observation model, and noise. In most cases, this set cannot be accurately described, so a feasible set is often approximated using geometric shapes that can encompass it. Common geometric shapes include intervals, ellipsoids, parallelepipeds, ordinary polyhedra, and fully symmetric polyhedra. Among fully symmetric polyhedra, a regular convex polyhedron is typically used to represent the current noise boundary matrix. Based on the aforementioned geometric shape comparison, the vehicle state variables are relatively high-dimensional in three-dimensional space and contain a wealth of vehicle information, necessitating a polyhedron that can fit these state variables. Furthermore, the update process of the noise boundary matrix can be transformed into simple matrix operations.

[0080] Exemplarily, step S104 is a step of determining a priori boundary matrix based on the aforementioned vehicle state space matrix and the current noise boundary corresponding to the current moment, including:

[0081] In step 2.1, the vehicle state space matrix is ​​multiplied by the current noise boundary matrix to obtain the first matrix.

[0082] The first part of the prior boundary matrix is ​​the vehicle state space matrix F x Multiply the current noise boundary matrix to obtain the first 24*24 matrix.

[0083] In step 2.2, the noise upper bound matrix of the current noise boundary matrix is ​​first multiplied by the current noise boundary matrix, and then each row of the matrix is ​​summed up, and each row sum is converted into a diagonal element of the second matrix to obtain the second matrix.

[0084] The noise upper bound matrix is ​​a diagonal matrix of noise used in the current prediction process.

[0085] Set the noise upper bound of the current noise boundary matrix H to the A_plus matrix, initialize the A_plus matrix to the identity matrix, multiply the current noise boundary matrix by A_plus, and then perform a row sum diagonalization operation. That is, sum each row element of the multiplied matrix and place the sum of each row element in the diagonal position of the corresponding row of the second matrix to fill the second matrix, thereby obtaining a 24*24 second matrix.

[0086] In step 2.3, first multiply the noise upper bound matrix and the vehicle's motion state at the previous moment, and then perform row and diagonal matrix processing to obtain the third matrix.

[0087] After the noise upper bound matrix A_plus is multiplied by the vehicle's motion state x at the previous moment, row sum diagonalization is performed. The result is a 24*1 vector, which is placed on the diagonal of the third matrix, thereby obtaining a 24*24 third matrix.

[0088] In step 2.4, the first matrix, the second matrix, and the third matrix are sequentially concatenated column by column to generate the prior boundary matrix in the current prediction process.

[0089] From left to right, the rows of the first matrix, the second matrix, and the third matrix are aligned and spliced ​​in turn to reconstruct a new boundary matrix, which is called the prior boundary matrix in the prediction process.

[0090] It should be noted that the process of constructing the prior boundary matrix is ​​the process of generating the augmented matrix. The linear operation of the matrix during the operation is actually the scaling of the size of the regular convex polyhedron used to represent the noise boundary matrix.

[0091] The three matrices corresponding to the prior boundary matrix are actually based on the linearized vehicle state space matrix F x The matrix expression corresponding to each part of the expanded variable is as follows:

[0092] First, assume that the noise of the motion process is unknown but bounded, that is, the noise at time t |W t |≤W max ; Regarding the vehicle motion process, the vehicle state space matrix F x Unknown but bounded |ΔF x |≤A max ; The motion process noise W and the initial state quantity X0 are both located within the initial boundary matrix H0, that is, X0∈H0,W∈H0;

[0093] Here, the noise upper bound matrix W max =diag{W 1max , W 2max ,...W nmax}, that is, the noise in each prediction process will correspond to an upper bound, and the corresponding upper bound is stored in the diagonal elements of a matrix.

[0094] Again, assuming that the state quantity X at the previous moment t is located in the boundary matrix H, X t ∈H t ; At this time, the vehicle state equation is X t+1 ∈H t+1 ;

[0095] In general, it is impossible to solve the current vehicle state prediction set (the vehicle's motion state at the current moment) based on this t+1 Accurate range, so calculate the noise boundary matrix H at the next moment t+1 As an approximate set of vehicle state predictions;

[0096] The state quantity of the vehicle changes during motion and there is noise. The state space expression of the model is:

[0097]

[0098] Among them, C represents the coefficient matrix of the output corresponding to the effective point observed by the radar at time t, V t represents the observation noise, E is the noise distribution matrix, that is, W max , F X is the state coefficient matrix, ΔF X is the unknown uncertainty matrix, W t It is the noise during the movement. t is the observed value;

[0099] According to the above conditions, we can think of solving the boundary matrix H corresponding to the state prediction set. t+1 , that is, X t+1 ∈H t+1 ;X t+1 Expanded into three parts

[0100] Therefore, the new boundary matrix is ​​approximately represented as three parts:

[0101] 1)X t The corresponding boundary matrix is ​​H X , that is, the first part F X H X ;

[0102] 2) ΔF X The unknown is uncertain, so let’s set an upper bound A-plus, A-plus·H X Part II;

[0103] 3) Using A-plus X t As the third part, it serves as the upper limit of the noise boundary under this state quantity.

[0104] Step S106 , obtaining a vehicle state observation value with observation noise introduced based on the residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimation value at the current moment.

[0105] Here, the residual fusion result is the observation value. Step S106 can be specifically implemented by the following steps, including:

[0106] Step 3.1: Convert each feature point obtained after preprocessing the current radar point cloud data into a world coordinate system; perform target plane fitting based on a preset number of neighboring points corresponding to each feature point in the world coordinate system, and determine the distance between each feature point and the corresponding fitted target plane; and determine each feature point whose distance is less than a preset threshold as a valid point.

[0107] Among them, the distance is the residual value. The feature points whose distance from the fitted target plane exceeds the preset threshold are understood as feature points with large residuals, and the feature points whose residual values ​​are within the preset threshold range are selected as valid points.

[0108] In step 3.2, whenever a valid point is identified from the current radar point cloud data, the residual value corresponding to the valid point is substituted into the observation value calculation formula to generate an output matrix and a residual fusion result. This process continues until all valid points in the radar point cloud data at the current moment are traversed, and the output matrix corresponding to each valid point and a set of residual fusion results used to characterize the vehicle state are obtained.

[0109] Among them, the residual fusion result set can be understood as a matrix, which includes the first residual fusion result Z before the current moment t-1 And the second residual fusion result y at the current moment t .

[0110] During the sampling time, through the time sequence within the sampling interval, each feature point is judged to be valid and the residual of the valid point is immediately calculated. Whenever a residual is calculated, it is substituted into the observation value calculation formula and fused to obtain a new residual h, that is, the residual fusion result set. This residual represents the error between the current observation and the true value.

[0111] As an optional embodiment, the order in which each feature point is judged to be a valid point can be saved. After the residuals of all valid points are calculated, the residuals corresponding to each valid point are substituted into the observation value calculation formula in this order to generate an output matrix and residual fusion result.

[0112] Step 3.3, the first residual fusion result Z t-1 and the second residual fusion result y t The augmented measurement equation is calculated by the least square method to obtain the vehicle state estimation value with the introduction of observation noise.

[0113] Here, the fusion result Z of the first t-1 measurements is t-1 and the tth measurement value y t The following augmented measurement equation is formed:

[0114]

[0115] Among them, V t-1 The measurement noise of the fusion at the previous t-1 time; therefore, the estimated value can be obtained using the least squares algorithm as follows:

[0116]

[0117] Here, C represents the output coefficient matrix of the effective point of radar observation at time t, which has been calculated when calculating the residual, y t It is the residual of the K-th fusion, Z t-1 It is the result of the fusion at the previous t-1 moment. Substituting the residuals of all valid points into the calculation, we get a total value, the residual fusion result set h, that is, Z t , X next is the estimated value of the vehicle state with observation noise introduced at time t, i.e. the current time.

[0118] In step 3.4, the vehicle state estimation value with the introduction of observation noise and the residual fusion result set are substituted into the observation equation, and the vehicle state observation value with the introduction of observation noise is output.

[0119] X next and Z t Substitute into the observation equation y next =CX next +Z t , because the front and rear sensors are the same, C_indentity is the unit matrix, this y next Represents the vehicle state observation value with observation noise added, that is, the state quantity output by the vehicle after observation, which is used to correct the boundary matrix and state quantity in subsequent steps to provide correction values.

[0120] The LiDAR sensor on a vehicle continuously observes and calibrates the vehicle's state. This embodiment of the present invention uses a sampling time-sequential fusion method. This method uses the time sequence of the sensor's observed point cloud data within the sampling interval, i.e., the order in which each point is determined as valid. The residual results (the vehicle's state after observation) calculated from the point cloud data are fused one by one (by sequentially adding the observations to the boundary matrix), deriving new fused estimated observations. Compared to the batch calculation method used in previous solutions, this embodiment of the present invention eliminates the need to wait until all point cloud data for the current frame is available before performing fusion, thus reducing latency and computational burden.

[0121] Step S108 , based on the prior boundary matrix, the residual fusion result, the vehicle state observation value and the vehicle's current motion state, estimate the vehicle state estimation result of the posterior state at the current moment.

[0122] In the above prediction process, the vehicle state estimation value is obtained After the state prediction set is formed, the vehicle state observation value y is added with the observation noise using the measurement value at time t+1 next Update the boundary matrix to obtain the set of posterior state estimates at time t+1, that is, the final accurate and reliable state of the car, that is, the optimal linear state estimate under the boundary matrix.

[0123] At time i+1, with the observed value y next The consistent state is also within a range, but the intersection of this range and the state prediction set does not meet the boundary matrix conditions, so it is necessary to update the boundary matrix to approximate the state estimation set. The correction matrix is ​​constructed using the matrix constructed by the prior boundary matrix in the prediction process and the residual of the effective point in the observation process (the residual fusion result):

[0124] Step 4.1: Construct the correction matrix based on the prior boundary matrix and the residual fusion result.

[0125] λ=H·H T (H.H T +h·h T ) -1

[0126] Among them, h represents the calculated new residual matrix of 24*1, and each element is placed on the diagonal of the 24*24 zero matrix; the correction matrix λ can be combined with the observation value y_next and the vehicle's current motion state X t+1 .

[0127] In step 4.2, the difference between the vehicle state observation value and the vehicle's current motion state is multiplied by the correction matrix, and then added to the vehicle's current motion state to estimate the vehicle state estimation result at the current moment under the action of the correction matrix, as shown in the following formula:

[0128] X t+1 final=X t+1 +λ(y next -X t+1 )

[0129] Among them, the vehicle state estimation result at the current moment under the action of the correction matrix is ​​X t+1 final.

[0130] In a preferred embodiment of practical application, the vehicle motion discrete model and the vehicle state equation are combined, and the sensor and vehicle data at the previous moment are input to obtain the vehicle state estimate at the current moment, as well as the vehicle state space matrix used to characterize the vehicle state and the sensor collected values. Combined with the noise boundary matrix corresponding to the current moment, the prior boundary matrix in the vehicle state prediction process can be generated; on the basis of the vehicle state estimate at the current moment, the observation value in the radar point cloud data, that is, the residual fusion result of each valid point, is combined to obtain the vehicle state observation value with the introduction of observation noise; finally, based on the aforementioned prior boundary matrix, the residual fusion result constructs a correction matrix, as well as the vehicle state observation value and the vehicle motion state quantity at the current moment, to estimate the vehicle state estimate result of the posterior state at the current moment. The vehicle state estimation result obtained in this way is more accurate.

[0131] In some embodiments, in order to prevent the boundary matrix from gradually accumulating columns as the observation values ​​gradually accumulate, it is necessary to reduce the order of the boundary matrix and update it. The order is consistent with the original boundary matrix order, and finally a stable state quantity and a boundary matrix updated throughout the entire process are obtained. The reduced-order boundary matrix can participate in the update process of the motion state quantity at the next moment. The method further includes:

[0132] In step 5.1, the posterior boundary matrix constructed based on the correction matrix and the prior boundary matrix is ​​reduced in order to serve as the current noise boundary matrix corresponding to the next moment.

[0133] At the same time, the first part of the new boundary matrix is ​​calculated using the prior boundary matrix and the correction matrix λ, and then the residual fusion result matrix constructed by the correction matrix and the residual of the effective points in the observation process is used to calculate the second part of the new boundary matrix. The rows are aligned and the columns are summed in order from left to right, and finally the boundary matrix H of the entire prediction and observation process is obtained. next .

[0134] Assume that at time t+1, the measurement equation has been obtained:

[0135] Z t+1 =C t+1 X t+1 +V t+1

[0136] Among them, C t+1 is the sensor output matrix, X t+1 is the motion state quantity at the current moment, V t+1 is the observation noise, which is fused with the residual result Z t+1 The set of states that remain consistent will also be within a range, and a polyhedron is used to represent ζ-lambda. During the prediction process, the intersection of the state prediction set and the polyhedron ζ is not located in the boundary matrix, so the noise boundary matrix H needs to be updated;

[0137] The final state set There exists a correction matrix such that Established;

[0138] Among them, H next The first part represents the boundary matrix after adding the observation value correction, and the second part represents the error between the true value and the observed value during observation. In this way, the observation information is added to the matrix that meets the boundary conditions.

[0139] It's important to note that the boundary matrix given during the initial prediction process is a specific matrix that determines the size and shape of the vehicle's state set during operation. The number of rows in the boundary matrix represents the spatial dimension, while the number of columns represents the number of edges of a regular convex polyhedron. A regular convex polyhedron has 24 edges, corresponding to a 24-dimensional state variable. In three-dimensional space, the boundary matrix is ​​initially padded with a 3x24 matrix. To ensure dimensional alignment throughout the subsequent update process, it is padded to 24x24, using a zero matrix.

[0140] Initially, the boundary matrix is ​​calculated through the prediction process to obtain a 24*72 matrix. After correction and fusion calculation in the observation process, a 24*96 matrix is ​​obtained. After further order reduction, a 24*24 matrix is ​​obtained. This boundary matrix is ​​used as the input of the boundary matrix in the prediction process at the next moment, and this cycle is repeated.

[0141] Based on the above embodiment, this method can also be used to accurately determine the vehicle state estimation result and implement the corresponding vehicle application to ensure application reliability, including:

[0142] Step 6.1: Based on the vehicle state estimation results at the current moment, vehicle positioning and mapping are performed.

[0143] In the embodiment of the present invention, when the lidar point cloud noise does not satisfy the normal distribution, a priori matrix is ​​constructed through noise boundary information. The state estimation value obtained by combining the state quantity prediction and the correction matrix constructed by the residual fusion result obtained based on the unique fusion sequence of point cloud observation are combined. A reduced-order algorithm is used without iterative optimization to calculate the vehicle state estimation result, thereby obtaining better positioning and mapping results and inertial odometry acquisition results.

[0144] In some embodiments, as Figure 2 As shown, an embodiment of the present invention further provides a vehicle state estimation device 200, comprising:

[0145] Determination module 201 inputs the sensor values ​​and the vehicle's motion state at the previous moment into a vehicle motion discrete model, and then uses the vehicle state equation to perform predictions to determine a linear vehicle state space matrix and an estimated value of the vehicle state at the current moment. The vehicle state space matrix is ​​a matrix that represents changes in vehicle state and sensor values ​​during vehicle motion.

[0146] A generation module 202 generates a priori boundary matrix for a current prediction process based on the vehicle state space matrix and the current noise boundary matrix;

[0147] An observation module 203 obtains a vehicle state observation value with observation noise introduced based on a residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimation value at the current moment;

[0148] The estimation module 204 estimates the vehicle state estimation result of the posterior state at the current moment based on the prior boundary matrix, the residual fusion result, the vehicle state observation value and the vehicle's current motion state.

[0149] Furthermore, the determination module 201 is specifically used to determine the state transfer matrix of the vehicle at the previous moment based on the acceleration and angular velocity output by the sensor at the previous moment, and the motion state quantity of the vehicle at the previous moment; determine the motion state quantity at the current moment based on the vehicle motion discrete model, the motion state quantity of the vehicle at the previous moment, the state transfer matrix, and the time difference between the previous moment and the current moment; based on the motion state quantity at the current moment, the vehicle motion discrete model and the vehicle state equation at the current moment are combined to determine the vehicle state space matrix in a linear state; the vehicle state space matrix is ​​combined with the vehicle state equation at the current moment to predict the vehicle state estimate value at the current moment.

[0150] Furthermore, the generation module 202 is specifically used to multiply the vehicle state space matrix by the current noise boundary matrix to obtain a first matrix; first multiply the noise upper bound matrix of the current noise boundary matrix by the current noise boundary matrix, and then perform row and diagonal matrix processing to obtain a second matrix; wherein, the noise upper bound matrix is ​​a diagonal matrix of noise used in the current prediction process; first multiply the noise upper bound matrix and the motion state quantity of the vehicle at the previous moment, and then perform row and diagonal matrix processing to obtain a third matrix; and sequentially splice the first matrix, the second matrix, and the third matrix by column to generate a priori boundary matrix in the current prediction process.

[0151] Furthermore, the observation module 203 is specifically used to, whenever a valid point is identified from the current radar point cloud data, substitute the residual value corresponding to the valid point into the observation value calculation formula to generate an output matrix and a residual fusion result, until all valid points in the radar point cloud data at the current moment are traversed, and obtain the output matrix corresponding to each valid point and a residual fusion result set for characterizing the vehicle state; wherein the residual fusion result set includes the first residual fusion result before the current moment and the second residual fusion result at the current moment; perform the least squares calculation on the augmented measurement equation composed of the first residual fusion result and the second residual fusion result to obtain the vehicle state estimation value with the introduction of observation noise; substitute the vehicle state estimation value with the introduction of observation noise and the residual fusion result set into the observation equation, and output the vehicle state observation value with the introduction of observation noise.

[0152] Furthermore, the estimation module 204 is specifically used to construct a correction matrix based on the prior boundary matrix and the residual fusion result; multiply the difference between the vehicle state observation value and the motion state quantity of the vehicle at the current moment by the correction matrix, and then add it to the motion state quantity of the vehicle at the current moment to estimate the vehicle state estimation result at the current moment under the action of the correction matrix.

[0153] Furthermore, the device is also used to reduce the order of the posterior boundary matrix constructed based on the correction matrix and the prior boundary matrix to serve as the current noise boundary matrix corresponding to the next moment.

[0154] Furthermore, the device is also used to perform vehicle positioning and mapping based on the vehicle state estimation result at the current moment.

[0155] Figure 3 Schematic diagram of the hardware architecture of the electronic device 300 provided in an embodiment of the present invention. Figure 3 As shown, the electronic device 300 includes a machine-readable storage medium 301 and a processor 302. It may also include a non-volatile storage medium 303, a communication interface 304, and a bus 305. The machine-readable storage medium 301, the processor 302, the non-volatile storage medium 303, and the communication interface 304 communicate with each other via the bus 305. The processor 302 reads and executes the machine-executable instructions for vehicle state estimation in the machine-readable storage medium 301 to perform the vehicle state estimation method described in the above embodiment.

[0156] The machine-readable storage medium referred to herein may be any electronic, magnetic, optical, or other physical storage device that can contain or store information, such as executable instructions, data, and the like. For example, the machine-readable storage medium may be: RAM (Random Access Memory), volatile memory, non-volatile memory, flash memory, a storage drive (such as a hard disk drive), any type of storage disk (such as a CD, DVD, etc.), or similar storage media, or a combination thereof.

[0157] The non-volatile medium may be a non-volatile memory, a flash memory, a storage drive (such as a hard drive), any type of storage disk (such as an optical disk, a DVD, etc.), or similar non-volatile storage medium, or a combination thereof.

[0158] It can be understood that the specific operation methods of each functional module in this embodiment can refer to the detailed description of the corresponding steps in the above method embodiment, and will not be repeated here.

[0159] The computer-readable storage medium provided in the embodiment of the present invention stores a computer program. When the computer program code is executed, the vehicle state estimation method described in any of the above embodiments can be implemented. For specific implementation, please refer to the method embodiment, which will not be repeated here.

[0160] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described systems and devices can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0161] In addition, in the description of the embodiments of the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they may refer to fixed connections, detachable connections, or integral connections; they may refer to mechanical connections or electrical connections; they may refer to direct connections or indirect connections through an intermediate medium; and they may refer to internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on the specific circumstances.

[0162] In the description of the present invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings and are intended solely to facilitate and simplify the description of the present invention. They are not intended to indicate or imply that the devices or components referred to must have, be constructed, or operate in a specific orientation, and therefore should not be construed as limitations on the present invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0163] Finally, it should be noted that the above-described embodiments are only specific implementation methods 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 has been described in detail with reference to the above-mentioned embodiments, ordinary technicians in this field should understand that any technician familiar with this technical field can still modify the technical solutions recorded in the above-mentioned embodiments within the technical scope disclosed by the present invention, or make equivalent replacements for some of the technical features therein; and these modifications, changes or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered by the protection scope of the present invention.

Claims

1. A vehicle state estimation method, characterized in that: include: The sensor values ​​and the vehicle's motion state at the previous moment are input into the vehicle motion discrete model, and then a prediction is performed using the vehicle state equation to determine a linear vehicle state space matrix and an estimated value of the vehicle state at the current moment. The vehicle state space matrix is ​​used to represent the changes in the vehicle state and the changes in the sensor collected values ​​during the vehicle's motion. generating a priori boundary matrix for a current prediction process based on the vehicle state space matrix and the current noise boundary matrix; Obtaining a vehicle state observation value with introduced observation noise based on a residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimation value at the current moment; Based on the prior boundary matrix, the residual fusion result, the vehicle state observation value and the motion state of the vehicle at the current moment, the vehicle state estimation result of the posterior state at the current moment is estimated.

2. The method according to claim 1, characterized in that The steps of inputting sensor values ​​and vehicle motion state quantities at the previous moment into a vehicle motion discrete model, and then performing prediction using a vehicle state equation to determine a linear vehicle state space matrix and a vehicle state estimate at the current moment include: Determine the state transfer matrix of the vehicle at the previous moment based on the acceleration and angular velocity output by the sensor at the previous moment and the motion state of the vehicle at the previous moment; Determine the motion state of the vehicle at the current moment based on the vehicle motion discrete model, the motion state of the vehicle at the previous moment, the state transfer matrix, and the time difference between the previous moment and the current moment; Based on the motion state quantity at the current moment, the vehicle motion discrete model and the vehicle state equation at the current moment are combined to determine a vehicle state space matrix in a linear state; The vehicle state space matrix is ​​combined with the vehicle state equation at the current moment to predict the vehicle state estimation value at the current moment.

3. The method according to claim 1, characterized in that The step of generating a priori boundary matrix in the current prediction process based on the vehicle state space matrix and the current noise boundary matrix includes: Multiplying the vehicle state space matrix by the current noise boundary matrix to obtain a first matrix; First, multiply the noise upper bound matrix of the current noise boundary matrix by the current noise boundary matrix, and then perform row and diagonal matrix processing to obtain a second matrix; wherein the noise upper bound matrix is ​​a diagonal matrix of noise used in the current prediction process; First, the noise upper bound matrix is ​​multiplied by the motion state of the vehicle at the previous moment, and then row and diagonal matrix processing is performed to obtain a third matrix; The first matrix, the second matrix, and the third matrix are sequentially concatenated column by column to generate a priori boundary matrix in the current prediction process.

4. The method according to claim 1, wherein The step of obtaining a vehicle state observation value with observation noise introduced according to a residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimation value at the current moment comprises: Whenever a valid point is identified from the current radar point cloud data, the residual value corresponding to the valid point is substituted into the observation value calculation formula to generate an output matrix and a residual fusion result. This is done until all valid points in the radar point cloud data at the current moment are traversed, obtaining an output matrix corresponding to each valid point and a residual fusion result set for representing the vehicle state. The residual fusion result set includes a first residual fusion result before the current moment and a second residual fusion result at the current moment. performing a least squares calculation on an augmented measurement equation formed by the first residual fusion result and the second residual fusion result to obtain a vehicle state estimation value with observation noise introduced; The vehicle state estimation value with the observation noise introduced and the residual fusion result set are substituted into the observation equation, and the vehicle state observation value with the observation noise introduced is output.

5. The method according to claim 1, wherein The step of estimating a vehicle state estimation result of a posterior state at a current moment based on the prior boundary matrix, the residual fusion result, the vehicle state observation value, and the motion state quantity of the vehicle at a current moment comprises: Constructing a correction matrix based on the prior boundary matrix and the residual fusion result; The difference between the vehicle state observation value and the vehicle's current motion state is multiplied by the correction matrix, and then added to the vehicle's current motion state to estimate the vehicle state estimation result at the current moment under the action of the correction matrix.

6. The method according to claim 5, characterized in that The method further comprises: The posterior boundary matrix constructed based on the correction matrix and the prior boundary matrix is ​​reduced in order to serve as the current noise boundary matrix corresponding to the next moment.

7. The method according to claim 1, characterized in that The method further comprises: Based on the vehicle state estimation result at the current moment, vehicle positioning and mapping are performed.

8. A vehicle state estimation device, characterized in that: include: A determination module inputs the sensor values ​​and the vehicle's motion state at the previous moment into a vehicle motion discrete model, and then predicts using the vehicle state equation to determine a linear vehicle state space matrix and an estimated value of the vehicle state at the current moment; wherein the vehicle state space matrix is ​​a matrix used to represent changes in vehicle state and sensor collected values ​​during vehicle motion; A generation module, which generates a priori boundary matrix in a current prediction process based on the vehicle state space matrix and the current noise boundary matrix; An observation module obtains a vehicle state observation value with introduced observation noise based on a residual fusion result calculated for each valid point in the current radar point cloud data and the vehicle state estimate at the current moment; The estimation module estimates the vehicle state estimation result of the posterior state at the current moment based on the prior boundary matrix, the residual fusion result, the vehicle state observation value and the motion state of the vehicle at the current moment.

9. An electronic device comprising a memory and a processor, wherein the memory stores a computer program that can be run on the processor, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A machine-readable storage medium, characterized in that The machine-readable storage medium stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement the steps of the method according to any one of claims 1 to 7.

Images