Positioning method and system based on adaptive multi-sensor fusion

Abstract

The invention relates to a positioning method and system based on adaptive multi-sensor fusion, belongs to the technical field of positioning, and aims to solve the problems of low positioning precision and poor stability in a complex environment in the existing positioning technology. According to the method, a VIW system and a GPS are deeply fused, and the advantages of all sensors are effectively integrated. The method comprises the following steps: firstly, evaluating, correcting and screening GPS data through an improved GPS precision factor model and an anomaly detection model based on fuzzy logic and statistical learning; and secondly, acquiring a local pose estimation result by using the VIW system, and realizing time synchronization of the GPS and the VIW system through a time difference compensation strategy. And finally, constructing an optimization objective function containing a VIW system residual term and a GPS residual term, and updating the transformation matrix of the VIW system and the GPS coordinate system. The method is suitable for the positioning requirements of intelligent robots, pilotless automobiles and other equipment in urban canyons, tunnels, indoor and long-distance and high-dynamic scenes.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning, and relates to a positioning method and system based on adaptive multi-sensor fusion. Background Art

[0002] In the process of the vigorous development of intelligent robots and driverless cars, positioning and navigation technology has become the core key. Traditional SLAM algorithms have obvious limitations in complex environments.

[0003] Visual SLAM algorithms such as ORB-SLAM and LSD-SLAM mainly rely on the camera to collect image information for feature point detection and matching to achieve positioning and mapping. They can achieve good results in scenes with sufficient lighting and rich textures, but in scenes with drastic lighting changes (such as direct strong light and alternating shadow areas), unclear textures (such as monotonous walls and solid-color floors), or many dynamic objects (such as crowded areas and busy traffic intersections), the accuracy of feature matching is greatly reduced, causing serious damage to positioning accuracy and stability. For example, in a warehouse environment with flickering indoor lights, the ORB-SLAM algorithm may cause positioning deviations of several meters or even more due to mismatching of feature points, which cannot meet the needs of precise positioning.

[0004] LiDAR SLAM algorithms such as LOAM and LIO-SAM rely on the high-precision distance measurement of LiDAR for environmental modeling and positioning. High-precision positioning can be achieved in static environments, but the high cost of LiDAR limits its widespread application. In addition, in environments with low visibility (such as factories filled with smoke, outdoors in rainy and snowy weather), the laser beam is severely interfered and the performance drops sharply. In fire rescue scenarios with thick smoke, LiDAR SLAM algorithms may not be able to effectively detect the surrounding environment, causing robots or unmanned vehicles to lose their positioning capabilities.

[0005] Visual inertial SLAM algorithms such as VINS-Mono and OKVIS combine visual sensors with inertial measurement units (IMUs) and use IMU high-frequency data to compensate for the high-speed motion delay of visual sensors, which improves real-time performance and stability to a certain extent. However, since the long-term integration error of the IMU is difficult to eliminate, the positioning drift problem is still prominent 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 execution of the task.

[0006] As a common positioning method, GPS can provide relatively accurate location information in open environments. However, in complex environments such as urban canyons, tunnels, and indoors, the signal is easily affected by building obstruction and electromagnetic interference, resulting in signal failure, instability, or loss of lock, and extremely unstable positioning performance. In urban streets with tall buildings, GPS signals may be frequently interrupted or produce large errors, making it impossible to provide reliable positioning services for vehicles or robots. Summary of the invention

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

[0008] In order to achieve the above object, the present invention provides the following technical solutions:

[0009] A positioning method based on adaptive multi-sensor fusion, comprising the following steps:

[0010] Collect GPS absolute position information and process it using an improved GPS precision factor model and an anomaly detection model based on fuzzy logic and statistical learning. The precision factor model combines geometric precision factor, atmospheric delay correction term and multipath effect correction term. The anomaly detection model uses fuzzy logic rules to determine the possibility of signal anomaly 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 the local pose estimation result. At the same time, the GPS longitude and latitude data are converted into UTM rectangular coordinates and transformed into VIW system coordinates to determine whether the GPS and VIW system timestamps match. If they do not match, the compensation function constructed based on the linear speed information of the VIW system wheel speed meter is used to compensate for the time difference, solve the time difference and optimize the time synchronization relationship.

[0012] An optimization objective function including VIW system residual term and GPS residual term is constructed. 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 measured position and the converted position in the UTM coordinate system. During the optimization process, its weight is dynamically adjusted according to the number of GPS satellite searches 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, and the transformation matrix of the VIW system and the GPS coordinate system is updated to achieve locally accurate and globally consistent positioning.

