A fusion positioning method, system and electronic device based on adaptive iterative filtering

Abstract

The present invention relates to the field of autonomous driving technology. The present invention provides a fusion positioning method, system, and electronic device using adaptive iterative filtering, wherein the method comprises: obtaining a vehicle state and assigning a value to the vehicle state; constructing a Kalman filter model and initializing the Kalman filter model; performing inertial calculation on the vehicle state, predicting the vehicle's error state, and outputting the vehicle's error state variance, wherein the vehicle's error state variance includes a priori variance and a posteriori variance; and introducing an attenuation factor to reduce the weight of the predicted value in the priori variance, thereby improving the convergence of the Kalman filter model. By improving and optimizing the existing Kalman filter model, the present invention can more accurately estimate the vehicle positioning result, thereby improving the safety and reliability of the autonomous driving process.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a fusion positioning method, system and electronic equipment for adaptive iterative filtering. Background Art

[0002] Positioning technology is a crucial component of autonomous driving solutions. Autonomous driving has the following specific positioning requirements: 1. High precision: only centimeter-level accuracy can provide accurate navigation support; 2. Availability: Positioning services should be available in all scenarios to the greatest extent possible; and 3. Robustness: Positioning results must be reliable and stable even when input data is contaminated by noise.

[0003] To achieve positioning requirements, observation data from sensors such as GPS, IMUs, cameras, ultrasonic radar, and lidar are required. However, different sensors have different strengths and weaknesses. For example, GPS provides accurate location information, but the signal is poor in obstructed environments such as tunnels and underground garages. IMUs can stably output acceleration and angular velocity at a high frequency, but wander errors accumulate over time. Cameras are relatively low-cost but are easily affected by light. To ensure that positioning covers all scenarios, it is necessary to combine the strengths of different sensors and fuse the data provided by all of them for positioning. Currently, multi-sensor fusion positioning has become the mainstream solution for autonomous driving.

[0004] Due to inconsistencies in observation data from different sensors, obtaining a true estimate of the vehicle's state from this data is a core issue in fusion positioning. The Kalman filter (KF) technique, proposed in the 1960s, is a fundamental approach to addressing this problem. Due to its clear principles, simplicity, reliability, and ease of programming, the KF technique has been widely used in aerospace, navigation, transportation, control, and other fields. Since the KF technique targets linear models and cannot address nonlinear positioning problems, the improved extended Kalman filter (EKF) has been proposed. The EKF's iterative formula is equivalent to the nonlinear optimization algorithm Gauss-Newton. Using local linearization techniques, the nonlinear model is converted into a linear model and then processed using the Kalman filter. Since the EKF linearizes the nonlinear model, the error increases with increasing nonlinearity. However, since the linearization operating point is close to the true value, a solution has been proposed: using an optimized iterative algorithm to gradually find the exact linearization point, thereby improving accuracy. This is the iterative extended Kalman filter (IEKF). While the EKF increases the computational complexity compared to the EKF, it improves estimation accuracy.

[0005] With the development of robotics, the Error Kalman Filter (ESKF) has been developed based on the Kalman filter to better estimate the robot's motion state. The EKF estimates the error between the actual state and the nominal state to obtain the true state. The EKF has the following advantages: 1. The number of parameters is equal to the degrees of freedom of motion, avoiding over-parameterization. 2. The error state is always close to 0, and the linear system operates near the origin, reducing the effects of singular values and universal lock, ensuring the rationality and effectiveness of linearization. 3. The error state is small, so the second-order terms can be ignored, making the Jacobian matrix calculation simple and fast. 4. The error state changes smoothly, and the filter correction frequency does not need to be too high.

[0006] Because the error Kalman filter model is a linearization of the error model, it also faces issues with extending the Kalman filter. Furthermore, the aforementioned Kalman filter model suffers from the following two problems: 1. The error in the IMU-based prediction value accumulates over time, while the error in the observation value is relatively stable. However, the role played by the observation value in the Kalman filter process decreases, and the model tends to make increasingly incorrect predictions. 2. The noise statistical models vary in different scenarios. For example, the vehicle body vibration on cement roads and asphalt roads varies. When the noise models do not match, the Kalman filter model may diverge. Therefore, these issues in the improved Kalman filter have a significant impact on the quality of vehicle positioning results.

