Multi-terminal cooperative indoor autonomous inspection robot control method and system

Abstract

The application discloses a kind of multi-terminal collaborative indoor autonomous inspection robot control method and system, specifically relates to autonomous mobile robot control technical field, constructs environmental disturbance observation matrix and calculates its rank as disturbance linear independent dimension number to judge disturbance richness, joint dimension number and each physical quantity change rate calculate dynamic compensation gain coefficient to adjust navigation positioning filter and vision enhancement parameter, to three kinds of heterogeneous data are executed space alignment and two two consistency check, according to the confidence coefficient of the proportion of check through generation and is weighted fusion to obtain optimal pose estimation;The application calculates the number of disturbance linear independent dimensions and dynamically compensates the filter and image enhancement parameters accordingly, improves the availability of sensory data from the source, then generates real-time confidence coefficients for each source and performs dynamic weighted fusion to suppress low-quality data pollution, and finally feeds the residual error of the steady-state pose and the original observation to the disturbance determination reference to achieve dynamic self-tuning.

Description

Technical Field

[0001] This invention relates to the field of autonomous mobile robot control technology, and more specifically, to a multi-terminal collaborative indoor autonomous inspection robot control method and system. Background Technology

[0002] With the intelligent development of urban gas management, gas companies are increasingly demanding regular meter readings of gas meters in residential and commercial areas. Traditional manual meter reading methods suffer from low efficiency, high costs, and difficulties in accessing homes. Meanwhile, gas meter reading solutions based on autonomous mobile inspection robots are gradually becoming a viable alternative to manual reading. These robots are typically equipped with multiple sensors, including LiDAR, vision cameras, and inertial measurement units. By fusing multi-source sensor data, they autonomously navigate and locate themselves in residential corridors or indoor environments, and use their vision cameras to identify gas meter readings. The complexity of corridors and indoor environments places high demands on the robot's autonomous positioning accuracy: corridors are narrow and contain many repetitive textures; lighting conditions vary drastically with day and night and different corridor orientations; dynamic obstacles such as residents entering and exiting and clutter frequently appear; and wireless communication signals are easily blocked by walls, causing fluctuations. These factors all have varying degrees of impact on the point cloud data quality of LiDAR, the image clarity and feature matching accuracy of visual cameras, and the cumulative integral drift of inertial measurement units. This reduces the reliability of multi-source sensor data fusion and the robot's autonomous positioning accuracy, causing the robot to be unable to accurately reach the meter reading location or to complete the meter reading recognition in a stable posture.

[0003] Existing multi-source sensor data fusion methods generally treat each sensor as a relatively independent signal source. When fusing these signals, the lack of effective coordination and linkage between the internal processing stages, with the sensing and feedback stages operating independently, makes it difficult for the system to dynamically adjust key internal parameters and judgment criteria based on real-time changes in the corridor environment. When drastic changes in corridor lighting cause visual image degradation, the confidence level of the visual data decreases significantly. However, existing methods often fail to identify these confidence level changes and feed them back to the front-end data correction stage, resulting in low-quality visual data continuously participating in the fusion process and contaminating the positioning results. When the robot's positioning exhibits continuous deviations, existing methods also lack a channel to feed back deviation information to the disturbance judgment criteria, failing to achieve dynamic self-tuning of the judgment criteria. Consequently, positioning errors accumulate over time, ultimately leading to meter reading location deviations or meter recognition failures. This open-loop structure, lacking coordination and linkage between stages, is a major factor limiting the long-term stable operation of gas meter reading robots in complex corridor environments. Therefore, this invention proposes a multi-terminal collaborative indoor autonomous inspection robot control method to address these problems. Summary of the Invention

[0004] To achieve the above objectives, the present invention provides the following technical solution: A control method for a multi-terminal collaborative indoor autonomous inspection robot includes the following steps: Step 1: Collect the environmental disturbance observation matrix: Using multiple types of environmental perception sensors on the robot, collect in real time the wireless signal strength, ambient light intensity, and dynamic obstacle position offset in the current inspection environment. The multiple types of environmental perception sensors include at least a signal strength detection sensor, an illuminance sensor, a lidar, a vision camera, and an inertial measurement unit. Arrange the above-mentioned physical quantities collected in chronological order and robot spatial pose into a multi-dimensional observation vector, and combine the multi-dimensional observation vectors of multiple consecutive moments into an environmental disturbance observation matrix. Step 2, calculate the number of linearly independent disturbances: perform linear correlation analysis on the environmental disturbance observation matrix, calculate the rank of the matrix, define the rank of the matrix as the number of linearly independent disturbances, and judge the richness of the types of dynamic disturbance sources in the current environment based on the number of linearly independent disturbances. Step 3: Calculate the dynamic compensation gain coefficient: Based on the number of linear independent dimensions of the disturbance and the instantaneous change rate of each physical quantity in the environmental disturbance observation matrix, calculate the dynamic compensation gain coefficient to counteract the dynamic nonlinearity of the environment. Based on the dynamic compensation gain coefficient, adaptively adjust the noise covariance parameter of the navigation and positioning filter inside the robot and the image enhancement parameter of the vision camera to obtain the original sensing data frame after dynamic compensation correction. Step 4: Generate confidence coefficients for multi-source data: Align the three heterogeneous data sources from LiDAR, visual camera and inertial measurement unit in the original sensor data frame after dynamic compensation and correction with spatial coordinates under a unified time reference, and perform physical consistency logic verification between each pair of the three heterogeneous data after alignment. Generate the real-time confidence coefficient corresponding to each data source based on the verification pass rate. Step 5: Generate steady-state weighted fusion output field: Based on the real-time confidence coefficient, dynamically weight and fuse the three types of heterogeneous data to generate the optimal steady-state weighted fusion output field at the current moment. The steady-state weighted fusion output field contains the robot's optimal steady-state pose estimate. Step 6, Closed-loop feedback self-tuning: The steady-state weighted fusion output field and the original observation data in the environmental disturbance observation matrix are differentially calculated to obtain the residual feedback driving quantity. The residual feedback driving quantity is used as the feedback input to act on the judgment criterion of the linear independent dimension of the disturbance in Step 2, so as to realize the dynamic self-tuning of the judgment criterion and form the full-link closed-loop adaptive control.

[0005] In step one, the multi-type environmental perception sensors include at least a signal strength detection sensor for acquiring wireless signal strength, a light intensity sensor for acquiring ambient light intensity, a lidar for detecting the positional offset of dynamic obstacles, a visual camera for acquiring visual images, and an inertial measurement unit for acquiring acceleration and angular velocity information.

[0006] In a preferred embodiment, the specific method for performing linear correlation analysis on the environmental disturbance observation matrix and calculating the rank of the matrix in step two is as follows: The environmental disturbance observation matrix is ​​transformed into a row echelon form matrix using Gaussian elimination. The number of non-zero rows in the row echelon form matrix is ​​counted, and the number of non-zero rows is used as the rank of the matrix. The rank is then used as the number of linearly independent dimensions of the disturbance.

[0007] In a preferred embodiment, in step three, the instantaneous rate of change of each physical quantity in the environmental disturbance observation matrix is ​​obtained in the following way: Forward difference calculation is performed on the physical quantity values ​​of each dimension in the continuously acquired multidimensional observation vector. The instantaneous rate of change of each physical quantity is obtained by subtracting the value of the previous time from the value of the current time and then dividing by the sampling time interval. The value of the dynamic compensation gain coefficient is positively correlated with the number of linear independent dimensions of the disturbance and also positively correlated with the absolute value of the instantaneous rate of change.

[0008] In a preferred embodiment, step four, the pairwise physical consistency logical verification includes: The robot's displacement change measured by the LiDAR is compared with the displacement change calculated by integrating the acceleration measured by the inertial measurement unit. A pass is determined when the absolute value of the difference is less than a preset first pose verification threshold. Similarly, the robot's displacement change obtained by the visual camera through image feature matching is compared with the displacement change measured by the LiDAR. A pass is determined when the absolute value of the difference is less than a preset second pose verification threshold. The robot's displacement change obtained by the visual camera is compared with the displacement change calculated by integrating the acceleration measured by the inertial measurement unit. A pass is determined when the absolute value of the difference is less than a preset third pose verification threshold. The ratio of the number of successful verifications to the total number of verifications is the verification pass rate.