[0013] Furthermore, in the improved GPS precision factor model, the atmospheric delay correction term and the multipath effect correction term are used to correct the positioning accuracy of the GPS signal in real time; assuming that the position of the receiver is (x a ,y a ,za ), the satellite’s position is (x s ,y s ,z s ), the traditional pseudo-range measurement model is The improved pseudorange measurement formula is: The atmospheric delay correction term Multipath correction θ is the angle between the satellite signal and the ground, Δd is the offset distance caused by the multipath effect, T std and D max They are standard scales for atmospheric and multipath effects, respectively. The positioning accuracy of GPS signals is corrected in real time by adaptively adjusting these correction items.

[0014] Furthermore, the time difference compensation is specifically as follows: constructing a compensation function using the linear speed information of the wheel speed meter of the VIW system to achieve time synchronization between the GPS and the VIW system;

[0015] When the time difference between the VIW system time point and the GPS time point is unknown and is Δt, the constructed GPS compensation function is: The objective function after compensation is because The speed information comes from the wheel speed meter linear speed information in the VIW system, and the compensation objective function is expressed as After simplification, This formula is used to achieve time difference compensation and time synchronization between the VIW system and the GPS system.

[0016] Further, 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 measured position and the converted position in the UTM coordinate system;

[0017] The optimization objective function is in is the Mahalanobis norm expression, h is the corresponding measurement matrix, and the local pose residual factor expression is The calculation formula of GPS residual factor is:

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

[0019] Sensor module, including GPS sensor, camera, IMU and wheel speed meter, used to collect position, image, inertial and motion information;

[0020] A data processing unit, comprising a GPS data processing module, a time compensation module and a pose graph optimization module, wherein the GPS data processing module is used to process GPS data using an improved GPS precision factor model and an anomaly detection model, the time compensation module is used to compensate for the time difference between the GPS and the VIW system according to the linear speed information of the wheel speed meter of the VIW system, and the pose graph optimization module is used to construct and solve an optimization objective function including a VIW system residual term and a GPS residual term;

[0021] The control unit is used to receive the results of the pose graph optimization module, control the movement of the device or perform other tasks, and coordinate the workflow of each module.

[0022] Furthermore, when processing GPS data, the GPS data processing module corrects the data by combining the geometric precision factor, atmospheric delay correction term and multipath effect correction term, and converts the longitude and latitude data into UTM rectangular coordinates and then into VIW system coordinates, while using fuzzy logic and support vector machines to screen out high-quality GPS data.

[0023] Furthermore, the time compensation module performs time difference compensation according to the constructed GPS compensation function. By solving the time difference Δt and using the linear speed information of the wheel speed meter of the VIW system Realize time synchronization between GPS and VIW system.

[0024] Furthermore, when constructing the optimization objective function, the pose graph optimization module constructs a function including the VIW system residual term and the GPS residual term in the manner described in claim 4, dynamically adjusts the GPS signal weight according to the number of GPS satellite searches and the signal quality, and uses the Ceres Solver open source library to iteratively solve and update the VIW system and GPS coordinate system transformation matrix.

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

[0026] Equipment installation and initialization steps: Install GPS sensor, camera, IMU and wheel speed meter on the target device, set parameters and calibrate them, and perform self-test on each sensor after starting the device;

[0027] Data collection and transmission steps: When the equipment is running, each sensor synchronously collects data and transmits it to the data processing unit;

[0028] GPS data preprocessing steps: The data processing unit uses the improved GPS precision factor model and anomaly detection model to evaluate, correct and filter the GPS data, and convert the data coordinates and coordinate system;

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

[0030] Pose graph optimization fusion steps: Construct an optimization objective function that includes the VIW system residual term and the GPS residual term, dynamically adjust the weight according to the number of GPS satellite searches and signal quality, use the Ceres Solver open source library to iteratively solve, and update the transformation matrix between the VIW system and the GPS coordinate system;

[0031] Positioning result application and feedback steps: The positioning results are output to the control system, which performs path planning while continuously monitoring the equipment operating status and positioning effect, and feeds the data back to the data processing unit for optimization and adjustment.

[0032] Furthermore, during the equipment installation and initialization steps, ensure that each sensor is firmly installed and reasonably positioned to avoid mutual interference, and record the initial parameter values;

[0033] In the data collection and transmission step, 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 precision factor model are adjusted in real time according to the environment in which the device is located;

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

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

[0037] The beneficial effects of the present invention are:

[0038] 1. The present invention effectively integrates the advantages of each sensor by deeply integrating the visual / inertial navigation / wheel speed meter tightly coupled positioning system (VIW system) with GPS. In complex environments, it overcomes the limitations of a single sensor, such as the reduced accuracy of visual SLAM in scenes with lighting and texture problems, the high cost of LiDAR SLAM and poor adaptability to bad weather, and the long-term integration error of IMU in visual inertial SLAM. It achieves local precision and global consistent positioning. Compared with the current mainstream visual SLAM algorithm, the average positioning accuracy is improved by at least 15%, which significantly improves the positioning accuracy.

[0039] 2. The constructed GPS precision factor model and anomaly detection mechanism can effectively deal with the problem of GPS signals being easily blocked and interfered in complex environments such as urban canyons, tunnels, and indoors, and select high-quality GPS data for fusion. At the same time, in the optimization process, the weights are dynamically adjusted according to the number of GPS satellite searches and signal quality, and the auxiliary role of the VIW system when the GPS signal is poor, ensuring that the system can still maintain stable positioning performance in complex environments and when the GPS signal is weak or temporarily lost, effectively reducing posture drift and cumulative errors, and enhancing the robustness of the system.

[0040] 3. The pose graph optimization strategy is used to replace the traditional bundle adjustment method, which greatly reduces the computational burden when dealing with the growth of mobile platform trajectories and dense accumulation of feature points in large-scale scenes. By locking the spatial feature points as static constraints and focusing on pose refinement, the real-time performance of the system is improved while maintaining high-precision pose estimation, meeting the requirements of intelligent robots and unmanned vehicles for the high efficiency of the positioning system, so that it can adapt to the real-time positioning needs in dynamic environments.

[0041] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

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

[0044] Figure 2 It is a workflow diagram of a positioning method and system based on adaptive multi-sensor fusion;

[0045] Figure 3 It is a VIG-SLAM adaptive multi-sensor fusion framework diagram of a positioning method and system based on adaptive multi-sensor fusion;

[0046] Figure 4 This is the corresponding relationship diagram between the VIW system and the GPS timestamp;

[0047] Figure 5 Optimize the model graph for the global pose graph with GPS constraints;

[0048] Figure 6 This is a comparison chart of the translation error and rotation error of Urban27;

[0049] Figure 7 Comparison chart of translation error and rotation error of Urban34;

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

[0051] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways 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 only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0052] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0053] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0054] See also Figures 1 to 8, the present invention aims to solve the problems of low positioning accuracy and poor stability of existing positioning technologies in complex environments. Specifically, it overcomes the positioning errors caused by poor lighting, lack of texture and difficulty in feature matching in dynamic scenes caused by visual SLAM algorithms; solves the application limitations of lidar SLAM algorithms affected by cost and bad weather; improves the positioning drift caused by long-term IMU integration errors in visual inertial SLAM algorithms; and copes with the situation where GPS positioning is unstable due to signal interference in complex environments. Through innovative adaptive multi-sensor fusion methods, it achieves highly robust positioning with local accuracy and global consistency in large scenes, improves positioning accuracy, reduces cumulative errors, enhances the adaptability and reliability of the system in complex environments, and meets the high-precision positioning needs of intelligent robots, driverless cars and other equipment in complex scenes such as urban canyons, tunnels, indoors, long distances, and high dynamics.

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

[0056] Preferably, the camera, IMU, and wheel speed meter information are input into the VIW positioning system to obtain the local pose estimation result, and the GPS longitude and latitude data are converted into UTM rectangular coordinates and transformed into the VIW system coordinates to determine whether the GPS and VIW system timestamps match. If they do not match, a time difference compensation strategy is adopted. This strategy uses the VIW system wheel speed meter linear speed information (because GPS and wheel speed meter are bound to the same rigid body, the wheel speed meter speed information is highly reliable) to construct a compensation function, solve the time difference and optimize the time synchronization relationship.

[0057] Preferably, an optimization objective function including VIW system residual term and GPS residual term is constructed, wherein 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 measured position and the converted position in the UTM coordinate system. During the optimization process, its weight is dynamically adjusted according to the number of GPS satellite searches and signal quality, and the Ceres Solver open source library is used to iteratively solve based on the Gauss-Newton method and the Levenberg-Marquardt algorithm, update the VIW system and GPS coordinate system transformation matrix, and achieve local precision and global consistent positioning.