[0007] In view of this, overcoming the defects of the prior art is an urgent problem to be solved in this technical field. Summary of the Invention

[0008] The present invention provides a solution to the technical problem that the existing Kalman filter model tends to have prediction errors accumulated over time.

[0009] Furthermore, the present invention provides a solution to the technical problem that the existing Kalman filter model may have a divergence risk when the noise model does not match.

[0010] Furthermore, the present invention provides a solution to the technical problem that the existing Kalman filter model may be affected by singular values.

[0011] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0012] In a first aspect, the present invention provides a fusion positioning method of adaptive iterative filtering, comprising:

[0013] Get the status of the vehicle and assign a value to the vehicle status;

[0014] Construct a Kalman filter model and initialize the Kalman filter model;

[0015] Perform inertial calculation on the vehicle state, predict the vehicle error state, and output the vehicle error state variance, where the vehicle error state variance includes prior variance and posterior variance;

[0016] The attenuation factor is introduced to reduce the weight of the predicted value in the prior variance and improve the convergence of the Kalman filter model.

[0017] Preferably, the method for constructing the Kalman filter model includes:

[0018] Represent the state variables of the vehicle as a vector; wherein the state variables of the vehicle include nominal variables and real variables;

[0019] Calculate the error state variable between the nominal variable and the real variable of the vehicle;

[0020] Determine the vehicle's error state equation and observation equation.

[0021] Preferably, the method for initializing the Kalman filter model includes:

[0022] Set the vehicle's error state variable to 0;

[0023] The error state variance of the vehicle is set to the square of the noise of each variable;

[0024] Initialize the variance of the initial error state, the variance of the process noise, and the variance of the observation noise.

[0025] Preferably, after introducing the attenuation factor to reduce the weight of the predicted value in the prior variance and improve the convergence of the Kalman filter model, the method further includes:

[0026] The Sage-Husa adaptive filtering algorithm is introduced to correct the noise model.

[0027] Preferably, the noise model is corrected by the Sage-Husa adaptive filtering algorithm, and the method includes:

[0028] Set the adjustment factor and update the observation variance through the adjustment factor;

[0029] When there are observations, the predicted values are used as initial values, and the Kalman gain and the posterior state are updated by the iterative method derived from the LM algorithm;

[0030] Update the posterior variance of the error state, update the state quantity, and clear the error state to zero.

[0031] Preferably, the iterative method derived from the LM algorithm is used to calculate the Kalman gain and the update of the posterior state, and the method includes:

[0032] Update the damping coefficient using the Nielsen strategy;

[0033] Set the threshold for stopping iteration. When the optimal error state value between two iterations is less than the threshold, stop the iteration.

[0034] The error state when the iteration is stopped is used as the estimate of the posterior state, and the gain when the iteration is stopped is used as the Kalman gain at the current moment.

[0035] Preferably, the state of the vehicle includes one or more of an initial position, an initial speed, and an initial posture.

[0036] In a second aspect, the present invention provides an adaptive iterative filtering fusion positioning system, using the adaptive iterative filtering fusion positioning method as described in the first aspect, the system comprising:

[0037] A sensor module with a data acquisition function; the sensor module is used to collect the status of the vehicle;

[0038] An adaptive iterative filtering module with a Kalman filter model; the adaptive iterative filtering module is communicatively connected to the sensor module; wherein the Kalman filter model is provided with an attenuation factor, the attenuation factor is used to reduce the weight of the predicted value in the Kalman gain; the adaptive iterative filtering module includes a Sage-Husa adaptive filtering algorithm, a Gauss-Newton algorithm and an LM iterative algorithm, the Sage-Husa adaptive filtering algorithm is used to correct the noise model; the LM iterative algorithm is used to correct the Gauss-Newton algorithm.

[0039] Preferably, the sensor module includes one or more of GPS, IMU, camera, ultrasonic radar, and lidar.

