Multi-modal fusion robot anti-strong magnetic field interference self-adaptive positioning method

Abstract

The invention discloses a multi-modal fusion robot high-intensity magnetic field interference resisting self-adaptive positioning method. The method comprises the following steps that S1, nine-axis magnetic field data are collected through three sets of three-axis magnetometers arranged in an inclined redundancy mode; s2, constructing a Kalman filtering framework based on motion feature dynamic weighting, and performing real-time denoising on the nine-axis magnetic field data; s3, performing course estimation on the denoised magnetometer data and the motion acceleration and inclination angle data of the robot by adopting a correlation fusion algorithm; and S4, under the condition of not depending on a gyroscope, outputting a course angle positioning root mean square error of 0-15 DEG. The method has the beneficial effects that the inclined redundant magnetometer array is adopted, and dynamic weighted Kalman filtering is combined, so that the method can stably work in a strong magnetic field environment; under the condition that a gyroscope is not introduced, the root mean square error of course estimation is 13.64, compared with a traditional non-filtering magnetometer, data are greatly improved, and positioning failure caused by magnetic field distortion is avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of special robots, and specifically to a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method. Background Art

[0002] In strong magnetic field environments such as nuclear power plants, ship maintenance, and large storage tanks, magnetic adsorption wall-climbing robots require stable and reliable positioning systems to achieve precise navigation and operation. However, such environments usually have complex and variable magnetic field interferences (such as strong radiation magnetic fields near nuclear power plant reactors, electromagnetic noises in ship welding areas, and residual magnetization of large storage tanks), resulting in severe challenges for traditional positioning methods.

[0003] Traditional magnetic adsorption wall-climbing robots rely on magnetic encoders or inertial navigation systems (INS) for positioning. However, in strong magnetic interference environments, magnetic field distortion causes magnetic sensor data to fail, and in multi-sensor fusion positioning, it is vulnerable to multi-sensor data coupling interference, resulting in cumulative positioning errors. Moreover, existing anti-interference magnetometer solutions use hard magnetic shielding or software filtering to suppress interference, but static filtering algorithms cannot adapt to dynamic magnetic field changes. Therefore, we propose a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method. Summary of the Invention

[0004] The purpose of the present invention is to provide a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method to solve the problems raised in the above background art.

[0005] To achieve the above purpose, a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method includes the following steps:

[0006] S1. Collect nine-axis magnetic field data through three sets of triaxial magnetometers configured with tilt redundancy;

[0007] S2. Construct a Kalman filter framework with dynamic weighting based on motion characteristics to perform real-time denoising on the nine-axis magnetic field data;

[0008] S3. Use a correlation fusion algorithm to estimate the heading by fusing the denoised magnetometer data with the robot's motion acceleration and inclination data;

[0009] S4. Without relying on a gyroscope, output the root mean square error of the heading angle positioning against strong magnetic field interference within 0 - 15°.

[0010] Preferably, the tilt redundancy magnetometer configuration in step S1 is specifically: three sets of magnetometers are arranged in a non-coplanar triangle, and the axes of each magnetometer have a preset inclination angle with the robot body coordinate system, and the preset inclination angle is 10° - 30°.

[0011] Preferably, the dynamic weighting process of the Kalman filter framework in step S2 is specifically as follows:

[0012] S21. Dynamically allocate weights according to the real-time signal-to-noise ratio of each magnetometer;

[0013] S22. When it is detected that the uniaxial data is interfered by a strong magnetic field, automatically reduce the weight of this axis below the threshold.

[0014] Preferably, the correlation fusion algorithm in step S3 is specifically as follows:

[0015] S31. Establish the time-domain cross-correlation function of the magnetometer data and the accelerometer data;

[0016] S32. When the correlation coefficient is lower than 0.7, use the motion acceleration data to dominate the heading estimation.

[0017] Preferably, it further includes:

[0018] S5. Construct a dynamic magnetic field interference map through the spatial gradient detection of the magnetometer array;

[0019] S6. Adjust the process noise matrix Q of the filter in real time according to the interference map.

[0020] Preferably, when constructing the Kalman filter framework with dynamic weighting based on motion characteristics in step S2, preprocess the nine-axis data, including data denoising and normalization operations.

[0021] Preferably, for complex magnetic interference scenarios, use a single-axis verification path to verify and calibrate the heading positioning result. By comparing with the preset single-axis motion trajectory, discover and correct the positioning error.

[0022] Preferably, the update frequency of the dynamic magnetic field interference map is 10 - 100 Hz.

[0023] According to the positioning system of a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to any one of the above, it includes an inclined redundant magnetometer array module, a low-power six-axis IMU module, an embedded real-time processing unit, and an anti-magnetic interference packaging structure;

