Robot multi-sensor fusion positioning method based on GPS-guided particle filter

Abstract

The present invention relates to a robot multi-sensor fusion positioning method based on GPS-guided particle filtering. In a 3D point cloud map, the fusion of differential GPS, laser radar and IMU is realized based on the GPS-guided particle filtering. The method of judging the number of effective GPS particles determines whether to add a small number of new particles near the GPS measurement value, thereby reducing the dependence of the positioning method on the GPS signal. GPS only plays a role in guiding the direction of particle movement when the signal is good. The particle weights are calculated using the measurement data of the laser radar or IMU instead of using the GPS weighting method, thereby avoiding that the particles generated by GPS will obtain higher weights under GPS measurement. When GPS positioning data is unavailable, the added GPS particles will obtain lower weights under laser measurement. At the same time, in order to avoid the problem of the amount of calculation caused by adding GPS particles, the particles are randomly downsampled before the resampling step to ensure that the number of particles is always maintained within a set range, thereby ensuring the real-time performance of the positioning system.

Description

Technical Field

[0001] The invention belongs to the field of outdoor positioning of mobile robots and relates to a robot multi-sensor fusion positioning method based on GPS-guided particle filtering. Background Art

[0002] High-precision positioning of mobile robots in complex outdoor environments is a prerequisite for realizing advanced functions such as inspection, security, and more. Positioning accuracy and stability directly determine whether the robot can safely and stably complete autonomous navigation tasks, which in turn is the foundation for the robot to achieve its desired advanced functions.

[0003] In outdoor environments, current mainstream positioning methods can be divided into map-based and non-map-based methods, depending on whether they use pre-established environmental maps. Non-map-based positioning methods rely on the accuracy of the sensor itself, making it difficult to utilize the texture characteristics of the environment, which is not conducive to the robot's subsequent path planning. Map-based positioning methods can be further divided into single-sensor positioning and multi-sensor fusion positioning. Common positioning methods that rely on a single sensor include: GPS positioning, LiDAR positioning, visual positioning, and inertial navigation positioning technology.

[0004] GPS positioning is a high-precision radio navigation positioning method based on artificial satellites. It is capable of omnidirectional real-time positioning in open outdoor locations. However, when there are numerous obstructions in the environment, multipath effects can lead to relatively large positioning errors, making it unsuitable for scenarios requiring high accuracy. There are two methods for LiDAR positioning. One is based on map matching, which matches a pre-built environmental map with LiDAR measurement data to determine the robot's position within the environment. The accuracy of this positioning method depends on the richness of the environment's texture features. The other method uses inter-frame point cloud data changes to perform dead reckoning to obtain the robot's position. Visual positioning estimates the robot's position by matching image data between camera frames, but this method is significantly affected by ambient lighting. Inertial navigation positioning uses inertial sensors to measure the robot's posture, velocity, acceleration, and other parameters, combined with relevant algorithms, to infer the robot's position. This method can only achieve high-precision positioning for a short period of time, but cannot provide stable, high-precision positioning over a long period of time, making it difficult to use in large outdoor environments.

[0005] Therefore, in complex outdoor scenarios, relying on a single sensor is difficult to provide stable, high-precision positioning. Multi-sensor fusion positioning is currently the mainstream trend in outdoor positioning for mobile robots. GPS and LiDAR have also attracted widespread attention due to their complementary advantages. GPS offers high positioning accuracy in open areas, but low accuracy in areas with dense buildings. LiDAR positioning relies on matching its own measurements with a map of the environment, resulting in high positioning accuracy in scenes with rich environmental features and dense buildings, while GPS offers the opposite. Researchers typically implement multi-sensor data fusion based on Bayesian probability theory, using algorithms such as Kalman filtering and particle filtering. Particle filtering, due to its superior performance in nonlinear and non-Gaussian systems, is widely used in autonomous positioning for mobile robots.

[0006] Among the existing fusion positioning technologies, the literature (Perea-Strom D, Morell A, Toledo J, et al. GNSS Integration in the Localization System of an Autonomous Vehicle Based on Particle Weighting [J]. IEEE Sensors Journal, 2019, PP (99): 1-1.) implements the fusion positioning of lidar and GPS sensors based on the particle filter framework. It obtains the probability distribution of particles under lidar and GPS measurement data respectively, and updates the particle weights by multiplying the probability distributions without generating new particles. This fusion positioning method will affect the overall positioning effect when the positioning data of one of the sensors is poor or unavailable, and the positioning accuracy and robustness cannot be guaranteed.

[0007] The literature (Miguel M, F García, Armingol J M. Improved LiDAR Probabilistic Localization for Autonomous Vehicles Using GNSS [J]. Sensors (Basel, Switzerland), 2020, 20(11).) implements fusion positioning of LiDAR and GPS sensors based on the Adaptive Monte Carlo Positioning (AMCL) framework. It calculates the weights of particles under LiDAR and GPS measurements respectively, combines the two weights in a weighted manner, and finally determines whether to generate GPS particles based on the mean of the particle weights under GPS measurements. The positioning accuracy of this method depends on the weights assigned by the algorithm to LiDAR data and GPS data. At the same time, the generated GPS particles will receive a larger weight in the GPS measurement, resulting in a greater influence of the GPS signal on the positioning accuracy and poor positioning robustness.