[0009] In a preferred embodiment, the preset first pose verification threshold, second pose verification threshold, and third pose verification threshold are all dynamically adjusted values, and their values ​​are adjusted in the same direction based on the value of the dynamic compensation gain coefficient and the preset adjustment strategy.

[0010] In a preferred embodiment, step four involves generating the real-time confidence coefficient for each data source as follows: Nonlinear correlation analysis was performed on three types of heterogeneous data after dynamic compensation correction using the information entropy calculation method. The numerical sequences of each data source were divided into multiple intervals with equal numerical ranges. The frequency of data occurrence within each interval was used as the probability value. The information entropy value of each of the three heterogeneous data types was calculated based on these probability values ​​using a logarithmic operation with a base of two. When the probability value was zero, the defined term value was zero. The pass rate of all pairwise verification items involving each data source was used as the information entropy probability correction factor for the data source. A weighted attenuation correction was applied to the probability values ​​within each interval of the data source, i.e., the original probability value within each interval was multiplied by the pass rate to obtain the corrected probability value. The corrected information entropy value of the data source is recalculated by replacing the original probability value; the joint information entropy value between any two heterogeneous data is calculated; the corrected information entropy values ​​of the two data are added together and then the joint information entropy value of the two data is subtracted to obtain the mutual information between the two data sources; the three sets of mutual information values ​​obtained by combining the three data sources in pairs are normalized into three sets of correlation coefficients with values ​​ranging from zero to one; based on the initial acquisition accuracy weight of each data source and the three sets of correlation coefficients, a comprehensive calculation is performed to generate the final real-time confidence coefficient for each of the three heterogeneous data; in the calculation, the more balanced and larger the mean of the mutual information between any data source and the other two data sources, the higher the real-time confidence coefficient assigned to the data source.

[0011] In a preferred embodiment, step five involves dynamically weighting and fusing the three types of heterogeneous data as follows: The pose data output by the LiDAR, the pose data output by the visual camera, and the pose data output by the inertial measurement unit are multiplied by their respective real-time confidence coefficients to obtain three weighted pose values. The three weighted pose values ​​are added together and divided by the sum of the three real-time confidence coefficients to obtain the optimal pose value after weighted averaging. The optimal pose value after weighted averaging is the optimal steady-state pose estimate. The optimal steady-state pose estimate and its corresponding covariance information together constitute the steady-state weighted fusion output field.

[0012] In a preferred embodiment, in step six, the residual feedback driving quantity is obtained through time-frequency joint analysis logic calculation in the migration mechanical vibration analysis: First, the optimal steady-state pose estimate in the steady-state weighted fusion output field is interpolated point-by-point with the original observation data in the environmental disturbance observation matrix at the same time node to obtain the residual time series. Then, time-domain features are extracted from the residual time series to obtain the peak and mean values ​​of the residual amplitude. Next, frequency-domain features are extracted from the residual time series, dividing it into multiple segments according to the time window. Fourier transform is performed on each segment to obtain the spectrum, and the center frequency value of the energy-concentrated frequency band in the spectrum is extracted. Finally, the difference between the peak and mean values ​​of the residual amplitude is used as the time-domain excitation value, and the center frequency value of the energy-concentrated frequency band is used as the frequency-domain influence factor. The product of the time-domain excitation value and the frequency-domain influence factor is used as the residual feedback driving quantity and applied to the determination criterion of the linear independent dimension of the disturbance in step two to achieve dynamic self-tuning of the determination criterion.

[0013] The technical effects and advantages of this invention are as follows: This invention arranges wireless signal strength, ambient light intensity, and dynamic obstacle position offsets into multi-dimensional observation vectors according to time sequence and robot spatial pose, and combines them into an environmental disturbance observation matrix. Linear correlation analysis is performed on the environmental disturbance observation matrix to calculate the rank of the matrix and obtain the number of linearly independent disturbance dimensions. Based on the number of dimensions, the richness of the types of dynamic disturbance sources in the current environment is determined. Then, in step three, a dynamic compensation gain coefficient is calculated based on the number of linearly independent disturbance dimensions and the instantaneous change rate of each physical quantity in the environmental disturbance observation matrix. Based on the gain coefficient, the noise covariance parameter of the robot's internal navigation and positioning filter and the image enhancement parameters of the visual camera are adaptively adjusted to obtain the original sensor data frame after dynamic compensation correction. Through the above mechanism, this invention enables the robot's navigation and positioning filter and visual image enhancement parameters to synchronously and adaptively adjust with the real-time changes in the richness and intensity of environmental disturbance types. When there are drastic changes in lighting in the corridor or fluctuations in the wireless signal, the image enhancement intensity and the filter noise covariance are optimized synchronously, improving the usability of the original data from each sensor from the source and avoiding the problem of low-quality data contaminating the subsequent fusion results due to independent open-loop operation of each stage.

[0014] This invention aligns the spatial coordinates of three heterogeneous data sources—LiDAR, visual camera, and inertial measurement unit—from the original sensor data frame after dynamic compensation and correction under a unified time reference. It then performs pairwise physical consistency checks on the aligned heterogeneous data, generating a real-time confidence coefficient for each data source based on the pass rate of these checks. Finally, in step five, it dynamically weights and fuses the three heterogeneous data sources based on these real-time confidence coefficients to generate a steady-state weighted fusion output field containing the optimal steady-state pose estimate. Through this mechanism, the fusion weights of each data source are no longer fixed preset values ​​but are dynamically generated in real-time based on the pass rate of the pairwise physical consistency checks between LiDAR, visual camera, and inertial measurement unit. When drastic changes in lighting in the corridor cause visual image degradation, the pass rate of the visual camera's check automatically decreases, and its real-time confidence coefficient decreases accordingly. The contribution of low-quality visual data in the weighted fusion is automatically suppressed, avoiding the problem of low-quality data continuously participating in the fusion and contaminating the positioning results. This ensures that the steady-state pose estimate output by the fusion always maintains a high level of reliability.

[0015] This invention obtains the residual feedback driving quantity by differentially calculating the steady-state weighted fusion output field and the original observation data in the environmental disturbance observation matrix. This residual feedback driving quantity is then used as feedback input to determine the linear independent dimension of the disturbance in step two, enabling the determination criterion to dynamically self-tune. This forms a full-link closed-loop adaptive control system, from environmental perception through dynamic compensation, confidence generation, weighted fusion, and feedback self-tuning. Through this mechanism, the determination criterion for the linear independent dimension of the disturbance is no longer a preset fixed value, but rather dynamically adjusted in real time according to the residual between the steady-state fusion positioning result and the original observation. When the robot's positioning continuously deviates due to repeated dynamic obstacles in the corridor, the residual feedback driving quantity increases accordingly and acts on the determination criterion, causing the determination criterion to adaptively rise. The system's judgment of disturbance richness becomes more conservative, thus proactively adjusting the dynamic compensation gain coefficient and confidence verification threshold in subsequent cycles. This ensures that the front-end perception, intermediate compensation, and end-end fusion of the entire control link work together around the same positioning target, effectively suppressing the problem of continuous accumulation of positioning error over time in an open-loop structure. Attached Figure Description

[0016] To facilitate understanding by those skilled in the art, the present invention will be further described below with reference to the accompanying drawings; Figure 1 This is a schematic diagram of a multi-terminal collaborative indoor autonomous inspection robot control method according to the present invention.