[0058] Preferably, the GPS DOP model is used to evaluate the reliability and accuracy of GPS positioning. In the process of fusion with other sensors, the availability and accuracy of GPS signals are particularly important. The traditional GPS DOP model only considers the geometric relationship between the receiver and the satellite, ignoring the influence of external factors such as atmospheric delay and signal multipath effect. This paper proposes a dynamic adaptive correction model, which not only combines the geometric DOP (GDOP), but also introduces the environmental correction factor, which significantly improves the accuracy of the model.

[0059] Assume the receiver's position is (x a ,y a ,z a ), the satellite’s position is (x s ,y s ,z s ), the traditional pseudo-range measurement model is:

[0060]

[0061] In order to model more accurately, this paper introduces the atmospheric delay correction term f atm (t a ) and the multipath effect correction term f mp (x a ,y a ,z a ), the improved pseudo-range measurement formula is constructed as:

[0062]

[0063] f atm (t a ) and the multipath effect correction term f mp (x a ,y a ,z a ) is calculated as follows:

[0064]

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

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

[0067]

[0068] in, is the movement speed of GPS, and the time difference Δt is a fixed value. The objective function after compensation is:

[0069]

[0070] in, is the transformation relationship between the compensated ideal GPS coordinates and the original GPS. The speed information comes from the wheel speed meter linear speed information in the VIW system, and the compensation objective function can be further expressed as:

[0071]

[0072] Therefore, the goal of the GPS time difference compensation model is to solve the fixed time difference Δt and optimize the time synchronization relationship between the GPS and VIW systems through the linear velocity information of the VIW system. After simplifying the formula, we can get:

[0073]

[0074] This formula represents the conversion matrix of the time difference compensation between the VIW system and the GPS system, which can achieve high-precision time synchronization by fusing the wheel speed meter linear speed and GPS observation. Through the above 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 the real-time positioning needs in dynamic environments.

[0075] Preferably, in a large-scale scene, the growth of the mobile platform trajectory and the dense accumulation of feature points will greatly increase the optimization calculation load of the visual SLAM backend. Although the traditional bundle adjustment (BA) can finely optimize the pose and feature points, it is inefficient due to the continuous update of a large number of state variables, which affects real-time performance. The pose graph optimization locks the spatial feature points after preliminary optimization, focuses on pose refinement, reduces the computational burden and maintains high precision, which is suitable for large-scale positioning. Assuming that all nodes in the global pose graph are the pose state quantities of the mobile platform at adjacent times, each of which contains local pose and global coordinate position information, the observation of the local pose is set to, the global position observation is set to, and the state quantity observation at time is set to, then the optimization objective function is as follows:

[0076]

[0077] In the above formula It is expressed in Markov norm. Because the measurement units of different sensors are not the same during the optimization process, it is necessary to use the Markov norm method for unified conversion. h is the corresponding measurement matrix.

[0078] Based on this, this paper conducts multi-sensor data fusion. The VIG-SLAM algorithm introduces GPS global constraints on the basis of the relative constraints of adjacent frames of traditional visual inertial positioning, and combines the visual, inertial measurement and wheel speed meter tightly coupled system for local positioning. In its optimization objective function, the Mahalanobis norm is used to convert the units of different sensor measurements. The local pose and global position observations are set to obtain the local pose residual factor expression (involving quaternion operations to represent adjacent time motion and attitude constraints) and the GPS residual factor calculation formula. Assume that the position of adjacent time nodes is Posture Then the local pose residual factor expression is as follows:

[0079]

[0080] in, represents the change operator between adjacent positions and postures. Since the posture in this paper is expressed as a quaternion, the operation is the product of two quaternions. In the matrix of the above formula, the first row represents the relative motion constraints at adjacent moments, and the second row represents the relative posture constraints at adjacent moments. After the conversion of GPS from longitude and latitude to the local VIW system coordinate system data, the calculation of GPS residual factors is relatively easy. Assume that the position measured by GPS in the UTM coordinate system is The calculation formula of GPS residual factor is as follows:

[0081]

[0082] Since GPS positioning accuracy is related to the number of searched satellites and signal quality, this paper dynamically adjusts their weights during optimization, 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 large-scale, high-precision and robust positioning.

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

[0084] S100, Equipment Installation and Initialization: Correctly install GPS sensors, cameras, inertial measurement units (IMUs), and wheel speed meters on the target equipment (such as robots or driverless cars), ensuring that each sensor is firmly installed and positioned appropriately to avoid mutual interference. Set and calibrate the parameters of each sensor according to the equipment manual to ensure that the sensor is in the best working state, and record the initial parameter values ​​as the basis for subsequent data processing.