[0008] After a comprehensive search, the inventors discovered that most existing outdoor fusion positioning technologies have the following problems: First, positioning methods are mostly implemented in 2D grid maps. In complex outdoor environments, 3D point cloud maps are converted into 2D grid maps through projection, which loses a large amount of environmental information, hindering the matching of lidar data with environmental maps and affecting positioning accuracy. Second, positioning methods are highly dependent on GPS signals. When integrating GPS data, the impact of poor or completely unavailable GPS signals on positioning accuracy is not fully considered, resulting in poor positioning robustness. Third, the inevitable mapping errors during map construction can lead to mismatches between the environmental map and GPS positioning data. Directly mapping GPS data into the map will cause large positioning errors. Summary of the Invention

[0009] Technical problems to be solved

[0010] To overcome the shortcomings of existing technologies, this paper proposes a multi-sensor fusion positioning method for robots based on GPS-guided particle filtering. To enable autonomous robot navigation in complex outdoor scenarios, this paper provides a GPS-guided particle filtering algorithm for multi-sensor fusion positioning within 3D point cloud maps. This algorithm, based on a particle filtering framework, achieves multi-sensor data fusion, combining the advantages of each sensor to achieve high positioning accuracy and strong robustness.

[0011] Technical Solution

[0012] In this invention, the professional terms involved are explained as follows:

[0013] IMU (Inertial Measurement Unit): A device that measures the three-axis attitude angle (or angular rate) and acceleration of an object;

[0014] SLAM (Simultaneous Localization and Mapping): A robot starts moving from an unknown location in an unknown environment, locates itself based on its location and map, and constructs an incremental map based on its own positioning, enabling autonomous positioning and navigation.

[0015] UTM (Universal Transverse Mercator Grid System) coordinates are a type of plane rectangular coordinate system. This coordinate grid system and the projection it is based on have been widely used in topographic maps, as a reference grid for satellite imagery and natural resource databases, and other applications that require precise positioning.

[0016] A robot multi-sensor fusion positioning method based on GPS-guided particle filtering is characterized by the following steps:

[0017] Step 1: Preprocess the sensor data, that is, smooth and filter the IMU data, and convert the latitude and longitude coordinates and yaw angle obtained by differential GPS into the UTM coordinate system;

[0018] Step 2: Remotely control the robot and record the GPS location information at the starting point Construct a GPS odometer and establish the global point cloud map coordinate system (Map system) and the GPS northeast sky coordinate system (GPS system) at the starting point; use the preprocessed IMU data, lidar data, and GPS odometer data as input to the 3D SLAM algorithm, output a global point cloud map, and save the global point cloud map in pcd format. This map is a 3D point cloud map and does not require 2D projection;

[0019] Step 3: Calculate the robot's positioning data in the GPS Northeast Sky Coordinate System:

[0020]

[0021] Where, is the GPS positioning data in the UTM coordinate system at time k;

[0022] Transform the robot's position information in the GPS Northeast Sky coordinate system (GPS) to the global point cloud map coordinate system (Map):

[0023]

[0024] Calculate the GPS data and transform it into the positioning data in the global point cloud map coordinate system, and publish the positioning data. The GPS positioning data used in subsequent steps is this data:

[0025]

[0026] Step 4: Load the constructed global point cloud map and use the positioning data of LiDAR, IMU and GPS to achieve fusion positioning based on the particle filter framework. The specific steps are as follows:

[0027] Step (1) Initialize particles

[0028] Step (2) predicts the state of each particle

[0029] Step (3) Calculate the weight of each particle by obtaining the lidar data and IMU data

[0030] For lidar data, voxel downsampling is used to filter the point cloud data. The filtered point cloud data is matched with the point cloud at each particle pose in the 3D point cloud map using the normal distribution transform (NDT). The score function h1(·) is used to calculate the score of an iterative registration and the particle weight is calculated:

[0031]

[0032] in, is the particle weight at the previous moment, is the measurement probability, z laser refers to the lidar data after voxel downsampling at the current moment, h1(·) refers to the score function of the lidar data after one iteration of NDT registration, Refers to the predicted value of the particle state;

[0033] For IMU data, calculate particle weights:

[0034]

[0035] in, is the particle weight at the previous moment, To measure the probability, Refers to the predicted value of the particle state, z imu is the IMU measurement data at the current moment, h2(·) is the IMU observation equation, Q k+1 is the IMU observation covariance matrix;

[0036] The observation equation h2(·) of the IMU is:

[0037]

[0038] Among them, δ k+1 is the IMU observation noise, δ k+1 The covariance of Q k+1 , Q k+1 Used to measure the magnitude of the IMU measurement error at the current moment;

[0039] Step (4) normalizes the weight of each particle to obtain the normalized weight of each particle

[0040]

[0041] Step (5) resamples the particles, copies the particles with higher weights, removes the particles with lower weights, and obtains the true probability distribution of the robot's position;

[0042] Step (6) Publish the robot's current positioning result Φ k+1 :

[0043]

[0044] Step (7) adds GPS particles and executes step (2) to make the next prediction until the end;

[0045] The weight of the particle after resampling under GPS measurement

[0046]

