Indoor and outdoor robot positioning method based on multi-sensor fusion

Abstract

The invention relates to the technical field of robot positioning, and discloses an indoor and outdoor robot positioning method based on multi-sensor fusion. According to the method, original data streams generated by multiple sensors in the moving process of the robot are collected, and due to the fact that time of data generation of different sensors is different and is prone to interference, time alignment and filtering processing are carried out on the original data streams to generate a synchronous and denoised sensor data sequence. The motion characteristics of the robot are extracted from the sequence, position change, speed fluctuation, attitude offset and the like are covered, then the time evolution rule of the motion characteristics is analyzed, and the stability index and the environmental fitness of the motion characteristics of the robot are calculated. And according to the stability index and the environmental fitness, multi-sensor data are fused, so that positioning coordinates of the robot are generated. According to the method, the positioning accuracy and the environment adaptability of the robot are effectively improved, and the robot can be stably and accurately positioned in complex indoor and outdoor environments.

Description

Technical Field

[0001] This invention relates to the field of robot positioning technology, specifically to an indoor and outdoor robot positioning method based on multi-sensor fusion. Background Technology

[0002] Existing robot localization technologies widely rely on data fusion from multiple sensors, including GPS, inertial measurement units, LiDAR, and vision sensors. Common fusion algorithms, such as extended Kalman filtering or particle filtering, integrate sensor data from different sources by establishing a fixed kinematic model or environmental observation model to estimate the robot's pose. These methods largely presuppose the noise characteristics of the sensor data and the stability of the environment, using static weights or model parameters for data fusion calculations.

[0003] Static fusion methods of this type suffer a significant drop in positioning accuracy and robustness when faced with changes in indoor / outdoor environments, dynamic obstacles, or drastic changes in the robot's own motion state. Due to the lack of online evaluation of the robot's long-term motion characteristics and environmental changes, fixed-parameter fusion algorithms struggle to adaptively adjust their strategies. When a sensor generates unreliable data due to a sudden environmental change (such as GPS failure caused by moving from outdoors to indoors), the system cannot promptly identify and reduce its weight, potentially leading to divergence in the positioning trajectory. Similarly, sudden changes in the robot's speed or severe posture jitter can cause inaccurate predictions based on fixed models, introducing cumulative errors.

[0004] The drawback of existing technical solutions lies in their forward-open-loop data fusion process, which fails to incorporate the historical evolution of robot motion characteristics as feedback information to optimize the fusion process. The system only processes current sensor readings, ignoring the patterns and trends of motion states (such as position changes, velocity fluctuations, and attitude shifts) over time. This neglect prevents the system from quantifying the stability of the current motion state and assessing the environmental friendliness to various sensors, resulting in a lack of intelligence and adaptability in the fusion strategy and difficulty in maintaining consistently reliable positioning performance in complex and ever-changing indoor and outdoor scenarios. Summary of the Invention

[0005] The purpose of this invention is to provide an indoor and outdoor robot positioning method based on multi-sensor fusion to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention provides an indoor and outdoor robot localization method based on multi-sensor fusion, the method comprising:

[0007] Collect raw data streams generated by multiple sensors during the robot's movement;

[0008] The raw data stream is time-aligned and filtered to produce a synchronized and denoised sensor data sequence;

[0009] Extract robot motion features, including position changes, velocity fluctuations, and posture shifts, from synchronized and denoised sensor data sequences.

[0010] Analyze the temporal evolution of robot motion characteristics and calculate the stability index and environmental adaptability of robot motion characteristics;

[0011] Based on stability indicators and environmental adaptability, multi-sensor data is fused to generate the robot's positioning coordinates.

[0012] Preferably, the raw data stream generated by the robot from multiple sensors during its movement includes the following steps: initializing the inertial measurement unit, global navigation satellite system receiver, lidar, and vision sensor; setting the sampling rate and data format of each sensor; starting a parallel data acquisition thread to continuously read angular velocity and acceleration values ​​from the inertial measurement unit, obtain latitude and longitude coordinates from the global navigation satellite system receiver, acquire point cloud data from the lidar scanning environment, and capture image sequences from the vision sensor; adding a high-precision timestamp to each data packet; and storing the timestamp data stream in a circular buffer.

[0013] Preferably, the time alignment and filtering of the original data stream includes: aligning the data from each sensor to a unified time axis using a linear interpolation method based on the timestamps; applying a mean filter to the aligned inertial measurement unit data to remove impulse noise; smoothing the global navigation satellite system data using a moving average filter; downsampling the lidar point cloud data using a voxel grid filter; and eliminating noise from the visual sensor image sequence using a bilateral filter.

[0014] Preferably, the extraction of robot motion features includes: calculating displacement increments and attitude changes from filtered inertial measurement unit data; extracting position change trajectories from smoothed global navigation satellite system data; extracting surface normal vectors and edge features from downsampled lidar point cloud data; and calculating optical flow vectors and feature point descriptors from denoised image sequences.

[0015] Preferably, the stability index and environmental adaptability of the computation robot motion features include: for displacement increment features, calculating its variance within the sliding time window as a stability index; for position change trajectory features, calculating its autocorrelation coefficient as another stability index; for environmental features, calculating the matching error between the lidar surface normal vector and the pre-stored map as environmental adaptability; and calculating the consistency score of the visual optical flow vector as another environmental adaptability.

[0016] Preferably, the fusion of multi-sensor data based on stability indices and environmental adaptability specifically includes: assigning a dynamic weight to each sensor data source, wherein the higher the stability index of the displacement increment, the greater the weight of the inertial measurement unit data; the higher the autocorrelation coefficient of the position change trajectory, the greater the weight of the global navigation satellite system data; the smaller the lidar matching error and the higher the visual optical flow consistency score, the greater the fusion weight of lidar and visual sensors; and based on the dynamic weights, using a weighted average method to fuse the position estimates obtained from each sensor.

