Adaptive vehicle navigation filtering method and apparatus

Abstract

The application discloses a kind of self-adapting vehicle navigation filtering method and device, belong to intelligent driving technical field.It includes: according to the abruptness of actual vehicle tracking state, the nonlinearity of sensor measurement equation and the non-Gaussian of measurement error, construct nonlinear uncertainty tracking system model;For nonlinear uncertainty tracking system model, combined with strong tracking filter and maximum entropy criterion, construct the cost function of unmanned vehicle optimal state estimation;Based on the orthogonality of measurement residual sequence, determine the fading factor in cost function;According to cost function and fading factor, combined with unscented kalman filter framework, construct new self-adapting navigation filtering algorithm;According to the data processing of unmanned vehicle tracking system of the self-adapting navigation filtering algorithm constructed.This application can simultaneously suppress the abruptness, nonlinearity and non-Gaussian in nonlinear tracking system and other problems, improve the tracking precision and reliability of unmanned vehicle in complex environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to an adaptive vehicle navigation filtering method and apparatus. Background Technology

[0002] Autonomous driving technology, as a crucial component of future intelligent transportation systems, is gradually moving from the laboratory to practical applications. In complex environments, such as smart mines, autonomous vehicles (including unmanned mining trucks) require highly accurate environmental perception and target tracking capabilities to ensure operational safety and efficiency. However, these environments often present numerous uncertainties, such as dynamic obstacles, complex terrain, variable weather conditions, and the limitations of sensors themselves, leading to a significant decrease in the accuracy of traditional tracking algorithms in complex environments.

[0003] Autonomous vehicles utilize onboard sensors (cameras, lidar, millimeter-wave radar, infrared cameras, GPS, and inertial navigation systems, etc.) to perceive their surroundings. Based on the perceived information about roads, vehicle positions, and obstacles, they automatically plan routes and control steering and speed to safely and reliably reach their destinations. High-precision and reliable state estimation and tracking of pedestrians, other vehicles, and the vehicle itself is one of the key tasks that autonomous driving perception systems must address.

[0004] Vehicle tracking performance depends not only on the performance and accuracy of the onboard sensor hardware but also on the tracking filtering algorithm used. Commonly used onboard sensors include cameras, LiDAR, millimeter-wave radar, infrared cameras, GPS, and inertial navigation systems. As for filtering algorithms, the most widely used is the Kalman filter.

[0005] Kalman filtering is used for linear system state equations. For linear Gaussian processes, Kalman filtering can obtain the optimal recursive estimate based on the minimum mean square error. However, in most practical applications, the tracking system model (mainly reflected in the vehicle sensor measurement model) is often nonlinear, and the Kalman filtering algorithm cannot be directly used to solve the state estimation problem of nonlinear systems. Furthermore, due to the complex operating environment faced by autonomous vehicles, the vehicle itself or its onboard sensors often encounter various uncertainties and disturbances, leading to sudden changes in vehicle state (turning, acceleration / deceleration, uphill, downhill, etc.) or a large number of anomalies (outliers) in sensor data. The generation of these disturbances and changes in error characteristics inevitably affect the design of vehicle tracking filtering algorithms, reducing vehicle tracking accuracy and performance, and consequently affecting the vehicle's subsequent behavioral decisions and motion planning.

[0006] Existing filtering algorithms do not consider sudden changes in system state and non-Gaussianity of measurement error, and there is no navigation filtering algorithm that can simultaneously handle nonlinearity of dynamic systems, sudden changes in system state, and non-Gaussianity of measurement error. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides an adaptive vehicle navigation filtering method and apparatus that can simultaneously suppress the impact of abrupt changes in the actual vehicle tracking state, the nonlinearity of sensor measurement equations, and the non-Gaussianity of measurement errors on vehicle tracking accuracy, thereby improving the tracking accuracy and reliability of unmanned vehicles in complex environments.

[0008] The technical solution provided by this invention is as follows:

[0009] An adaptive vehicle navigation filtering method, the method comprising:

[0010] S1: Based on the abrupt changes in the actual vehicle tracking state, the nonlinearity of the sensor measurement equation, and the non-Gaussianity of the measurement error, a nonlinear uncertainty tracking system model for autonomous vehicles is constructed.

[0011] S2: For the aforementioned nonlinear uncertainty tracking system model, a cost function for estimating the optimal state of the autonomous vehicle is constructed by combining strong tracking filtering and the maximum entropy criterion.

[0012] S3: Based on the orthogonality of the measured residual sequences, determine the fading factor in the cost function;

[0013] S4: Based on the cost function and fading factor, and combined with the unscented Kalman filter framework, a novel adaptive navigation filter algorithm is constructed.

[0014] S5: Perform data processing on the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

[0015] Furthermore, S1 includes:

[0016] S11: Based on the abrupt changes in the actual vehicle tracking state, obtain the state equation of the vehicle tracking system with state abrupt changes;

[0017]

[0018] in, and Let these represent the vehicle's state vectors at time k and time k-1, respectively, where k = 1, 2, 3, ... Here is the state transition matrix. This is the state change vector. To satisfy zero mean and error covariance matrix as The process noise vector;

[0019] S12: Based on the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error, obtain the nonlinear measurement equation of the vehicle tracking system with measurement outliers;

[0020]

[0021] in, This represents the output vector measured by the sensor. This represents the corresponding nonlinear measurement function of the sensor. To measure the outlier vector, To satisfy zero mean and error covariance matrix as Gaussian noise vector;

[0022] S13: Construct a nonlinear uncertainty tracking system model for unmanned vehicles based on the state equation and the nonlinear measurement equation;

[0023] .

[0024] Furthermore, the cost function is:

[0025]

[0026] in, This is the estimated value of the state vector. For the predicted value of the state vector, The state prediction error covariance matrix is... The fading factor in strong tracking filtering, symbol Represents the 2-norm operator. Indicates For the weighted matrix The 2-norm, Let be the Gaussian kernel function under the maximum entropy criterion, j=1, 2, …,m, where m is the dimension of the sensor's output vector. For vectors The j-th component, To measure the predicted value.

[0027] Furthermore, the fading factor for:

[0028]

[0029] in, , This represents the trace operation of a matrix. , To measure the residual, , Forgetting factor, To measure the predicted residual covariance matrix, for The value at time k-1.

[0030] Furthermore, S4 includes:

[0031] S41: Set the relevant entropy kernel window parameters The given value initial state Given initial state Given initial state ,make Perform initialization;

[0032] in, Indicates taking the expected value;

[0033] S42: Using the unscented transformation, calculate Sigma sampling points and its weight coefficients;

[0034]

[0035] Where i is the label of the Sigma sampling point, i = 0, 1, 2, … 2n, and n is the dimension of the state vector. for The value at time k-1, For the shrinkage parameter, , The parameters used to control the distribution of Sigma sampling points , , Representative matrix The square root of the first List;

[0036] The weighting coefficients include mean-weighted values. Sum of variance weights :

[0037]

[0038] in, Weight parameters that take positive values;

[0039] S43: Use the following formula for time updates, calculate... ;

[0040] ;

[0041] in, ;

[0042] S44: Calculate the improved covariance using the maximum entropy criterion. ;

[0043]

[0044] in, , , , ;

[0045] S45: Combine strong tracking filtering to calculate the fading factor. ;

[0046] S46: Use the following formula to update the measurement;

[0047]

[0048]

[0049]

[0050] ;

[0051] in, for The updated value, for The updated value, This is the Kalman gain matrix.

[0052] Furthermore, S5 includes:

[0053] The vehicle motion state is generated through the CV motion model of the autonomous vehicle, and the vehicle tracking data is generated by simulating the distance measurement model of LiDAR. The data is then processed by the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