[0017] Figure 2 This is a schematic diagram of a multi-terminal collaborative indoor autonomous inspection robot control system according to the present invention. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0019] Reference Figure 1-2 The following examples were obtained: Example 1: A control method for a multi-terminal collaborative indoor autonomous inspection robot, comprising the following steps: Step 1: Collect the environmental disturbance observation matrix: Using multiple types of environmental perception sensors on the robot, collect in real time the wireless signal strength, ambient light intensity, and dynamic obstacle position offset in the current inspection environment. The multiple types of environmental perception sensors include at least a signal strength detection sensor, an illuminance sensor, a lidar, a vision camera, and an inertial measurement unit. Arrange the above-mentioned physical quantities collected in chronological order and robot spatial pose into a multi-dimensional observation vector, and combine the multi-dimensional observation vectors of multiple consecutive moments into an environmental disturbance observation matrix. Step 2, calculate the number of linearly independent disturbances: perform linear correlation analysis on the environmental disturbance observation matrix, calculate the rank of the matrix, define the rank of the matrix as the number of linearly independent disturbances, and judge the richness of the types of dynamic disturbance sources in the current environment based on the number of linearly independent disturbances. Step 3: Calculate the dynamic compensation gain coefficient: Based on the number of linear independent dimensions of the disturbance and the instantaneous change rate of each physical quantity in the environmental disturbance observation matrix, calculate the dynamic compensation gain coefficient to counteract the dynamic nonlinearity of the environment. Based on the dynamic compensation gain coefficient, adaptively adjust the noise covariance parameter of the navigation and positioning filter inside the robot and the image enhancement parameter of the vision camera to obtain the original sensing data frame after dynamic compensation correction. Step 4: Generate confidence coefficients for multi-source data: Align the three heterogeneous data sources from LiDAR, visual camera and inertial measurement unit in the original sensor data frame after dynamic compensation and correction with spatial coordinates under a unified time reference, and perform physical consistency logic verification between each pair of the three heterogeneous data after alignment. Generate the real-time confidence coefficient corresponding to each data source based on the verification pass rate. Step 5: Generate steady-state weighted fusion output field: Based on the real-time confidence coefficient, dynamically weight and fuse the three types of heterogeneous data to generate the optimal steady-state weighted fusion output field at the current moment. The steady-state weighted fusion output field contains the robot's optimal steady-state pose estimate. Step 6, Closed-loop feedback self-tuning: The steady-state weighted fusion output field and the original observation data in the environmental disturbance observation matrix are differentially calculated to obtain the residual feedback driving quantity. The residual feedback driving quantity is used as the feedback input to act on the judgment criterion of the linear independent dimension of the disturbance in Step 2, so as to realize the dynamic self-tuning of the judgment criterion and form the full-link closed-loop adaptive control.

[0020] During data acquisition, the robot is equipped with various environmental perception sensors, including signal strength sensors, illuminance sensors, LiDAR, vision cameras, and inertial measurement units (IMUs). The signal strength sensors collect wireless signal strength, the illuminance sensors collect ambient light levels, the LiDAR detects the distance and angle of dynamic obstacles, the vision cameras capture visual images, and the IMU collects acceleration and angular velocity information. Wireless signal strength reflects the level of electromagnetic interference in the current environment, ambient light levels affect the image quality of the vision cameras, and the dynamic obstacle position offset is obtained by differentiating the LiDAR detection results of the current frame from the previous frame; its value is the combined vector length of the obstacle's lateral and longitudinal displacements in the robot's coordinate system.

[0021] The sampling frequency of the signal strength detection sensor is 1 Hz to 5 Hz, based on the slow rate of change of wireless signal strength in indoor environments, with a typical value of 2 Hz. The sampling frequency of the illuminance sensor is 10 Hz to 50 Hz, based on the fact that changes in illuminance under indoor lighting conditions mainly come from changes in shadows caused by the robot's own movement, with a typical value of 20 Hz. The sampling frequency of the lidar is 10 Hz to 30 Hz, based on the moderate rate of change of the position of dynamic obstacles, with a typical value of 20 Hz. The sampling frequency of the vision camera is 15 Hz to 60 Hz, based on the requirements of human visual persistence and motion blur suppression, with a typical value of 30 Hz. The sampling frequency of the inertial measurement unit is 100 Hz to 500 Hz, based on the high-frequency dynamic characteristics of angular velocity and acceleration signals, with a typical value of 200 Hz. All sensors perform data acquisition under the trigger of a unified time synchronization signal to ensure the consistency of each physical quantity on the time reference.

[0022] When arranging the collected physical quantities into a multidimensional observation vector according to time sequence and robot spatial pose, the robot's heading angle at the current moment is first calculated based on the integral value of the angular velocity provided by the inertial measurement unit. The robot's current position coordinates and heading angle are then defined together as its spatial pose. Next, using the current moment as the reference moment, the wireless signal strength, ambient light intensity, dynamic obstacle position offset, environmental contour features extracted from the LiDAR, and image texture features extracted from the visual camera are all converted to the robot's spatial pose coordinate system corresponding to the reference moment, ensuring that all physical quantities are aligned under the same spatial reference. The aligned physical quantity values ​​are then arranged into a multidimensional observation vector in a fixed order: wireless signal strength value first, ambient light intensity value second, dynamic obstacle position offset third, LiDAR distance feature value fourth to fourth + N minus one, and visual camera image grayscale average value last, where N is the number of effective scanning points of the LiDAR.

[0023] The multidimensional observation vectors from multiple consecutive time points are combined into an environmental disturbance observation matrix. The combination method involves taking all multidimensional observation vectors within a predetermined time window before the current time point. The multidimensional observation vector from the first time point within the predetermined time window is placed as the first row of the matrix, the multidimensional observation vector from the second time point as the second row, and so on, until the multidimensional observation vector from the last time point within the predetermined time window is placed as the last row. The number of rows is equal to the predetermined time window length divided by the sampling period. The predetermined time window length is set based on the time required to completely capture one environmental disturbance change within the robot's motion control cycle. A typical value is a time window length of 30 sampling times, meaning the environmental disturbance observation matrix is ​​a matrix with 30 rows multiplied by the dimension of the multidimensional observation vectors. For example, when the wireless signal strength is -65 dBmW, the ambient light intensity is 350 lux, the dynamic obstacle position offset is 0.15 m, the lidar distance feature values ​​are 2.5 m and 3.0 m, and the average grayscale value of the visual camera image is 128, the multidimensional observation vector at time 128 is arranged as [-65, 350, 0.15, 2.5, 3.0, 128]. The class vectors of the 30 consecutive time steps before time 128 are arranged vertically in chronological order to obtain a 30-row, 6-column environmental disturbance observation matrix.

[0024] In step two, when performing linear correlation analysis and calculating the rank of the environmental disturbance observation matrix, the matrix is ​​first transformed into a row echelon form matrix using Gaussian elimination. The Gaussian elimination process involves selecting the element with the largest absolute value in the first column as the pivot, swapping the row containing the pivot to the first row, and then using the pivot to eliminate the values ​​in the same column of all rows below it, making all values ​​in the second to last rows zero. This process of selecting the pivot and elimination is repeated for the remaining submatrix until all columns have been processed or the remaining submatrix is ​​completely zero. A row echelon form matrix has the characteristic that the column containing the first non-zero element of each row is strictly higher than the column containing the first non-zero element of the previous row, and all rows containing zeros are located at the bottom of the matrix. The number of non-zero rows in the row echelon matrix is ​​counted. A non-zero row is defined as a row in which at least one element has an absolute value greater than a preset numerical threshold. The preset numerical threshold is set based on ten times the sensor quantization noise level, with a typical value of 0.001. If the absolute values ​​of all elements in a row are less than or equal to 0.001, the row is considered to be a row with all zeros. The number of non-zero rows is used as the rank of the environmental disturbance observation matrix, and the rank is defined as the number of linearly independent dimensions of the disturbance.

[0025] The richness of dynamic disturbance sources in the current environment is determined based on the number of linear independent dimensions (LIDs). The determination method is as follows: if the LID is greater than a preset richness threshold, the richness of dynamic disturbance sources in the current environment is considered high; if the LID is less than or equal to the preset richness threshold, it is considered low. The preset richness threshold is set based on the minimum requirement for the robot's localization and navigation algorithm to identify disturbance sources. The minimum requirement is set according to the statistical results of the number of typical disturbance sources in the indoor inspection environment, with a typical value of 3. A higher LID indicates a greater variety of independent dynamic disturbance sources in the current environment, while a lower LID indicates a more homogeneous variety of dynamic disturbance sources or a more uniform disturbance intensity. For example, if the environmental disturbance observation matrix is ​​a 5x3 matrix and the number of non-zero rows in the row echelon matrix obtained by Gaussian elimination is 2, then the rank of the matrix is ​​2, and the number of linearly independent disturbance dimensions is 2. Since 2 is less than the preset richness judgment threshold of 3, the type richness of the dynamic disturbance source in the current environment is determined to be low. If the number of non-zero rows in the row echelon matrix is ​​4, then the number of linearly independent disturbance dimensions is 4. Since 4 is greater than the preset richness judgment threshold of 3, the type richness of the dynamic disturbance source in the current environment is determined to be high.