[0040] In a third aspect, the present invention provides an adaptive iterative filtering fusion positioning electronic device, comprising:

[0041] at least one processor; and,

[0042] A memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the processor to perform the adaptive iterative filtering fusion positioning method as described in the first aspect.

[0043] In view of the deficiencies in the prior art, the present invention can achieve the following beneficial effects:

[0044] The present invention reduces the weight of the predicted value in the Kalman gain by introducing an attenuation factor into the error Kalman filter model. As time accumulates, the predicted value and its error are always confined to a suitable weight range, effectively preventing the prediction error tendency existing in the error Kalman filter model.

[0045] Furthermore, the present invention introduces the Sage-Husa adaptive filtering algorithm into the Kalman filter model, and corrects the noise model by setting adjustment factors, updating the observation value variance, etc., so as to solve the problem of noise model mismatch during scene switching, thereby preventing the error Kalman filter model from diverging.

[0046] Furthermore, the present invention introduces the LM iterative algorithm into the Kalman filter model, modifies the Gauss-Newton algorithm, re-derives the iterative formula, solves the error caused by local linearization, avoids the singular value problem in the Gauss-Newtom algorithm, and ensures the stability of the calculation process.

[0047] In general, the present invention improves and optimizes the existing Kalman filter model in the above-mentioned multiple aspects, which can more accurately estimate the vehicle positioning results, thereby improving the safety and reliability of the autonomous driving process. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] To more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive effort.

[0049] Figure 1 This is a flow chart of an adaptive iterative filtering fusion positioning method provided in Example 1;

[0050] Figure 2 This is a schematic diagram of a fusion positioning fusion process of adaptive iterative filtering provided in Example 1;

[0051] Figure 3 This is a flow chart of an adaptive iterative filtering fusion positioning method provided in Example 2;

[0052] Figure 4 This is a Kalman filter flow chart in the overall fusion positioning process of Example 1 and Example 2;

[0053] Figure 5 This is a schematic diagram of an adaptive iterative filtering fusion positioning system provided in Example 3;

[0054] Figure 6This is a schematic diagram of an adaptive iterative filtering fusion positioning electronic device provided in Example 4. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. In addition, the technical features in the various embodiments or single embodiments provided by the present invention can be arbitrarily combined with each other to form a feasible technical solution. This combination is not restricted by the sequence of steps and / or structural composition mode, but must be based on the ability of ordinary technicians in this field to implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0056] Example 1:

[0057] In order to solve the technical problem of the prediction error tendency of the existing Kalman filter model over time, this embodiment 1 provides a fusion positioning method of adaptive iterative filtering, such as Figure 1 FIG. 1 is a flow chart of a fusion positioning method using adaptive iterative filtering provided in Example 1. The method includes:

[0058] S100, obtaining the state of the vehicle and assigning a value to the state of the vehicle.

[0059] In this step, the state of the vehicle specifically refers to the state of the vehicle at a certain moment when it starts or is driving. The state data of the vehicle is collected by a sensor module with a data acquisition function. Depending on the sensor module, the collected state of the vehicle is also different. During the application process, the state of the vehicle includes one or more of the initial position, initial speed, and initial posture. Since the state of the vehicle changes with time, one or more of the above-mentioned initial position, initial speed, and initial posture respectively corresponds to one or more time-related variables; the process of assigning a value to the state of the vehicle is the process of assigning the value of the data collected by the sensor module to the above-mentioned variables.

[0060] like Figure 2 As shown, it is a schematic diagram of the fusion positioning fusion process of an adaptive iterative filtering provided in Example 1, where the initial position, initial velocity, and initial attitude are obtained by GPS, camera, and IMU, respectively.

[0061] The following example uses the data collected by the sensor module, including the initial position, initial velocity, and initial posture, as an example. In this step, the specific implementation is as follows:

[0062] = initial position;

[0063] = initial velocity;

[0064] The posture is represented by a rotation vector, and its corresponding SO(3) rotation matrix is represented by R(θ0).

[0065] S200: construct a Kalman filter model and initialize the Kalman filter model.