[0024] The inclined redundant magnetometer array module is used for multi-dimensional magnetic field distortion detection and data redundancy fault tolerance in a strong magnetic field environment;

[0025] The low-power six-axis IMU module is used for establishing a reference for magnetic field interference discrimination assisted by motion characteristics;

[0026] The embedded real-time processing unit is used for hardware-level synchronization of multi-modal data and acceleration of anti-interference algorithms;

[0027] The anti-magnetic interference encapsulation structure is used for local magnetic field shielding and thermal stability guarantee of the sensor module.

[0028] Preferably, the anti-magnetic interference encapsulation structure adopts a three-dimensionally printed honeycomb magnetic shielding layer.

[0029] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0030] 1. The present invention adopts an inclined redundant magnetometer array and combines dynamic weighted Kalman filtering, which can work stably in a strong magnetic field environment. Without introducing a gyroscope, the root mean square error of heading estimation is 13.64, which is greatly improved compared with the traditional unfiltered magnetometer data. It is applicable to high-interference scenarios such as nuclear power plants and ship welding, avoiding positioning failure caused by magnetic field distortion.

[0031] 2. Through the gyroscope-free architecture and embedded optimization algorithm of the present invention, the total weight of the system becomes smaller and the power consumption is lower. Compared with the traditional INS / GPS fusion scheme, the hardware burden can be greatly reduced, making it suitable for long-term operation of battery-powered wall-climbing robots.

[0032] 3. The present invention innovatively adopts the correlation fusion of magnetometer - accelerometer. When the magnetometer is interfered, it automatically switches to the motion feature-dominated heading estimation, avoiding positioning collapse caused by the failure of a single sensor. The system supports online update of the magnetic interference map and can adapt to magnetic field environments with different intensities and frequencies. Description of the Drawings

[0033] Figure 1 It is a flowchart of the method of the present invention. Detailed Embodiments

[0034] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0035] Please refer to Figure 1 , the present invention provides a technical solution: a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method, including the following steps:

[0036] S1. Collect nine-axis magnetic field data through three groups of three-axis magnetometers with inclined redundancy configuration; S2. Construct a Kalman filter framework with dynamic weighting based on motion characteristics to perform real-time denoising on the nine-axis magnetic field data; S3. Use a correlation fusion algorithm to perform heading estimation on the denoised magnetometer data, robot motion acceleration, and inclination data; S4. Output the root mean square error of heading angle positioning with anti-strong magnetic field interference of 0-15° without relying on gyroscopes; S5. Construct a dynamic magnetic field interference map through spatial gradient detection of the magnetometer array; S6. Adjust the process noise matrix Q of the filter in real time according to the interference map.

[0037] In step S1, the specific configuration of the inclined redundant magnetometers is as follows: three groups of magnetometers are arranged in a non-coplanar triangle, and the axes of each magnetometer have a preset inclination angle with the robot body coordinate system. The preset inclination angle is 10°-30°; the specific process of dynamic weighting of the Kalman filter framework in step S2 is as follows: S21. Dynamically allocate weights according to the real-time signal-to-noise ratio of each magnetometer; S22. When it is detected that the single-axis data is affected by strong magnetic field interference, automatically reduce the weight of this axis below the threshold; the specific correlation fusion algorithm in step S3 is as follows: S31. Establish a time-domain cross-correlation function between the magnetometer data and the accelerometer data; S32. When the correlation coefficient is lower than 0.7, use the motion acceleration data to dominate the heading estimation; when constructing a Kalman filter framework with dynamic weighting based on motion characteristics in step S2, preprocess the nine-axis data, including data denoising and normalization operations; for complex magnetic interference scenarios, use a single-axis verification path to verify and calibrate the heading positioning result. By comparing with the preset single-axis motion trajectory, discover and correct the positioning error; the update frequency of the dynamic magnetic field interference map is 10-100Hz.

[0038] It should be noted that in step S1, three groups of TLV493D three-axis magnetometers are installed in the non-magnetic area of the robot body, arranged in an equilateral triangle, and the Z axis of each module has an inclination angle of 20° with the normal direction of the robot. Synchronously collect nine-axis data at a frequency of 1kHz through the SPI bus, and use CRC check to ensure transmission reliability. Each group of magnetometers is wrapped with a 0.2mm thick MuMetal shielding layer, and it is measured that 60% of the local magnetic field distortion can be suppressed. In the data preprocessing stage, outliers are removed. The formula is:

[0039] [[ID=IO]] [

[0040] where B i is the magnetometer data at the current moment, is the mean value of the historical data, and τ is the standard deviation of the historical data; when the two-norm distance between the current data and the mean value exceeds 3 times the standard deviation, it is determined as an outlier, and the linear interpolation method interp(B i-1 , B i+1 ) is used for replacement to eliminate the influence of outliers on subsequent processing;

[0041] During the Kalman filtering process, a dynamic weighting mechanism is implemented to dynamically adjust the weights of each magnetometer according to the motion acceleration a of the wall-climbing robot. The specific weight calculation formula is as follows:

[0042] W[i] = softmax(-||a × B i ||),

[0043] where W[i] is the weight of the i-th magnetometer, a is the motion acceleration vector, and B i is the magnetic field measurement vector of the i-th magnetometer; by calculating the angle between the motion direction and the magnetic field direction, the larger the angle, the lower the weight, thereby reducing the influence of the magnetometer data severely interfered by strong magnetic fields during the filtering process; the filtering process is as follows: the state vector X = [θ, θ'], the observation matrix H = weighted average magnetic field direction, the process noise, Q = diag[0.01 + 0.1 × ΔB, 0.1], where ΔB is the magnetic field gradient change rate;

[0044] During the multi-modal data alignment process, the data synchronization between the magnetometer and the inertial measurement unit (IMU) is achieved through the hardware timestamp to ensure that the time deviation between the two data is less than 10 μs, so as to improve the accuracy of subsequent fusion processing;

[0045] A time-domain cross-correlation function is established to evaluate the correlation between the magnetometer data and the accelerometer data. The specific formula is as follows:

[0046]

[0047] where B(t) is the magnetic field measurement value of the magnetometer at time t; a(t + τ) is the acceleration measurement value of the accelerometer at time t + τ; N is the total number of data points; τ is the time delay; when the peak value of the calculated correlation coefficient is less than 0.6, it is determined that the magnetometer data is severely interfered by strong magnetic fields, and at this time, the pure accelerometer heading estimation method is adopted;

[0048] During the heading estimation process, the heading deviation caused by the inclination of the wall-climbing robot is compensated. The specific formula is as follows:

[0049]

[0050] where θ corrected is the compensated heading angle; θ is the original heading angle; is the pitch angle; ψ is the roll angle; through this formula, the influence of inclination on heading estimation is effectively eliminated;

[0051] During the gyroscope-free heading output process, when it is detected that the duration of continuous magnetic field interference exceeds 3 seconds, a heading prediction strategy based on the kinematic model is enabled. The specific formula is as follows:

[0052]

[0053] Among them, is the predicted heading angle at the current moment; θ k-1 is the heading angle at the previous moment; v is the wheel speed of the wall-climbing robot; Δt is the time interval; R is the turning radius; β is the differential speed ratio; the prediction result is processed by moving average filtering, the length of the filtering window is 5, and the root mean square error (RMSE) of the final heading output is controlled within 12.5°;

[0054] During the construction of the dynamic magnetic field map, the magnetic field gradient is calculated through the spatial difference of three groups of magnetometers. The specific formula is:

[0055]

[0056] Among them, B ix 、B iy 、B iz are the magnetic field measurement values of the i-th group of magnetometers on the x, y, and z axes respectively, and d ij is the spatial distance between the i-th group and the j-th group of magnetometers; when the calculated magnetic field gradient modulus is greater than 0.5 T / m, mark this area as a strong magnetic field interference area;

[0057] During the Kalman filtering process. Dynamically adjust the process noise covariance matrix Q k in the noise model according to the magnetic field gradient information constructed by the dynamic magnetic field map. The specific formula is:

[0058]

[0059] Among them, Q0 is the initial process noise covariance matrix; α is an empirical coefficient (the value is 0.05); I2 is a 2×2 identity matrix; through this formula, the Kalman filter can adaptively adjust the filtering parameters to cope with magnetic field interferences of different intensities.

[0060] According to the positioning system of the robot anti-strong magnetic field interference adaptive positioning method of any one of the above, it includes an inclined redundant magnetometer array module, a low-power six-axis IMU module, an embedded real-time processing unit, and an anti-magnetic interference packaging structure;

[0061] The inclined redundant magnetometer array module is used for multi-dimensional magnetic field distortion detection and data redundancy fault tolerance in a strong magnetic field environment; the low-power six-axis IMU module is used for establishing a magnetic field interference discrimination benchmark assisted by motion characteristics; the embedded real-time processing unit is used for hardware-level synchronization of multi-modal data and acceleration of anti-interference algorithms; the anti-magnetic interference packaging structure is used for local magnetic field shielding and thermal stability guarantee of the sensor module; the anti-magnetic interference packaging structure adopts a three-dimensional printed honeycomb magnetic shielding layer.

[0062] It should be noted that in the tilt redundant magnetometer array module, at least 3 groups of TDK TLV493D three-axis magnetometers are used and installed on the aluminum chassis of the robot in an asymmetric manner. The first group is 50 mm in front of the center point, with the Z-axis tilted by 20°. The second and third groups are 60 mm on the left and right sides of the center point respectively, with the Z-axis tilted by +15° / -15°. In the low-power six-axis IMU module, STLSM6DSOXTR is selected (the acceleration measurement range is ±16 g, which can withstand climbing impacts. When the mutation frequency of the magnetometer data is greater than 2 times the change rate of the IMU angular velocity, it is determined as magnetic field interference; the gyroscope measurement range is ±2000 dps; it has a built-in machine learning core and runs the LSTM vibration classification algorithm); the main control chip of the embedded processing unit is the STM32H743 dual-core MCU, where the Cortex-M7 core is dedicated to magnetic field interference detection, and the Cortex-M4 core executes the EKF fusion algorithm.

[0063] In summary: The present invention realizes the high-precision synchronization of multi-modal data through hardware timestamps, combines the time-domain cross-correlation function to evaluate data correlation, and improves the accuracy of heading estimation; by introducing the tilt compensation formula, it effectively eliminates the influence of tilt on heading estimation and improves the positioning accuracy; in the absence of a gyroscope, a kinematic model is used to predict the heading, and the output stability is improved through moving average filtering; the magnetic field gradient is calculated by the spatial difference of three groups of magnetometers to construct a dynamic magnetic field map, and the noise model of the Kalman filter is dynamically adjusted accordingly to achieve adaptive filtering.

[0064] Although the embodiments of the present invention have been shown and described, for those of ordinary skill in the art, it can be understood that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method, characterized in that It includes the following steps: S1. Collect nine-axis magnetic field data through three sets of three-axis magnetometers configured with tilt redundancy; S2. Construct a Kalman filter framework with dynamic weighting based on motion characteristics to perform real-time denoising on the nine-axis magnetic field data; S3. Use a correlation fusion algorithm to perform heading estimation on the denoised magnetometer data, robot motion acceleration, and inclination data; S4. Without relying on a gyroscope, output the root mean square error of heading angle positioning against strong magnetic field interference within 0 - **********; 2. The adaptive positioning method for a multi-modal fusion robot to resist strong magnetic field interference according to claim 1, characterized in that In step S1, the specific configuration of the tilt-redundant magnetometer is as follows: three sets of magnetometers are arranged in a non-coplanar triangle, and the axis of each magnetometer has a preset inclination angle with the robot body coordinate system, and the preset inclination angle is 10° - 30°; 3. A multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to claim 1, characterized in that, In step S2, the dynamic weighting process of the Kalman filter framework is specifically as follows: S21. Dynamically allocate weights according to the real-time signal-to-noise ratio of each magnetometer; S22. When it is detected that the single-axis data is interfered by a strong magnetic field, automatically reduce the weight of this axis below the threshold; 4. A multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to claim 1, characterized in that, In step S3, the correlation fusion algorithm is specifically as follows: S31. Establish a time-domain cross-correlation function between the magnetometer data and the accelerometer data; S32. When the correlation coefficient is lower than 0.7, use the motion acceleration data to dominate the heading estimation; 5. A multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to claim 1, characterized in that It also includes: S5. Construct a dynamic magnetic field interference map through the spatial gradient detection of the magnetometer array; S6. Adjust the process noise matrix Q of the filter in real time according to the interference map; 6. A multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to claim 1, characterized in that When constructing the Kalman filter framework with dynamic weighting based on motion characteristics in step S2, preprocess the nine-axis data, including data denoising and normalization operations; 7. A multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to claim 1, characterized in that, For complex magnetic interference scenarios, use a single-axis verification path to verify and calibrate the heading positioning result. By comparing with the preset single-axis motion trajectory, discover and correct the positioning error; 8. A multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to claim 5, characterized in that, The update frequency of the dynamic magnetic field interference map is 10 - 100Hz; 9. A positioning system for a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to any one of claims 1-8, characterized in that, It includes a tilt-redundant magnetometer array module, a low-power six-axis IMU module, an embedded real-time processing unit, and an anti-magnetic interference packaging structure; The tilt-redundant magnetometer array module is used for multi-dimensional magnetic field distortion detection and data redundancy fault tolerance in a strong magnetic field environment; The low-power six-axis IMU module is used to establish a benchmark for magnetic field interference discrimination assisted by motion characteristics; The embedded real-time processing unit is used for hardware-level synchronization of multi-modal data and acceleration of anti-interference algorithms; The anti-magnetic interference packaging structure is used for local magnetic field shielding and thermal stability guarantee of the sensor module; 10. The positioning system of a multi-modal fusion robot anti-strong magnetic field interference adaptive positioning method according to claim 9, characterized in that, The anti-magnetic interference packaging structure uses a three-dimensionally printed honeycomb magnetic shielding layer;

Images