[0026] It should be noted that determining the richness of dynamic disturbance sources in the current environment is significant for providing a preliminary decision-making basis for calculating the dynamic compensation gain coefficient in step three, regarding whether fine-grained compensation is necessary. When the richness is determined to be high, it indicates the presence of multiple independent dynamic disturbance sources in the current environment. In this case, the robot's navigation and positioning filter needs to adopt a compensation strategy with a higher gain coefficient to cope with the nonlinear effects caused by the superposition of multiple disturbances. That is, the value of the dynamic compensation gain coefficient is set to a relatively large range under high richness conditions. When the richness is determined to be low, it indicates that the types of disturbance sources in the current environment are relatively simple or that there is a high correlation between the disturbance sources. In this case, a compensation strategy with a medium or low gain coefficient is sufficient and there is no need for excessive adjustment. The determination of type richness serves as a preliminary screening condition for the dynamic compensation gain coefficient in step three. Specifically, before step three is executed, a set of preset initial search ranges for gain coefficients are selected based on the determination result. When the determination result is high richness, both the upper and lower limits of the initial search range are higher than the corresponding values ​​under low richness conditions. Within the selected initial search range, fine optimization is performed based on the instantaneous change rate of each physical quantity to obtain the final dynamic compensation gain coefficient. Therefore, the determination result of type richness does not directly participate in subsequent calculations but serves as the basis for decision branches before gain coefficient calculation. It plays a role in optimizing computational efficiency and preventing excessive gain in low richness scenarios within the complete method flow.

[0027] In step three, the instantaneous rate of change of each physical quantity in the environmental disturbance observation matrix is ​​obtained in the following way: Forward difference calculations are performed on the physical quantity values ​​of each dimension in the continuously acquired multidimensional observation vector. The forward difference calculation method involves subtracting the physical quantity value from the value at the previous moment from the value at the current moment, and then dividing the difference by the sampling time interval. The quotient is the instantaneous rate of change of the physical quantity at the current moment. The sampling time interval is the time difference between two adjacent acquisitions, and its value is equal to the reciprocal of the sampling period of the corresponding sensor for the physical quantity. For example, if the illuminance sensor acquires a value of 350 lux at the current moment, 340 lux at the previous moment, and the sampling time interval is 0.05 seconds, then the instantaneous rate of change of the illuminance physical quantity is 200 lux per second. If the angular velocity signal of the inertial measurement unit is 2.5 radians per second at the current moment, 2.3 radians per second at the previous moment, and the sampling time interval is 0.005 seconds, then the instantaneous rate of change of the angular velocity physical quantity is 40 radians per second.

[0028] Based on the determination results of the number of linearly independent dimensions and the richness of the types of disturbances, the dynamic compensation gain coefficient is calculated in conjunction with the instantaneous rate of change of each physical quantity. The dynamic compensation gain coefficient is a dimensionless value greater than zero, and its value is positively correlated with the number of linearly independent dimensions of the disturbance, and also positively correlated with the absolute value of the instantaneous rate of change of each physical quantity. That is, the larger the number of linearly independent dimensions of the disturbance, the larger the value of the dynamic compensation gain coefficient; the larger the absolute value of the instantaneous rate of change of each physical quantity, the larger the value of the dynamic compensation gain coefficient. The specific calculation method of the dynamic compensation gain coefficient is as follows: first, average the absolute values ​​of the instantaneous rate of change of each physical quantity to obtain the average instantaneous rate of change value; then divide the average instantaneous rate of change value by a preset normalized reference value to obtain the normalized rate of change factor. The preset normalized reference value is set based on the root mean square value of each sensor range, and a typical value is a normalized reference value of 100.

[0029] Simultaneously, the number of linearly independent dimensions of the disturbance is divided by a preset dimension normalization constant to obtain the normalized dimension factor. The preset dimension normalization constant is set based on the theoretical maximum value of the number of sensor types, with a typical value of 6. Then, the normalized rate of change factor and the normalized dimension factor are added together to obtain a sum. This sum is divided by two to obtain the baseline gain coefficient. The baseline gain coefficient is then multiplied by a preset gain adjustment coefficient. The gain adjustment coefficient is set based on the empirical trade-off between system response speed and stability, with a typical value of 0.8. The result is the dynamic compensation gain coefficient. The typical range of the dynamic compensation gain coefficient is between 0.3 and 3.5. When the number of linearly independent dimensions of the disturbance is 4 and the average instantaneous rate of change is 150, the normalized rate of change factor is 150 divided by 100, which equals 1.5; the normalized dimension factor is 4 divided by 6, which is approximately 0.67; the sum is 2.17, divided by 2, which gives 1.085; multiplied by 0.8, we get 0.868, so the dynamic compensation gain coefficient is 0.868. When the number of linearly independent dimensions of the disturbance is 2 and the average instantaneous rate of change is 50, the normalized rate of change factor is 50 divided by 100, which equals 0.5; the normalized dimension factor is 2 divided by 6, which is approximately 0.33; the sum is 0.83, divided by 2, which gives 0.415; multiplied by 0.8, we get 0.332, so the dynamic compensation gain coefficient is 0.332. It can be seen that the dynamic compensation gain coefficient calculated when the number of linearly independent dimensions of the disturbance is larger is relatively larger, and the dynamic compensation gain coefficient calculated when the number of linearly independent dimensions of the disturbance is smaller is relatively smaller. This is a concrete manifestation of the positive correlation between the two.

[0030] The noise covariance parameters of the robot's internal navigation and positioning filter and the image enhancement parameters of the vision camera are adaptively adjusted based on the dynamic compensation gain coefficient. The navigation and positioning filter is an extended Kalman filter, and its noise covariance parameters include the process noise covariance matrix and the observation noise covariance matrix. The diagonal elements of the process noise covariance matrix represent the uncertainty variance of velocity error and attitude angle error in the robot's motion model, and the diagonal elements of the observation noise covariance matrix represent the uncertainty variance of the observations from each sensor.

[0031] The adaptive adjustment method involves multiplying all diagonal elements of the process noise covariance matrix and the observation noise covariance matrix by a dynamic compensation gain coefficient to obtain updated process noise covariance matrices and updated observation noise covariance matrices. When the dynamic compensation gain coefficient is greater than 1, the diagonal element values ​​increase, indicating that the filter trusts real-time observation data more and reduces the weight of motion model predictions. When the dynamic compensation gain coefficient is less than 1, the diagonal element values ​​decrease, indicating that the filter trusts motion model predictions more and reduces the weight of observation data. The image enhancement parameters of the visual camera include contrast stretching intensity and sharpening intensity values. The adaptive adjustment method involves multiplying both contrast stretching intensity and sharpening intensity values ​​by a dynamic compensation gain coefficient to obtain updated contrast stretching intensity and updated sharpening intensity values. When the dynamic compensation gain coefficient is greater than 1, the image enhancement intensity increases to cope with scenarios where drastic changes in lighting cause image degradation. When the dynamic compensation gain coefficient is less than 1, the image enhancement intensity decreases to avoid over-enhancement and the introduction of false textures.

[0032] After adaptive adjustment, the updated contrast stretching intensity value and the updated sharpening intensity value will be used to perform contrast stretching and sharpening processing on the original image of the current frame of the visual camera. The updated process noise covariance matrix and the updated observation noise covariance matrix will be applied to the navigation and positioning filter to filter the current frame of sensor data. All the sensor data processed in the above way will together constitute the original sensor data frame after dynamic compensation and correction.

[0033] In step four, before generating the confidence coefficient of multi-source data, the three heterogeneous data from LiDAR, visual camera and inertial measurement unit in the original sensor data frame after dynamic compensation and correction are first aligned in spatial coordinates under a unified time reference. The unified time reference is established by using the hardware timestamp provided by the inertial measurement unit as the global time reference, and unifying the scanning time of LiDAR and the exposure time of visual camera to the global time reference through linear interpolation.