[0066] In this step, the Kalman filter model is constructed, and the method includes:

[0067] The state variables of the vehicle are represented as vectors; wherein the state variables of the vehicle include nominal variables and real variables.

[0068] The vehicle's state variables are represented as vectors. Specifically, the position, velocity, attitude, acceleration bias, and angular acceleration bias are represented as vectors:

[0069] x=(p,v,R(θ),a b ,ω b ) T

[0070] Among them, acceleration bias and angular acceleration bias are calculated through IMU calculations; the nominal variable and real variable are for one of the above variables under different premises. The nominal variable is the value determined under the existing premise or conditions, and the real variable is the value after the existing premise or conditions change. The nominal variable and the real variable are used to compare the changes in the variable when the conditions change.

[0071] Calculate the error between the nominal and true state variables of the vehicle.

[0072] Let the error state between the nominal variable and the real variable be δx k , the error state equation can be expressed as:

[0073] δx k = k-1 δx k-1 + k-1 w k

[0074] The specific expression is:

[0075] δxk =(δp k ,v k ,θ k ,a bk ,ω bk ) T

[0076] Among them, F k-1 and B k-1 They represent the predicted value at the initial moment (or the previous moment) and the error value at the initial moment (or the previous moment) respectively.

[0077] Determine the vehicle's error state equation and observation equation.

[0078] The specific processes of determining the vehicle's error state equation and observation equation are as follows:

[0079] Among them, δx k The posterior variance estimate is express.

[0080] w k is the process noise, w k The expression is:

[0081] w k =(n a ,n ω ,n ab ,n ωb )

[0082] Among them, n a 、n ω 、n ab 、n ωb They represent the noise of the measured acceleration, the noise of the measured angular velocity, the noise of the measured acceleration bias, and the noise of the measured angular velocity bias, respectively.

[0083] w k The variance is recorded as Q k .

[0084] Through a series of calculations, F k-1 equal:

[0085]

[0086]

[0087] The observation equation is:

[0088] y k =G(δx k )+n k

[0089] Among them, y kis the observed value, R k Represents the rotation matrix corresponding to the posture, n k is the noise of the observation value, the noise of the observation value n k The variance of express.

[0090] It should be noted that the above observation equation is a general expression. For different sensor modules, the observation value and the noise model of the observation value are different.

[0091] In this step, the Kalman filter model is initialized, and the method includes:

[0092] Set the vehicle's error state variable to 0.

[0093] Right now:

[0094]

[0095] The error state variance of the vehicle is set to the square of the noise of each variable.

[0096] Initialize the variance of the initial error state, the variance of the process noise, and the variance of the observation noise.

[0097] Initial error state variance:

[0098]

[0099] Among them, P δp 、P δv 、P δθ 、 They represent the error state variance of the initial position, the error state variance of the initial velocity, the error state variance of the initial attitude, the error state variance of the initial bias, and the error state variance of the initial angular velocity bias, which together constitute the initial error state variance. matrix.

[0100] Variance of process noise: (can be obtained from IMU)

[0101]

[0102] Among them, Q a , Q ω 、 They represent the variance of the measured acceleration, the variance of the measured angular velocity, the variance of the measured acceleration bias, and the variance of the measured angular velocity bias, respectively.

[0103] Variance of observation noise: (can be obtained from GPS, camera)

[0104]

[0105] Among them, represent the variance of the observation noise of position, the variance of the observation noise of velocity, and the variance of the observation noise of attitude, respectively.

[0106] S300 , performing inertial calculation on the state of the vehicle, predicting the error state of the vehicle, and outputting the error state variance of the vehicle, wherein the error state variance of the vehicle includes a priori variance and a posteriori variance.

[0107] In this step, the inertial solution process predicts the vehicle condition at the next moment through the actual value of the vehicle condition at the current moment (i.e., the observed value) and the predicted value of the vehicle condition at the current moment (i.e., the predicted value). The prior variance refers to the error state variance of the vehicle before fusion positioning, and correspondingly, the posterior variance refers to the error state variance of the vehicle after fusion positioning.

