Multi-sensor fusion positioning robustness method based on trajectory error

Abstract

This invention discloses a robust multi-sensor fusion positioning method based on trajectory error. This method determines sensor interference or failure by analyzing the trajectory errors of each sensor's positioning trajectory. Then, it reduces the fusion weight of positioning results from interfering or malfunctioning sensors, or removes the fusion weight of such sensors and uses the remaining sensors for fusion positioning. This invention improves the robustness of the positioning algorithm, ensuring good positioning results even when sensors are interfered with but their own evaluation metrics remain good. It solves the problem of decreased positioning accuracy after fusion positioning and significantly improves positioning accuracy.

Description

Technical Field

[0001] This invention relates to the field of sensor positioning technology, and in particular to a robust multi-sensor fusion positioning method based on trajectory error. Background Technology

[0002] Sensor positioning is currently used in vehicle driving. However, single-sensor positioning is insufficient to meet the long-term positioning requirements across all scenarios. Multi-sensor fusion positioning, which fully integrates the advantages of various sensor positioning methods, is gradually becoming the mainstream positioning method. However, when a sensor fails, the accuracy of sensor fusion positioning will decrease. Similarly, when a certain type of sensor is interfered with, such as GNSS positioning near tall buildings, it is prone to multipath interference, resulting in significant positioning deviations while its own evaluation indicators remain good. This also leads to a decrease in positioning accuracy after fusion positioning. Summary of the Invention

[0003] The technical problem to be solved by the present invention is to provide a robust multi-sensor fusion positioning method based on trajectory error that still has good positioning performance when the sensor is interfered with.

[0004] To solve the above-mentioned technical problems, the technical solution of the present invention is: a multi-sensor fusion positioning robustness method based on trajectory error. This method determines the interference or failure status of the sensors by acquiring the trajectory error of the positioning trajectory of each sensor, and then reduces the fusion weight of the positioning results of the interfering or failed sensors, or removes the fusion weight of the positioning results of the interfering or failed sensors and uses the remaining sensors for fusion positioning.

[0005] As a preferred technical solution, a robust multi-sensor fusion positioning method based on trajectory error includes:

[0006] Step 1. Acquire and cache the sensor's pose trajectory;

[0007] Step 2. Interpolate the trajectory pose;

[0008] Step 3. Calculate the trajectory error;

[0009] Step 4. Determine if the sensor has been interfered with or is malfunctioning;

[0010] Step 5. Adjust the weights. If the judgment that the sensor is interfered with or malfunctions is true, then reduce the fusion weight of the sensor's positioning trajectory.

[0011] As a preferred technical solution, the pose trajectory of the sensor acquired and cached in step 1 is P,

[0012] As a preferred technical solution, in step 2, the trajectory is located as P1, ..., P after interpolation of the trajectory pose. nAnd Q1, ..., Q n The time difference is Δt.

[0013] As a preferred technical solution, the trajectory error calculation in step 3 uses relative pose error to calculate the change in two poses separated by a time difference of Δ.

[0014]

[0015] The translation component of the relative pose error, trans(E) i Calculate the RMS value to represent the trajectory error between P and Q.

[0016]

[0017] As a preferred technical solution, in step 4, t1 to t are obtained by the following formula. n Trajectory error sequence Err at time step t1 (P, Q), ..., Err tn (P, Q),

[0018]

[0019] When t n If the trajectory error at any given time exceeds a predetermined threshold σ, then the P trajectory is considered to have a large error, and the sensor is considered to be interfered with or malfunctioning.

[0020] As a preferred technical solution, in the weight adjustment of step 5, if a sensor is considered to be interfering or malfunctioning in step 4, the fusion weight of the sensor trajectory is reduced, or the fusion weight of the sensor trajectory is removed and the remaining sensors are used for fusion positioning.

[0021] This robust multi-sensor fusion positioning method based on trajectory error, employing the aforementioned technical solution, determines sensor interference or failure by analyzing the trajectory errors of each sensor's positioning trajectory. It then reduces or eliminates the fusion weight of positioning results from interfering or malfunctioning sensors, using the remaining sensors for fusion positioning. By calculating the trajectory errors of each sensor's positioning trajectory and analyzing whether sensors are interfered with or malfunctioning, this invention reduces or even eliminates the fusion weight of positioning results from interfering or malfunctioning sensors, thereby improving the robustness of the positioning algorithm. This ensures good positioning performance even when sensors are interfered with but their own evaluation indicators remain good, solving the problem of decreased positioning accuracy after fusion positioning and significantly improving positioning accuracy. Attached Figure Description

[0022] The following figures are intended only to illustrate and explain the present invention and do not limit the scope of the invention. Wherein:

[0023] Figure 1 This is a flowchart illustrating the present invention. Detailed Implementation