[0085] After starting the device, the system automatically performs self-tests on each sensor to check whether the sensor is working properly, whether the data transmission is stable, etc. If a sensor failure or abnormal data is found, an alarm will be issued in time to prompt the user to perform maintenance or repair.

[0086] S200, Data Collection and Transmission: When the device is running, each sensor collects data synchronously. The GPS sensor obtains satellite signals in real time and calculates the absolute position information, the camera continuously captures images of the surrounding environment, the IMU measures the acceleration and angular velocity of the device, and the wheel speed meter monitors the rotation speed of the wheel to obtain the movement information of the device.

[0087] The collected data is transmitted to the data processing unit in a high-speed and stable manner through the communication module inside the equipment (such as CAN bus, Ethernet, etc.), ensuring the integrity and timeliness of the data and avoiding data loss or delay.

[0088] S300, GPS data preprocessing: After receiving the GPS data, the data processing unit first evaluates and corrects the data using a pre-built GPS precision factor model. The model combines the geometric precision factor (GDOP), atmospheric delay correction term, and multipath correction term, and dynamically adjusts the correction parameters according to the real-time situation of the environment in which the device is located to improve the accuracy of GPS positioning.

[0089] Convert GPS data in the form of longitude and latitude into UTM (Universal Transverse Mercator) rectangular coordinates to facilitate subsequent fusion calculations with other sensor data. At the same time, convert the GPS coordinate system to a coordinate system consistent with the VIW (visual / inertial navigation / wheel speed meter) system to ensure that the data is processed in the same reference system.

[0090] GPS data is screened through an anomaly detection model based on fuzzy logic and statistical learning. The model uses fuzzy logic rules to judge the possibility of GPS signal anomalies based on parameters trained with historical data, and combines the support vector machine (SVM) classification algorithm to further improve detection accuracy, eliminate abnormal data, and ensure the quality of subsequent fused GPS data.

[0091] S400, VIW system positioning and time difference compensation: The data from the camera, IMU and wheel speed meter 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 speed meter motion information estimation to calculate the local posture estimation results of the device in real time.

[0092] Compare the timestamps of GPS and VIW systems. If a time difference is found, use the time difference compensation algorithm based on the linear speed information of the wheel speed meter to process it. Based on the characteristics that GPS and wheel speed meter are bound to the same rigid body and the wheel speed meter speed information is highly reliable, a compensation function is constructed to calculate the time difference, and the GPS observation value is compensated accordingly to achieve time synchronization between GPS and VIW systems and ensure the consistency of multi-sensor data in the time dimension.

[0093] S500, pose graph optimization fusion: Based on the local pose estimation results of the VIW system and the preprocessed GPS position information, an optimization objective function including the VIW system residual term and the GPS residual term is constructed. Among them, the VIW system residual factor is calculated based on the position and attitude changes of adjacent time nodes, 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 converted to the same coordinate system.

[0094] During the optimization process, the weight of the GPS signal is dynamically adjusted according to the number of GPS satellites searched and the signal quality. When the number of searched satellites is large and the signal quality is good, the weight of GPS data in the optimization is increased; otherwise, the weight is appropriately reduced to ensure the robustness of the system in different environments.

[0095] Using the Ceres Solver open source library, the optimization objective function is iteratively solved based on the Gauss-Newton method and the Levenberg-Marquardt algorithm, and the device's pose estimation value is continuously adjusted to make it 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 the consistency of the local accurate estimation and the global coordinates, and finally obtain a high-precision device pose result.

[0096] S600, application and feedback of positioning results: The final high-precision positioning results are output to the control system of the device. The control system performs path planning, obstacle avoidance decisions, task execution and other operations based on the positioning information to ensure that the device can operate accurately and stably in complex environments.

[0097] Continuously monitor the operating status and positioning effect of the equipment, collect actual operating data and feed it back to the data processing unit. The data processing unit optimizes and adjusts the parameters of each sensor, data processing algorithm, etc. based on the feedback data, and continuously improves the performance and adaptability of the system.

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

[0099] 1. Experimental equipment and environment preparation

[0100] (1) Equipment installation and commissioning