[0034] The specific method for spatial coordinate alignment is as follows: taking the robot's current spatial pose as the reference coordinate system, the distance and angle data in polar coordinate format output by the LiDAR are converted into Cartesian coordinates through trigonometric functions, the pixel coordinates output by the vision camera are converted into three-dimensional coordinates in the camera coordinate system through the inverse projection transformation of the pinhole camera model, and then the three-dimensional coordinates are transformed into the robot's spatial pose coordinate system through a pre-calibrated rotation matrix and translation vector. The incremental data output by the inertial measurement unit relative to its own initial state is integrated into the robot's spatial pose coordinate system through dead reckoning, thus completing the spatial coordinate alignment.

[0035] After alignment, the three types of heterogeneous data undergo pairwise physical consistency logic checks. The checks use the robot's displacement change as a unified check metric. The robot's displacement change is defined as the distance the robot moves in three-dimensional space between two adjacent sampling times. The pairwise physical consistency logic checks include three independent check sub-items: The first verification item involves calculating the difference between the displacement change measured by the LiDAR and the displacement change obtained by double integration of the acceleration measured by the inertial measurement unit (IMU). The displacement change measured by the LiDAR is directly extracted by performing iterative nearest-point registration on two consecutive frames of LiDAR point cloud data. The displacement change obtained by double integration of the acceleration measured by the IMU is obtained by performing two integration operations on the acceleration signal on the time axis. The first integration is from the acceleration value to the velocity value, and the second integration is from the velocity value to the displacement value. The first verification item is considered passed when the absolute value of the difference is less than the preset first attitude verification threshold. The preset first attitude verification threshold is set based on twice the root mean square of the nominal ranging error of the LiDAR and the integral drift error of the IMU, with a typical value of 0.05 meters.

[0036] The second verification item involves calculating the difference between the robot's displacement change obtained from image feature matching by the visual camera and the displacement change measured by the lidar. The displacement change obtained from image feature matching is calculated by extracting and matching feature points from two consecutive images, using epipolar geometry constraints to calculate the essential matrix, and then decomposing it into a rotation matrix and a translation vector. The magnitude of the translation vector is the displacement change. The second verification item is considered passed when the absolute value of the difference is less than a preset second pose verification threshold. The preset second pose verification threshold is set based on the ternary digits of the statistical distribution of the combined pixel reprojection error from visual feature matching and the lidar ranging error; a typical value is 0.08 meters.

[0037] The third verification item involves calculating the difference between the robot's displacement change obtained from the visual camera and the displacement change obtained by integrating the acceleration measured by the inertial measurement unit. If the absolute value of the difference is less than a preset third pose verification threshold, the third verification item is considered passed. The preset third pose verification threshold is set based on three times the root mean square of both the visual feature matching displacement error and the inertial measurement unit integral drift error. A typical value is 0.10 meters. There is no mandatory restriction on the verification order of the three sub-items, but all three sub-items are executed completely at each sampling time.

[0038] The ratio of the number of items that pass the three checks to the total number of checks is the pass rate. The total number of checks is three. The pass rate is a dimensionless fractional value between 0 and 1. The specific value is the result of dividing the number of checks that pass the three sub-items by three.

[0039] The preset first pose verification threshold, the preset second pose verification threshold, and the preset third pose verification threshold are all dynamically adjusted values. Their values ​​are adjusted in the same direction based on the value of the dynamic compensation gain coefficient and the preset adjustment strategy. The meaning of the same direction adjustment is that when the dynamic compensation gain coefficient increases, all three pose verification thresholds increase, and when the dynamic compensation gain coefficient decreases, all three pose verification thresholds decrease.

[0040] The specific implementation method of the preset adjustment strategy is as follows: First, initial reference values ​​are set for the three pose verification thresholds: 0.05 meters for the first threshold, 0.08 meters for the second, and 0.10 meters for the third. Then, during online operation, each initial reference value is multiplied by a dynamic compensation gain coefficient to obtain its current adjustment value. This current adjustment value is the actual pose verification threshold used at that moment. Specifically, the current adjustment value for the first threshold is 0.05 meters multiplied by the dynamic compensation gain coefficient, the current adjustment value for the second threshold is 0.08 meters multiplied by the dynamic compensation gain coefficient, and the current adjustment value for the third threshold is 0.10 meters multiplied by the dynamic compensation gain coefficient.

[0041] For example, when the dynamic compensation gain coefficient is 0.868, the current adjustment value of the first pose verification threshold is 0.05 multiplied by 0.868, which equals 0.0434 meters; the current adjustment value of the second pose verification threshold is 0.08 multiplied by 0.868, which equals 0.0694 meters; and the current adjustment value of the third pose verification threshold is 0.10 multiplied by 0.868, which equals 0.0868 meters. At this time, all three thresholds are less than their respective initial reference values. Since the three thresholds adjust dynamically in the same direction as the dynamic compensation gain coefficient, when the dynamic disturbance in the environment is severe, the dynamic compensation gain coefficient increases accordingly, and the three pose verification thresholds increase synchronously. This avoids the phenomenon of properly usable data being misjudged as inconsistent and excessively downweighted due to overly strict fixed thresholds. When the dynamic disturbance in the environment is relatively gentle, the dynamic compensation gain coefficient decreases accordingly, and the three pose verification thresholds narrow synchronously. This avoids the phenomenon of abnormal data with deviations being incorrectly accepted and contaminating the fusion result due to overly wide fixed thresholds, thus achieving dynamic adaptation of the verification thresholds.

[0042] In step four, when performing nonlinear correlation analysis on the three types of heterogeneous data after dynamic compensation and correction using the information entropy calculation method, the information entropy calculation method is used to quantify the dispersion of numerical distribution within each data source and the amount of information shared between different data sources. The numerical sequences of each data source are divided into multiple intervals at equal intervals according to the numerical range. The number of intervals is set based on the square root of the total number of data samples, typically ranging from 8 to 16. For example, when the total number of data samples is 100, the number of intervals is 10; when the total number of data samples is 64, the number of intervals is 8. The frequency of data occurrence within each interval is used as the probability value. The frequency is the ratio of the number of data samples within the interval to the total number of data samples in the data source. The sum of the probability values ​​is 1. Based on the probability values, the information entropy values ​​of the three types of heterogeneous data are calculated separately, using a logarithmic operation with base 2. The formula for calculating the information entropy value of a single data source is: multiply the probability value of each interval of the data source by the base-2 logarithm of the probability value, take the negative number, and then add the results of all intervals. The defined term is zero when the probability is zero. This definition is based on the mathematical principle that the limit of zero multiplied by infinity is zero, thus avoiding the undefined nature of logarithmic operations when the input is zero. Information entropy represents the degree of dispersion of data within a single data source. A larger information entropy value indicates a more dispersed numerical distribution within the data source, while a smaller information entropy value indicates a more concentrated numerical distribution.

[0043] The pass rate of all pairwise verification items involving each data source is used as the information entropy probability correction factor for the data source, and a weighted attenuation correction is applied to the probability values ​​within each interval of the data source. The pass rate is the ratio of the number of verification items in the three pairwise verification items mentioned above that the data source participated in and was judged to pass to the total number of verification items in the data source. The specific method of weighted attenuation correction is to multiply the original probability value in each interval of the data source by the pass rate to obtain the corrected probability value.

[0044] The sum of the corrected probability values ​​equals the pass rate, which is no longer 1. The corrected probability values ​​replace the original probability values, and the corrected information entropy value of the data source is recalculated. The calculation method for the corrected information entropy value is the same as the aforementioned information entropy value calculation method: multiply the corrected probability value of each interval by the base-2 logarithm of the corrected probability value, take the opposite number, and then sum them. For example, when the pass rate of the LiDAR verification is 0.5, assuming the LiDAR data is divided into 10 intervals and the original probability value of each interval is uniformly distributed at 0.1, then the corrected probability value of each interval is 0.1 multiplied by 0.5 equals 0.05, and the sum of the corrected probability values ​​of each interval is 0.5. The corrected information entropy value is 10 multiplied by negative 0.05 multiplied by the base-2 logarithm of 0.05, approximately equal to 10 multiplied by 0.216 equals 2.16 bits. The corrected information entropy value is greater than the information entropy value under the original uniform distribution by 3.32 bits, indicating that the pass rate reduces the statistical reliability of the values ​​within the data source.