[0024] The present invention will be further described below with reference to the accompanying drawings and embodiments. In the following detailed description, only certain exemplary embodiments of the invention are described by way of illustration. Undoubtedly, those skilled in the art will recognize that various modifications can be made to the described embodiments without departing from the spirit and scope of the invention. Therefore, the drawings and description are illustrative in nature and not intended to limit the scope of the claims.

[0025] like Figure 1 As shown, a robust multi-sensor fusion positioning method based on trajectory error is used for multi-sensor fusion positioning. This method determines the interference or failure status of sensors by measuring the trajectory errors of the positioning trajectories of each sensor. Then, it reduces the fusion weight of positioning results from interfering or failed sensors, or removes the fusion weight of positioning results from interfering or failed sensors and uses the remaining sensors for fusion positioning. Specifically, it includes:

[0026] After starting this method, perform the following steps:

[0027] Step 1. Obtain and cache the sensor's pose trajectory, where the obtained and cached sensor pose trajectory is P,

[0028] Step 2. Interpolate the trajectory pose. After interpolation, the localized trajectory is P1, ..., P n And Q1, ..., Q n The time difference is Δt;

[0029] Step 3. Calculate the trajectory error. The trajectory error is calculated by using relative pose error to calculate the change in two poses separated by a time difference Δ.

[0030]

[0031] The translation component of the relative pose error, trans(E) i Calculate the RMS value to represent the trajectory error between P and Q.

[0032]

[0033] Step 4. Determine if the sensor is interfered with or malfunctioning, and obtain t1 to t2 using the following formula. n Trajectory error sequence Err at time step t1 (P, Q), ..., Err tn (P, Q),

[0034]

[0035] When t n If the trajectory error at time t exceeds the predetermined threshold σ, then the P trajectory is considered to have a large error, indicating that the sensor is either interfered with or malfunctioning, and the next step is initiated; if when t n If the trajectory error at any given time does not exceed the predetermined threshold σ, then the trajectory error of P is considered to be small or non-existent, and the sensor is considered to be neither interfered with nor malfunctioning, thus ending this method.

[0036] It can also calculate other trajectories from t1 to t2. n The error sequence at each time step is used for determination, such as...

[0037]

[0038]

[0039] Step 5. Adjust the weights. If the judgment that a sensor is considered to be interfering or malfunctioning in Step 4 is valid, then reduce the fusion weight of the sensor trajectory, or remove the fusion weight of the sensor trajectory and use the remaining sensors for fusion positioning. This method ends.

[0040] This invention is a robust multi-sensor fusion positioning method based on trajectory similarity or trajectory error. This method not only ensures the advantages of multi-sensor fusion positioning, but also solves the problem that when a sensor is near a tall building and GNSS positioning is subject to multipath interference, resulting in a large positioning deviation while its own evaluation index remains good, the positioning accuracy decreases after fusion positioning, thus greatly improving the positioning accuracy.

[0041] As described above, embodiments of the present invention have been specifically described, but the invention is not limited thereto. Those skilled in the art should understand that various modifications, combinations, sub-combinations, or substitutions can be made according to design requirements or other factors, and these are within the scope of the appended claims and their equivalents.

Claims

1. A robust method of multi-sensor fusion positioning based on trajectory error, characterized in that: This method determines sensor interference or failure by analyzing the trajectory errors of each sensor's positioning trajectory. It then reduces the fusion weight of positioning results from interfering or malfunctioning sensors, or removes the fusion weight of these sensors and uses the remaining sensors for fusion positioning. This includes: Step 1. Acquire and cache the sensor's pose trajectory; Step 2. Interpolate the trajectory pose; Step 3. Calculate the trajectory error; Step 4. Determine if the sensor has been interfered with or is malfunctioning; Step 5. Adjust the weights. If the judgment that the sensor is interfered with or malfunctions is valid, then reduce the fusion weight of the sensor's positioning trajectory; whereby... In step 1, the pose trajectories of the sensor are obtained and cached as P, Q∈SE(3); In step 2, the trajectory is located after interpolation of the trajectory pose. and The time difference is ; In step 3, the trajectory error calculation is performed by calculating the relative pose error. Change in pose between two positions over time The translation part of the relative pose error Calculate the RMSE value to represent the trajectory error between P and Q. In step 4, the following formula is used to obtain... arrive Trajectory error sequence at time step , when The trajectory error at any given time exceeds the predetermined threshold. If the error is large, it is considered that the sensor is being interfered with or is malfunctioning.

2. The robust multi-sensor fusion positioning method based on trajectory error as described in claim 1, characterized in that: In step 5, if a sensor is considered to be interfering or malfunctioning in step 4, the fusion weight of the sensor's positioning trajectory is reduced, or the fusion weight of the sensor's positioning trajectory is removed and the remaining sensors are used for fusion positioning.

Images