[0108] S400, introduces an attenuation factor to reduce the weight of the predicted value in the prior variance and improve the convergence of the Kalman filter model.

[0109] In this step, the weight of the predicted value in the prior variance is reduced, that is, the prior variance in the variance prediction step is multiplied by the attenuation factor.

[0110] In the specific implementation, assume:

[0111]

[0112] Its meaning is: use the predicted value at the previous moment and the error state variance at the previous moment to estimate the error state variance at the current moment.

[0113]

[0114] Where γ represents the attenuation factor, 0<γ<1; Represents the initial error state variance after fusion estimated value.

[0115] This embodiment 1 provides an adaptive iterative filtering fusion positioning method, which reduces the weight of the predicted value in the Kalman gain by introducing an attenuation factor into the error Kalman filter model, thereby effectively preventing the prediction error tendency existing in the error Kalman filter model.

[0116] Example 2:

[0117] On the basis of Example 1, in order to solve the technical problem that the existing Kalman filter model may have the risk of divergence when the noise model does not match, this Example 2 provides a fusion positioning method of adaptive iterative filtering, such as Figure 3FIG. 1 is a flow chart of a fusion positioning method for adaptive iterative filtering provided in Example 2. After introducing an attenuation factor to reduce the weight of the predicted value in the prior variance and improve the convergence of the Kalman filter model, the method further includes:

[0118] S500 introduces the Sage-Husa adaptive filtering algorithm to correct the noise model.

[0119] In this step, the noise model is corrected by the Sage-Husa adaptive filtering algorithm, and the method includes:

[0120] Set the adjustment factor and update the observation variance through the adjustment factor;

[0121] Assume that the variance of the observations is The adjustment factor is d k ,in:

[0122] 0 <b<1

[0123] Preferably, b is generally between 0.95 and 0.99.

[0124]

[0125]

[0126] in, G x represents the Jacobian matrix of G(x) for x, K k-1 is the Kalman filter gain at the last iteration, The overall representative observation's prior variance.

[0127] When there are observations, the predicted values are used as the initial values, and the Kalman gain and the posterior state are updated by the iterative method derived from the LM algorithm.

[0128] The LM algorithm, namely the Levenberg-Marquardt algorithm, is an optimization algorithm and an iterative algorithm for solving least squares problems. It can be regarded as a combination of the steepest descent method and the Gauss-Newton method. In practical applications, the damping μ is switched by adjusting the damping μ.

[0129] When implementing it specifically, let: Since the iterative Kalman filter requires the minimum value of F(x), we have:

[0130]

[0131]

[0132] in, for The variance of , assuming that the observation noise and process noise are independent, expression:

[0133]

[0134] The point where F(x) takes the minimum (optimal) value is used as the posterior estimate of the current state. According to the LM algorithm, the iterative format can be derived as follows:

[0135]

[0136] δx op, =δx op, +K i (y k -G x (δx op, ))

[0137] Among them, δx op Represents the optimal error state value, and i represents the number of iterations.

[0138] G x Denotes the Jacobian matrix of G(x) with respect to x.

[0139] Since the error Kalman filter is to locally linearize the nonlinear model, the stronger the nonlinearity, the greater the error. This problem can be solved by the iterative method of the extended Kalman filter. However, the iterative formula of the extended Kalman filter is equivalent to the nonlinear optimization algorithm Gauss-Newton algorithm. When the Jacobian matrix J of the nonlinear function is multiplied by its transpose product J T When J is a singular matrix, the stability of the Gauss-Newton algorithm is very poor. In order to solve the technical problem that the existing Kalman filter model may be affected by singular values, the iterative method derived from the LM algorithm is used to calculate the Kalman gain and the update of the posterior state. The method includes:

[0140] Update the damping coefficient using the Nielsen strategy;

[0141] Let the damping coefficient be μ, where μ is updated as follows:

[0142] Let L(Δx) be:

[0143]

[0144] Where J is the Jacobian matrix of F(x) with respect to x, L is the second-order Taylor expansion of F(x), and ρ is equal to:

[0145]

[0146] Where ρ is the scaling factor for updating the damping coefficient.