[0045] Calculate the joint information entropy between any two heterogeneous data sources. The joint information entropy is calculated as follows: Divide each of the two data sources into intervals, then take combinations of these intervals. Calculate the joint frequency of the two data sources occurring simultaneously under each interval combination. Use this joint frequency as the joint probability value. The joint probability value is calculated as the ratio of the number of data samples in each combination to the total number of data samples. Then, perform a base-2 logarithmic operation on the joint probability value for each combination, take the negative of the result, and sum the results. Add the corrected information entropy values ​​of the two data sources together, then subtract the joint information entropy value to obtain the mutual information between the two data sources. The mutual information value characterizes the correlation strength between the two data sources; a larger mutual information value indicates more shared information between the two data sources, while a smaller mutual information value indicates stronger independence between the two data sources.

[0046] The three sets of mutual information values ​​obtained by combining the three data sources in pairs are normalized into three sets of correlation coefficients with values ​​ranging from zero to one. The normalization method is to divide each set of mutual information values ​​by the sum of the corrected information entropy values ​​of the two data sources in the group; the resulting ratio is the normalized correlation coefficient. A comprehensive calculation is performed based on the initial acquisition accuracy weights of each data source and the three sets of correlation coefficients. The initial acquisition accuracy weights are set according to the normalized values ​​of the nominal measurement accuracy levels specified in the manuals of each sensor device. Typical values ​​are 0.4 for LiDAR, 0.3 for visual cameras, and 0.3 for inertial measurement units. The comprehensive calculation method is to average the two sets of normalized correlation coefficients between any data source and the other two data sources to obtain the average correlation coefficient of the data source. Then, multiply the average correlation coefficient by the initial acquisition accuracy weight of the data source to obtain the weighted correlation coefficient of the data source. Finally, divide the weighted correlation coefficient of the data source by the sum of the weighted correlation coefficients of the three data sources to obtain the real-time confidence coefficient of the data source.

[0047] The more balanced and higher the mean of mutual information between any data source and the other two data sources, the higher the average correlation coefficient of the data source, and the higher the real-time confidence coefficient assigned to the data source under the same initial acquisition accuracy weight. For example, when the normalized correlation coefficient between LiDAR and the visual camera is 0.8, the normalized correlation coefficient between LiDAR and the inertial measurement unit is 0.7, the average correlation coefficient of LiDAR is 0.75, and the initial acquisition accuracy weight of LiDAR is 0.4, then the weighted correlation coefficient of LiDAR is 0.3. When the average correlation coefficient of the visual camera is 0.6 and its initial acquisition accuracy weight is 0.3, its weighted correlation coefficient is 0.18; when the average correlation coefficient of the inertial measurement unit is 0.5 and its initial acquisition accuracy weight is 0.3, its weighted correlation coefficient is 0.15; and the sum of the weighted correlation coefficients of the three is 0.63, then the real-time confidence coefficient of the lidar is approximately 0.476 (0.3 divided by 0.63), the real-time confidence coefficient of the visual camera is approximately 0.286 (0.18 divided by 0.63), and the real-time confidence coefficient of the inertial measurement unit is approximately 0.238 (0.15 divided by 0.63).

[0048] In step five, when dynamically weighting and fusing the three types of heterogeneous data, the pose data output by the LiDAR includes: the robot's position coordinates and heading angle values ​​obtained by the LiDAR through iterative nearest-point algorithm registration of two consecutive frames of point cloud data; the pose data output by the vision camera includes the robot's position coordinates and heading angle values ​​obtained by the vision camera through image feature matching and epipolar geometric constraint solving; and the pose data output by the inertial measurement unit (IMU) includes the robot's position coordinates and heading angle values ​​obtained by the IMU through dead reckoning integration of acceleration and angular velocity. These three pose data are then multiplied by their respective real-time confidence coefficients to obtain three weighted pose values. The real-time confidence coefficients are the dimensionless weight values ​​generated in step four, ranging from zero to one, and the sum of the real-time confidence coefficients of the three data sources is 1.

[0049] The optimal pose value is obtained by summing the three weighted pose values ​​and then dividing by the sum of the three real-time confidence coefficients. Since the sum of the three real-time confidence coefficients is 1, the result of the division operation is the sum of the three weighted pose values. For example, when the pose data output by the LiDAR is position coordinates (1.00 m, 0.50 m, 0.00 m) and heading angle of 0.10 radians, the pose data output by the visual camera is position coordinates (0.98 m, 0.52 m, 0.00 m) and heading angle of 0.12 radians, and the pose data output by the inertial measurement unit is position coordinates (1.02 m, 0.48 m, 0.00 m) and heading angle of 0.09 radians, and the real-time confidence coefficients of the three data sources are... When the coordinates are 0.476, 0.286, and 0.238 respectively, the weighted pose values ​​for the lidar are (0.476 m, 0.238 m, 0.00 m) and the heading angle is 0.0476 radians; for the visual camera, they are (0.280 m, 0.149 m, 0.00 m) and the heading angle is 0.0343 radians; and for the inertial measurement unit, they are (0.243 m, 0.114 m, 0.00 m) and the heading angle is 0.0214 radians. Adding these three weighted pose values ​​yields the position coordinates (0.999 m, 0.501 m, 0.00 m) and the heading angle is 0.1033 radians. This value represents the optimal pose value after weighted averaging.

[0050] The optimal steady-state pose estimate is the optimal pose value after weighted averaging. The optimal steady-state pose estimate and its corresponding covariance information together constitute the steady-state weighted fusion output field. The corresponding covariance information is obtained by weighted summation of the pose covariance matrices of the three heterogeneous data sources, multiplied by the squares of their corresponding real-time confidence coefficients. The pose covariance matrix comes from the error model output of each sensor, and its diagonal elements represent the uncertainty variance of each coordinate axis direction and heading angle. The steady-state weighted fusion output field is a data structure containing the timestamp, the three-dimensional position coordinates and heading angle of the optimal steady-state pose estimate, and the corresponding covariance information. This data structure is used as a benchmark reference value for feedback comparison in subsequent step six.

[0051] In step six, the optimal steady-state pose estimate in the steady-state weighted fusion output field is compared with the original observation data in the environmental disturbance observation matrix at the same time node. The point-by-point difference operation is to subtract the position coordinates and heading angle values ​​in the original observation data in the environmental disturbance observation matrix at the corresponding time from the position coordinates and heading angle values ​​in the optimal steady-state pose estimate. The resulting difference sequence is the residual time series, which reflects the time-varying pattern of the deviation between the fused positioning result and the original sensor observation.

[0052] Time-domain feature extraction is performed on the residual time series to obtain the peak value and mean value of the residual amplitude. The peak value of the residual amplitude is the largest absolute value in the residual time series, and the mean value of the residual amplitude is the quotient obtained by dividing the algebraic sum of all values ​​in the residual time series by the total number of data points. Then, frequency-domain feature extraction is performed on the residual time series, specifically as follows: The residual time series is divided into multiple segments according to a time window. The length of the time window is set as an integer multiple of the sampling frequency to cover at least one complete perturbation period; a typical value is a time window length of 128 sampling points. A Fourier transform is performed on each segment to obtain the spectrum. The Fourier transform converts the segment from a time-domain signal to a frequency-domain signal, outputting the amplitude corresponding to each frequency component. The center frequency value of the energy-concentrated frequency band in the spectrum is extracted. The energy-concentrated frequency band is determined by finding the frequency component with the largest amplitude in the spectrum and expanding outwards from this frequency component until the amplitude decreases to half of the largest amplitude. The expanded frequency band is the energy-concentrated frequency band, and the arithmetic mean of the starting and ending frequencies of the energy-concentrated frequency band is the center frequency value.

[0053] The difference between the peak value and the mean value of the residual amplitude is used as the time-domain excitation value, which represents the difference between the extreme deviation and the average deviation in the residual time series. The center frequency value of the energy concentration band is used as the frequency domain influence factor, which represents the frequency position of the dominant energy concentration region in the residual signal. The product of the time-domain excitation value and the frequency domain influence factor is used as the residual feedback driving quantity.

[0054] For example, suppose that in a certain calculation, the peak value of the residual amplitude is 0.15 meters and the mean value of the residual amplitude is 0.02 meters, then the time-domain excitation value is 0.13 meters. Assuming that the frequency component with the largest amplitude in the spectrum appears at 1.5 Hz, and extending outwards from 1.5 Hz until the amplitude drops to half of the maximum amplitude, the energy concentration band is found to be from 0.8 Hz to 2.2 Hz, with a center frequency of 1.5 Hz. Therefore, the frequency domain influence factor is 1.5 Hz, and the residual feedback drive is 0.13 multiplied by 1.5, which equals 0.195 m / Hertz.