[0101] Choose a suitable mobile platform, such as a driverless car or intelligent robot, and install a high-precision GPS sensor on its top (for example, choose a model with a high-sensitivity receiving antenna and a low-noise amplifier to ensure stable reception of satellite signals) so that it can receive GPS signals in the sky without obstacles. Install multiple cameras (such as industrial cameras with high resolution and wide dynamic range) at appropriate positions in front, behind and on the sides of the vehicle or robot to ensure that image information of the surrounding environment can be captured in all directions. At the same time, install an inertial measurement unit (IMU) and a wheel speed meter inside the equipment. The IMU needs to be accurately calibrated so that its coordinate axis direction is consistent with the movement direction of the equipment, and the wheel speed meter must be closely connected to the wheel transmission system to ensure the accuracy of the measurement.

[0102] Connect each sensor to the central processing unit (CPU) or dedicated computing chip of the device to establish a stable data transmission channel, such as using a high-speed CAN bus or Ethernet interface, to ensure that the data can be transmitted to the processing unit in real time and accurately for subsequent analysis. Perform initial calibration on each sensor, including GPS satellite signal search and positioning initialization, camera focal length and optical center calibration, IMU zero bias calibration, and wheel speed meter circumference and pulse number calibration, and record the initial calibration parameters 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 tall buildings, narrow streets, intersections, etc.), parks (with trees, lakes, pedestrians, etc.) and suburban roads (where there may be undulating terrain and signal blocking areas). Pre-set some landmarks or reference points with known locations in the experimental site. The location information of these reference points is accurately measured by 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 accuracy of the positioning system.

[0105] Different lighting conditions are set in different environmental areas, including strong direct light, shadow areas, dimly lit indoor scenes, etc., to test the performance of the visual sensor in various lighting environments. At the same time, some dynamic scenes are simulated, such as arranging pedestrians and vehicles to move in the experimental area, to observe the positioning effect of the positioning system when dynamic obstacles exist.

[0106] 2. Implementation of Data Collection and Processing Process

[0107] (1) Data collection

[0108] Start the mobile platform, and each sensor begins to collect data synchronously. The GPS sensor receives satellite signals at a set frequency (e.g. 1Hz), and parses the location information (longitude, latitude, altitude) and satellite-related parameters (such as satellite number, signal strength, signal-to-noise ratio, etc.). The camera captures images of the surrounding environment at a high frame rate (e.g. 30fps), and compresses and encodes the image data before transmitting it to the processing unit. The IMU measures the acceleration and angular velocity information of the device at an extremely high frequency (e.g. 100Hz), and preliminarily calculates the posture change of the device through an internal integration algorithm. The wheel speedometer monitors the rotation of the wheel in real time, calculates the driving speed and distance of the device based on the circumference of the wheel 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, and processes it using the improved GPS precision factor model in combination with the receiver's position information. For atmospheric delay correction, by querying local meteorological data (such as atmospheric pressure, temperature, humidity, etc.), the atmospheric delay model is substituted to calculate the delay time of the signal propagating in the atmosphere, and the pseudo-range measurement value is corrected accordingly. For multipath effect correction, a multipath identification algorithm based on signal strength and arrival time is used to analyze whether there are multipath reflection components in the received satellite signal. If so, the error caused by the multipath effect is estimated based on the characteristics of the reflected signal and the terrain information of the equipment's surrounding environment, and the positioning result is compensated.

[0111] The GPS longitude and latitude data are converted into UTM rectangular coordinates, and the geographic coordinate conversion algorithm is used for accurate conversion. At the same time, according to the initial posture and position information of the device, the GPS coordinate system is converted to a coordinate system consistent with the VIW system, and the conversion calculation is performed through the coordinate transformation matrix to ensure that the two are in the same reference system for subsequent processing.

[0112] GPS data is screened using an anomaly detection model based on fuzzy logic and statistical learning. In the fuzzy logic part, according to the pre-set fuzzy rules, such as when the position precision factor (PPD) is greater than 10 and the number of satellites searched is less than 5, it is considered that the GPS signal is likely to be abnormal. The membership value of the signal anomaly is calculated through fuzzy reasoning, and then further judgment is made in combination with the support vector machine (SVM) classifier. The SVM classifier is trained with a large amount of historical GPS data to learn the characteristic patterns of normal and abnormal signals, and finally determines whether there is abnormal data based on the characteristics of the current GPS data and the training model. If abnormal data is detected, it will be marked and temporarily excluded from the subsequent fusion process, waiting for further processing or re-acquisition of data.

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