[0017] Preferably, the dynamic weights are calculated as follows: each stability index and environmental adaptability are normalized and input into a predefined weight mapping function. This function is configured to output a higher weight coefficient for the corresponding sensor data source when a certain index indicates high data reliability, and to ensure that the sum of all weight coefficients is 1.

[0018] Preferably, the fusion of multi-sensor data further includes using an extended Kalman filter algorithm to take the weighted averaged position estimate as the observation value, and the displacement increment and attitude change calculated by the inertial measurement unit as the state prediction value, to update the state and generate the optimal robot positioning coordinates.

[0019] Preferably, after generating the robot's positioning coordinates, a positioning optimization step is further included: constructing a graph model containing the robot's pose as nodes, using the positioning coordinates generated by multi-sensor fusion as nodes of the graph model, and using the constraint relationships between pose nodes at different times as edges, smoothing and optimizing the entire motion trajectory through a graph optimization algorithm, and outputting the final optimized robot motion trajectory.

[0020] Preferably, the method further includes an environmental pattern recognition step: based on the environmental adaptability, determining whether the robot is currently in an indoor environment, an open outdoor environment, or an outdoor canyon environment; when identified as an indoor environment, reducing the fusion weight of the global navigation satellite system data; when identified as an open outdoor environment, increasing the fusion weight of the global navigation satellite system data; when identified as an outdoor canyon environment, enabling a point cloud registration algorithm based on lidar point cloud and visual features for assisted positioning.

[0021] Compared with the prior art, the beneficial effects of the present invention are:

[0022] By analyzing the temporal evolution of robot motion characteristics (including position changes, velocity fluctuations, and posture deviations) and calculating stability indices and environmental fitness, a quantitative perception of the robot's motion state and environmental conditions is achieved. Unlike conventional techniques that focus only on instantaneous data, this process captures trend changes in motion patterns, such as whether the motion is stabilizing or becoming unstable, and whether environmental perception conditions are deteriorating or improving. This time-series-based deep analysis enables the system to obtain contextual information beyond instantaneous data, thus providing accurate and reliable decision-making basis for the dynamic adjustment of fusion strategies and enhancing the system's cognitive depth in complex operating scenarios.

[0023] This system dynamically fuses multi-sensor data based on calculated stability indices and environmental adaptability, changing the traditional fixed-weight fusion model. When the stability index indicates smooth robot movement, the fusion algorithm can confidently rely on the motion prediction model; when the index shows severe jitter, the system automatically increases the suppression of inertial sensor noise or increases the weight of environmental observation data. Environmental adaptability directly guides the contribution of different sensors in the fusion process. For example, it assigns higher weights to sensors in outdoor environments with good GPS signals, while automatically switching to a fusion mode dominated by LiDAR and vision when entering indoor environments. This dynamic weight allocation mechanism based on real-time evaluation enables the positioning system to autonomously adapt to changes in the internal and external environment and the uncertainty of its own motion state, improving the accuracy and reliability of positioning results in various scenarios. Attached Figure Description

[0024] Figure 1 This is a schematic diagram illustrating the working principle of the indoor and outdoor robot positioning method based on multi-sensor fusion described in this invention.

[0025] Figure 2 Flowchart of the steps for collecting raw data streams;

[0026] Figure 3 A flowchart illustrating the steps involved in extracting robot motion features;

[0027] Figure 4 A comparison chart of positioning stability during environment switching;

[0028] Figure 5 This is a graph showing the dynamic changes in sensor weights under different environmental modes. Detailed Implementation

[0029] 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 skilled in the art without creative effort are within the scope of protection of the present invention.

[0030] Please see Figure 1 This invention provides an indoor and outdoor robot localization method based on multi-sensor fusion. The method includes: collecting raw data streams generated by multiple sensors during robot movement, which may be asynchronous in time and contain noise. Next, the raw data streams are time-aligned and filtered to generate a synchronized and denoised sensor data sequence, laying the foundation for subsequent feature extraction. From the processed synchronized and denoised sensor data sequence, features characterizing the robot's motion state need to be extracted. These features include, but are not limited to, position changes, velocity fluctuations, and posture deviations. The system analyzes the evolution of these robot motion features over time and calculates a stability index reflecting data reliability and an environmental fitness index reflecting the degree of sensor-environment compatibility. Based on the calculated stability index and environmental fitness index, data from different sensors are intelligently fused to generate the robot's optimal positioning coordinates.

[0031] Example 1: See Figure 2In practical implementation, initializing the inertial measurement unit (IMU), global navigation satellite system (GNSS) receiver, lidar, and vision sensor is the first step. This process involves powering on each sensor module and loading the corresponding driver firmware to bring it into an operational state. Setting the sampling rate and data format of each sensor requires configuration based on the sensor's characteristics and the accuracy requirements of the positioning task. For example, the IMU's sampling rate is typically set to 100 Hz to 1000 Hz to capture high-frequency motion; the GNSS receiver's sampling rate may be set to 1 Hz to 10 Hz; the lidar's sampling rate is consistent with its scanning frequency; and the vision sensor's frame rate is set to 15 to 60 frames per second, depending on ambient lighting and computing resources. Regarding data format, the IMU outputs six-axis or nine-axis data packets, the GNSS receiver outputs NMEA-0183 protocol strings, the lidar outputs a 3D point cloud array, and the vision sensor outputs an RGB or grayscale image matrix. Initiating parallel data acquisition threads is key to leveraging the multi-core processor architecture. The system creates multiple independent threads to manage data inflows from the inertial measurement unit (IMU), GPS receiver, LiDAR, and vision sensor. Angular velocity and acceleration values ​​continuously read from the IMU are buffered at a fixed frequency. Latitude and longitude coordinates acquired from the GPS receiver are received asynchronously via serial port or network interface. Point cloud data acquired from the LiDAR scanning environment is transmitted via Ethernet protocol. Image sequences captured by the vision sensor are retrieved via USB or CameraLink interface. Adding high-precision timestamps to each data packet is fundamental to ensuring time synchronization. These timestamps originate from the system's high-resolution clock or an external GPS clock source, with an accuracy down to the microsecond level. The timestamp data stream is stored in a circular buffer, designed to accommodate data streams ranging from seconds to minutes, preventing data overflow and allowing older data to be overwritten by newer data.