[0047] Where g(·) is the GPS observation equation:

[0048]

[0049] z k+1 Refers to the GPS measurement data at the current moment, Σ k+1 refers to the covariance matrix of GPS measurement noise, Refers to the state of the i-th particle at the current moment;

[0050] Defined under GPS measurement data, weight The particles are called effective GPS particles, and the percentage of effective GPS particles to the total number of particles is calculated as N g :

[0051]

[0052] Judgment: When the number of effective GPS particles accounts for N g Less than the threshold N th When , add m particles to the particle swarm;

[0053] When the number of effective GPS particles accounts for N g Greater than the threshold N th When , the current particle's position is close to the GPS positioning data, and there is no need to add new particles. Return to (2) for the next prediction;

[0054] The m particles obey N(μ k+1 ,Σ k+1 ), μ k+1 =(x k+1 ,y k+1 ,θ k+1 ) and Σ k+1 They are the current GPS positioning result and positioning covariance matrix, from the normal distribution N(μ k+1 ,Σ k+1 ) randomly generates the m particles:

[0055] m=[(N th -N g )·n],

[0056] Where: [·] is the rounding function, and n is the total number of particles in the particle swarm.

[0057] The global point cloud map coordinate system Map system and the GPS northeast celestial coordinate system gps system are both stationary coordinate systems.

[0058] The global point cloud map coordinate system Map is a right-handed coordinate system, the x-axis is consistent with the direction of the robot's starting point, and the z-axis is upward.

[0059] The GPS northeast celestial coordinate system has an x-axis pointing due east, a y-axis pointing due north, and a z-axis pointing upward.

[0060] The step (1) initializes particles: at the initial moment, k=0, the distribution of particles is initialized using GPS positioning data; the initial position Φ0 provided by the GPS positioning data obeys the normal distribution of N(μ0,Σ0), where μ0=(x0,y0,θ0) and Σ0 are the initial positioning result of GPS and the positioning covariance matrix respectively; according to Sampling from this distribution, the state of each particle By vector Indicates that, is the position of the i-th particle in space at the initial moment, and are the yaw angle, linear velocity, angular velocity and linear acceleration of the i-th particle at the initial moment.

[0061] The step (2) predicts the state of each particle: the state of each particle Substitute into the system state prediction equation Get the predicted value of the particle state at the next moment For each particle Status prediction.

[0062] The step (5) resamples the particles, copies particles with higher weights, removes particles with lower weights, and obtains the true probability distribution of the robot's position:

[0063] Step 1) Count the total number of particles n. When n is greater than the upper threshold value n of the total number of particles, th When , the number of particles is randomly downsampled to reduce the total number of particles;

[0064] Step 2) According to the weight of each particle Calculate the effective number of particles N eff , let N low is the lower limit threshold of the effective particle number; when the effective particle number N eff Greater than N low, execute the next step; when the number of effective particles N eff Lower than N low When , resampling is performed, particles with higher weights are copied, particles with lower weights are eliminated, and the weights of all particles are set to 1 / n, where n is the total number of particles:

[0065]

[0066] The n th Select 1.1 to 1.2 times the total number of particles n.

[0067] The N th Take between 1% and 6%.

[0068] The lower limit threshold N of the effective particle number low Set to 0.5 times the total number of particles n.

[0069] Beneficial effects

[0070] This invention proposes a multi-sensor fusion positioning method for robots based on GPS-guided particle filtering. This method integrates differential GPS, lidar, and IMU sensors within a 3D point cloud map. This method directly loads a pre-constructed 3D point cloud map without converting a 2D grid map, ensuring that map information is not lost and providing a prerequisite for improving positioning accuracy. By determining the number of valid GPS particles, it determines whether to add a small number of new particles near the GPS measurement value, reducing the positioning method's dependence on the GPS signal. GPS only guides particle movement when the signal is good. Particle weights are calculated using lidar or IMU measurement data, rather than using GPS weighting. This prevents GPS-generated particles from receiving higher weights under GPS measurements and mitigates the impact of poor GPS signal data on overall positioning. The lidar measurement method uses an NDT (normal distribution transformation) algorithm to match the current point cloud data with a known point cloud map. Particles are evaluated using registration scores. This algorithm offers advantages such as high speed and insensitivity to initial values. When the GPS positioning data matches the environmental map with a large mismatch, or the GPS signal is poor or even completely unavailable, the added GPS particles will receive a lower weight under laser measurement. These particles will be eliminated through resampling without affecting overall positioning accuracy, thus improving the robustness of the positioning system. Furthermore, to avoid the computational overhead associated with adding GPS particles, the particles are randomly downsampled before the resampling step, ensuring that the particle count remains within the set range and guaranteeing the real-time performance of the positioning system.

[0071] Since the rotation transformation relationship is used in step three to transform the GPS positioning data into the global point cloud map coordinate system, the GPS positioning data can be accurately mapped to the global point cloud map, reducing the negative impact of the matching error between the global point cloud map and the GPS positioning data on the robot positioning and path planning.

[0072] Since the global point cloud map loaded in step 4 is a 3D point cloud map, subsequent positioning is also completed in the 3D point cloud map, avoiding the impact of environmental information lost during the reprojection process to the 2D map on the positioning accuracy.