[0147] The update formula of μ is:

[0148]

[0149] Set the threshold for stopping iteration. When the optimal error state value between two iterations is less than the threshold, stop the iteration.

[0150] In the specific implementation, let the threshold for stopping iteration be ∈, when |Fδx op, )-F(δx op, )|<∈, stop iteration.

[0151] The error state when the iteration is stopped is used as the estimate of the posterior state, and the gain when the iteration is stopped is used as the Kalman gain at the current moment.

[0152] Update the posterior variance of the error state, update the state quantity, and clear the error state to zero.

[0153] Update the posterior variance of the new error state:

[0154]

[0155] Update state:

[0156]

[0157] The pose state update method is:

[0158] R k =R k-1 δR k

[0159] Error status clear:

[0160]

[0161] This embodiment 2 provides a fusion positioning method of adaptive iterative filtering, which introduces the Sage-Husa adaptive filtering algorithm into the Kalman filter model to correct the noise model to solve the problem of noise model mismatch during scene switching, thereby preventing the error Kalman filter model from diverging and enhancing the accuracy of fusion positioning; further, by introducing the LM iterative algorithm into the Kalman filter model, the Gauss-Newton algorithm is corrected to solve the error caused by local linearization, avoid the singular value problem in the Gauss-Newtom algorithm, ensure the stability of the calculation process, and further improve the accuracy and reliability of fusion positioning.

[0162] like Figure 4 FIG. 1 is a flow chart of the Kalman filter in the overall fusion positioning process of Example 1 and Example 2.

[0163] Example 3:

[0164] Based on the same general technical solution as Example 1 and Example 2, as Figure 5 FIG. 1 is a schematic diagram of a fusion positioning system using adaptive iterative filtering provided in Example 3. The system includes:

[0165] A sensor module with a data acquisition function; and an adaptive iterative filtering module with a Kalman filter model; the adaptive iterative filtering module is in communication with the sensor module; the sensor module is used to collect the status of the vehicle. In actual application, the sensor module includes one or more of GPS, IMU, camera, ultrasonic radar, and lidar. GPS is used to collect the position of the vehicle, IMU is used to calculate the posture of the vehicle, such as acceleration and angular velocity, camera is used to collect the image of the vehicle, and obtain the speed of the vehicle by performing feature extraction and calculation on the image, and lidar is used to collect the distance of the vehicle. The above sensor modules can be combined and selected according to the actual application needs; wherein, the Kalman filter model An attenuation factor is provided in the Kalman filter, which is used to reduce the weight of the predicted value in the Kalman gain, thereby preventing the error Kalman filter model from having a tendency to make prediction errors. The adaptive iterative filtering module includes a Sage-Husa adaptive filtering algorithm, a Gauss-Newton algorithm and a LM iterative algorithm. Among them, the Sage-Husa adaptive filtering algorithm is used to correct the noise model, thereby solving the problem of mismatch of the noise model during scene switching, thereby preventing the error Kalman filter model from diverging. The LM iterative algorithm is used to correct the Gauss-Newton algorithm, avoiding the singular value problem in the Gauss-Newtom algorithm, thereby ensuring the stability of the calculation process.

[0166] Example 4:

[0167] Based on the same general technical solution as Example 1 and Example 2, as Figure 6 The figure shows a schematic diagram of an adaptive iterative filtering fusion positioning electronic device provided in Example 4, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the processor to execute the adaptive iterative filtering fusion positioning method of Examples 1 and 2.

[0168] In summary, the present invention provides a fusion positioning method, system and electronic device for adaptive iterative filtering. By improving and optimizing the existing Kalman filter model in the above-mentioned multiple aspects, it can more accurately estimate the vehicle positioning results, thereby improving the safety and reliability of the autonomous driving process.

[0169] It should be noted that, in the above embodiments, the description of each embodiment has its own focus. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant description of other embodiments.

[0170] It will be understood by those skilled in the art that embodiments of the present invention may be provided as methods, systems, electronic devices, or computer software program products. Thus, the present invention may take the form of a fully hardware embodiment, a fully software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0171] The present invention is described with reference to flowcharts and / or block diagrams of methods, systems, electronic devices, or computer software program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded computer, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A system that specifies the functions of a box or boxes.