[0032] In practical implementation, time alignment and filtering of the raw data stream are core steps to improve data quality. Based on timestamps, linear interpolation is used to align the data from each sensor to a unified time axis. Linear interpolation calculates estimates of intermediate times from data points with known timestamps, ensuring all sensor data have the same time index. A median filter is applied to the aligned inertial measurement unit (IMU) data to remove impulse noise. The median filter window size can be selected as 3 or 5 data points, effectively suppressing spike noise without significantly smoothing the effective signal. A moving average filter is used to smooth the GNSS data. The window length of the moving average filter is adjusted according to signal fluctuations; for example, a 5-point or 10-point moving average is used to reduce random errors in latitude and longitude coordinates. A voxel grid filter is used to downsample the lidar point cloud data. The voxel grid filter divides the three-dimensional space into uniform voxel grids, replacing all points within each grid with their centroids, thus significantly reducing the data volume while maintaining the point cloud shape. Noise reduction is achieved by using a bilateral filter on the visual sensor image sequence. The bilateral filter considers both spatial proximity and pixel value similarity, smoothing the image while preserving edge information. The filtered image sequence is more conducive to subsequent feature extraction. In some embodiments, the time alignment process may involve more complex interpolation methods, such as spline interpolation, to accommodate non-linear time shifts. Optionally, the filter parameters can be dynamically adjusted based on real-time data quality; for example, the median filter window can be increased when the inertial measurement unit data has high noise.

[0033] It is understandable that the entire data collection and preprocessing process is the foundation of the positioning system. The use of a circular buffer allows for efficient data pipeline operation, while time alignment and filtering together ensure data consistency in subsequent processing steps. In practical implementation, the order of sensor initialization may affect system startup time; therefore, a parallel initialization strategy is usually adopted to shorten the preparation cycle. When setting the sampling rate, a trade-off between data accuracy and computational load must be struck. Too high a sampling rate may lead to data congestion, while too low a sampling rate may result in the loss of critical motion information. The scheduling of parallel data acquisition threads requires real-time support from the operating system to avoid thread blocking and ensure continuous data flow. The accuracy of adding high-precision timestamps directly determines the accuracy of multi-sensor fusion; therefore, a reliable clock source must be used. Linear interpolation methods are commonly used for time alignment due to their simplicity and efficiency, but higher-order interpolation may be required for high-dynamic motion. Median filters have a good suppression effect on impulse noise in inertial measurement units, while moving average filters are suitable for slowly varying signals in GNSS data. The voxel size selection for voxel grid filters needs to be a trade-off between point cloud density and positioning accuracy requirements, and the parameter settings for bilateral filters need to balance noise cancellation and detail preservation. In some embodiments, for resource-constrained systems, simplified filtering algorithms can be used, but this may come at the cost of sacrificing some accuracy. Optionally, a data quality monitoring module can be introduced to trigger re-acquisition or an alarm when data is abnormal. It is understood that the robustness of the preprocessing stage is directly related to the performance of the entire positioning system; therefore, each parameter and algorithm selection must be fully validated.

[0034] Example 2: See Figure 3 Extracting robot motion features from synchronized and denoised sensor data sequences involves deep analysis of multi-source data. In practice, calculating displacement increments and attitude changes from filtered inertial measurement unit (IMU) data is achieved through integration. Integrating the angular velocity measured by the IMU yields the attitude change, typically represented by quaternions or rotation matrices. The displacement increment is obtained by quadratically integrating the acceleration measured by the IMU and subtracting the gravitational acceleration component. Extracting position change trajectories from smoothed Global Navigation Satellite System (GNSS) data directly utilizes its latitude and longitude coordinate sequences. Coordinate transformation converts the latitude and longitude coordinates to points in a local Cartesian coordinate system, and connecting these points forms the position change trajectory. Extracting surface normals and edge features from downsampled LiDAR point cloud data requires point cloud processing algorithms. Surface normals are typically calculated using principal component analysis (PCA) to estimate the normals of points within the local neighborhood of the point cloud. Edge feature extraction is achieved by detecting regions with significant curvature changes in the point cloud. Calculating optical flow vectors and feature point descriptors from denoised image sequences relies on computer vision techniques. Feature point descriptor extraction involves detecting corner or blob features in the image and calculating their descriptors.

[0035] Analyzing the temporal evolution of robot motion features and calculating their stability indices and environmental adaptability is a process of quantifying data reliability. In practice, for displacement increment features, the variance within a sliding time window is calculated as a stability index. The length of the sliding time window is dynamically adjusted according to the robot's motion speed. The variance calculation uses the standard deviation formula to measure the dispersion of the displacement increment sequence within the window. For position change trajectory features, the autocorrelation coefficient is calculated as another stability index. The calculation of the autocorrelation coefficient involves correlation analysis between the position trajectory sequence and itself after time offset; a higher autocorrelation coefficient indicates better temporal continuity of the trajectory. For environmental features, the matching error between the lidar surface normal vector and the pre-stored map is calculated as environmental adaptability. The matching error is calculated by registering the current frame point cloud features with the pre-stored map using an iterative nearest-point algorithm or a normal distribution transformation algorithm, resulting in a mean squared error. The consistency score of the visual optical flow vector is calculated as another environmental adaptability metric. The consistency score is evaluated by statistically analyzing the dispersion of the direction and magnitude of the optical flow vector within the image region; a lower dispersion results in a higher consistency score.

