A positioning method and system based on adaptive multi-sensor fusion

Abstract

This invention relates to a positioning method and system based on adaptive multi-sensor fusion, belonging to the field of positioning technology, and aims to solve the problems of low positioning accuracy and poor stability in complex environments of existing positioning technologies. This method effectively integrates the advantages of each sensor by deeply fusing the VIW system with GPS. First, GPS data is evaluated, corrected, and filtered using an improved GPS accuracy factor model and an anomaly detection model based on fuzzy logic and statistical learning. Second, local pose estimation results are obtained using the VIW system, and time synchronization between GPS and the VIW system is achieved through a time difference compensation strategy. Finally, an optimization objective function containing VIW system residuals and GPS residuals is constructed to update the transformation matrix between the VIW system and the GPS coordinate system. This invention is applicable to the positioning needs of intelligent robots, autonomous vehicles, and other devices in urban canyons, tunnels, indoor environments, and long-distance, high-dynamic scenarios.

Description

Technical Field

[0001] This invention belongs to the field of positioning technology and relates to a positioning method and system based on adaptive multi-sensor fusion. Background Technology

[0002] In the booming development of intelligent robots and self-driving cars, localization and navigation technologies have become crucial. Traditional SLAM algorithms have significant limitations in complex environments.

[0003] Visual SLAM algorithms, such as ORB-SLAM and LSD-SLAM, primarily rely on camera-acquired image information to detect and match feature points for localization and mapping. They perform well in well-lit, textured scenes, but in scenarios with drastic lighting changes (e.g., direct sunlight, areas with alternating shadows), indistinct textures (e.g., monotonous walls, solid-color floors), or with many dynamic objects (e.g., densely populated areas, busy intersections), the accuracy of feature matching drops significantly, severely compromising localization precision and stability. For example, in a warehouse environment with flickering indoor lighting, the ORB-SLAM algorithm may experience localization deviations of several meters or more due to mismatched feature points, failing to meet the requirements for precise localization.

[0004] LiDAR SLAM algorithms, such as LOAM and LIO-SAM, leverage the high-precision distance measurement of LiDAR for environmental modeling and localization. They can achieve high-precision localization in static environments; however, the high cost of LiDAR limits its widespread application. Furthermore, in low-visibility environments (such as smoke-filled factories or outdoor conditions in rain or snow), the laser beam is severely interfered with, leading to a sharp decline in performance. In fire rescue scenarios with dense smoke, LiDAR SLAM algorithms may fail to effectively detect the surrounding environment, causing robots or autonomous vehicles to lose their localization capabilities.

[0005] Visual-inertial SLAM algorithms such as VINS-Mono and OKVIS, while combining visual sensors with inertial measurement units (IMUs) and utilizing high-frequency IMU data to compensate for the high-speed motion delay of the visual sensors, have improved real-time performance and stability to some extent. However, due to the difficulty in eliminating long-term integration errors of the IMU, positioning drift remains a significant problem in the absence of external reference signal calibration. For example, in long-distance outdoor autonomous inspection tasks, after a period of operation, the positioning results of the VINS-Mono algorithm may deviate from the actual trajectory by several meters, affecting the mission's performance.

[0006] GPS, as a commonly used positioning method, can provide relatively accurate location information in open environments. However, in complex environments such as urban canyons, tunnels, and indoor spaces, signals are easily affected by building obstruction and electromagnetic interference, leading to signal failure, instability, or loss of lock, resulting in highly unstable positioning performance. In urban streets filled with tall buildings, GPS signals may frequently interrupt or produce significant errors, making it impossible to provide reliable positioning services for vehicles or robots. Summary of the Invention

[0007] In view of this, the purpose of the present invention is to provide a positioning method and system based on adaptive multi-sensor fusion.

[0008] To achieve the above objectives, the present invention provides the following technical solution:

[0009] A localization method based on adaptive multi-sensor fusion includes the following steps:

[0010] GPS absolute position information is collected and processed using an improved GPS accuracy factor model and an anomaly detection model based on fuzzy logic and statistical learning. The accuracy factor model combines geometric accuracy factor, atmospheric delay correction term and multipath effect correction term. The anomaly detection model uses fuzzy logic rules to judge the probability of signal anomalies and combines support vector machine classification algorithm to improve detection accuracy, so as to screen out high-quality GPS data.

[0011] The camera, inertial measurement unit (IMU), and wheel speed meter information are input into the VIW positioning system to obtain local pose estimation results. At the same time, the GPS latitude and longitude data are converted into UTM rectangular coordinates and transformed into the VIW system coordinate system. It is determined whether the GPS and VIW system timestamps match. If they do not match, a compensation function based on the VIW system wheel speed meter linear velocity information is used to compensate for the time difference, solve for the time difference, and optimize the time synchronization relationship.

[0012] An optimization objective function is constructed, which includes VIW system residuals and GPS residuals. The VIW system residual factor is calculated based on the position and attitude of adjacent time nodes, while the GPS residual factor is calculated based on the GPS measurement position and the transformed position in the UTM coordinate system. During the optimization process, the weights of the residuals are dynamically adjusted according to the number of GPS satellites searched and the signal quality. The solution is obtained iteratively using the Ceres Solver open-source library based on the Gauss-Newton method and the Levenberg-Marquardt algorithm, and the transformation matrix between the VIW system and the GPS coordinate system is updated to achieve local accuracy and global consistency in positioning.