[0055] The residual feedback driving force is applied to the criterion for determining the number of linear independent dimensions in step two, thereby achieving dynamic self-tuning of the criterion. The criterion is the preset richness determination threshold in step two, and the initial value of the preset richness determination threshold is set to 3. The specific method of dynamic self-tuning is as follows: The residual feedback driving quantity is converted into an adjustment step size value using a preset normalized mapping function. This function divides the residual feedback driving quantity by a preset normalized denominator value, which is set based on an estimate of the maximum expected residual feedback driving quantity from empirical data, typically 1.0 MHz. This yields a normalized ratio. When the normalized ratio is greater than 0, the adjustment step size is set to a positive value, equal to the rounded-down normalized ratio. When the normalized ratio is less than 0, the adjustment step size is set to a negative value, equal to the rounded-down normalized ratio. When the normalized ratio is 0, the adjustment step size is 0. The adjustment step size is then added to the initial value of a preset richness judgment threshold; the sum is used as the dynamic judgment threshold for the current moment. The dynamic judgment threshold is limited to a range of 1 to 6. When the sum is less than 1, the value is 1, and when the sum is greater than 6, the value is 6. This is to prevent the judgment benchmark from drifting too much and causing the judgment to become unstable.

[0056] In subsequent step two, the dynamic judgment threshold replaces the initial value of the preset richness judgment threshold, serving as a new benchmark for judging the richness of dynamic disturbance source types in the current environment. Specifically, when the number of linearly independent dimensions of the disturbance is greater than the dynamic judgment threshold, it is judged as high richness; when the number of linearly independent dimensions of the disturbance is less than or equal to the dynamic judgment threshold, it is judged as low richness. For example, when the residual feedback driving quantity is 0.195 MHz and the normalized denominator is 1.0 MHz, the normalization ratio is 0.195, and the rounded value of the normalization ratio is 0, so the adjustment step size is 0, and the dynamic judgment threshold remains 3. When the residual feedback driving quantity is 0.80 MHz, the normalization ratio is 0.80, and the rounded value is 1, so the adjustment step size is 1, and the dynamic judgment threshold is 3 plus 1 equals 4. This means that in subsequent judgments, a high richness judgment is only made when the number of linearly independent dimensions of the disturbance is greater than 4, thereby reducing the system's sensitivity to disturbance richness under high residual conditions. When the residual feedback driving quantity is -0.50 MHz, the normalization ratio is -0.50, which rounds down to -1. Therefore, the adjustment step size is -1, and the dynamic judgment threshold is 3 plus -1, which equals 2. This means that in subsequent judgments, a disturbance with a linearly independent dimension greater than 2 is considered high richness, thus improving the system's sensitivity to disturbance richness under low residual conditions. The self-tuning process is executed once per sampling period, allowing the judgment benchmark to dynamically evolve with the real-time changes in the system's operating state.

[0057] Example 2: A multi-terminal collaborative indoor autonomous inspection robot control system, comprising: The observation matrix construction module is used to collect wireless signal strength, ambient light intensity, and dynamic obstacle position offset in the current inspection environment in real time through multiple types of environmental perception sensors on the robot. The multiple types of environmental perception sensors include at least a signal strength detection sensor, an illuminance sensor, a lidar, a vision camera, and an inertial measurement unit. The collected physical quantities are arranged into multi-dimensional observation vectors according to the time sequence and the robot's spatial pose, and the multi-dimensional observation vectors at multiple consecutive moments are combined into an environmental disturbance observation matrix. The independent dimension calculation module is used to perform linear correlation analysis on the environmental disturbance observation matrix, calculate the rank of the matrix, define the rank of the matrix as the number of linear independent dimensions of the disturbance, and determine the richness of the types of dynamic disturbance sources in the current environment based on the number of linear independent dimensions of the disturbance. The compensation gain adjustment module is used to calculate the dynamic compensation gain coefficient to counteract the dynamic nonlinearity of the environment based on the number of linear independent dimensions of the disturbance and the instantaneous change rate of each physical quantity in the environmental disturbance observation matrix. Based on the dynamic compensation gain coefficient, the noise covariance parameter of the navigation and positioning filter inside the robot and the image enhancement parameter of the vision camera are adaptively adjusted to obtain the original sensing data frame after dynamic compensation correction. The confidence generation module is used to align the three heterogeneous data from lidar, vision camera and inertial measurement unit in the original sensing data frame after dynamic compensation and correction under a unified time reference, and perform a pairwise physical consistency logic check on the three heterogeneous data after alignment, and generate the real-time confidence coefficient corresponding to each data source according to the check pass ratio. The weighted fusion output module is used to dynamically weight and fuse three types of heterogeneous data based on real-time confidence coefficients to generate the optimal steady-state weighted fusion output field at the current moment. The steady-state weighted fusion output field contains the robot's optimal steady-state pose estimate. The feedback self-tuning module is used to perform differential calculations on the steady-state weighted fusion output field and the original observation data in the environmental disturbance observation matrix to obtain the residual feedback driving quantity. The residual feedback driving quantity is then used as feedback input to the determination criterion of the number of linear independent dimensions of disturbance in the independent dimension calculation module, thereby realizing dynamic self-tuning of the determination criterion and forming a full-link closed-loop adaptive control.

[0058] All numerical calculations in the above algorithms or formulas are dimensionless, using only numerical substitution for computation. The values ​​of each preset parameter are derived from statistical analysis and experimental verification of data; the specific values ​​can be adjusted within a certain range according to the actual application scenario.

[0059] It should be understood that in the various embodiments of this application, the order of the above-mentioned processes does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0060] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0061] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the devices and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0062] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A control method for a multi-terminal collaborative indoor autonomous inspection robot, characterized in that, Includes the following steps: Step 1: Collect the environmental disturbance observation matrix: Using multiple types of environmental perception sensors on the robot, collect in real time the wireless signal strength, ambient light intensity, and dynamic obstacle position offset in the current inspection environment. The multiple types of environmental perception sensors include at least a signal strength detection sensor, an illuminance sensor, a lidar, a vision camera, and an inertial measurement unit. Arrange the above-mentioned physical quantities collected in chronological order and robot spatial pose into a multi-dimensional observation vector, and combine the multi-dimensional observation vectors of multiple consecutive moments into an environmental disturbance observation matrix. Step 2, calculate the number of linearly independent disturbances: perform linear correlation analysis on the environmental disturbance observation matrix, calculate the rank of the matrix, define the rank of the matrix as the number of linearly independent disturbances, and judge the richness of the types of dynamic disturbance sources in the current environment based on the number of linearly independent disturbances. Step 3: Calculate the dynamic compensation gain coefficient: Based on the number of linear independent dimensions of the disturbance and the instantaneous change rate of each physical quantity in the environmental disturbance observation matrix, calculate the dynamic compensation gain coefficient to counteract the dynamic nonlinearity of the environment. Based on the dynamic compensation gain coefficient, adaptively adjust the noise covariance parameter of the navigation and positioning filter inside the robot and the image enhancement parameter of the vision camera to obtain the original sensing data frame after dynamic compensation correction. Step 4: Generate confidence coefficients for multi-source data: Align the three heterogeneous data sources from LiDAR, visual camera and inertial measurement unit in the original sensor data frame after dynamic compensation and correction with spatial coordinates under a unified time reference, and perform physical consistency logic verification between each pair of the three heterogeneous data after alignment. Generate the real-time confidence coefficient corresponding to each data source based on the verification pass rate. Step 5: Generate steady-state weighted fusion output field: Based on the real-time confidence coefficient, dynamically weight and fuse the three types of heterogeneous data to generate the optimal steady-state weighted fusion output field at the current moment. The steady-state weighted fusion output field contains the robot's optimal steady-state pose estimate. Step 6, Closed-loop feedback self-tuning: The steady-state weighted fusion output field and the original observation data in the environmental disturbance observation matrix are differentially calculated to obtain the residual feedback driving quantity. The residual feedback driving quantity is then used as the feedback input to act on the judgment criterion of the linear independent dimension of the disturbance in Step 2, thereby realizing the dynamic self-tuning of the judgment criterion.