[0036] In some embodiments, the calculation of displacement increments may employ more complex inertial navigation algorithms, such as using direction cosine matrices for coordinate transformation. Optionally, the calculation of attitude changes may incorporate magnetometer data to compensate for gyroscope drift. Extraction of position change trajectories may require consideration of coordinate projection distortion, employing UTM projection or Gauss-Kruger projection for accurate coordinate transformation. Surface normal vector estimation may utilize different neighborhood search strategies, such as radius-based neighborhood search or K-nearest neighbor-based search methods. Edge feature detection may use gradient-based methods or surface shape indexing methods. The calculation of optical flow vectors may incorporate an image pyramid model to improve the estimation capability for large displacements. Feature point descriptor extraction may employ learned descriptors to enhance robustness to illumination variations.

[0037] It is understandable that the completeness of feature extraction directly affects the accuracy of subsequent fusion decisions; therefore, the calculation of each feature needs to fully consider its physical meaning and computational efficiency. The variance calculation of the sliding time window effectively reflects the short-term measurement stability of the inertial measurement unit, while the autocorrelation coefficient evaluates the smoothness of the global navigation satellite system trajectory from the perspective of time series analysis. The lidar point cloud matching error directly reflects the degree of consistency between the current perception environment and prior knowledge, while the visual optical flow consistency score reflects the internal consistency level of visual observation. In some embodiments, the stability index can be calculated using different statistics, such as using mean absolute deviation instead of variance. Optionally, the environmental fitness calculation can combine multiple feature errors for a comprehensive score. The matching error calculation may use a point-to-surface distance metric instead of a point-to-point distance metric to improve accuracy. The consistency score calculation can introduce a weight matrix to assign different importance to the optical flow vectors of different image regions.

[0038] It is understandable that stability indicators and environmental adaptability together constitute a multi-dimensional evaluation system for sensor data reliability. The variance of displacement increment mainly targets the short-term accuracy of the inertial measurement unit, the autocorrelation coefficient of the position change trajectory targets the long-term stability of the global navigation satellite system, the lidar matching error assesses the matching quality of geometric features, and the visual optical flow consistency score assesses the internal consistency of motion features. In practical implementation, the calculation frequency of these indicators needs to be synchronized with the sensor data update rate to ensure real-time performance. The length of the sliding time window needs to be balanced between response speed and smoothing effect; a window that is too short will cause drastic fluctuations in the indicators, while a window that is too long will cause the indicators to lag. The time lag in the calculation of the autocorrelation coefficient needs to be reasonably set according to the robot's motion characteristics; too large or too small a lag will affect the evaluation effect. The calculation of the matching error requires the pre-stored map to have sufficient accuracy and coverage, and the calculation of the consistency score needs to exclude interference from image edge areas and dynamic object areas.

[0039] Optionally, an adaptive threshold mechanism can be introduced into the calculation of environmental adaptability, dynamically adjusting the judgment criteria based on historical data. A quality check can be added to the feature extraction process, triggering re-extraction or the adoption of a backup scheme when feature quality is too low. The calculation of stability indicators can consider performance at different time scales, such as simultaneously calculating the variance values ​​of short-term and long-term windows. In some embodiments, dedicated feature extraction algorithms can be enabled for specific motion modes, such as employing different posture calculation methods when the robot is detected to be rotating. It is understood that feature extraction and reliability assessment are continuous optimization processes, requiring constant adjustment of algorithm parameters and judgment logic based on the actual application scenario.

[0040] See Figure 4This figure illustrates the changes in robot localization error during different environment transitions with and without environmental adaptation mechanisms. The horizontal axis represents time (s), and the vertical axis represents localization error (m). Dashed lines represent no environmental adaptation, and solid lines represent environmental adaptation. The scenarios are, in order, indoor, open outdoor, and outdoor canyon, with accompanying environmental transitions. In the indoor scenario, the peak localization error without environmental adaptation is approximately 2.7m, while with environmental adaptation it stabilizes at around 1.3m. Upon entering the open outdoor environment, the error without environmental adaptation first decreases to 0.3m and then gradually increases, while with environmental adaptation it remains within a low error range of 0.3-0.6m. When switching to the outdoor canyon environment, the error without environmental adaptation spikes to 3.0m and then slowly decreases, while with environmental adaptation it remains within a controllable range of 0.5-1.4m. This figure visually demonstrates the crucial role of environmental adaptation mechanisms in suppressing localization errors and improving robot localization stability during multi-scene transitions, providing quantitative support for the effectiveness of environmental adaptation strategies in indoor and outdoor robot localization methods based on multi-sensor fusion.

[0041] Example 3: In specific implementation, the process of assigning a dynamic weight to each sensor data source is based on real-time calculated reliability indicators. A higher stability index for displacement increments indicates greater reliability of the inertial measurement unit (IMU) data in short-term motion estimation; therefore, a larger weight is assigned to the IMU data. A higher autocorrelation coefficient for the position change trajectory indicates better trajectory smoothness of the Global Navigation Satellite System (GNSS) data; therefore, the weight of the GNSS data is correspondingly increased. A smaller matching error for the lidar and a higher visual optical flow consistency score indicate stronger consistency between the lidar and visual sensors in environmental perception; therefore, a larger fusion weight is assigned to the lidar and visual sensors. The position estimates obtained from each sensor are fused using a weighted average method based on the dynamic weights. The weighted average method multiplies the position estimate provided by each sensor by its corresponding dynamic weight and then sums the results to obtain the fused position estimate. This method is simple, effective, and computationally inexpensive.