[0073] Because step 4 (3) uses lidar and IMU data for measurement updates, it avoids the drawback of using weighted calculations of GPS measurement data and laser measurement data in existing fusion methods, which sacrifices positioning accuracy to obtain stable positioning. It fully exploits the matching ability of lidar data and global point cloud maps and the positioning advantages of each sensor. This achieves high-precision and high-robust positioning of the robot in complex outdoor environments.

[0074] Since step 4 (6) uses the percentage of valid GPS particles to the total number of particles to determine whether to add a small number of GPS particles, the positioning accuracy of the lidar is poor in open environments. By adding a small number of particles distributed near the GPS measurement, it can provide guidance for the exploration of the entire particle. The added GPS particles receive a higher weight under the laser measurement, and the particles will gradually transition to the optimal position near the GPS measurement value. This solves the problem of poor lidar positioning in open environments.

[0075] Since the particles are randomly downsampled before resampling in step 4 (5), the number of particles is always maintained within the set range, which avoids the computational complexity caused by adding GPS particles and ensures the real-time performance of the positioning system.

[0076] Since step four uses a multi-sensor fusion positioning framework based on GPS-guided particle filtering, the particle filter measurement update direction is guided by particles distributed in a small proportion near the GPS measurement value, and the measurement update is performed using data from the lidar and IMU. In environments with obvious environmental features and dense buildings, GPS positioning accuracy is lower than that of lidar. Using lidar data to evaluate particle weights and resampling can maximize the positioning advantages of lidar, so that most particles are distributed near the lidar measurement value, and the small number of particles distributed near the GPS measurement value will not adversely affect positioning. In open environments, GPS accuracy is better than lidar, and a small number of particles distributed near the GPS measurement value can provide guidance for particle detection and search. Under the lidar measurement, the weight of particles distributed near the GPS measurement value increases, and the particles will gradually transition to an optimal position near the GPS measurement value, avoiding the situation where particles are trapped in the local optimum due to particle degradation. This avoids the problem of low positioning accuracy and easy trapping of local optimum of particle filters in open scenes. By fully exploiting the positioning advantages of each sensor, using GPS data as guidance and lidar positioning data as the evaluation criteria for particle updates, high-precision and high-robust positioning of mobile robots in complex outdoor environments can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] Figure 1 This is a schematic diagram of the installation of the mobile robot and sensors of the present invention;

[0078] Figure 2 It is the wiring diagram of the positioning system of the present invention;

[0079] Figure 3 This is a schematic diagram of the matching between the GPS northeast sky coordinate system and the global point cloud map coordinate system of the present invention;

[0080] Figure 4 It is a flow chart of the positioning strategy of the present invention; DETAILED DESCRIPTION

[0081] The present invention will now be further described with reference to the embodiments and accompanying drawings:

[0082] A GPS-guided particle filter algorithm for multi-sensor fusion positioning in 3D point cloud maps. Based on a particle filter framework, this algorithm achieves multi-sensor data fusion, combining the advantages of each sensor to achieve high positioning accuracy and strong robustness.

[0083] Step 1: Preprocess the sensor data, smooth and filter the IMU data, and convert the latitude and longitude coordinates and yaw angle obtained by differential GPS into the UTM coordinate system.

[0084] Step 2: Remotely control the robot and record the GPS location information at the starting point Construct a GPS odometry system and establish a global point cloud map coordinate system (Map) and a GPS northeast celestial coordinate system (GPS) at the starting point. Both are stationary coordinate systems. The Map system is a right-handed coordinate system, with the x-axis aligned with the robot's starting point and the z-axis pointing upward. The GPS northeast celestial coordinate system (GPS) has the x-axis pointing due east, the y-axis pointing due north, and the z-axis pointing upward. The preprocessed IMU data, lidar data, and GPS odometry data serve as input to the 3D SLAM algorithm, outputting a global point cloud map. This global point cloud map is saved in pcd format. This map is a 3D point cloud map, eliminating the need for 2D projection.

[0085] Step 3: Calculate the robot's positioning data in the GPS Northeast Sky Coordinate System

[0086]

[0087] Where, is the GPS positioning data in the UTM coordinate system at time k

[0088] Use formula (2) to transform the robot's position information in the GPS northeast sky coordinate system into the global point cloud map coordinate system Map system

[0089]

[0090] Calculate the positioning data of the GPS data transformed into the global point cloud map coordinate system and publish the positioning data. The GPS positioning data used in subsequent steps will be this data.

[0091]

[0092] Step 4: Load the constructed global point cloud map and use the positioning data of LiDAR, IMU and GPS to achieve fusion positioning based on the particle filter framework. The specific steps are as follows:

[0093] (1) Initialize particles

[0094] At the initial moment, k = 0, the distribution of particles is initialized using GPS positioning data. Assume that the initial position Φ0 provided by GPS positioning data obeys the normal distribution of N(μ0,Σ0), where μ0 = (x0, y0, θ0) and Σ0 are the initial positioning result of GPS and the positioning covariance matrix respectively. The state of each particle can be sampled from this distribution according to formula (1). Vector Indicates that, is the position of the i-th particle in space at the initial moment, and are the yaw angle, linear velocity, angular velocity and linear acceleration of the i-th particle at the initial moment.

[0095]