2. The control method for a multi-terminal collaborative indoor autonomous inspection robot according to claim 1, characterized in that, In step one, the multi-type environmental perception sensors include at least a signal strength detection sensor for acquiring wireless signal strength, a light intensity sensor for acquiring ambient light intensity, a lidar for detecting the positional offset of dynamic obstacles, a visual camera for acquiring visual images, and an inertial measurement unit for acquiring acceleration and angular velocity information.

3. The control method for a multi-terminal collaborative indoor autonomous inspection robot according to claim 2, characterized in that, In step two, the specific method for performing linear correlation analysis on the environmental disturbance observation matrix and calculating the rank of the matrix is ​​as follows: The environmental disturbance observation matrix is ​​transformed into a row echelon form matrix using Gaussian elimination. The number of non-zero rows in the row echelon form matrix is ​​counted, and the number of non-zero rows is used as the rank of the matrix. The rank is then used as the number of linearly independent dimensions of the disturbance.

4. The control method for a multi-terminal collaborative indoor autonomous inspection robot according to claim 3, characterized in that, In step three, the instantaneous rate of change of each physical quantity in the environmental disturbance observation matrix is ​​obtained in the following way: Forward difference calculation is performed on the physical quantity values ​​of each dimension in the continuously acquired multidimensional observation vector. The instantaneous rate of change of each physical quantity is obtained by subtracting the value of the previous time from the value of the current time and then dividing by the sampling time interval. The value of the dynamic compensation gain coefficient is positively correlated with the number of linear independent dimensions of the disturbance and also positively correlated with the absolute value of the instantaneous rate of change.

5. The control method for a multi-terminal collaborative indoor autonomous inspection robot according to claim 4, characterized in that, Step four, the pairwise physical consistency logical verification includes: The difference between the displacement change of the robot measured by the lidar and the displacement change obtained by double integration of the acceleration measured by the inertial measurement unit is calculated. When the absolute value of the difference is less than the preset first pose verification threshold, it is judged as passing. The difference between the robot's displacement change obtained by the visual camera through image feature matching and the robot's displacement change measured by the lidar is calculated. When the absolute value of the difference is less than the preset second pose verification threshold, it is determined to pass. The difference between the displacement change of the robot obtained by the visual camera and the displacement change obtained by the second integration of the acceleration measured by the inertial measurement unit is calculated. When the absolute value of the difference is less than the preset third pose verification threshold, it is judged as passing. The ratio of the number of passes out of the three checks to the total number of checks is the pass rate.

6. The control method for a multi-terminal collaborative indoor autonomous inspection robot according to claim 5, characterized in that, The preset first pose verification threshold, second pose verification threshold, and third pose verification threshold are all dynamically adjusted values, and their values ​​are adjusted in the same direction based on the value of the dynamic compensation gain coefficient and the preset adjustment strategy.

7. The control method for a multi-terminal collaborative indoor autonomous inspection robot according to claim 6, characterized in that, In step four, the method for generating the real-time confidence coefficient for each data source is as follows: The information entropy calculation method is used to perform nonlinear correlation analysis on three types of heterogeneous data after dynamic compensation and correction. The numerical sequences of each data source are divided into multiple intervals according to the numerical range. The frequency of data occurrence in each interval is used as the probability value. Based on the probability value, the information entropy value of each of the three types of heterogeneous data is calculated. The pass rate of all pairwise verification items in each data source is used as the information entropy probability correction factor of the data source. The probability values ​​in each interval of the data source are weighted and attenuated to obtain the corrected probability value. The corrected probability value is used to replace the original probability value to recalculate the corrected information entropy value of the data source. Calculate the joint information entropy value between any two heterogeneous data, add the modified information entropy values ​​of the two data respectively, and then subtract the joint information entropy value of the two data to obtain the mutual information between the two data sources. The three sets of mutual information obtained by combining the three data sources in pairs are normalized into three sets of correlation coefficients with values ​​ranging from zero to one. Based on the initial acquisition accuracy weight of each data source and the three sets of correlation coefficients, the final real-time confidence coefficients of the three heterogeneous data are generated.

8. The control method for a multi-terminal collaborative indoor autonomous inspection robot according to claim 7, characterized in that, In step five, the specific method for dynamically weighting and fusing the three types of heterogeneous data is as follows: The pose data output by the LiDAR, the pose data output by the visual camera, and the pose data output by the inertial measurement unit are multiplied by their respective real-time confidence coefficients to obtain three weighted pose values. The three weighted pose values ​​are added together and divided by the sum of the three real-time confidence coefficients to obtain the optimal pose value after weighted averaging. The optimal pose value after weighted averaging is the optimal steady-state pose estimate. The optimal steady-state pose estimate and its corresponding covariance information together constitute the steady-state weighted fusion output field.

9. A multi-terminal collaborative indoor autonomous inspection robot control method according to claim 8, characterized in that, In step six, the residual feedback driving quantity is obtained through time-frequency joint analysis logic calculation in the migration mechanical vibration analysis: The optimal steady-state pose estimate in the steady-state weighted fusion output field is compared with the original observation data in the environmental disturbance observation matrix at the same time node to obtain the residual time series. Then, time-domain feature extraction is performed to obtain the peak value and mean value of the residual amplitude. Frequency domain features are extracted from the residual time series. The residual time series is divided into multiple segments according to the time window. Fourier transform is performed on each segment to obtain the spectrum. The center frequency value of the energy concentration band in the spectrum is extracted. The difference between the peak value and the mean value of the residual amplitude is used as the time-domain excitation value, the center frequency value of the energy concentration band is used as the frequency-domain influence factor, and the product of the time-domain excitation value and the frequency-domain influence factor is used as the residual feedback driving quantity, which is applied to the determination criterion of the number of linear independent dimensions of the disturbance in step two.

10. A multi-terminal collaborative indoor autonomous inspection robot control system, used to implement the multi-terminal collaborative indoor autonomous inspection robot control method as described in any one of claims 1-9, characterized in that, include: The observation matrix construction module is used to collect wireless signal strength, ambient light intensity, and dynamic obstacle position offset in the current inspection environment in real time through multiple types of environmental perception sensors on the robot. The multiple types of environmental perception sensors include at least a signal strength detection sensor, an illuminance sensor, a lidar, a vision camera, and an inertial measurement unit. The collected physical quantities are arranged into multi-dimensional observation vectors according to the time sequence and the robot's spatial pose, and the multi-dimensional observation vectors at multiple consecutive moments are combined into an environmental disturbance observation matrix. The independent dimension calculation module is used to perform linear correlation analysis on the environmental disturbance observation matrix, calculate the rank of the matrix, define the rank of the matrix as the number of linear independent dimensions of the disturbance, and determine the richness of the types of dynamic disturbance sources in the current environment based on the number of linear independent dimensions of the disturbance. The compensation gain adjustment module is used to calculate the dynamic compensation gain coefficient to counteract the dynamic nonlinearity of the environment based on the number of linear independent dimensions of the disturbance and the instantaneous change rate of each physical quantity in the environmental disturbance observation matrix. Based on the dynamic compensation gain coefficient, the noise covariance parameter of the navigation and positioning filter inside the robot and the image enhancement parameter of the vision camera are adaptively adjusted to obtain the original sensing data frame after dynamic compensation correction. The confidence generation module is used to align the three heterogeneous data from lidar, vision camera and inertial measurement unit in the original sensing data frame after dynamic compensation and correction under a unified time reference, and perform a pairwise physical consistency logic check on the three heterogeneous data after alignment, and generate the real-time confidence coefficient corresponding to each data source according to the check pass ratio. The weighted fusion output module is used to dynamically weight and fuse three types of heterogeneous data based on real-time confidence coefficients to generate the optimal steady-state weighted fusion output field at the current moment. The steady-state weighted fusion output field contains the robot's optimal steady-state pose estimate. The feedback self-tuning module is used to perform differential calculations on the steady-state weighted fusion output field and the original observation data in the environmental disturbance observation matrix to obtain the residual feedback driving quantity. The residual feedback driving quantity is then used as feedback input to the determination criterion of the number of linear independent dimensions of disturbance in the independent dimension calculation module, thereby realizing dynamic self-tuning of the determination criterion and forming a full-link closed-loop adaptive control.

Images