[0042] The calculation of dynamic weights requires standardized mathematical processing. In practice, the first step is to normalize the stability indicators and environmental adaptability. Normalization transforms indicators with different dimensions into a numerical range between zero and one. For example, the min-max normalization method can be used to scale each indicator to the zero-one interval. The normalized indicators are then input into a predefined weight mapping function, which is configured to have monotonically increasing characteristics. When the value of a normalized indicator increases, indicating higher data reliability, the weight mapping function outputs a higher weight coefficient corresponding to the sensor data source. The design of the weight mapping function must ensure that the sum of all weight coefficients is always one. This can be achieved by using the softmax function or simple scaling. An exemplary weight mapping function can be expressed as:

[0043]

[0044] Where: symbol Represents the dynamic weighting coefficient assigned to the i-th sensor data source, with the symbol... , These represent the normalized stability index or environmental fitness values ​​corresponding to the i-th and j-th sensor data sources, respectively, with symbols... Represents the total number of sensor data sources participating in the fusion, symbol This represents an adjustable gain factor used to control the sensitivity of the weights to changes in the metric.

[0045] The fusion of multi-sensor data further includes using the Extended Kalman Filter (EKF) algorithm to improve positioning accuracy. In practice, the EKF algorithm introduces the weighted averaged position estimate as the observation value into the filtering system, while simultaneously using the displacement increment and attitude change calculated by the inertial measurement unit (IMU) for state prediction through the system's nonlinear motion model. The prediction step of the EKF algorithm uses IMU data to calculate the state vector and its covariance matrix for the next moment. The update step compares the weighted averaged position observation value with the state prediction value, calculates the Kalman gain, and updates the state estimate and covariance matrix to generate the optimal robot positioning coordinates. The EKF algorithm can effectively handle nonlinear systems and account for various uncertainties.

[0046] In some embodiments, dynamic weight allocation may consider the correlation between sensor data sources; for example, when both LiDAR and vision sensors indicate high reliability, their weights can be adjusted collaboratively. The weighted averaging method may employ a weighted median or other robust estimation method to resist the influence of outliers. Normalization may employ Z-score normalization or other standardization methods to accommodate different index distributions. The weight mapping function may be implemented as a piecewise linear function or a lookup table to simplify computation. The motion model of the extended Kalman filter algorithm may include more complex robot kinematic equations, such as considering slip compensation for wheeled robots. The observation model may directly fuse the raw observations from each sensor rather than a pre-weighted averaged position estimate.

[0047] In some embodiments, gain factor The values ​​can be dynamically adjusted based on the environment type, for example, increasing the weight sensitivity of LiDAR and vision sensors in indoor environments. The normalization process may introduce sliding window statistics to adapt to the time-varying characteristics of the index values. The output of the weight mapping function may be low-pass filtered to smooth drastic changes in weights. The initial value of the covariance matrix of the extended Kalman filter algorithm may be pre-calibrated based on sensor accuracy. The dimension of the state vector may include multiple variables such as position, velocity, and attitude angle. The dynamic weight calculation module can introduce a weight lower bound mechanism to prevent any sensor data source from becoming completely ineffective due to excessively low weights. Before weighted average fusion, a consistency check can be performed on the position estimates of each sensor to eliminate significantly deviating estimates. The extended Kalman filter algorithm can be combined with other filtering algorithms, such as the unscented Kalman filter, to handle highly nonlinear problems. The core of the dynamic weight allocation mechanism is to enable the fusion system to adaptively rely on the most reliable sensor information, thereby improving the robustness of the overall positioning. The design of the weight mapping function directly affects the fusion effect; a smooth and monotonic function can avoid abrupt weight changes. The extended Kalman filter algorithm quantifies and integrates the uncertainty of prediction and observation through optimal estimation theory, and obtains the statistically optimal solution.

[0048] In some embodiments, for multi-sensor systems, weight calculation may employ a hierarchical structure, first assigning weights within similar sensor groups and then performing inter-group fusion. Normalized indices may be weighted and combined to generate a comprehensive reliability index before being input into the weight mapping function. The extended Kalman filter algorithm may be implemented using an error-state Kalman filter to improve numerical stability. The entire fusion process is a closed-loop system; dynamic weights depend on feature indices, which in turn originate from sensor data, and the fusion result affects the robot's motion, thus influencing subsequent sensor data. In practical implementation, the weight update frequency needs to match the sensor data update rate; too high a frequency may lead to weight oscillations, while too low a frequency may result in response lag. Gain factor Careful adjustment is required; excessively large values ​​will make the weights overly sensitive to changes in the metric, while excessively small values ​​will result in slow weight adjustments. The covariance parameter of the extended Kalman filter algorithm needs to accurately model the characteristics of system noise and observation noise to ensure filter convergence.

[0049] Example 4: The localization optimization step performed after generating the robot's localization coordinates is implemented using graph optimization technology. In specific implementation, constructing a graph model containing robot poses as nodes is the basic framework of the optimization process. Robot pose nodes typically contain position coordinates and attitude angle information in three-dimensional space, with each node corresponding to a timestamp. Using the localization coordinates generated by multi-sensor fusion as nodes in the graph model means that the initial value of each node originates from the fusion result output by the extended Kalman filter algorithm. Constraints between pose nodes at different times are added as edges to the graph model. These constraints originate from observation data from various sensors. For example, edges between pose nodes at adjacent times can be composed of odometry constraints based on displacement increments and attitude changes calculated by the inertial measurement unit, while edges between pose nodes at non-adjacent times can be composed of closed-loop detection constraints generated by lidar point cloud matching or visual feature matching. The entire motion trajectory is smoothed and optimized using a graph optimization algorithm. The graph optimization algorithm represents the pose graph as a nonlinear least squares problem, adjusting the state variables of all pose nodes through iterative optimization to minimize the error between the actual relative transformation between nodes and the expected transformation defined by the constraint edges. The final optimized robot trajectory is a continuous path formed by connecting all the optimized pose nodes in chronological order. This trajectory has higher global consistency and smoothness.