[0054] An adaptive vehicle navigation filter device, the device comprising:

[0055] The model building module is used to construct a nonlinear uncertainty tracking system model for autonomous vehicles based on the abrupt changes in the actual vehicle tracking state, the nonlinearity of the sensor measurement equation, and the non-Gaussianity of the measurement error.

[0056] The cost function construction module is used to construct a cost function for estimating the optimal state of the autonomous vehicle, combining strong tracking filtering and the maximum entropy criterion, for the nonlinear uncertainty tracking system model.

[0057] The fading factor determination module is used to determine the fading factor in the cost function based on the orthogonality of the measurement residual sequence.

[0058] The filtering algorithm construction module is used to construct a novel adaptive navigation filtering algorithm based on the cost function and the fading factor, combined with the unscented Kalman filtering framework.

[0059] The navigation module is used to process data for the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

[0060] Furthermore, the model building module includes:

[0061] The state equation establishment unit is used to obtain the state equation of a vehicle tracking system with state changes based on the abrupt changes in the actual vehicle tracking state.

[0062]

[0063] in, and Let these represent the vehicle's state vectors at time k and time k-1, respectively, where k = 1, 2, 3, ... Here is the state transition matrix. This is the state change vector. To satisfy zero mean and error covariance matrix as The process noise vector;

[0064] The measurement equation establishment unit is used to obtain the nonlinear measurement equation of the vehicle tracking system with measurement outliers based on the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error.

[0065]

[0066] in, This represents the output vector measured by the sensor. This represents the corresponding nonlinear measurement function of the sensor. To measure the outlier vector, To satisfy zero mean and error covariance matrix as Gaussian noise vector;

[0067] The model building unit is used to build a nonlinear uncertainty tracking system model for unmanned vehicles based on the state equation and the nonlinear measurement equation.

[0068] .

[0069] Furthermore, the cost function is:

[0070]

[0071] in, This is the estimated value of the state vector. For the predicted value of the state vector, The state prediction error covariance matrix is... The fading factor in strong tracking filtering, symbol Represents the 2-norm operator. Indicates For the weighted matrix The 2-norm, Let be the Gaussian kernel function under the maximum entropy criterion, j=1, 2, …,m, where m is the dimension of the sensor's output vector. For vectors The j-th component, To measure the predicted value.

[0072] Furthermore, the fading factor for:

[0073]

[0074] in, , This represents the trace operation of a matrix. , To measure the residual, , Forgetting factor, To measure the predicted residual covariance matrix, for The value at time k-1.

[0075] Furthermore, the filtering algorithm construction module includes:

[0076] The initialization unit is used to set the relevant entropy kernel window parameters. The given value initial state Given initial state Given initial state ,make Perform initialization;

[0077] in, Indicates taking the expected value;

[0078] Unscented transform unit, used to calculate using unscented transform. Sigma sampling points and its weight coefficients;

[0079]

[0080] Where i is the label of the Sigma sampling point, i = 0, 1, 2, … 2n, and n is the dimension of the state vector. for The value at time k-1, For the shrinkage parameter, , The parameters used to control the distribution of Sigma sampling points , , Representative matrix The square root of the first List;

[0081] The weighting coefficients include mean-weighted values. Sum of variance weights :

[0082]

[0083] in, Weight parameters that take positive values;

[0084] The time update unit is used to perform time updates using the following formula, and calculates... ;

[0085] ;

[0086] in, ;

[0087] The covariance calculation unit is used to calculate the improved covariance by combining the maximum entropy criterion. ;

[0088]

[0089] in, , , , ;

[0090] The fading factor calculation unit is used to calculate the fading factor in conjunction with strong tracking filtering. ;

[0091] A measurement update unit is used to update measurements using the following formula;

[0092]

[0093]

[0094]

[0095] ;

[0096] in, for The updated value, for The updated value, This is the Kalman gain matrix.

[0097] Furthermore, the navigation module includes:

[0098] The vehicle motion state is generated through the CV motion model of the autonomous vehicle, and the vehicle tracking data is generated by simulating the distance measurement model of LiDAR. The data is then processed by the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

[0099] The present invention has the following beneficial effects:

[0100] This invention is an adaptive navigation filtering method to improve the tracking accuracy of autonomous vehicles. It can simultaneously suppress the impact of abrupt changes in the actual vehicle tracking state, the nonlinearity of sensor measurement equations, and the non-Gaussianity of measurement errors on vehicle tracking accuracy, thereby improving the tracking accuracy and reliability of autonomous vehicles in complex environments (such as smart mines) and ensuring the safe and efficient operation of autonomous vehicles (including unmanned mining trucks). Attached Figure Description

[0101] Figure 1 This is a flowchart of the adaptive vehicle navigation filtering method of the present invention;

[0102] Figure 2 This is a schematic diagram of the adaptive vehicle navigation filtering device of the present invention. Detailed Implementation

[0103] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0104] Example 1:

[0105] This invention provides an adaptive vehicle navigation filtering method aimed at improving the tracking accuracy of unmanned vehicles (including unmanned mining trucks) in complex environments. Taking vehicle-mounted LiDAR tracking as an example, this invention utilizes single-station LiDAR distance measurement information to track moving vehicles in a two-dimensional Cartesian coordinate system. The vehicle's state vector includes two-dimensional position and two-dimensional velocity information.

[0106] like Figure 1 As shown, the method includes:

[0107] S1: Based on the abrupt changes in the actual vehicle tracking state, the nonlinearity of the sensor measurement equation, and the non-Gaussianity of the measurement error, a nonlinear uncertainty tracking system model for autonomous vehicles is constructed.

[0108] Specifically, S1 includes:

[0109] S11: Based on the abrupt changes in the actual vehicle tracking state, obtain the state equation of the vehicle tracking system with state abrupt changes.

[0110] Based on the constant velocity (CV) motion model of the vehicle, the state equation of the vehicle motion is: .

[0111] in, and Let K and K-1 represent the state vectors of the vehicle at time k and k-1, respectively, where k = 1, 2, 3, ... , . and These represent the Cartesian coordinates (i.e., position information) of the vehicle at time k and time k-1, respectively. and They represent and The corresponding speed.

[0112] Here is the state transition matrix. , This represents the sampling time interval.

[0113] This is the state change vector. That is, in two different time periods and There is a state mutation.

[0114] To satisfy zero mean and error covariance matrix as The process noise vector, .

[0115] S12: Based on the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error, obtain the nonlinear measurement equation of the vehicle tracking system with measurement outliers.

[0116] Specifically, based on the ranging and speed information from the lidar, the measurement equation for vehicle tracking is as follows:

[0117]

[0118] in, This represents the output vector (distance measurement, velocity measurement) of the sensor. This represents the corresponding nonlinear measurement function of the sensor.

[0119] To measure the outlier vector, That is, in two different time periods and There are outliers in the measurement.

[0120] To satisfy zero mean and error covariance matrix as Gaussian noise vector, .

[0121] S13: Construct a nonlinear uncertainty tracking system model for unmanned vehicles based on the state equation and the nonlinear measurement equation.

[0122] .

[0123] S2: For nonlinear uncertain tracking system models, a cost function for optimal state estimation of autonomous vehicles is constructed by combining strong tracking filtering and the maximum entropy criterion.

[0124] In order to obtain the optimal state vector estimate, the aforementioned model of the nonlinear uncertainty tracking system for autonomous vehicles... The cost function of the classic Kalman filter is shown below:

[0125]

