Adaptive vehicle navigation filtering method and device

Abstract

The invention discloses a self-adaptive vehicle navigation filtering method and device, and belongs to the technical field of intelligent driving. The method comprises the following steps: constructing a nonlinear uncertainty tracking system model according to the mutability of an actual vehicle tracking state, the nonlinearity of a sensor measurement equation and the non-Gaussian property of a measurement error; aiming at a nonlinear uncertainty tracking system model, combining strong tracking filtering and a maximum entropy criterion, and constructing a cost function for estimating the optimal state of the unmanned vehicle; determining a fading factor in the cost function based on the orthogonality of the measurement residual sequence; constructing a novel adaptive navigation filtering algorithm by combining an unscented Kalman filtering framework according to the cost function and the fading factor; and carrying out data processing on the unmanned vehicle tracking system according to the constructed adaptive navigation filtering algorithm. According to the method, the problems of mutability, nonlinearity, non-Gaussian property and the like in a nonlinear tracking system can be inhibited at the same time, and the tracking precision and reliability of the unmanned vehicle in a complex environment are improved.

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, 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.

[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 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.

[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, 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.

[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 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.

[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{a}$. 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, 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.

[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 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 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.

[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. A method of adaptive vehicle navigation filtering, the method comprising: The method comprises: S1: according to the abruptness of the actual vehicle tracking state, the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error, a nonlinear uncertain tracking system model of the unmanned vehicle is constructed; S2: for the nonlinear uncertain tracking system model, a cost function of optimal state estimation of the unmanned vehicle is constructed in combination with strong tracking filtering and maximum entropy criterion; S3: based on the orthogonality of the measurement residual sequence, a fading factor in the cost function is determined; S4: according to the cost function and the fading factor, a new adaptive navigation filtering algorithm is constructed in combination with an unscented Kalman filtering framework; S5: the adaptive navigation filtering algorithm is used for data processing of the unmanned vehicle tracking system; The S1 comprises: S11: according to the abruptness of the actual vehicle tracking state, a state equation of the vehicle tracking system with state mutation is obtained; wherein, and respectively represent the state vectors of the vehicle at time k and k-1, k = 1, 2, 3,..., is a state transition matrix, is a state jump vector, is a process noise vector satisfying zero mean and error covariance matrix . S12: according to the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error, a nonlinear measurement equation of the vehicle tracking system with measurement outliers is obtained; wherein, represents an output vector measured by the sensor, represents a corresponding non-linear measurement function of the sensor, is a measurement outlier vector, is a Gaussian noise vector satisfying zero mean and error covariance matrix . S13: according to the state equation and the nonlinear measurement equation, a nonlinear uncertain tracking system model of the unmanned vehicle is constructed; 。 2. The adaptive vehicle navigation filtering method of claim 1, wherein, The cost function is: wherein is the state vector estimate, is the state vector prediction, is the state prediction error covariance matrix, is the fading factor in the strong tracking filter, the symbol denotes the 2-norm operator, denotes the 2-norm of with the weighting matrix , the 2-norm of is the Gaussian kernel function under the maximum entropy criterion, j = 1, 2, …, m, m is the dimension of the output vector of the sensor measurement, is the jth component of the vector , and is the measurement prediction.

3. The adaptive vehicle navigation filtering method of claim 2, wherein, The evanescent factor is: wherein , denotes the matrix trace operation, , is the measurement residual, , is the forgetting factor, is the measurement prediction residual covariance matrix, is the value at time k - 1.

4. The adaptive vehicle navigation filtering method of claim 3, wherein, The S4 comprises: S41: set the related entropy kernel window parameters the value of the initial state of the initial state of the initial state of the initial state of the initial state of Let , initialize; wherein represents to take; S42: Calculate Sigma samples and their weight coefficients; where i is the index of Sigma sampling point, i = 0, 1, 2, …2n, n is the dimension of state vector, is the value at k-1 time, is the shrinkage parameter, , is the parameter of controlling the distribution state of Sigma sampling point, , , represents the matrix square root of the first column; The weight coefficients include mean weight values and variance weight values : wherein is a weight parameter taking positive values; S43: Time update is performed using the following equation, calculating ; ； wherein ; S44: Calculate the improved covariance combined with the maximum entropy criterion ; wherein , , , ; S45: Calculate the fading factor in conjunction with the strong tracking filter ; S46: measurement update is performed by using the following formula; ； wherein is the updated value, is the updated value, is the Kalman gain matrix.

5. The adaptive vehicle navigation filtering method according to any one of claims 1-4, characterized in that, The S5 comprises: Through the unmanned vehicle CV motion model, vehicle motion state is generated, vehicle tracking data is simulated by using a laser radar distance measurement model, and the adaptive navigation filtering algorithm is used for data processing of the unmanned vehicle tracking system.

6. An adaptive vehicle navigation filtering apparatus characterized by comprising: The device comprises: A model construction module is configured to construct a nonlinear uncertain tracking system model of the unmanned vehicle according to the abruptness of the actual vehicle tracking state, the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error; A cost function construction module is configured to construct a cost function of optimal state estimation of the unmanned vehicle in combination with strong tracking filtering and maximum entropy criterion for the nonlinear uncertain tracking system model; A fading factor determination module is configured to determine a fading factor in the cost function based on the orthogonality of the measurement residual sequence; A filtering algorithm construction module is configured to construct a new adaptive navigation filtering algorithm in combination with an unscented Kalman filtering framework according to the cost function and the fading factor; A navigation module is configured to use the adaptive navigation filtering algorithm for data processing of the unmanned vehicle tracking system; The model construction module comprises: A state equation establishment unit is configured to obtain a state equation of the vehicle tracking system with state mutation according to the abruptness of the actual vehicle tracking state; wherein, and respectively represent the state vector of the vehicle at time k and k-1, k = 1, 2, 3,..., is a state transition matrix, is a state jump vector, is a process noise vector satisfying zero mean and error covariance matrix . A measurement equation establishment unit is configured to obtain a nonlinear measurement equation of the vehicle tracking system with measurement outliers according to the nonlinearity of the sensor measurement equation and the non-Gaussianity of the measurement error; wherein, represents an output vector measured by the sensors, represents a corresponding non-linear measurement function of the sensors, is a measurement outlier vector, is a Gaussian noise vector satisfying zero mean and error covariance matrix . A model construction unit is configured to construct a nonlinear uncertain tracking system model of the unmanned vehicle according to the state equation and the nonlinear measurement equation. 。

Images