[0050] In practical implementation, the construction of the pose graph model requires clearly defining the types of node variables and constraint edges. Node variables are typically represented using six-degree-of-freedom pose, including 3D translation and 3D rotation. Rotation can be represented using quaternions or Lie algebras to avoid gimbal lock problems. The construction of constraint edges requires accurate modeling of the uncertainties in sensor observations. Each constraint edge is associated with an information matrix to represent the reliability weight of the constraint; the larger the information matrix, the higher the weight of the constraint in the optimization. Graph optimization algorithms typically employ iterative algorithms such as the Gauss-Newton method or the Levenberg-Marquardt method. These algorithms require calculating the Jacobian matrix to linearize the error function. The optimization process requires solving large-scale sparse linear equation systems, which can be handled using numerical methods such as Cholisky decomposition or preprocessing conjugate gradient methods. After optimization convergence, the output trajectory not only corrects the accumulated error but also achieves global trajectory alignment through loop closure detection constraints. Refer to Table 1 for commonly used constraint types and their characteristics in pose graph optimization.

[0051] Table 1: Constraint Types and Characteristics in Pose Graph Optimization

[0052] Constraint Type Source of constraints State variable dimension Typical information matrix structure Odometer constraints Inertial measurement unit integrator, wheel encoder 6-DOF relative pose transformation For diagonal matrices, the translation component has higher accuracy than the rotation component. Point cloud matching constraints LiDAR scanning matching algorithm 6-DOF relative pose transformation Full-rank matrices are affected by point cloud quality and matching algorithms. Closed-loop detection constraints Visual bag-of-words model, point cloud feature matching 6-DOF relative pose transformation Diagonal-dominated matrices have higher reliability than odometry-constrained matrices. Global position constraints Global Navigation Satellite System Positioning Results 3-DOF planar coordinates Diagonal matrices have high weights in open environments.

[0053] In some embodiments, graph optimization algorithms can be implemented using open-source optimization libraries such as g2o or GTSAM, which provide efficient sparse matrix operations and optimization solvers. The parameterization of node variables may affect the optimization convergence speed; using Lie group and Lie algebra representations can avoid singularity problems. The information matrix of constraint edges can be obtained through offline calibration or online estimation using a sensor noise model. The size of the optimization problem may increase with the extension of the motion trajectory, requiring the use of sliding window optimization or subgraph construction strategies to control computational complexity. Robust kernel functions can be introduced into the graph optimization process to handle abnormal constraint edges; for example, the Huber kernel or Cauchy kernel can reduce the negative impact of erroneous loop detection on the optimization results. The stopping condition for optimization iteration can be set to the relative error change being less than a threshold or reaching the maximum number of iterations. The initial pose graph can be constructed incrementally, gradually adding nodes and edges during robot motion.

[0054] The core advantage of graph optimization technology lies in its ability to utilize global information from all available constraints to correct local positioning errors, achieving overall trajectory optimization by minimizing the total error of all constraint edges. Odometry constraints ensure short-term trajectory continuity, point cloud matching constraints provide geometric consistency, loop closure detection constraints enable global correction across time periods, and global position constraints anchor the trajectory's position in the absolute coordinate system. Graph optimization algorithms iteratively adjust each pose node, enabling the entire trajectory network to reach optimal configuration while satisfying all constraints. For applications with high real-time requirements, fixed-hysteresis smoothers or incremental smoothing and graphing methods can be used to reduce computational latency while maintaining optimization effectiveness. The optimized trajectory can be fed back to the front-end positioning module as prior information, forming a closed-loop positioning system. When computational resources allow, multiple global optimizations can be performed on the entire motion trajectory to further improve accuracy. The positioning optimization step connects the discrete positioning points obtained from front-end sensor fusion into an optimal trajectory through global optimization. Pose graph models can flexibly incorporate various types of sensor constraints, and graph optimization algorithms can effectively solve large-scale nonlinear optimization problems. The optimized motion trajectory not only smooths the robot's movement path, but more importantly, it significantly reduces accumulated errors through mechanisms such as closed-loop detection, making long-distance navigation possible. In actual deployment, the settings of the optimization parameters need to be carefully adjusted according to the specific robot platform and sensor configuration to achieve a balance between accuracy and computational efficiency.

[0055] Example 5: In specific implementation, the logic for determining whether the robot is currently in an indoor environment, an open outdoor environment, or an outdoor canyon environment based on environmental adaptability is achieved by setting multiple sets of threshold conditions. For example, when the matching error between the LiDAR point cloud and the pre-stored map is continuously higher than threshold A and the visual optical flow consistency score is continuously lower than threshold B, the system determines it to be an indoor environment. When the LiDAR matching error is lower than threshold C and the signal-to-noise ratio of the Global Navigation Satellite System (GNSS) signal is higher than threshold D, the system determines it to be an open outdoor environment. When the LiDAR matching error is at a moderate level and the GNSS signal-to-noise ratio fluctuates periodically, the system determines it to be an outdoor canyon environment. This judgment requires a comprehensive analysis of the combination patterns of multiple sensor indicators rather than a single indicator. The judgment result will trigger a corresponding fusion weight adjustment strategy. When the environment is identified as indoor, the system automatically reduces the fusion weight of the global navigation satellite system data to zero, because the global navigation satellite system signal is usually unavailable in indoor environments. When the environment is identified as outdoor open environment, the system significantly increases the fusion weight of the global navigation satellite system data to make full use of its stable absolute positioning information. When the environment is identified as outdoor canyon environment, the system moderately reduces the weight of the global navigation satellite system data while using a point cloud registration algorithm based on lidar point cloud and visual features for assisted positioning.