[0172] These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture including an instruction system that is implemented in the process. Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0173] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1The steps for the function specified in one or more boxes.

[0174] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

Claims

1. A fusion positioning method based on adaptive iterative filtering, characterized in that: Methods include: Get the status of the vehicle and assign a value to the vehicle status; Construct a Kalman filter model and initialize the Kalman filter model; Perform inertial calculation on the vehicle state, predict the vehicle error state, and output the vehicle error state variance, where the vehicle error state variance includes prior variance and posterior variance; Introducing an attenuation factor to reduce the weight of the predicted value in the prior variance and improve the convergence of the Kalman filter model; The Sage-Husa adaptive filtering algorithm is introduced to correct the noise model; The noise model is corrected by the Sage-Husa adaptive filtering algorithm, and the method includes: Set the adjustment factor and update the observation variance through the adjustment factor; When there are observations, the predicted values are used as initial values, and the Kalman gain and the posterior state are updated by the iterative method derived from the LM algorithm; Update the posterior variance of the error state, update the state quantity, and clear the error state to zero.

2. The adaptive iterative filtering fusion positioning method according to claim 1, characterized in that: The method for constructing the Kalman filter model includes: Represent the state variables of the vehicle as a vector; wherein the state variables of the vehicle include nominal variables and real variables; Calculate the error state variable between the nominal variable and the real variable of the vehicle; Determine the vehicle's error state equation and observation equation.

3. The adaptive iterative filtering fusion positioning method according to claim 2, characterized in that: The method for initializing the Kalman filter model includes: Set the vehicle's error state variable to 0; The error state variance of the vehicle is set to the square of the noise of each variable; Initialize the variance of the initial error state, the variance of the process noise, and the variance of the observation noise.

4. The adaptive iterative filtering fusion positioning method according to claim 1, characterized in that: The iterative method derived by the LM algorithm is used to calculate the Kalman gain and the update of the posterior state, and the method includes: Update the damping coefficient using the Nielsen strategy; Set the threshold for stopping iteration. When the optimal error state value between two iterations is less than the threshold, stop the iteration. The error state when the iteration is stopped is used as the estimate of the posterior state, and the gain when the iteration is stopped is used as the Kalman gain at the current moment.

5. The adaptive iterative filtering fusion positioning method according to claim 1, characterized in that: The state of the vehicle includes one or more of an initial position, an initial speed, and an initial posture.

6. An adaptive iterative filtering fusion positioning system, characterized in that: The system using the fusion positioning method of adaptive iterative filtering according to any one of claims 1 to 5 includes: A sensor module with a data acquisition function; the sensor module is used to collect the status of the vehicle; An adaptive iterative filtering module with a Kalman filter model; the adaptive iterative filtering module is in communication with the sensor module; wherein the Kalman filter model is provided with an attenuation factor, the attenuation factor being used to reduce the weight of the predicted value in the Kalman gain; the adaptive iterative filtering module includes a Sage-Husa adaptive filtering algorithm, a Gauss-Newton algorithm, and an LM iterative algorithm; the Sage-Husa adaptive filtering algorithm is used to correct the noise model; and the LM iterative algorithm is used to correct the Gauss-Newton algorithm; The Sage-Husa adaptive filtering algorithm is specifically used for: Set the adjustment factor and update the observation variance through the adjustment factor; When there are observations, the predicted values are used as initial values, and the Kalman gain and the posterior state are updated by the iterative method derived from the LM algorithm; Update the posterior variance of the error state, update the state quantity, and clear the error state to zero.

7. The adaptive iterative filtering fusion positioning system according to claim 6, characterized in that: The sensor module includes one or more of GPS, IMU, camera, ultrasonic radar, and lidar.

8. An adaptive iterative filtering fusion positioning electronic device, characterized in that: include: at least one processor; as well as, A memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the processor to perform the adaptive iterative filtering fusion positioning method according to any one of claims 1 to 5.

Images