[0114] The image data collected by the camera is subjected to feature extraction and matching operations in the processing unit. Advanced feature extraction algorithms (such as the ORB feature extraction algorithm) are used to extract key points with obvious features from the image and calculate their descriptors. Through the feature matching algorithm, matching feature point pairs are found between adjacent image frames. According to the position changes of the feature points and the internal and external parameter information of the camera, the motion posture and position changes of the device are preliminarily estimated. At the same time, the acceleration and angular velocity data measured by the IMU are integrated and filtered to obtain the posture and speed information of the device, which is fused with the camera data. The speed and distance information provided by the tachometer is also involved in the fusion process. Through fusion algorithms such as Kalman filtering, the visual, inertial and tachometer information are comprehensively processed to obtain the local posture estimation result of the VIW system.

[0115] (4) Pose graph optimization fusion

[0116] During the optimization process, the number of GPS satellite searches and signal quality are monitored in real time. The quality of the GPS signal is determined by the number of satellite searches, signal strength, signal-to-noise ratio and other indicators provided by the GPS receiver. When the number of satellite searches is large (e.g., greater than 8) and the signal quality is good (e.g., the signal-to-noise ratio is greater than 30dB), the weight of the GPS signal in the optimization objective function is increased; conversely, when the number of satellite searches decreases or the signal quality deteriorates, the GPS weight is reduced. The optimization objective function is iteratively solved based on the Gauss-Newton method and the Levenberg-Marquardt algorithm using the Ceres Solver open source library. In each round of iteration, the gradient and Hessian matrix (approximate) 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 the minimum value, and the optimal device pose result is obtained. At the same time, after each round of optimization, the transformation matrix between the VIW system and the GPS coordinate system is updated to ensure the consistency between the local precise estimate and the global coordinates.

[0117] 3. Experimental results verification and system optimization

[0118] (1) Positioning accuracy verification

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

[0120] At the same time, analyze the stability and continuity of the positioning results. Observe whether there are obvious jumps or drifts in the positioning trajectory, especially when the device is running for a long time or passing through complex environmental areas. By drawing a positioning trajectory diagram, the performance of the positioning system of the present invention is intuitively displayed, and compared with the trajectories of other existing positioning technologies, the advantages of the present invention are further verified.

[0121] (2) System optimization and adjustment

[0122] Optimize and adjust the system based on experimental results and data feedback. If it is found that the performance of a certain sensor has declined in a specific environment, such as difficulty in feature extraction under strong light, or frequent loss of GPS signals in certain areas, the parameters of the sensor are adjusted or a more suitable sensor model is replaced. For data processing algorithms, such as feature matching algorithms and fusion algorithms, if the convergence speed is slow or the accuracy is not high, the algorithm is improved and optimized, such as adjusting the feature matching threshold and improving the parameter setting of the Kalman filter.

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

[0124] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A positioning method based on adaptive multi-sensor fusion, characterized by: The following steps are involved: Collect GPS absolute position information and process it using an improved GPS precision factor model and an anomaly detection model based on fuzzy logic and statistical learning. The precision factor model combines geometric precision factor, atmospheric delay correction term and multipath effect correction term. The anomaly detection model uses fuzzy logic rules to determine the possibility of signal anomaly 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 the local pose estimation result. At the same time, the GPS longitude and latitude data are converted into UTM rectangular coordinates and transformed into VIW system coordinates to determine whether the GPS and VIW system timestamps match. If they do not match, the compensation function constructed based on the linear speed information of the VIW system wheel speed meter is used to compensate for the time difference, solve the time difference and optimize the time synchronization relationship. An optimization objective function including VIW system residual term and GPS residual term is constructed. 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 measured position and the converted position in the UTM coordinate system. During the optimization process, its weight is dynamically adjusted according to the number of GPS satellite searches 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, and the transformation matrix of the VIW system and the GPS coordinate system is updated to achieve locally accurate and globally consistent positioning.

2. The positioning method based on adaptive multi-sensor fusion according to claim 1, characterized in that: In the improved GPS precision factor model, the positioning accuracy of the GPS signal is corrected in real time using the atmospheric delay correction term and the multipath effect correction term; assuming the position of the receiver is (x a ,y a ,z a ), the satellite’s position is (x s ,y s ,z s ), the traditional pseudo-range measurement model is The improved pseudorange measurement formula is: The atmospheric delay correction term Multipath correction θ is the angle between the satellite signal and the ground, Δd is the offset distance caused by the multipath effect, T std and D max They are standard scales for atmospheric and multipath effects, respectively. The positioning accuracy of GPS signals is corrected in real time by adaptively adjusting these correction items.