[0056] In practical implementation, the point cloud registration algorithm based on LiDAR point clouds and visual features includes a multimodal data alignment process. The algorithm first extracts geometric features such as corners and edges from the LiDAR point cloud data, and simultaneously extracts visual features such as SIFT feature points from the visual sensor image. Then, it establishes a correspondence between LiDAR point cloud features and visual features through feature descriptor matching. Using these correspondences, it calculates the transformation matrix between the LiDAR coordinate system and the visual coordinate system. Finally, it optimizes the transformation matrix using an iterative nearest-point algorithm to achieve accurate registration. This registration result can be used as an independent positioning observation input to the fusion filter, or used to correct the matching process between the LiDAR point cloud and a pre-stored map, thereby providing reliable pose constraints in canyon environments where GNSS signals are obstructed. The environmental pattern recognition module uses a state mechanism to manage the switching of environmental modes. Each mode state is associated with a sensor weight combination and an algorithm enable flag. The state transition conditions are based on time-series analysis of environmental fitness indicators; for example, a mode switch is triggered only after several consecutive samplings meet the conditions to avoid frequent oscillations.

[0057] In some embodiments, environmental pattern recognition may employ machine learning classifiers instead of threshold rules, such as using support vector machines or random forests to automatically classify multidimensional environmental fitness indicators. Threshold conditions may be set considering seasonal variations and weather factors; for example, the signal quality of global navigation satellite systems generally deteriorates during the rainy season, requiring adjustments to the judgment threshold. Point cloud registration algorithms may employ a tightly coupled approach to directly fuse LiDAR point clouds and visual feature points for joint registration, rather than extracting features independently before matching. Mode switching mechanisms may introduce hysteresis intervals to prevent frequent switching in environmental boundary areas. The environmental pattern recognition module can integrate motion characteristics of the inertial measurement unit as auxiliary judgment criteria; for example, in indoor environments, robots typically turn frequently while their straight-line movements are relatively short. The point cloud registration process can incorporate semantic information to improve registration reliability, such as prioritizing the matching of stable features like building facades or road signs. During mode switching, a gradual animation can be used to adjust sensor weights to avoid abrupt changes in positioning results. For mixed environments such as underground parking lot entrances, transition modes can be defined and special fusion strategies can be employed.

[0058] Optionally, the point cloud registration algorithm can employ a multi-scale registration strategy, performing coarse registration followed by fine registration to improve computational efficiency. Environmental pattern recognition can consider historical pattern information; for example, if the system has primarily been in indoor environments in the past, the confidence level of indoor patterns can be increased. Visual feature extraction can adaptively adjust the number of feature points, extracting more feature points in texture-rich areas to improve registration accuracy. Mode switching decisions can incorporate confidence assessment, only executing mode changes when the confidence level exceeds a threshold. The essence of the environmental pattern recognition steps is to enable the positioning system to have environmental awareness, thereby adaptively adjusting the technical approach to adapt to positioning challenges in different scenarios. In indoor environments, due to the failure of the Global Navigation Satellite System (GNSS) signal, the system relies on a relative positioning system composed of LiDAR and visual sensors, maintaining positioning continuity through point cloud registration and visual odometry. In open outdoor environments, the GNSS provides an absolute positioning reference, with LiDAR and visual sensors primarily playing an auxiliary correction role. In outdoor canyon environments, the GNSS signal degrades due to multipath effects, requiring tight coupling registration of LiDAR point clouds and visual features to compensate for the decrease in positioning accuracy.

[0059] Optionally, the environmental pattern recognition module can add a mechanism for handling unknown environmental patterns, employing a conservative default fusion strategy when existing patterns cannot match. The point cloud registration algorithm can fuse inertial measurement unit data to predict the initial registration position, accelerating the convergence process. Pattern recognition results can be visualized for operator monitoring and intervention. For special application scenarios such as tunnel navigation, dedicated environmental patterns can be predefined and corresponding sensor configurations loaded. The sampling frequency of the environmental fitness index may affect the real-time performance of pattern recognition; too high a frequency may lead to excessive computational load, while too low a frequency may cause mode switching delays. The selection of the point cloud registration algorithm may consider computational resource limitations, employing a lightweight registration algorithm on embedded devices. The mode switching threshold may need to be calibrated on-site based on the specific sensor performance. The visual feature extraction algorithm may be optimized for different environments; for example, indoor environments may focus on artificial sign recognition, while outdoor environments may focus on natural feature extraction.

[0060] It is understandable that environmental pattern recognition and adaptive fusion constitute the core closed loop of an intelligent positioning system. The system identifies environmental features by evaluating the quality of sensor data in real time, selects the optimal fusion strategy based on these features, and the fusion result, in turn, influences the robot's motion and environmental perception, forming a virtuous cycle of continuous optimization. This design enables a single positioning system to handle positioning needs in various complex scenarios, from indoors to outdoors, significantly improving the robot's environmental adaptability and task completion rate. In practical applications, the parameters of the environmental pattern recognition algorithm need to undergo extensive scenario testing and optimization to ensure the accuracy and robustness of pattern judgment.