[0013] Furthermore, in the improved GPS accuracy factor model, atmospheric delay correction and multipath effect correction terms are used to correct the positioning accuracy of the GPS signal in real time; let the receiver's position be (x a ,y a ,za The satellite's position is (x s ,y s ,z s The traditional pseudorange measurement model is The improved pseudorange measurement formula is as follows: Among them, atmospheric delay correction term Multipath effect correction term θ is the angle between the satellite signal and the ground, Δd is the offset distance caused by multipath effect, and T std and D max These are standard scales for atmospheric and multipath effects, respectively, and the positioning accuracy of GPS signals is corrected in real time by adaptively adjusting these correction terms.

[0014] Furthermore, the time difference compensation specifically involves: constructing a compensation function using the linear velocity information from the wheel speedometer of the VIW system to achieve time synchronization between the GPS and the VIW system;

[0015] When the time difference between the VIW system and GPS is unknown and Δt, the constructed GPS compensation function is: The objective function after compensation is because The speed information comes from the wheel speedometer linear velocity information in the VIW system, and the compensation objective function is expressed as: After simplification This method enables time difference compensation and time synchronization between the VIW system and the GPS system.

[0016] Furthermore, in the optimization objective function, the local pose residual factor is calculated based on the position and attitude of adjacent time nodes, and the GPS residual factor is calculated based on the GPS measurement position and the transformed position in the UTM coordinate system.

[0017] The objective function to be optimized is in The expression is given by the Mahalanobis norm, where h is the corresponding measurement matrix, and the local pose residual factor is expressed as follows: The formula for calculating the GPS residual factor is:

[0018] A positioning system based on adaptive multi-sensor fusion includes:

[0019] The sensor module, which includes a GPS sensor, camera, IMU, and wheel speedometer, is used to acquire position, image, inertial, and motion information.

[0020] The data processing unit includes a GPS data processing module, a time compensation module, and a pose graph optimization module. The GPS data processing module is used to process GPS data using an improved GPS accuracy factor model and an anomaly detection model. The time compensation module is used to compensate for the time difference between GPS and the VIW system based on the wheel speed meter linear velocity information of the VIW system. The pose graph optimization module is used to construct and solve an optimization objective function that includes VIW system residuals and GPS residuals.

[0021] The control unit receives the results from the pose graph optimization module, controls the movement of the device or performs other tasks, and coordinates the workflow of each module.

[0022] Furthermore, when processing GPS data, the GPS data processing module combines geometric accuracy factor, atmospheric delay correction term and multipath effect correction term to correct the data, converts latitude and longitude data into UTM rectangular coordinates and converts them into VIW system coordinates, and uses fuzzy logic and support vector machine to filter out high-quality GPS data.

[0023] Furthermore, when performing time difference compensation, the time compensation module uses the constructed GPS compensation function. By solving for the time difference Δt and utilizing the linear velocity information from the VIW system wheel speedometer... To achieve time synchronization between GPS and the VIW system.

[0024] Furthermore, when constructing the objective function, the pose graph optimization module constructs a function containing VIW system residuals and GPS residuals in accordance with the manner described in claim 4, and dynamically adjusts the GPS signal weights based on the number of GPS satellites searched and the signal quality, and iteratively solves the problem using the Ceres Solver open-source library to update the transformation matrix between the VIW system and the GPS coordinate system.

[0025] A method for using a positioning system based on adaptive multi-sensor fusion includes the following steps:

[0026] Equipment installation and initialization steps: Install GPS sensors, cameras, IMUs and wheel speedometers on the target device, set and calibrate parameters, and perform self-tests on each sensor after starting the device;

[0027] Data acquisition and transmission steps: During equipment operation, each sensor synchronously acquires data and transmits it to the data processing unit;

[0028] GPS data preprocessing steps: The data processing unit uses an improved GPS accuracy factor model and anomaly detection model to evaluate, correct, and filter GPS data, and transforms data coordinates and coordinate systems;

[0029] VIW system positioning and time difference compensation steps: Input camera, IMU and wheel speed meter data into the VIW system to obtain local pose estimation results, compare GPS and VIW system timestamps and perform time difference compensation;

[0030] Pose graph optimization and fusion steps: Construct an optimization objective function containing VIW system residuals and GPS residuals, dynamically adjust the weights according to the number of GPS satellites searched and signal quality, iteratively solve the problem using the Ceres Solver open-source library, and update the transformation matrix between the VIW system and the GPS coordinate system.

[0031] Positioning result application and feedback steps: The positioning result is output to the control system, which performs path planning and continuously monitors the equipment operation status and positioning effect. The data is then fed back to the data processing unit for optimization and adjustment.

[0032] Furthermore, during the device installation and initialization steps, ensure that each sensor is securely installed and positioned appropriately to avoid mutual interference, and record the initial parameter values.

[0033] In the data acquisition and transmission steps, data is transmitted in a high-speed and stable manner through the internal communication module of the device;

[0034] In the GPS data preprocessing step, the correction parameters of the GPS accuracy factor model are adjusted in real time according to the environment in which the device is located.

[0035] In the pose graph optimization and fusion step, the transformation matrix is ​​updated after each round of optimization to ensure the consistency between local accurate estimation and global coordinates;

[0036] In the positioning result application and feedback step, the data processing unit optimizes and adjusts the sensor parameters and data processing algorithm based on the feedback data.

[0037] The beneficial effects of this invention are as follows:

[0038] 1. This invention effectively integrates the advantages of various sensors by deeply fusing a tightly coupled visual / inertial / wheel velocity measurement (VIW) positioning system with GPS. In complex environments, it overcomes the limitations of single sensors, such as the accuracy degradation of visual SLAM in scenarios with lighting and texture problems, the high cost and poor adaptability to harsh weather conditions of lidar SLAM, and the long-term integration error of the IMU in visual-inertial SLAM. It achieves local precision and global consistency in positioning, improving the average positioning accuracy by at least 15% compared to current mainstream visual SLAM algorithms, significantly enhancing positioning accuracy.

[0039] 2. The constructed GPS accuracy factor model and anomaly detection mechanism can effectively address the problem of GPS signal susceptibility to obstruction and interference in complex environments such as urban canyons, tunnels, and indoor spaces, selecting high-quality GPS data for fusion. Simultaneously, during the optimization process, weights are dynamically adjusted based on the number of GPS satellites acquired and signal quality, along with the auxiliary role of the VIW system when GPS signals are weak. This ensures that the system maintains stable positioning performance even in complex environments and under conditions of weakened or temporarily lost GPS signals, effectively mitigating pose drift and accumulated errors, and enhancing the system's robustness.

[0040] 3. By employing a pose graph optimization strategy instead of the traditional bundle adjustment method, the computational burden is significantly reduced when dealing with situations involving the growth of mobile platform trajectories and the dense accumulation of feature points in large-scale scenes. By locking spatial feature points as static constraints and focusing on pose refinement, the system's real-time performance is improved while maintaining high-precision pose estimation. This meets the efficiency requirements of positioning systems in intelligent robots and autonomous vehicles, enabling them to adapt to real-time positioning needs in dynamic environments.

[0041] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0042] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0043] Figure 1 This is a system architecture diagram of a localization method and system based on adaptive multi-sensor fusion;