3. The positioning method based on adaptive multi-sensor fusion according to claim 1, characterized in that: The time difference compensation is specifically as follows: constructing a compensation function using the linear speed information of the wheel speed meter of the VIW system to achieve time synchronization between the GPS and the VIW system; When the time difference between the VIW system time point and the GPS time point is unknown and is Δt, the constructed GPS compensation function is: The objective function after compensation is because The speed information comes from the wheel speed meter linear speed information in the VIW system, and the compensation objective function is expressed as After simplification, This formula is used to achieve time difference compensation and time synchronization between the VIW system and the GPS system.

4. The positioning method based on adaptive multi-sensor fusion according to claim 1, characterized in that: 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 measured position and the converted position in the UTM coordinate system; The optimization objective function is in is the Mahalanobis norm expression, h is the corresponding measurement matrix, and the local pose residual factor expression is The calculation formula of GPS residual factor is:

5. A positioning system based on adaptive multi-sensor fusion, characterized in that: include: Sensor module, including GPS sensor, camera, IMU and wheel speed meter, used to collect position, image, inertial and motion information; A data processing unit, comprising a GPS data processing module, a time compensation module and a pose graph optimization module, wherein the GPS data processing module is used to process GPS data using an improved GPS precision factor model and an anomaly detection model, the time compensation module is used to compensate for the time difference between the GPS and the VIW system according to the linear speed information of the wheel speed meter of the VIW system, and the pose graph optimization module is used to construct and solve an optimization objective function including a VIW system residual term and a GPS residual term; The control unit is used to receive the results of the pose graph optimization module, control the movement of the device or perform other tasks, and coordinate the workflow of each module.

6. The positioning system based on adaptive multi-sensor fusion according to claim 5, characterized in that: When processing GPS data, the GPS data processing module corrects the data by combining the geometric precision factor, atmospheric delay correction term and multipath effect correction term, and converts the longitude and latitude data into UTM rectangular coordinates and then into VIW system coordinates, while using fuzzy logic and support vector machine to screen out high-quality GPS data.

7. The positioning system based on adaptive multi-sensor fusion according to claim 5, characterized in that: The time compensation module performs time difference compensation according to the constructed GPS compensation function By solving the time difference Δt and using the linear speed information of the wheel speed meter of the VIW system Realize time synchronization between GPS and VIW system.

8. The positioning system based on adaptive multi-sensor fusion according to claim 5, characterized in that: When constructing the optimization objective function, the pose graph optimization module constructs a function including the VIW system residual term and the GPS residual term in the manner described in claim 4, dynamically adjusts the GPS signal weight according to the number of GPS satellite searches and the signal quality, and uses the Ceres Solver open source library to iteratively solve and update the VIW system and GPS coordinate system transformation matrix.

9. A method for using a positioning system based on adaptive multi-sensor fusion, characterized in that: The following steps are involved: Equipment installation and initialization steps: Install GPS sensor, camera, IMU and wheel speed meter on the target device, set parameters and calibrate them, and perform self-test on each sensor after starting the device; Data collection and transmission steps: When the equipment is running, each sensor synchronously collects data and transmits it to the data processing unit; GPS data preprocessing steps: The data processing unit uses the improved GPS precision factor model and anomaly detection model to evaluate, correct and filter the GPS data, and convert the data coordinates and coordinate system; VIW system positioning and time difference compensation steps: input the camera, IMU and wheel speed meter data into the VIW system to obtain the local pose estimation result, compare the GPS and VIW system timestamps and perform time difference compensation; Pose graph optimization fusion steps: Construct an optimization objective function that includes the VIW system residual term and the GPS residual term, dynamically adjust the weight according to the number of GPS satellite searches and signal quality, use the Ceres Solver open source library to iteratively solve, and update the transformation matrix between the VIW system and the GPS coordinate system; Positioning result application and feedback steps: The positioning results are output to the control system, which performs path planning while continuously monitoring the equipment operating status and positioning effect, and feeds the data back to the data processing unit for optimization and adjustment.

10. The method for using the positioning method and system based on adaptive multi-sensor fusion according to claim 9, characterized in that: During the equipment installation and initialization steps, ensure that each sensor is firmly installed and reasonably positioned to avoid mutual interference, and record the initial parameter values; In the data collection and transmission step, 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 precision factor model are adjusted in real time according to the environment in which the device is located; In the pose graph optimization fusion step, the transformation matrix is ​​updated after each round of optimization to ensure the consistency between the local accurate estimation and the global coordinates; In the positioning result application and feedback step, the data processing unit optimizes and adjusts the sensor parameters and the data processing algorithm according to the feedback data.

Images