[0061] See Figure 5In a multi-sensor fusion robot localization system, the dynamic adjustment of sensor weights relies on environmental pattern recognition technology. Specifically, the graph uses time (seconds) as the horizontal axis and weight values ​​as the vertical axis, illustrating the weight changes of IMU, GPS, LiDAR, and vision sensors in indoor, open outdoor, and outdoor canyon environments. In indoor environments (0-20 seconds, 80-100 seconds), the GPS weight drops to a very low level (approximately 0.05) because GPS signals are unavailable indoors; the LiDAR weight remains at 0.4, the vision sensor weight is approximately 0.25, and the IMU weight is approximately 0.3, indicating the system relies on the relative positioning capabilities of LiDAR, vision, and IMU. Upon entering an open outdoor environment (20-50 seconds), the GPS weight jumps to 0.6, becoming the core positioning benchmark; the IMU weight drops to 0.1, and the LiDAR and vision weights drop to approximately 0.15 each, fully utilizing the stable absolute positioning characteristics of GPS. When in an outdoor canyon environment (50-80 seconds), the GPS weight drops to 0.2, the LiDAR weight rises to 0.35, the visual sensor weight is about 0.25, and the IMU weight is 0.2. At this time, due to the degradation of GPS signal caused by multipath effect, the system relies on the point cloud registration of LiDAR and vision, visual odometry to assist in positioning, while retaining the motion continuity compensation of IMU.

[0062] This dynamic adjustment of weights is achieved through real-time evaluation of environmental adaptability indicators (such as lidar matching error, GPS signal-to-noise ratio, and visual optical flow consistency), ensuring that the system can select the optimal sensor fusion strategy in different scenarios, thereby improving the robustness and accuracy of positioning.

[0063] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

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

Claims

1. A method for indoor and outdoor robot localization based on multi-sensor fusion, characterized in that, This can be achieved through the following steps: Collect raw data streams generated by multiple sensors during the robot's movement; The raw data stream is time-aligned and filtered to produce a synchronized and denoised sensor data sequence; Extract robot motion features, including position changes, velocity fluctuations, and posture shifts, from synchronized and denoised sensor data sequences. Analyze the temporal evolution of robot motion characteristics and calculate the stability index and environmental adaptability of robot motion characteristics; Based on stability indicators and environmental adaptability, multi-sensor data is fused to generate the robot's positioning coordinates.

2. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 1, characterized in that, The raw data stream generated by the robot during its movement from multiple sensors includes the following steps: initializing the inertial measurement unit, global navigation satellite system receiver, lidar, and vision sensor; setting the sampling rate and data format of each sensor; starting a parallel data acquisition thread to continuously read angular velocity and acceleration values ​​from the inertial measurement unit, obtain latitude and longitude coordinates from the global navigation satellite system receiver, acquire point cloud data from the lidar scanning environment, and capture image sequences from the vision sensor; adding a high-precision timestamp to each data packet; and storing the timestamped data stream in a circular buffer.

3. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 2, characterized in that, The time alignment and filtering of the raw data stream includes: aligning the data from each sensor to a unified time axis using a linear interpolation method based on the timestamps; applying a medium-range filter to the aligned inertial measurement unit data to remove impulse noise; smoothing the global navigation satellite system data using a moving average filter; downsampling the lidar point cloud data using a voxel grid filter; and eliminating noise from the visual sensor image sequences using a bilateral filter.

4. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 3, characterized in that, The extraction of robot motion features includes: calculating displacement increments and attitude changes from filtered inertial measurement unit data; extracting position change trajectories from smoothed global navigation satellite system data; extracting surface normal vectors and edge features from downsampled lidar point cloud data; and calculating optical flow vectors and feature point descriptors from denoised image sequences.

5. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 4, characterized in that, The stability index and environmental fitness of the computational robot motion features include: for displacement increment features, calculating its variance within the sliding time window as a stability index; for position change trajectory features, calculating its autocorrelation coefficient as another stability index; for environmental features, calculating the matching error between the lidar surface normal vector and the pre-stored map as environmental fitness; and calculating the consistency score of the visual optical flow vector as another environmental fitness.

6. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 5, characterized in that, The process of fusing multi-sensor data based on stability indices and environmental adaptability specifically includes: assigning a dynamic weight to each sensor data source, where a higher stability index for displacement increments results in a greater weight for inertial measurement unit data; a higher autocorrelation coefficient for position change trajectories results in a greater weight for global navigation satellite system data; and a smaller lidar matching error and a higher visual optical flow consistency score result in a greater fusion weight between lidar and visual sensors. Based on these dynamic weights, a weighted average method is used to fuse the position estimates obtained from each sensor.

7. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 6, characterized in that, The dynamic weights are calculated as follows: each stability index and environmental adaptability are normalized and input into a predefined weight mapping function. This function is configured to output a higher weight coefficient for the corresponding sensor data source when a certain index indicates high data reliability, and to ensure that the sum of all weight coefficients is 1.

8. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 6, characterized in that, The fusion of multi-sensor data further includes using an extended Kalman filter algorithm to take the weighted averaged position estimate as the observation value, and the displacement increment and attitude change calculated by the inertial measurement unit as the state prediction value, to update the state and generate the optimal robot positioning coordinates.

9. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 8, characterized in that, After generating the robot's positioning coordinates, a positioning optimization step is also included: constructing a graph model containing the robot's pose as nodes, using the positioning coordinates generated by multi-sensor fusion as nodes of the graph model, and using the constraint relationships between pose nodes at different times as edges, smoothing and optimizing the entire motion trajectory through a graph optimization algorithm, and outputting the final optimized robot motion trajectory.

10. The indoor / outdoor robot localization method based on multi-sensor fusion as described in claim 1, characterized in that, The method further includes an environmental pattern recognition step: based on the environmental adaptability, it is determined whether the robot is currently in an indoor environment, an open outdoor environment, or an outdoor canyon environment; when it is identified as an indoor environment, the fusion weight of the global navigation satellite system data is reduced; when it is identified as an open outdoor environment, the fusion weight of the global navigation satellite system data is increased; when it is identified as an outdoor canyon environment, a point cloud registration algorithm based on lidar point cloud and visual features is used for assisted positioning.

Images