[0044] Figure 2 This is a flowchart illustrating the workflow of a localization method and system based on adaptive multi-sensor fusion.

[0045] Figure 3 This is a VIG-SLAM adaptive multi-sensor fusion framework diagram for a localization method and system based on adaptive multi-sensor fusion.

[0046] Figure 4 A diagram showing the correspondence between the VIW system and GPS timestamps;

[0047] Figure 5 A global pose graph optimization model with GPS constraints;

[0048] Figure 6 A comparison chart of translation and rotation errors for Urban27;

[0049] Figure 7 A comparison chart of translation and rotation errors in Urban34;

[0050] Figure 8 This is a diagram showing the relative pose error of a small-scale outdoor experiment. Detailed Implementation

[0051] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0052] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0053] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0054] Please see Figures 1 to 8This invention aims to address the problems of low positioning accuracy and poor stability in existing positioning technologies under complex environments. Specifically, it overcomes the positioning errors caused by visual SLAM algorithms in poor lighting, lack of texture, and difficulty in feature matching in dynamic scenes; it addresses the application limitations of lidar SLAM algorithms due to cost and adverse weather conditions; it improves the positioning drift caused by long-term integration errors of IMU in visual inertial SLAM algorithms; and it addresses the instability of GPS positioning due to signal interference in complex environments. Through an innovative adaptive multi-sensor fusion method, it achieves highly robust positioning with accurate local positioning and global consistency in large scenes, improving positioning accuracy, reducing accumulated errors, and enhancing the system's adaptability and reliability in complex environments. This meets the high-precision positioning needs of intelligent robots, autonomous vehicles, and other devices in complex scenarios such as urban canyons, tunnels, indoor environments, and long-distance, highly dynamic environments.

[0055] Preferably, GPS absolute position information is collected and processed using an improved GPS accuracy factor model and an anomaly detection model based on fuzzy logic and statistical learning. This accuracy factor model combines a geometric accuracy factor, an atmospheric delay correction term, and a multipath effect correction term to correct GPS positioning accuracy in real time. The anomaly detection model uses fuzzy logic rules to determine the probability of signal anomalies and combines them with a support vector machine classification algorithm to improve detection accuracy, thus filtering out high-quality GPS data.

[0056] Preferably, camera, IMU, and wheel speedometer information are input into the VIW positioning system to obtain local pose estimation results. GPS latitude and longitude data are converted to UTM Cartesian coordinates and transformed to the VIW system coordinate system. The system then determines whether the GPS and VIW system timestamps match; if they do not match, a time difference compensation strategy is employed. This strategy utilizes the wheel speedometer linear velocity information from the VIW system (since GPS and the wheel speedometer are bound to the same rigid body, the wheel speedometer velocity information is highly reliable) to construct a compensation function, solve for the time difference, and optimize the time synchronization relationship.