[0096] (2) Predict the state of each particle. Substitute into the system state prediction equation (constant rotation rate and velocity quadratic motion model, CTRV) of formula (2) to obtain the particle state prediction value at the next moment For each particle Status prediction.

[0097]

[0098] (3) Calculate the weight of each particle by acquiring lidar data and IMU data Because LiDAR and IMU data have different frequencies, the present invention first determines the source of the currently acquired measurement data and then calculates particle weights using only that data and the corresponding observation model. This weight calculation method differs from existing fusion methods that use weighted calculations of GPS and laser measurement data.

[0099] If the current moment is LiDAR data, the point cloud data is filtered using voxel downsampling, and the filtered point cloud data is matched with the point cloud at each particle pose in the 3D point cloud map using the normal distribution transform (NDT). The score function h1(·) is used to calculate the score of an iterative registration, and the particle weight is calculated using formula (3):

[0100]

[0101] in, is the particle weight at the previous moment, is the measurement probability, z laser refers to the lidar data after voxel downsampling at the current moment, h1(·) refers to the score function of the lidar data after one iteration of NDT registration, Refers to the predicted value of the particle state.

[0102] If the IMU data is obtained at the current moment, the particle weight is calculated using formula (4):

[0103]

[0104] in, is the particle weight at the previous moment, To measure the probability Refers to the predicted value of the particle state, z imuis the IMU measurement data at the current moment, h2(·) is the IMU observation equation, as shown in formula (5), Q k+1 is the IMU observation covariance matrix.

[0105]

[0106] Among them, δ k+1 is the IMU observation noise, δ k+1 The covariance of Q k+1 , Q k+1 Used to measure the magnitude of the IMU measurement error at the current moment.

[0107] (4) Normalize the weight of each particle to obtain the normalized weight of each particle

[0108]

[0109] (5) Resample the particles, copy the particles with higher weights, remove the particles with lower weights, and obtain the true probability distribution of the robot's position. This step needs to be completed in two steps:

[0110] 1) Count the total number of particles n, when n is greater than the upper threshold value n of the total number of particles th (n th Generally, 1.1 to 1.2 times of the set total number of particles n is selected, and the number of particles is randomly downsampled to reduce the total number of particles.

[0111] 2) According to the weight of each particle Calculate the effective number of particles N eff , let N low is the lower limit of the effective particle number. eff Greater than N low , execute the next step; when the number of effective particles N eff Lower than N low When , resampling is performed, particles with higher weights are copied, particles with lower weights are eliminated, and the weights of all particles are set to 1 / n (n is the total number of particles).

[0112]

[0113] (6) Publish the robot's current positioning result Φ k+1 .

[0114]

[0115] (7) Add GPS particles and execute (2) to make the next prediction until the algorithm ends.

[0116] According to formula (9), the weight of the resampled particle under GPS measurement is calculated: Where g(·) is the GPS observation equation, as shown in Equation (9).

[0117]

[0118]

[0119] Note: k+1 Refers to the GPS measurement data at the current moment, Σ k+1 Refers to the covariance matrix of GPS measurement noise.

[0120] Defined under GPS measurement data, weight The particles are called effective GPS particles (ξ eff The value of can be set reasonably according to the measurement accuracy of GPS). Calculate the percentage of effective GPS particles to the total number of particles N g As shown in formula (11), this judgment method is different from the existing method that calculates the probability of generating new particles by taking the mean of particle weights measured by GPS and randomly generates new particles based on the probability.

[0121]

[0122] When the number of effective GPS particles accounts for N g Less than the threshold N th Time (N th It is recommended to take 1% to 6%), add m particles to the particle swarm (m is calculated by formula (12), where [·] is the rounding function and n is the total number of particles in the particle swarm), and these m particles obey N(μ k+1 ,Σ k+1 ), μ k+1 =(x k+1 ,y k+1 ,θ k+1 ) and Σ k+1 They are the current GPS positioning result and positioning covariance matrix. From the normal distribution N(μ k+1 ,Σ k+1 ) randomly generates the m particles.

[0123] m=[(N th -N g )·n] (12)

[0124] When the number of effective GPS particles accounts for N g Greater than the threshold N th When , it means that the current particle position is close to the GPS positioning data, and there is no need to add new particles. Return to (2) for the next prediction.

[0125] The above steps ensure that a certain proportion of particles in the filter are always distributed near the GPS positioning data. When the robot is in an environment with dense buildings, poor GPS positioning accuracy, and high lidar positioning accuracy, the generated GPS particles below 6% will have low scores under the lidar measurement and will be eliminated after resampling. This will not affect the positioning effect of the overall particles, thus ensuring that the algorithm can fully exploit the positioning effect of the lidar. When the robot is in an open environment with sparse environmental features, the accuracy of GPS positioning will be better than that of the lidar. At this time, the particles distributed near the GPS will also receive higher weights under the lidar measurement, so that the overall particles are quickly guided to the vicinity of the GPS measurement and obtain the optimal positioning result. This eliminates the impact of poor or completely unavailable GPS signals on positioning, reduces the adverse effects of errors in matching GPS positioning data with the environmental map, and ensures the accuracy and robustness of the positioning results. Specific embodiment:

[0127] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and examples. Obviously, the embodiments described are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0128] SS1: The robot mentioned in the present invention is used to implement the multi-sensor fusion positioning method based on guided particle filtering in 3D point cloud map. Figure 1 As shown, it is equipped with a 16-line laser radar, IMU and differential GPS. The installation position of each sensor and the orientation of the sensor coordinate system are shown in Figure 1 Given in.

[0129] SS2: Preprocess the IMU and GPS raw measurement data during robot movement.

[0130] SS21: Preprocess the raw measurement data of the IMU, correct and smooth the IMU data by calling the imu_tools function package in the ROS system, and publish the preprocessed data.

[0131] SS22: Convert the original latitude and longitude coordinates of GPS into coordinates in the UTM coordinate system (with due east as the x-axis and due north as the y-axis). The converted GPS positioning data is z UTM =(x UTM ,y UTM ,θ UTM ), where (x UTM ,y UTM) is the position in the UTM coordinate system, θ UTM is the yaw angle in the UTM coordinate system.

[0132] SS3: Remotely control the robot's movement, subscribe to sensor data, build a global point cloud map of the outdoor environment through a 3D SLAM mapping algorithm, and save the global point cloud map.

[0133] SS31: Before the robot moves, record the GPS position in the UTM coordinate system at the starting point And construct the GPS northeast celestial coordinate system (stationary coordinate system) gps system at the starting point;

[0134] SS32: The current k-time GPS position in the UTM coordinate system The posture at the starting point Make a difference, construct the GPS odometer, and obtain the GPS position in the northeast celestial coordinate system at the previous k moments

[0135] SS33: Uses lidar data, preprocessed IMU data, and GPS odometry data as input to the 3D SLAM algorithm LIO_SAM, outputs a global point cloud map of the outdoor environment, and saves the global point cloud map.

[0136] SS4: Before achieving positioning in the acquired global point cloud map, it is necessary to consider the alignment of GPS data with the global point cloud map and convert the GPS position in the Northeast Sky coordinate system to the global point cloud map coordinate system Map.

[0137] SS41: Reference Figure 3 The diagram of the matching between the GPS's Northeast Sky coordinate system (GPS) and the global point cloud map coordinate system (Map) is shown. The global point cloud map coordinate system (Map) is a right-handed coordinate system. The x-axis is in the same direction as the robot's mapping starting point, and the z-axis is upward. There is no translation relationship between the two coordinate systems. Therefore, the rotation matrix between the global point cloud map coordinate system (Map) and the GPS's Northeast Sky coordinate system (GPS) is used to transform the position coordinates in the GPS system to the Map system.

[0138]

[0139] Where, The position coordinates of GPS at time k in the GPS system are converted to the position coordinates in the Map system.

[0140] SS42: Calculate GPS positioning data converted from GPS system to Map system And publish the pose data.

[0141] SS5: Load the constructed global point cloud map, reference Figure 4 The positioning flowchart shown in the figure is based on the particle filter framework and integrates the lidar, IMU, and GPS sensor data to achieve fusion positioning in a known global point cloud map. The specific steps are as follows:

[0142] SS51: System power-on initialization, powering the computing unit and sensors;

[0143] SS52: Subscribes to pre-processed IMU data, GPS pose data in the map system, and lidar data, and publishes them in the ROS system;

[0144] SS53: Load the constructed global point cloud map;

[0145] SS54: Particle initialization, at time k = 0, the distribution of particles is initialized using the positioning data obtained by GPS. Assume that the initial position Φ0 of the robot provided by the GPS sensor obeys the normal distribution N(μ0,Σ0), where μ0 = (x0, y0, θ0) and Σ0 are the initial positioning data of GPS and the positioning covariance matrix respectively. The state of each particle can be sampled from this distribution according to formula (2). Vector Indicates that, is the position of the i-th particle in space at the initial moment, and are the yaw angle, linear velocity, angular velocity and linear acceleration of the i-th particle respectively.

[0146]

[0147] SS55: Predict the state of each particle. Substitute into the system state prediction equation (constant rotation rate and velocity quadratic motion model, CTRV) of formula (2) to obtain the particle state prediction value at the next moment For each particle Status prediction.

[0148]

[0149] SS56: Calculate the weight of each particle using LiDAR and IMU measurement data, and use the observation model to calculate particle weights. Due to frequency differences between LiDAR and IMU data, the present invention first determines the source of the currently acquired measurement data and then updates the particle weights using only that data and the corresponding observation model. This weight update method differs from existing fusion methods that use weighted updates based on GPS and laser measurement data.

[0150] (1) If the current moment is LiDAR data, first use voxel downsampling to filter the LiDAR point cloud data, then use the normal distribution transform (NDT) to match the filtered point cloud with the 3D point cloud map and calculate the registration score, and use formula (4) to update the particle weight;

[0151]

[0152] in, is the particle weight at the previous moment, z laser refers to the lidar data after voxel downsampling at the current moment, h1(·) refers to the score function of the lidar NDT one-iteration registration, Refers to the predicted value of the particle state.

[0153] (2) If the IMU data is obtained at the current moment, the particle weight is calculated using formula (5):

[0154]

[0155] in, is the particle weight at the previous moment, Refers to the predicted value of the particle state. h2(·) is the observation equation of the IMU, as shown in formula (6), Q k+1 is the IMU observation covariance matrix.