[0126] in, This is the estimated value of the state vector. For the predicted value of the state vector, Let $\mathbf{a}$ be the state prediction error covariance matrix, with the sign $\mathbf{ Represents the 2-norm operator. Indicates For the weighted matrix The 2-norm, representing the norm of , indicates that in Weighted influence and The differences between them. To measure the predicted value, Indicates For the weighted matrix The 2-norm, representing the norm of , indicates that in Weighted influence and The differences between them.

[0127] Considering the non-Gaussian nature of vehicle tracking measurement errors and measurement outliers, let Then, combining the maximum entropy criterion, the cost function in the above equation can be rewritten as:

[0128]

[0129] in, Let be the Gaussian kernel function under the maximum entropy criterion, j=1, 2, …,m, where m is the dimension of the sensor's output vector. For vectors The j-th component.

[0130] Furthermore, to accommodate sudden changes in system state, the fading factor of the strong tracking filter is introduced into the cost function of the above equation, thus obtaining the cost function for the optimal state estimation of the autonomous vehicle, described as follows:

[0131]

[0132] This is the fading factor in strong tracking filtering. Indicates For the weighted matrix The 2-norm, representing the norm of , indicates that in Weighted influence and The differences between them.

[0133] S3: Determine the fading factor in the cost function based on the orthogonality of the measurement residual sequences. .

[0134]

[0135] in, , This represents the trace operation of a matrix. , To measure the residual, , This is the forgetting factor, typically set to 0.95. To measure the predicted residual covariance matrix, for The value at time k-1

[0136] S4: Based on the cost function and fading factor, and combined with the unscented Kalman filter framework, a novel adaptive navigation filter algorithm is constructed.

[0137] The novel adaptive navigation filtering algorithm constructed in this invention consists of the following steps, as detailed below:

[0138] S41: Set the relevant entropy kernel window parameters The given value initial state Given initial state Given initial state ,make Initialize.

[0139] in, This indicates taking the expected value.

[0140] S42: Calculate using the Unscented Transform (UT) method. Sigma sampling points and its weight coefficients.

[0141]

[0142] Where i is the label of the Sigma sampling point, i = 0, 1, 2, … 2n, and n is the dimension of the state vector. for The value at time k-1, For the shrinkage parameter, , The parameters used to control the distribution of Sigma sampling points , , Representative matrix The square root of the first List.

[0143] The weighting coefficients include mean-weighted values. Sum of variance weights :

[0144]

[0145] in, For weight parameters to take positive values, they are generally taken as follows: .

[0146] S43: Use the following formula for time updates, calculate... .

[0147] ;

[0148] in, , These represent intermediate parameters in the calculation process.

[0149] S44: Calculate the improved covariance using the maximum entropy criterion. ;

[0150]

[0151] in, , , For vectors The j-th component, , , .

[0152] The above , , These represent intermediate parameters in the calculation process.

[0153] S45: Combine strong tracking filtering to calculate the fading factor. .

[0154] For the specific calculation formula, please refer to S3. The formula for calculating the measurement prediction residual covariance matrix is ​​as follows: Everything else is the same as S3, so I won't go into details.

[0155] S46: Use the following formula to update the measurement.

[0156]

[0157]

[0158]

[0159] ;

[0160] in, for The updated value, for The updated value, Here is the Kalman gain matrix. These represent intermediate parameters in the calculation process.

[0161] As can be seen from the process of the novel adaptive navigation filtering algorithm described above, the unscented Kalman filter framework can effectively handle the nonlinearity of the vehicle tracking system, the maximum entropy criterion can effectively suppress the non-Gaussianity of measurement errors and the influence of measurement outliers on tracking accuracy, and the introduction of the fading factor in the strong tracking filter can effectively suppress the influence of the abrupt change in the tracking system state on tracking performance. Therefore, compared with other traditional navigation filtering algorithms, the novel adaptive navigation filtering algorithm of this invention is more rationally designed and can obtain vehicle tracking results with higher accuracy and better performance.

[0162] S5: Perform data processing on the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

[0163] Specifically, the vehicle's motion state can be generated using a CV motion model, and vehicle tracking data can be simulated using a LiDAR distance measurement model. The data is then processed by the aforementioned adaptive navigation filtering algorithm for the autonomous vehicle tracking system, during which an initial state is set. Initial state covariance The simulation time was 150 seconds; the Gaussian kernel window was 0.8.

[0164] This invention addresses the complex operating environment of autonomous vehicles by combining strong tracking filtering with a maximum entropy criterion and utilizing an unscented Kalman filter framework. It presents a nonlinear adaptive navigation filtering algorithm that takes into account both the fading factor and the maximum entropy optimization criterion. This algorithm is designed to solve the target tracking problem under conditions of nonlinearity and uncertainty in the navigation system model, non-Gaussian noise, and measurement outliers in the vehicle operating environment.

[0165] This invention is an adaptive navigation filtering method to improve the tracking accuracy of autonomous vehicles. It can simultaneously suppress the impact of abrupt changes in the actual vehicle tracking state, the nonlinearity of sensor measurement equations, and the non-Gaussianity of measurement errors on vehicle tracking accuracy, thereby improving the tracking accuracy and reliability of autonomous vehicles in complex environments (such as smart mines) and ensuring the safe and efficient operation of autonomous vehicles (including unmanned mining trucks).

[0166] Example 2:

[0167] This invention provides an adaptive vehicle navigation filtering device, such as... Figure 2 As shown, the device includes:

[0168] Model building module 1 is used to build a nonlinear uncertainty tracking system model for autonomous vehicles based on the abrupt changes in the actual vehicle tracking state, the nonlinearity of the sensor measurement equation, and the non-Gaussianity of the measurement error.

[0169] Cost function construction module 2 is used to construct the cost function for optimal state estimation of autonomous vehicles by combining strong tracking filtering and maximum entropy criterion for nonlinear uncertain tracking system models.

[0170] The fading factor determination module 3 is used to determine the fading factor in the cost function based on the orthogonality of the measurement residual sequence.

[0171] Filtering algorithm construction module 4 is used to construct a novel adaptive navigation filtering algorithm based on the cost function and fading factor, combined with the unscented Kalman filtering framework.

[0172] Navigation module 5 is used to process data for the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

[0173] In one example, the model building module includes:

[0174] The state equation establishment unit is used to obtain the state equation of a vehicle tracking system with state abrupt changes based on the abrupt changes in the actual vehicle tracking state.

[0175]

[0176] in, and Let these represent the vehicle's state vectors at time k and time k-1, respectively, where k = 1, 2, 3, ... Here is the state transition matrix. This is the state change vector. To satisfy zero mean and error covariance matrix as The process noise vector.

[0177] The measurement equation establishment unit is used to obtain the nonlinear measurement equation of the vehicle tracking system with measurement outliers based on the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error.

[0178]

[0179] in, This represents the output vector measured by the sensor. This represents the corresponding nonlinear measurement function of the sensor. To measure the outlier vector, To satisfy zero mean and error covariance matrix as Gaussian noise vector.

[0180] The model building unit is used to construct a nonlinear uncertainty tracking system model for autonomous vehicles based on the state equation and the nonlinear measurement equation.

[0181] .

[0182] Specifically, the aforementioned cost function is:

[0183]

[0184] in, This is the estimated value of the state vector. For the predicted value of the state vector, The state prediction error covariance matrix is... The fading factor in strong tracking filtering, symbol Represents the 2-norm operator. Indicates For the weighted matrix The 2-norm, Let be the Gaussian kernel function under the maximum entropy criterion, j=1, 2, …,m, where m is the dimension of the sensor's output vector. For vectors The j-th component, To measure the predicted value.

[0185] Gradual decay factor for:

[0186]

[0187] in, , This represents the trace operation of a matrix. , To measure the residual, , Forgetting factor, To measure the predicted residual covariance matrix, for The value at time k-1.

[0188] As an improvement, the filtering algorithm construction module includes:

[0189] The initialization unit is used to set the relevant entropy kernel window parameters. The given value initial state Given initial state Given initial state ,make Initialize.

[0190] in, This indicates taking the expected value.

[0191] Unscented transform unit, used to calculate using unscented transform. Sigma sampling points and its weight coefficients.

[0192]

[0193] Where i is the label of the Sigma sampling point, i = 0, 1, 2, … 2n, and n is the dimension of the state vector. for The value at time k-1, For the shrinkage parameter, , The parameters used to control the distribution of Sigma sampling points , , Representative matrix The square root of the first List.

[0194] The weighting coefficients include mean-weighted values. Sum of variance weights :

[0195]

[0196] in, The weight parameters are to be positive.

[0197] The time update unit is used to perform time updates using the following formula, and calculates... .

[0198] ;

[0199] in, .

[0200] The covariance calculation unit is used to calculate the improved covariance by combining the maximum entropy criterion. .

[0201]

[0202] in, , , , .

[0203] The fading factor calculation unit is used to calculate the fading factor in conjunction with strong tracking filtering. .

[0204] The measurement update unit is used to perform measurement updates using the following formula.

[0205]

[0206]

[0207]

[0208] ;

[0209] in, for The updated value, for The updated value, This is the Kalman gain matrix.

[0210] In this invention, the navigation module includes:

[0211] The vehicle motion state is generated through the CV motion model of the autonomous vehicle, and the vehicle tracking data is generated by simulating the distance measurement model of LiDAR. The data is then processed by the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

[0212] The apparatus provided in this embodiment of the invention has the same implementation principle and technical effects as the aforementioned method embodiment. For the sake of brevity, any parts not mentioned in the apparatus embodiment can be referred to the corresponding content in the aforementioned method embodiment 1. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the apparatus and units described above can all be referred to the corresponding processes in the above method embodiments, and will not be repeated here.

[0213] Finally, it should be noted that the above-described embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit them. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments within the scope of the technology disclosed in the present invention, or make equivalent substitutions for some of the technical features; and these modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the scope of protection of the present invention.

Claims

1. An adaptive vehicle navigation filtering method, characterized in that, The method includes: S1: Based on the abrupt changes in the actual vehicle tracking state, the nonlinearity of the sensor measurement equation, and the non-Gaussianity of the measurement error, a nonlinear uncertainty tracking system model for autonomous vehicles is constructed. S2: For the aforementioned nonlinear uncertainty tracking system model, a cost function for estimating the optimal state of the autonomous vehicle is constructed by combining strong tracking filtering and the maximum entropy criterion. S3: Based on the orthogonality of the measured residual sequences, determine the fading factor in the cost function; S4: Based on the cost function and fading factor, and combined with the unscented Kalman filter framework, construct an adaptive navigation filter algorithm; S5: Perform data processing on the unmanned vehicle tracking system based on the constructed adaptive navigation filtering algorithm; Wherein, S1 includes: S11: Based on the abrupt changes in the actual vehicle tracking state, obtain the state equation of the vehicle tracking system with state abrupt changes; in, and Let these represent the vehicle's state vectors at time k and time k-1, respectively, where k = 1, 2, 3, ... Here is the state transition matrix. This is the state change vector. To satisfy zero mean and error covariance matrix as The process noise vector; S12: Based on the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error, obtain the nonlinear measurement equation of the vehicle tracking system with measurement outliers; in, This represents the output vector measured by the sensor. This represents the corresponding nonlinear measurement function of the sensor. To measure the outlier vector, To satisfy zero mean and error covariance matrix as Gaussian noise vector; S13: Construct a nonlinear uncertainty tracking system model for unmanned vehicles based on the state equation and the nonlinear measurement equation; ； The cost function is: in, This is the estimated value of the state vector. For the predicted value of the state vector, Here is the state prediction error covariance matrix. The fading factor in strong tracking filtering, symbol Represents the 2-norm operator. Indicates For the weighted matrix The 2-norm, Let be the Gaussian kernel function under the maximum entropy criterion, j=1, 2,…,m, where m is the dimension of the sensor's output vector. For vectors The j-th component, To measure the predicted value.

2. The adaptive vehicle navigation filtering method according to claim 1, characterized in that, The fading factor for: in, , This represents the trace operation of a matrix. , To measure the residual, , Forgetting factor, To measure the predicted residual covariance matrix, for The value at time k-1.

3. The adaptive vehicle navigation filtering method according to claim 2, characterized in that, S4 includes: S41: Set the relevant entropy kernel window parameters The given value initial state Given initial state Given initial state ,make Perform initialization; in, Indicates taking the expected value; S42: Using the unscented transformation, calculate Sigma sampling points and its weight coefficients; Where i is the label of the Sigma sampling point, i = 0, 1, 2, …2n, and n is the dimension of the state vector. for The value at time k-1, For the shrinkage parameter, , The parameters used to control the distribution of Sigma sampling points , , Representative matrix The square root of the first List; The weighting coefficients include mean-weighted values. Sum of variance weights : in, Weight parameters that take positive values; S43: Use the following formula for time updates, calculate... ; ； in, ; S44: Calculate the improved covariance using the maximum entropy criterion. ; in, , , , ; S45: Combine strong tracking filtering to calculate the fading factor. ; S46: Use the following formula to update the measurement; ； in, for The updated value, for The updated value, This is the Kalman gain matrix.

4. The adaptive vehicle navigation filtering method according to any one of claims 1-3, characterized in that, S5 includes: The vehicle motion state is generated using a CV motion model of an autonomous vehicle, and vehicle tracking data is generated by simulating a LiDAR distance measurement model. The data is then processed by the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm.

5. An adaptive vehicle navigation filter device, characterized in that, The device includes: The model building module is used to construct a nonlinear uncertainty tracking system model for autonomous vehicles based on the abrupt changes in the actual vehicle tracking state, the nonlinearity of the sensor measurement equation, and the non-Gaussianity of the measurement error. The cost function construction module is used to construct a cost function for estimating the optimal state of the autonomous vehicle, combining strong tracking filtering and the maximum entropy criterion, for the nonlinear uncertainty tracking system model. The fading factor determination module is used to determine the fading factor in the cost function based on the orthogonality of the measurement residual sequence. The filtering algorithm construction module is used to construct an adaptive navigation filtering algorithm based on the cost function and the fading factor, combined with the unscented Kalman filtering framework. The navigation module is used to process data for the autonomous vehicle tracking system based on the constructed adaptive navigation filtering algorithm; The model building module includes: The state equation establishment unit is used to obtain the state equation of a vehicle tracking system with state changes based on the abrupt changes in the actual vehicle tracking state. in, and Let these represent the vehicle's state vectors at time k and time k-1, respectively, where k = 1, 2, 3, ... Here is the state transition matrix. This is the state change vector. To satisfy zero mean and error covariance matrix as The process noise vector; The measurement equation establishment unit is used to obtain the nonlinear measurement equation of the vehicle tracking system with measurement outliers based on the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error. in, This represents the output vector measured by the sensor. This represents the corresponding nonlinear measurement function of the sensor. To measure the outlier vector, To satisfy zero mean and error covariance matrix as Gaussian noise vector; The model building unit is used to build a nonlinear uncertainty tracking system model for unmanned vehicles based on the state equation and the nonlinear measurement equation. ； The cost function is: in, This is the estimated value of the state vector. For the predicted value of the state vector, Here is the state prediction error covariance matrix. The fading factor in strong tracking filtering, symbol Represents the 2-norm operator. Indicates For the weighted matrix The 2-norm, Let be the Gaussian kernel function under the maximum entropy criterion, j=1, 2,…,m, where m is the dimension of the sensor's output vector. For vectors The j-th component, To measure the predicted value.

Images