[0057] Preferably, an optimization objective function is constructed that includes VIW system residuals and GPS residuals. The VIW system residual factor is calculated based on the position and attitude of adjacent time nodes, reflecting local motion and attitude constraints; the GPS residual factor is calculated based on the GPS measurement position and the transformed position in the UTM coordinate system. During the optimization process, the weights are dynamically adjusted according to the number of GPS satellites searched and the signal quality. The Ceres Solver open-source library is used to iteratively solve the problem based on the Gauss-Newton method and the Levenberg-Marquardt algorithm, updating the transformation matrix between the VIW system and the GPS coordinate system to achieve accurate local positioning and consistent global positioning.

[0058] Preferably, the GPS accuracy factor model is used to evaluate the reliability and accuracy of GPS positioning. In the process of fusing with other sensors, the availability and accuracy of GPS signals are particularly important. Traditional GPS accuracy factor models only consider the geometric relationship between the receiver and satellites, neglecting the influence of external factors such as atmospheric delay and signal multipath effects. This paper proposes a dynamic adaptive correction model that not only incorporates the geometrical accuracy factor (GDOP) but also introduces an environmental correction factor, significantly improving the model's accuracy.

[0059] Let the position of the receiver be (x a ,y a ,z a The satellite's position is (x s ,y s ,z s If so, then the traditional pseudorange measurement model is:

[0060]

[0061] To achieve more accurate modeling, this paper introduces an atmospheric delay correction term f. atm (t a ) and multipath effect correction term f mp (x a ,y a ,z a The improved pseudorange measurement formula is constructed as follows:

[0062]

[0063] f atm (t a ) and multipath effect correction term f mp (x a ,y a ,z a The calculation formula for ) is as follows:

[0064]

[0065] Where θ is the angle between the satellite signal and the ground, Δd is the offset distance caused by multipath effect, and T std and D max These are the standard scales for atmospheric and multipath effects, respectively. By adaptively adjusting these correction terms, the positioning accuracy of GPS signals can be corrected in real time, adapting to complex and changing environmental conditions.

[0066] Preferably, if the time difference between the VIW system and GPS is unknown and Δt, then the GPS compensation function is constructed as follows:

[0067]

[0068] in, Let be the speed of the GPS, and let Δt be a fixed value. The compensated objective function is:

[0069]

[0070] in, This relates to the transformation between the compensated ideal GPS coordinates and the original GPS coordinates. Because... Since the speed information comes from the wheel speedometer linear velocity information in the VIW system, the compensation objective function can be further expressed as:

[0071]

[0072] Therefore, the goal of the GPS time difference compensation model is to solve for a fixed time difference Δt and optimize the time synchronization relationship between the GPS and VIW systems using the linear velocity information of the VIW system. The simplified formula is as follows:

[0073]

[0074] This formula represents the transformation matrix for time difference compensation between the VIW and GPS systems. By fusing wheel speedometer linear velocity and GPS observations, high-precision time synchronization can be achieved. Through this compensation method, the time difference between the GPS and VIW systems can be effectively eliminated, making the multi-sensor fusion positioning system more accurate in the time dimension and adapting to real-time positioning needs in dynamic environments.

[0075] Preferably, in large-scale scenarios, the growth of the mobile platform's trajectory and the dense accumulation of feature points significantly increase the computational load on the visual SLAM backend optimization. While traditional bundle adjustment (BA) can finely optimize pose and feature points, its inefficiency due to the continuous updating of a large number of state variables impacts real-time performance. Pose graph optimization, on the other hand, locks spatial feature points after initial optimization, focusing on pose refinement, reducing computational burden while maintaining high accuracy, making it suitable for large-scale localization. Assuming all nodes in the global pose graph represent the pose state variables of the mobile platform at adjacent time points, where each state variable contains local pose and global coordinate position information, and the local pose observation is denoted as , the global position observation as , and the state variable observation at time is , then the optimization objective function is as follows:

[0076]

[0077] In the above formula The expression is expressed using the Mahalanobis norm, because the measurement units of different sensors are not the same during the optimization process, so it is necessary to use the Mahalanobis norm method for unified conversion, and h is the corresponding measurement matrix.

[0078] This paper proposes a multi-sensor data fusion approach. The VIG-SLAM algorithm, building upon the relative constraints between adjacent frames in traditional visual-inertial positioning, introduces global GPS constraints and combines a tightly coupled system of vision, inertial measurement, and wheel speed measurement for local positioning. Its objective function uses the Mahalanobis norm to convert the units of measurements from different sensors. Assuming local pose and global position observations, the expression for the local pose residual factor (involving quaternion operations to represent motion and attitude constraints between adjacent time points) and the calculation formula for the GPS residual factor are derived. Let the positions of adjacent time points be... posture as The expression for the local pose residual factor is as follows:

[0079]

[0080] in, The operator representing the change between adjacent poses is used. Since the pose is expressed as a quaternion in this paper, the operation is the product of two quaternions. In the matrix above, the first row represents the relative motion constraints between adjacent time points, and the second row represents the relative attitude constraints between adjacent time points. After the GPS data conversion from latitude and longitude to the local VIW system coordinate system described above, the calculation of the GPS residual factor is relatively easy. Let the GPS measured position in the UTM coordinate system be... The formula for calculating the residual factor of GPS is as follows:

[0081]

[0082] Since GPS positioning accuracy is related to the number of satellites searched and signal quality, this paper dynamically adjusts their weights during optimization. It uses the Ceres Solver open-source library to iteratively solve the nonlinear optimization problem based on the Gauss-Newton method and the Levenberg-Marquardt algorithm, and updates the transformation matrix between the VIW system and the GPS coordinate system to achieve high-precision and robust positioning over a wide area.

[0083] This invention also provides a method and system for using a positioning system based on adaptive multi-sensor fusion, which includes the following steps:

[0084] S100. Equipment Installation and Initialization: Correctly install the GPS sensors, cameras, inertial measurement units (IMUs), and wheel speedometers on the target equipment (such as robots or autonomous vehicles). Ensure that each sensor is securely installed and positioned appropriately to avoid mutual interference. Set and calibrate the parameters of each sensor according to the equipment manual to bring the sensors into optimal working condition. Record the initial parameter values ​​as the basis for subsequent data processing.

[0085] After the equipment is started, the system automatically performs self-tests on each sensor to check whether the sensors are working properly and whether the data transmission is stable. If a sensor malfunction or abnormal data is detected, an alarm will be issued in a timely manner and the user will be prompted to perform maintenance or repair.

[0086] S200, Data Acquisition and Transmission: During operation, all sensors synchronously acquire data. The GPS sensor obtains satellite signals in real time and calculates absolute position information; the camera continuously captures images of the surrounding environment; the IMU measures the device's acceleration and angular velocity; and the wheel speed meter monitors the rotational speed of the wheels to obtain the device's motion information.

[0087] The collected data is transmitted to the data processing unit in a high-speed and stable manner through the device's internal communication modules (such as CAN bus, Ethernet, etc.) to ensure data integrity and timeliness and avoid data loss or delay.

[0088] S300, GPS Data Preprocessing: After receiving GPS data, the data processing unit first evaluates and corrects the data using a pre-built GPS accuracy factor model. This model combines the geometrical factor of precision (GDOP), atmospheric delay correction, and multipath effect correction, dynamically adjusting the correction parameters according to the real-time conditions of the device's environment to improve the accuracy of GPS positioning.

[0089] GPS data in latitude and longitude format is converted to UTM (Universal Transverse Mercator) rectangular coordinate format to facilitate subsequent fusion calculations with other sensor data. Simultaneously, the GPS coordinate system is transformed to a coordinate system consistent with the VIW (Vision / Inertial / Wheel Speed ​​Measurement) system, ensuring that the data is processed within the same reference frame.

[0090] GPS data is filtered using an anomaly detection model based on fuzzy logic and statistical learning. This model uses parameters trained on historical data to determine the probability of anomalies in GPS signals using fuzzy logic rules, and further improves detection accuracy by combining it with a support vector machine (SVM) classification algorithm. This process eliminates anomalous data and ensures the quality of subsequently fused GPS data.

[0091] S400 and VIW system positioning and time difference compensation: Data from the camera, IMU and wheel speedometer are input into the VIW positioning system. The VIW system uses algorithms such as visual feature point detection and matching, inertial measurement data fusion and wheel speedometer motion information inference to calculate the local pose estimation results of the device in real time.

[0092] By comparing the timestamps of the GPS and VIW systems, if a time difference is found, a time difference compensation algorithm based on wheel speedometer linear velocity information is used to address it. Taking advantage of the fact that GPS and wheel speedometers are bound to the same rigid body and that wheel speedometer velocity information is highly reliable, a compensation function is constructed to calculate the time difference. This function is then used to compensate for the GPS observations, achieving time synchronization between the GPS and VIW systems and ensuring consistency of multi-sensor data in the time dimension.

[0093] S500, Pose Map Optimization and Fusion: Based on the local pose estimation results of the VIW system and the preprocessed GPS position information, an optimization objective function is constructed that includes VIW system residuals and GPS residuals. The VIW system residual factor is calculated based on the position and attitude changes at adjacent time points, reflecting the local motion and attitude constraints of the device; the GPS residual factor is calculated based on the GPS measurement position in the UTM coordinate system and the device position transformed to the same coordinate system.

[0094] During the optimization process, the weight of GPS signals is dynamically adjusted based on the number of GPS satellites acquired and the signal quality. When the number of satellites acquired is high and the signal quality is good, the weight of GPS data in the optimization is increased; conversely, the weight is appropriately reduced to ensure the robustness of the system under different environments.

[0095] Using the Ceres Solver open-source library, the objective function is iteratively solved based on the Gauss-Newton method and the Levenberg-Marquardt algorithm, continuously adjusting the device's pose estimate to approach the optimal solution. After each round of optimization, the transformation matrix between the VIW system and the GPS coordinate system is updated to ensure consistency between local accurate estimation and global coordinates, ultimately yielding a high-precision device pose result.

[0096] S600, Positioning Result Application and Feedback: The final high-precision positioning result is output to the equipment's control system. The control system performs path planning, obstacle avoidance decisions, and task execution based on the positioning information, ensuring that the equipment can operate accurately and stably in complex environments.

[0097] The system continuously monitors the equipment's operating status and positioning performance, collects actual operating data, and feeds it back to the data processing unit. Based on this feedback data, the data processing unit optimizes and adjusts the parameters of each sensor and the data processing algorithms, continuously improving the system's performance and adaptability.

[0098] As a preferred embodiment of the present invention, the present invention provides a positioning method and system based on adaptive multi-sensor fusion.

[0099] I. Preparation of Experimental Equipment and Environment

[0100] (1) Equipment installation and commissioning

[0101] Choose a suitable mobile platform, such as a self-driving car or intelligent robot, and install a high-precision GPS sensor on its top (e.g., a model with a high-sensitivity receiving antenna and low-noise amplifier to ensure stable satellite signal reception) to enable it to receive GPS signals from the sky without obstruction. Install multiple cameras (such as high-resolution and wide dynamic range industrial cameras) at appropriate locations in front, behind, and to the sides of the vehicle or robot to ensure comprehensive image capture of the surrounding environment. Simultaneously, install an inertial measurement unit (IMU) and wheel speedometers inside the device. The IMU must be precisely calibrated so that its coordinate axes align with the direction of the device's movement, and the wheel speedometers must be tightly connected to the wheel drive system to ensure measurement accuracy.

[0102] A central processing unit (CPU) or dedicated computing chip connects all sensors and devices, establishing a stable data transmission channel, such as using a high-speed CAN bus or Ethernet interface, to ensure that data can be transmitted to the processing unit in real time and accurately for subsequent analysis. Initial calibration is performed on each sensor, including GPS satellite signal search and positioning initialization, camera focal length and optical center calibration, IMU zero-bias calibration, and wheel speedometer circumference and pulse count calibration. Initial calibration parameters are recorded for comparison and correction during subsequent operation.

[0103] (2) Experimental Environment Setup

[0104] Select representative, complex environments as experimental sites, such as urban blocks (including high-rise buildings, narrow streets, intersections, etc.), parks (with trees, lakes, pedestrians, etc.), and suburban roads (which may have undulating terrain and areas with signal obstruction). Within the experimental site, pre-establish landmarks or reference points with known locations. The location information of these reference points is accurately measured using high-precision measuring instruments (such as total stations), and their geographic coordinates and coordinates in the local coordinate system are recorded for subsequent verification of the positioning system's accuracy.

[0105] Different lighting conditions were set up in various environmental areas, including direct sunlight, shaded areas, and dimly lit indoor scenes, to test the performance of the vision sensor under various lighting environments. Simultaneously, dynamic scenarios were simulated, such as arranging pedestrians and vehicles to move within the experimental area, to observe the positioning system's effectiveness in the presence of dynamic obstacles.

[0106] II. Implementation of Data Acquisition and Processing Procedures

[0107] (1) Data Acquisition

[0108] The mobile platform is activated, and all sensors begin synchronously collecting data. The GPS sensor receives satellite signals at a set frequency (e.g., 1Hz) and extracts location information (longitude, latitude, altitude) and relevant satellite parameters (such as satellite ID, signal strength, signal-to-noise ratio, etc.). The camera captures images of the surrounding environment at a high frame rate (e.g., 30fps), compresses and encodes the image data, and transmits it to the processing unit. The IMU measures the device's acceleration and angular velocity at an extremely high frequency (e.g., 100Hz) and uses its internal integration algorithm to preliminarily calculate the device's attitude changes. The wheel speed sensor monitors the wheel rotation in real time, calculates the device's speed and distance traveled based on the wheel circumference and the number of rotation pulses, and sends this data to the processing unit.

[0109] (2) GPS data preprocessing

[0110] After receiving GPS data, the data processing unit first calculates the satellite's position information based on the built-in ephemeris data and the current time. Combining this with the receiver's position information, it processes the data using an improved GPS accuracy factor model. For atmospheric delay correction, local meteorological data (such as atmospheric pressure, temperature, and humidity) is queried and substituted into the atmospheric delay model to calculate the signal propagation delay time in the atmosphere, and the pseudorange measurement value is corrected accordingly. For multipath effect correction, a multipath identification algorithm based on signal strength and time of arrival is used to analyze whether there are multipath reflection components in the received satellite signal. If so, the error caused by multipath effect is estimated based on the characteristics of the reflected signal and the terrain information of the surrounding environment, and the positioning result is compensated accordingly.

[0111] The GPS latitude and longitude data is converted into UTM rectangular coordinates using a geographic coordinate transformation algorithm for precise conversion. Simultaneously, based on the device's initial attitude and position information, the GPS coordinate system is transformed to a coordinate system consistent with the VIW system. This transformation is performed using a coordinate transformation matrix to ensure that both systems operate under the same reference frame for subsequent processing.

[0112] GPS data is filtered using an anomaly detection model based on fuzzy logic and statistical learning. In the fuzzy logic part, pre-defined fuzzy rules, such as a high probability of anomalies in GPS signals when the position precision factor (PPD) is greater than 10 and the number of satellites searched is less than 5, are applied. The membership values ​​of the signal anomalies are calculated through fuzzy inference, and then further judgment is made using a support vector machine (SVM) classifier. The SVM classifier is trained using a large amount of historical GPS data to learn the characteristic patterns of normal and abnormal signals. The current GPS data's features are compared with the trained model to ultimately determine whether abnormal data exists. If abnormal data is detected, it is marked and temporarily excluded from subsequent fusion processes, awaiting further processing or re-acquiring of the data.

[0113] (3) VIW system positioning and time difference compensation

[0114] Image data acquired by the camera undergoes feature extraction and matching operations in the processing unit. Advanced feature extraction algorithms (such as ORB feature extraction) are employed to extract key points with distinct features from the images and calculate their descriptors. Feature matching algorithms are used to find matching feature point pairs between adjacent image frames. Based on the positional changes of these feature points and the camera's intrinsic and extrinsic parameters, the device's motion attitude and positional changes are initially estimated. Simultaneously, acceleration and angular velocity data measured by the IMU are integrated and filtered to obtain the device's attitude and velocity information, which is then fused with the camera data. Velocity and distance information provided by the wheel speedometer also participate in the fusion process. Through fusion algorithms such as Kalman filtering, the visual, inertial, and wheel speedometer information are comprehensively processed to obtain the local pose estimation results of the VIW system.

[0115] (4) Pose graph optimization and fusion

[0116] During the optimization process, the number of GPS satellites acquired and the signal quality are monitored in real time. The quality of the GPS signal is assessed using information from the GPS receiver regarding the number of satellites acquired, signal strength, and signal-to-noise ratio (SNR). When the number of acquired satellites is high (e.g., more than 8) and the signal quality is good (e.g., SNR greater than 30dB), the weight of the GPS signal in the optimization objective function is increased; conversely, when the number of acquired satellites decreases or the signal quality deteriorates, the GPS weight is decreased. The Ceres Solver open-source library is used to iteratively solve the optimization objective function based on the Gauss-Newton method and the Levenberg-Marquardt algorithm. In each iteration, the gradient and Hessian matrix (approximately) of the objective function are calculated based on the current pose estimate and sensor data. By continuously adjusting the pose estimate, the objective function gradually converges to its minimum, yielding the optimal device pose result. Simultaneously, after each optimization round, the transformation matrix between the VIW system and the GPS coordinate system is updated to ensure consistency between local accurate estimation and global coordinates.

[0117] III. Experimental Results Verification and System Optimization

[0118] (1) Positioning accuracy verification

[0119] During the experiment, the location information output by the positioning system was compared with the location information of pre-set landmarks or reference points. The root mean square error (RMSE) and other metrics were used to evaluate the positioning accuracy. Under different experimental environments and scenarios, the positioning results of the positioning system were recorded over a period of time, and the deviation from the reference point position was calculated. For example, in an urban street environment, after multiple experiments and data statistics, the improvement in positioning accuracy of the positioning method of this invention compared to the traditional visual SLAM algorithm was calculated. If the RMSE of the traditional algorithm is 5 meters, while the RMSE of the positioning method of this invention is 2 meters in the same environment, it indicates that the positioning accuracy of this invention has been significantly improved in that environment.

[0120] Simultaneously, the stability and continuity of the positioning results are analyzed. The presence of obvious jumps or drifts in the positioning trajectory is observed, especially during prolonged operation or when traversing complex environments. By plotting the positioning trajectory, the performance of the positioning system of this invention is visually demonstrated, and the trajectories are compared with those of other existing positioning technologies, further validating the advantages of this invention.

[0121] (2) System optimization and adjustment

[0122] Based on experimental results and data feedback, the system was optimized and adjusted. If the performance of a particular sensor deteriorated under specific environments, such as difficulty in feature extraction by the camera under direct sunlight, or frequent signal loss of GPS in certain areas, the parameters of that sensor were adjusted accordingly, or a more suitable sensor model was replaced. For data processing algorithms, such as feature matching algorithms and fusion algorithms, if slow convergence speed or low accuracy occurred, the algorithms were improved and optimized, for example, by adjusting the feature matching threshold or improving the parameter settings of the Kalman filter.

[0123] Continuously collect and analyze system operating data under different environments and conditions to build data models and knowledge bases. Utilize technologies such as machine learning to enable the system to automatically learn and adapt to different environmental conditions, further improving the system's intelligence and performance stability. For example, by learning from a large amount of GPS anomaly data and environmental information, the system can automatically identify environmental characteristics that may lead to GPS anomalies and take corresponding measures in advance, such as switching to a VIW system-dominated positioning mode to ensure positioning reliability.

[0124] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A localization method based on adaptive multi-sensor fusion, characterized in that: Includes the following steps: GPS absolute position information is collected and processed using an improved GPS accuracy factor model and an anomaly detection model based on fuzzy logic and statistical learning. The accuracy factor model combines geometric accuracy factor, atmospheric delay correction term and multipath effect correction term. The anomaly detection model uses fuzzy logic rules to judge the probability of signal anomalies and combines support vector machine classification algorithm to improve detection accuracy, so as to screen out high-quality GPS data. The camera, inertial measurement unit (IMU), and wheel speed meter information are input into the VIW positioning system to obtain local pose estimation results. At the same time, the GPS latitude and longitude data are converted into UTM rectangular coordinates and transformed into the VIW system coordinate system. It is determined whether the GPS and VIW system timestamps match. If they do not match, a compensation function based on the VIW system wheel speed meter linear velocity information is used to compensate for the time difference, solve for the time difference, and optimize the time synchronization relationship. An optimization objective function is constructed that includes VIW system residuals and GPS residuals. The VIW system residual factor is calculated based on the position and attitude of adjacent time nodes, and the GPS residual factor is calculated based on the GPS measurement position and the transformed position in the UTM coordinate system. During the optimization process, the weights are dynamically adjusted according to the number of GPS satellites searched and the signal quality. The solution is obtained iteratively using the Ceres Solver open-source library based on the Gauss-Newton method and the Levenberg-Marquardt algorithm, and the transformation matrix between the VIW system and the GPS coordinate system is updated to achieve local accuracy and global consistency in positioning. In the optimization objective function, the local pose residual factor is calculated based on the position and attitude of adjacent time nodes, and the GPS residual factor is calculated based on the GPS measurement position and the transformed position in the UTM coordinate system. The objective function to be optimized is ,in For the Markov normal expression, h This is the corresponding measurement matrix, and the expression for the local pose residual factor is: The formula for calculating the GPS residual factor is: .

2. The positioning method based on adaptive multi-sensor fusion according to claim 1, characterized in that: In the improved GPS accuracy factor model, atmospheric delay correction and multipath effect correction terms are used to correct the positioning accuracy of the GPS signal in real time; let the receiver's position be... The satellite's position is The traditional pseudorange measurement model is The improved pseudorange measurement formula is as follows: Among them, atmospheric delay correction term Multipath effect correction term , The angle between the satellite signal and the ground. This refers to the offset distance caused by multipath effects. and These are standard scales for atmospheric and multipath effects, respectively, and the positioning accuracy of GPS signals is corrected in real time by adaptively adjusting these correction terms.

3. A positioning system based on adaptive multi-sensor fusion, characterized in that: include: The sensor module, which includes a GPS sensor, camera, IMU, and wheel speedometer, is used to acquire position, image, inertial, and motion information. The data processing unit includes a GPS data processing module, a time compensation module, and a pose graph optimization module. The GPS data processing module is used to process GPS data using an improved GPS accuracy factor model and an anomaly detection model. The time compensation module is used to compensate for the time difference between GPS and the VIW system based on the wheel speed meter linear velocity information of the VIW system. The pose graph optimization module is used to construct and solve an optimization objective function that includes VIW system residuals and GPS residuals. The control unit receives the results from the pose graph optimization module, controls the device movement or performs other tasks, and coordinates the workflow of each module. The pose graph optimization module constructs an objective function that includes VIW system residuals and GPS residuals, dynamically adjusts the GPS signal weights based on the number of GPS satellites acquired and signal quality, and iteratively solves the problem using the Ceres Solver open-source library, updating the transformation matrix between the VIW system and the GPS coordinate system. The objective function is: ,in For the Markov normal expression, h This is the corresponding measurement matrix, and the expression for the local pose residual factor is: The formula for calculating the GPS residual factor is: .

4. The positioning system based on adaptive multi-sensor fusion according to claim 3, characterized in that: When processing GPS data, the GPS data processing module combines geometric accuracy factor, atmospheric delay correction term and multipath effect correction term to correct the data, converts latitude and longitude data into UTM rectangular coordinates and then into VIW system coordinates, and uses fuzzy logic and support vector machine to filter out high-quality GPS data.

5. A method for using a positioning system based on adaptive multi-sensor fusion, characterized in that: Includes the following steps: Equipment installation and initialization steps: Install GPS sensors, cameras, IMUs and wheel speedometers on the target device, set and calibrate parameters, and perform self-tests on each sensor after starting the device; Data acquisition and transmission steps: During equipment operation, each sensor synchronously acquires data and transmits it to the data processing unit; GPS data preprocessing steps: The data processing unit uses an improved GPS accuracy factor model and anomaly detection model to evaluate, correct, and filter GPS data, and transforms data coordinates and coordinate systems; VIW system positioning and time difference compensation steps: Input camera, IMU and wheel speed meter data into the VIW system to obtain local pose estimation results, determine whether the GPS and VIW system timestamps match, if they do not match, use the compensation function based on the VIW system wheel speed meter linear velocity information to perform time difference compensation, solve the time difference and optimize the time synchronization relationship; Pose graph optimization and fusion steps: Construct an optimization objective function containing VIW system residuals and GPS residuals; dynamically adjust the weights based on the number of GPS satellites acquired and signal quality; iteratively solve the problem using the Ceres Solver open-source library; and update the transformation matrix between the VIW system and the GPS coordinate system. The optimization objective function is as follows: ,in For the Markov normal expression, h This is the corresponding measurement matrix, and the expression for the local pose residual factor is: The formula for calculating the GPS residual factor is: ; Positioning result application and feedback steps: The positioning result is output to the control system, which performs path planning and continuously monitors the equipment operation status and positioning effect. The data is then fed back to the data processing unit for optimization and adjustment.

6. The method of using the positioning system based on adaptive multi-sensor fusion according to claim 5, characterized in that: During the device installation and initialization steps, ensure that each sensor is securely installed and positioned appropriately to avoid mutual interference, and record the initial parameter values. In the data acquisition and transmission steps, data is transmitted in a high-speed and stable manner through the internal communication module of the device; In the GPS data preprocessing step, the correction parameters of the GPS accuracy factor model are adjusted in real time according to the environment in which the device is located. In the pose graph optimization and fusion step, the transformation matrix is ​​updated after each round of optimization to ensure the consistency between local accurate estimation and global coordinates; In the positioning result application and feedback step, the data processing unit optimizes and adjusts the sensor parameters and data processing algorithm based on the feedback data.

Images