[0156]

[0157] Among them, δ k+1 is the IMU observation noise, δ k+1 The covariance of Q k+1 , Q k+1 Used to measure the magnitude of the IMU measurement error at the current moment.

[0158] SS57: The weight of each particle is normalized using formula (7).

[0159]

[0160] SS58: First count the total number of particles at the current moment n, and determine whether the current total number of particles exceeds the set particle number threshold n th (n th Generally, 1.1 to 1.2 times of the set total number of particles n is selected. If it exceeds, the number of particles is randomly downsampled to reduce the total number of particles. Otherwise, proceed to the next step.

[0161] SS59: Particle resampling. Use formula (8) to calculate the number of valid particles and determine whether the calculated number of valid particles is less than the threshold. If it is less than the set threshold, resampling is performed, copying particles with higher weights and eliminating particles with lower weights. Then, the weights of all particles are set to 1 / n (n is the total number of particles). Otherwise, proceed to the next step.

[0162]

[0163] SS510: Calculate the robot's current positioning result

[0164] SS511: Calculate the weight of the particle under GPS measurement after resampling using formula (9)

[0165]

[0166] Where z k+1 Refers to GPS positioning measurement value, Σ k+1 is the covariance matrix of GPS measurement noise, and g(·) is the GPS observation equation, as shown in Equation (10).

[0167]

[0168] SS512: The weight calculated for each particle With the set threshold ξ eff Compare them and if the value is greater than the threshold, it is determined to be a valid GPS particle. The number of valid GPS particles is counted and their proportion is calculated using formula (11).

[0169]

[0170] SS513: Determine whether the percentage of valid GPS particles is less than 6%. If so, use formula (12) to calculate the number of particles m to be added near the GPS measurement value and randomly generate these particles near the measurement value. Otherwise, go to step SS55 and continue the loop until the positioning algorithm ends.

[0171] m=[(N th -N g )·n] (24)

[0172] This paper proposes a multi-sensor fusion positioning method based on GPS-guided particle filtering in a 3D point cloud map, achieving the integration of differential GPS, lidar, and IMU. This method directly loads a pre-constructed 3D point cloud map without converting a 2D grid map, ensuring that map information is not lost and providing a prerequisite for improving positioning accuracy. By determining the number of valid GPS particles, it determines whether to add a small number of new particles near the GPS measurement value, reducing the positioning method's dependence on the GPS signal. GPS only guides particle movement when the signal is good. Particle weights are calculated using lidar or IMU measurement data, rather than using a weighted method with GPS. This prevents GPS-generated particles from receiving higher weights under GPS measurement and mitigates the impact of poor GPS signal data on overall positioning. The lidar measurement method uses an NDT (normal distribution transformation) algorithm to match the current point cloud data with a known point cloud map. Particles are evaluated using the registration score. This algorithm has the advantages of being fast and insensitive to initial values. When the GPS positioning data matches the environmental map with a large mismatch, or the GPS signal is poor or even completely unavailable, the added GPS particles will receive a lower weight under laser measurement. These particles will be eliminated through resampling without affecting overall positioning accuracy, thus improving the robustness of the positioning system. Furthermore, to avoid the computational overhead associated with adding GPS particles, the particles are randomly downsampled before the resampling step, ensuring that the particle count remains within the set range and guaranteeing the real-time performance of the positioning system.

[0173] In this way, in environments with distinct features and dense buildings, GPS positioning accuracy is lower than that of lidar. Using lidar data to calculate particle weights and resampling maximizes the positioning advantage of lidar, ensuring that the majority of particles are distributed near the lidar measurement, while the small number of particles added near the GPS measurement does not adversely affect positioning. In open environments, GPS accuracy is superior to lidar, and a small number of particles distributed near the GPS measurement can provide guidance for particle detection and search. Under lidar measurements, the weights of particles distributed near the GPS measurement increase, and the particles gradually transition to an optimal position near the GPS measurement, avoiding the situation of being trapped in a local optimum due to particle degradation. This method fully exploits the positioning advantages of each sensor, using GPS data as a guide and lidar positioning data as the evaluation criterion for particle updates, and can achieve high-precision and robust positioning of mobile robots in complex outdoor environments.

Claims

1. A robot multi-sensor fusion positioning method based on GPS guided particle filtering, characterized in that Here are the steps: Step 1: Preprocess the sensor data, that is, smooth and filter the IMU data, and convert the latitude and longitude coordinates and yaw angle obtained by differential GPS into the UTM coordinate system; Step 2: Remotely control the robot and record the GPS location information at the starting point Construct a GPS odometer and establish the global point cloud map coordinate system (Map system) and the GPS northeast sky coordinate system (GPS system) at the starting point; use the preprocessed IMU data, lidar data, and GPS odometer data as input to the 3D SLAM algorithm, output a global point cloud map, and save the global point cloud map in pcd format. This map is a 3D point cloud map and does not require 2D projection; Step 3: Calculate the robot's positioning data in the GPS Northeast Sky Coordinate System: Where, is the GPS positioning data in the UTM coordinate system at time k; Transform the robot's position information in the GPS Northeast Sky coordinate system (GPS) to the global point cloud map coordinate system (Map): Calculate the GPS data and transform it into the positioning data in the global point cloud map coordinate system, and publish the positioning data. The GPS positioning data used in subsequent steps is this data: Step 4: Load the constructed global point cloud map and use the positioning data of LiDAR, IMU and GPS to achieve fusion positioning based on the particle filter framework. The specific steps are as follows: Step (1) Initialize particles Step (2) predicts the state of each particle Step (3) Calculate the weight of each particle by obtaining the lidar data and IMU data For lidar data, voxel downsampling is used to filter the point cloud data. The filtered point cloud data is matched with the point cloud at each particle pose in the 3D point cloud map using the normal distribution transform (NDT). The score function h1(·) is used to calculate the score of an iterative registration and the particle weight is calculated: in, is the particle weight at the previous moment, is the measurement probability, z laser refers to the lidar data after voxel downsampling at the current moment, h1(·) refers to the score function of the lidar data after one iteration of NDT registration, Refers to the predicted value of the particle state; For IMU data, calculate particle weights: in, is the particle weight at the previous moment, To measure the probability, Refers to the predicted value of the particle state, z imu is the IMU measurement data at the current moment, h2(·) is the IMU observation equation, Q k+1 is the IMU observation covariance matrix; The observation equation h2(·) of the IMU is: Among them, δ k+1 is the IMU observation noise, δ k+1 The covariance of Q k+1 , Q k+1 Used to measure the magnitude of the IMU measurement error at the current moment; Step (4) normalizes the weight of each particle to obtain the normalized weight of each particle Step (5) resamples the particles, copies the particles with higher weights, removes the particles with lower weights, and obtains the true probability distribution of the robot's position; Step (6) Publish the robot's current positioning result Φ k+1 : Step (7) adds GPS particles and executes step (2) to make the next prediction until the end; The weight of the particle after resampling under GPS measurement Where g(·) is the GPS observation equation: z k+1 Refers to the GPS measurement data at the current moment, Σ k+1 refers to the covariance matrix of GPS measurement noise, Refers to the state of the i-th particle at the current moment; Defined under GPS measurement data, weight The particles are called effective GPS particles, and the percentage of effective GPS particles to the total number of particles is calculated as N g : Judgment: When the number of effective GPS particles accounts for N g Less than the threshold N th When , add m particles to the particle swarm; When the number of effective GPS particles accounts for N g Greater than the threshold N th When , the current particle's position is close to the GPS positioning data, and there is no need to add new particles. Return to (2) for the next prediction; The m particles obey N(μ k+1 ,Σ k+1 ), μ k+1 =(x k+1 ,y k+1 ,θ k+1 ) and Σ k+1 They are the current GPS positioning result and positioning covariance matrix, from the normal distribution N(μ k+1 ,Σ k+1 ) randomly generates the m particles: m=[(N th -N g )·n], Where: [] is the rounding function, and n is the total number of particles in the particle swarm.

2. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 1 is characterized in that: The global point cloud map coordinate system Map system and the GPS northeast celestial coordinate system gps system are both stationary coordinate systems.

3. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 1 is characterized in that: The global point cloud map coordinate system Map is a right-handed coordinate system, the x-axis is consistent with the direction of the robot's starting point, and the z-axis is upward.

4. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 1 is characterized in that: The GPS northeast celestial coordinate system has an x-axis pointing due east, a y-axis pointing due north, and a z-axis pointing upward.

5. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 1 is characterized in that: The step (1) initializes particles: at the initial moment, k=0, the distribution of particles is initialized using GPS positioning data; the initial position Φ0 provided by the GPS positioning data obeys the normal distribution of N(μ0,Σ0), where μ0=(x0,y0,θ0) and Σ0 are the initial positioning result of GPS and the positioning covariance matrix respectively; according to Sampling from this distribution, the state of each particle By vector Indicates that, is the position of the i-th particle in space at the initial moment, and are the yaw angle, linear velocity, angular velocity and linear acceleration of the i-th particle at the initial moment.

6. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 1, characterized in that: The step (2) predicts the state of each particle: the state of each particle Substitute into the system state prediction equation Get the predicted value of the particle state at the next moment For each particle Status prediction.

7. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 1, characterized in that: The step (5) resamples the particles, copies particles with higher weights, removes particles with lower weights, and obtains the true probability distribution of the robot's position: Step 1) Count the total number of particles n. When n is greater than the upper threshold value n of the total number of particles, th When , the number of particles is randomly downsampled to reduce the total number of particles; Step 2) According to the weight of each particle Calculate the effective number of particles N eff , let N low is the lower limit threshold of the effective particle number; when the effective particle number N eff Greater than N low , execute the next step; when the number of effective particles N eff Lower than N low When , resampling is performed, particles with higher weights are copied, particles with lower weights are eliminated, and the weights of all particles are set to 1 / nn, which is the total number of particles:

8. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 7, characterized in that: The n th Select 1.1 to 1.2 times the total number of particles n.

9. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 1, characterized in that: The N th Take between 1% and 6%.

10. The robot multi-sensor fusion positioning method based on GPS-guided particle filtering according to claim 7, characterized in that: The lower limit threshold N of the effective particle number low Set to 0.5 times the total number of particles n.

Images