Multi-sensor fusion slam and magnetic guide-based combined navigation method and storage medium

Abstract

The application discloses a kind of based on multi-sensor fusion SLAM and magnetic guide combined navigation method and storage medium, comprising the following steps, tunnel includes first creating prior map, mapping algorithm is combined with odometer data, IMU inertial navigation data, depth camera and laser radar to realize, when inspection robot real-time positioning navigation, it is using the way of middle line navigation and prior map combined positioning navigation;Tunnel outside first is according to the inspection task to confirm inspection path, then lays magnetic strip and RFID, and RFID is numbered;When navigation, the magnetic induction device at the bottom of robot is according to the distance that magnetic strip deviates device center to calculate control speed in real time;When detecting that RFID number and target RFID number are consistent, it indicates to reach destination.The application uses magnetic navigation mode outside tunnel, avoids the influence of environmental change on positioning navigation, two kinds of navigation combination use, can effectively avoid the influence of long and narrow similar pipeline and environmental substantial change scene to SLAM.

Description

Technical Field

[0001] This invention relates to the field of intelligent inspection robot technology, specifically to a combined navigation method and storage medium based on multi-sensor fusion SLAM and magnetic guidance. Background Technology

[0002] With the continuous development of modern technology, many industries have formed a trend towards informatization and intelligentization. More and more places are introducing intelligent inspection robots to replace manual inspection of whether equipment is operating normally. This can reduce labor costs, and the robots can continuously perform inspections, improving work efficiency and reducing the risk of problems.

[0003] Currently, inspection robots mainly use LiDAR or depth camera technology for mapping and real-time positioning and navigation. However, LiDAR positioning and registration has its weaknesses, such as degradation in long corridors, sparse point cloud data, and susceptibility to interference from objects such as glass, which can lead to matching errors. Depth cameras, on the other hand, have disadvantages such as high computational load, susceptibility to lighting conditions, and low map accuracy. Therefore, no single technical solution can meet the requirements for use in narrow pipes or in scenarios with drastic environmental changes.

[0004] For example, the inspection tunnels for subway and high-speed rail chassis inspections are generally very long and narrow, with similar surrounding environmental characteristics. The tunnels are adjacent to each other on both sides, and the area between the tunnels is for maintenance personnel and equipment to pass through. However, because the train maintenance time is unpredictable, the train environment and personnel and equipment on both sides of the tunnel change frequently and significantly, which poses a great challenge to traditional laser and visual SLAM.

[0005] In general, intelligent inspection robots are being used in more and more scenarios. However, in some special scenarios, such as narrow and similar pipes, areas with drastic changes in the surrounding environment, and areas with large height differences in the inspection area, traditional navigation methods are difficult to use for normal mapping and positioning navigation, which limits the use of inspection robots.

[0006] Explanation of relevant terms:

[0007] SLAM: Simultaneous localization and mapping;

[0008] ICP: Iterative Closest Point algorithm;

[0009] Odom: odometer;

[0010] IMU: Inertial Measurement Unit;

[0011] EKF: Extended Kalman Filter;

[0012] RFID: Radio Frequency Identification. Summary of the Invention

[0013] The present invention proposes a combined navigation method based on multi-sensor fusion SLAM and magnetic guidance, which can solve the mapping and localization navigation problems in these special scenarios, ensure that the inspection robot can perform inspection work normally, and expand the application scenarios of the robot.

[0014] To achieve the above objectives, the present invention adopts the following technical solution:

[0015] A combined navigation method based on multi-sensor fusion SLAM and magnetic guidance, including combined navigation inside and outside the tunnel, involves the robot switching maps and navigation modes at appropriate locations. The method includes the following steps:

[0016] The process inside the tunnel involves first creating a priori map, which uses a mapping algorithm that combines odometry data, IMU inertial navigation data, depth camera and LiDAR. During inspection, the robot uses real-time positioning and navigation, employing a combination of centerline navigation and priori map positioning and navigation.

[0017] Outside the tunnel, the first step is to determine the inspection route based on the inspection task, then lay magnetic strips and RFID tags, and number the RFID tags.

[0018] During navigation, the magnetic induction device on the bottom of the robot calculates and controls the speed in real time based on the distance of the magnetic strip from the center of the device; when the detected RFID number matches the target RFID number, it indicates that the destination has been reached.

[0019] Furthermore, the prior map creation algorithm combines odometry data, IMU inertial navigation data, depth camera, and LiDAR to achieve the following steps:

[0020] S1.1 Correct the odometry data. The input data includes: odometry data collected by the encoder, IMU inertial navigation data, and visual odometry data calculated in real time by the depth camera. These three types of data are fused and estimated to obtain more accurate odometry data.

[0021] S1.2. The data from the depth camera and the LiDAR are fused and calculated to obtain more accurate point cloud data. The position coordinates of the depth camera and the LiDAR need to be calibrated beforehand. Then, the LiDAR data is filtered and converted to the depth camera coordinate system based on the calibrated coordinates. Then, the data of the two are smoothed to obtain the fused point cloud data.

[0022] S1.3 Perform coarse-to-fine inter-frame matching mapping. Use the more accurate odometry data obtained in step S1.1 to estimate the robot's real-time pose and obtain the robot's current coarse pose. Then, perform ICP matching calculation based on the current frame point cloud data and the current map data to correct the coarse pose and obtain a more accurate robot pose.

[0023] Furthermore, the robot's real-time positioning and navigation during the inspection uses a combination of centerline navigation and prior map positioning and navigation, including the following steps:

[0024] S2.1. The centerline navigation method is adopted to ensure that the robot always walks along the middle of the tunnel and does not hit the side walls of the tunnel; the data of the front depth camera is detected in real time to find the side walls, and the distance of the robot from the side walls and the angle with the parallel line of the wall are calculated. Based on the distance and the angle, the robot is controlled to strictly walk along the middle.

[0025] S2.2 Based on the pre-created prior map, a Monte Carlo localization strategy is adopted, and a particle filter algorithm combining the likelihood domain model and the beam model is used to calculate the robot's real-time posterior pose; the odometry data and point cloud data used for localization calculation are the data optimized in steps S1.1 and S1.2.

[0026] S2.3. Using the centerline navigation control of step S2.1 to move the robot forward and backward, and the calculation of the robot's real-time pose in step 2.2, determine whether the robot has reached the target point and control the robot to stop.

[0027] Furthermore, in step S1.1, the odometry data is corrected. The input data includes: odometry data collected by the encoder, IMU inertial navigation data, and visual odometry data calculated in real time by the depth camera. These three types of data are fused and estimated to obtain more accurate odometry data. Specifically, this includes:

[0028] Step 111. First, multi-sensor data is acquired as the front-end input for robot mapping and localization; odom and IMU data are directly read through the encoder, while the visual odom needs to be offset by ICP point cloud registration based on the visual point clouds of the previous and next frames; X′=R 3x3 X+T 3x1 Where X′ represents the point cloud of the current frame, X represents the point cloud of the previous frame, and R 3x3 T represents the rotation matrix of the point cloud transformation. 3x1 The point cloud transformation translation matrix is ​​represented by the rotation matrix and the translation matrix is ​​constrained in a plane to obtain the visual odometry (odom).

[0029] in This represents the x-coordinate, y-coordinate, and angle measurement values ​​of the odometer, (ρ imu () represents the measurement value of the IMU. This represents the calculated x-coordinate, y-coordinate, and angle values ​​of the visual odometry. This indicates the odometer readings after fusion;

[0030] Step 112. Perform EKF fusion calculation on odom, IMU, and visual odom.

[0031] The nonlinear motion and observation model is as follows:

[0032] x k =f(x) k-1 v k )+w k (1)

[0033] y k =g(x k )+n k (2)

[0034] x k For state variables, y k For the measurement, v k As control variables, both f() and g() are nonlinear functions. Assume x k The confidence function is a Gaussian distribution:

[0035]

[0036] Assumption: w k and n k Both are white noise, and f() and g() are differentiable.

[0037] w k ~N(0, Q) k )

[0038] n k ~N(0, R) k (4)

[0039] in v is the prior predicted state variable at time 0. 1：k Let y be the control quantity from time 1 to time k. 0：k The measurement quantity is from time 0 to time k. Let be the posterior state estimate at time k. Let be the covariance estimate at time k, and N be the mathematical representation of the Gaussian distribution, representing the expected value. covariance is Gaussian white noise, Qk is the state equation white noise covariance, R k For the white noise covariance of the observation equation, the entire expression represents wk The expected value is 0 and the covariance is Q. k Gaussian white noise, n k The expected value is 0 and the covariance is R. k Gaussian white noise;

[0040] Linearize the motion and observation models by performing a first-order Taylor expansion of f() and g();

[0041] Taylor expansion of the equations of motion:

[0042]

[0043] Taylor expansion of the observation equation:

[0044]

[0045] in:

[0046] w′ k For the first-order Taylor expansion of the motion white noise, F k-1 Let f() be the Jacobian matrix of the motion function f(). Let be the prior state quantity at time k. Let be the posterior state estimate at time k-1. Let k be the prior observations. Let G be the prior state variable at time k. k Let n′ be the Jacobian matrix of the observation equation g(). k The observational white noise is a first-order Taylor expansion;

[0047] The EKF algorithm is generally divided into two steps: prediction and update.

[0048] predict:

[0049]

[0050]

[0051] renew

[0052]

[0053]

[0054]

[0055] in Let be the prior covariance at time k. Let the posterior estimate of the covariance be at time k-1. Let Q′ be the transpose of the Jacobian matrix of the motion function at time k-1.k K is the covariance of the error between the actual and predicted values ​​of the state variables. k Let K be the Kalman gain matrix at time k. Let R′ be the transpose of the Jacobian matrix of the observation equation at time k. k Let k be the covariance of the error between the actual and predicted values ​​of the measured quantity. Let be the posterior estimated covariance at time k;

[0056] Prediction is based on updating the mean and variance according to the motion equation using the current posterior value. The update, in turn, corrects the mean and variance based on observation information, thereby estimating accurate pose data. The classic EKF recursive update equation is used to estimate the pose data. Calculate

[0057] Using Odom data as the observation and IMU data as the measurement, an EKF fusion is performed in the robot's Odom coordinate system to ensure the continuity of the fused data. Then, visual Odom data is used as the observation, and the IMU and Odom fused data are used as the measurement to perform a second EKF fusion in the global map coordinate system, thereby obtaining the global fused pose.

[0058] Further, in step S1.2, the data from the depth camera and the LiDAR are fused to obtain more accurate point cloud data. Beforehand, the position coordinates of the depth camera and the LiDAR are calibrated. Then, based on the calibrated coordinates, the LiDAR data is filtered and converted to the depth camera coordinate system. Finally, the data from both are smoothed to obtain the fused point cloud data, specifically including:

[0059] Step 121: Based on the layout of the LiDAR and depth camera on the robot, use a calibration algorithm to perform calibration and obtain the accurate coordinate transformation T1 = {x, y, z, roll, pitch, yaw} of the LiDAR and depth camera, where x represents the distance along the x-axis, y represents the distance along the y-axis, z represents the distance along the z-axis, roll represents the rotation angle around the x-axis, pitch represents the rotation angle around the y-axis, and yaw represents the rotation angle around the z-axis.

[0060] Step 122: During real-time mapping and localization, the LiDAR data is filtered. The filtering rule is that the range should not exceed the detection range of the depth camera, resulting in filtered LiDAR data; ∪ lidar = <P1|P2|...|P n >, where P i ={x i y i , z i} represents a specific point cloud, and xi <Max x y i <Max y , z i <Max z U lidar P represents the lidar data set. i Represents a specific point cloud, x i Represents the x-axis coordinates and y-axis coordinates of the point cloud. i Represents the y-axis coordinates and z-axis coordinates of the point cloud. i Represents the z-axis coordinate of the point cloud, Max x Max represents the maximum detection range along the x-axis of the depth camera. y Max represents the maximum detection range along the y-axis of the depth camera. z This indicates the maximum detection distance along the z-axis of the depth camera;

[0061] Step 123: Transform the LiDAR data to the depth camera coordinate system according to the coordinate transformation between sensors; U′ lidar =U lidar *T1; where U′ lidar Represents the laser point cloud in the depth camera coordinate system;

[0062] Step 1 24. Compare the depth values ​​of the LiDAR data and the depth camera data, perform least squares, smooth the depth of nearby points, and correct the depth camera data to obtain more accurate point cloud data;

[0063] U final =f(U l ′ idar U vision ), where U final U represents the final, more accurate point cloud. vision This represents depth camera data, and f() represents the smoothing function.

[0064] Further, in step S1.3, coarse-to-fine inter-frame matching mapping is performed. The more accurate odometry data obtained in step S1.1 is used to predict the robot's real-time pose to obtain the robot's current coarse pose. Then, ICP matching calculation is performed based on the current frame point cloud data and the current map data to correct the coarse pose and obtain a more accurate robot pose. Specifically, this includes:

[0065] Using the precise ODOM data obtained after EKF multi-sensor fusion, the robot pose is pre-estimated to obtain the robot's current coarse localization coordinates. Then, based on the current frame point cloud data and the current map, ICP matching calculation is performed to obtain the pose deviation. The coarse localization coordinates are then precisely corrected to obtain a more accurate robot pose.

[0066] The fused, precise point cloud data is added to the point cloud map using Q. k This represents the point cloud map established at time k. This indicates that at time k+1, the current frame has been transformed to the world coordinate system. This represents the robot's pose in the world coordinate system at time k. This represents the robot's pose increment at time k+1, which is also the output during the coarse matching process; the fine matching steps are as follows:

[0067] Step 131, Q k Using KD-tree description, for Each laser point in the search is located in Q. k Establish a pairing based on the nearest points in the data;

[0068] Step 132: Establish nonlinear equations with the goal of minimizing the distance between all pairs;

[0069] Step 133: Solve the nonlinear optimization problem using the LM method, and solve for T. opt = (R, t);

[0070] Step 134, then the robot pose at time k+1 is:

[0071] Furthermore, S2.1 employs centerline navigation to ensure the robot always travels along the center of the tunnel without encountering the side walls; it monitors data from the front depth camera in real time, identifies the side walls, and calculates the robot's distance from the side walls and the angle between the robot and the parallel lines of the walls. Based on the distance and angle, it controls the robot to strictly travel along the center; specifically including:

[0072] Step 211. The depth camera detects the pillars or walls on both sides in real time, and finds and extracts the edge information of the left and right sides of the tunnel;

[0073] Step 212. Calculate the coordinate information of the left and right sides, and perform offset calculation with the robot's current posture to obtain the distance difference dist between the robot and the left and right sides, and the angle θ between the robot and the center line;

[0074] Step 213. Then, based on the distance difference dist and the included angle θ, the robot's control speed (v, w) is calculated using a PID control algorithm;

[0075] e(t) = (dist t θ t ), where e t This represents the PID control input, namely the distance difference and the included angle.

[0076] u(t)=(vt w t ), where u(t) represents the output robot speed control quantity.

[0077] Where K represents the proportionality coefficient, and T i T represents the integral coefficient. d This represents the differential coefficient.

[0078] Furthermore, in step S2.2, based on a pre-created prior map, a Monte Carlo localization strategy is employed, using a particle filter algorithm combining a likelihood domain model and a beamforming model to calculate the robot's real-time posterior pose; the odometry data and point cloud data used for localization calculations are the optimized data from steps S1.1 and S1.2; specifically including:

[0079] The SLAM process can be summarized by mathematical formulas as motion equations and observation equations. The SLAM process is to perform optimal state estimation on the two equations.

[0080]

[0081] Equation of motion: The pose x at the current moment is estimated by using the x at the previous moment, the value u of the current motion measurement, and the noise w.

[0082] Observation equation: The observation data z is obtained by using the current pose x, the landmark point y, and the noise.

[0083] For the observation equation h(), there is a corresponding observation model, and the complete definition of the observation model is p(z). t |x t ,m), where m represents the environmental map. In mobile robots, the sensors used are ranging sensors, including ultrasonic and laser sensors.

[0084] A combination of beam model and likelihood domain model was used for particle filtering calculation;

[0085] Step 221. First, the odometer obtained by fusing multiple sensors is used as the input to the motion model to update the pose of all particles. U represents the pose of particle m at time k. k This represents the odometer noise at time k;

[0086] Step 222. Then load the fused point cloud data as input to the particle filter observation model;

[0087] Step 223. Use the updated particle and point cloud data to perform beam model particle filter estimation;

[0088] The beam model employs four types of measurement errors: errors caused by obstacles, errors caused by unexpected objects, random measurement errors, and errors caused by undetected objects. The beam model is a mixture of these four types of errors. The model considers what happens to the laser along this ray, and the expected value is also found along this ray. The probability is then calculated using the actual and expected values. The calculation uses a ray projection function. The model can then be expressed as the following equation based on the four types of measurement errors.

[0089]

[0090] Where α hit α represents the variance of the error caused by obstacles. unexp α represents the variance of the error caused by unexpected objects. max α represents the variance of the error caused by the failure to detect an object. rand P represents the variance of random measurement error. hit P represents the obstacle error observation model. unexp P represents the unexpected object error observation model. max This indicates that no object error observation model was detected, P rand Represent the observation model of random measurement error; obtain the particle weights W. k，m =ηP(z|x,m); where η is the normalization coefficient;

[0091] Step 224. Use the updated particles to perform particle filter estimation for the likelihood domain model;

[0092] The likelihood domain model maps the endpoint Zt of the sensor scan onto a grid map, calculates the distance between it and the nearest obstacle on the grid map, and then calculates the probability based on this distance. The expected value is the obstacle closest to the endpoint of the ray. The calculation uses a nearest neighbor function. First, the obstacle points detected by the sensor must be mapped onto a known map.

[0093]

[0094] Where x t =(xy θ) T It is the robot's pose state, (x k，sens y k，sens) T θ represents the local coordinate position of the installed sensor. k，sens This represents the deflection angle of the sensor beam relative to the robot's heading angle. Of course, this process requires the sensor to actually detect the obstacle; that is, the measured value cannot be the maximum value, so the maximum value measurement result is discarded. We first assume that the sensor only has measurement error, and represent this error with Gaussian noise, then calculate the obstacle point corresponding to the sensor data. Next, find the nearest obstacle on the map and denote the point as dist. The distance to the nearest obstacle is used to determine the sensor's measurement probability, which is represented by a Gaussian function with a mean of 0.

[0095]

[0096] Likelihood domain model particle weights Where η is the normalization coefficient;

[0097] Step 225. Based on the two model algorithms, particle filtering is performed on the real-time point cloud data frame to obtain their respective weights and poses. Then, weighted calculation is performed based on their respective weights to obtain the final pose Pk.

[0098] Step 226. Based on particle weights and poses, resample all particles, remove particles with lower weights, and re-sprinkle with new particles. Repeat the above operation; {P k，m W k，m}——>{P′ k，m , W′ k，m}, where P k，m W represents the pose of particle m at time k. k，m P′ represents the weight of particle m at time k. k，m W′ represents the pose of particle m after resampling at time k. k，m This represents the weight of the m particles after resampling at time k.

[0099] Furthermore, outside the tunnel, the inspection path is first confirmed according to the inspection task, then magnetic strips and RFID tags are laid and numbered; during navigation, the magnetic induction device on the bottom of the robot calculates and controls the speed in real time based on the distance of the magnetic strip from the center of the device; when the detected RFID tag number matches the target RFID tag number, it indicates that the destination has been reached, specifically including:

[0100] Step 31. Use magnetic navigation outside the pipeline. Stick magnetic strips on the ground near the inspection route and attach RFID carriers to the magnetic strips as parking points. Number the RFID carriers.

[0101] Step 32. The distance (dist) between the robot's center and the magnetic strip is detected in real time using the magnetic induction device on the bottom of the robot;

[0102] Step 33. Calculate the linear velocity v and angular velocity w of the magnetic strip based on its width d;

[0103] The calculation formula is as follows:

[0104] factor = dist / d, where factor represents the proportionality factor of dist's deviation from the center of the magnetic strip;

[0105] w t =v t-1 *(2 factor -1), v t =v t-1 *abs(1-factor)

[0106] Where w t v represents the angular velocity at time t. t Represents the linear velocity at time t;

[0107] Step 34. Control the robot to move according to the speed calculated in the previous step;

[0108] Step 35. Detect the current RFID number in real time and determine whether it is the same as the target RFID;

[0109] Step 36. Once the target number is detected, stop the vehicle and begin the task inspection.

[0110] In another aspect, the present invention also discloses a computer-readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the steps of the method described above.

[0111] As can be seen from the above technical solution, the combined navigation method based on multi-sensor fusion SLAM and magnetic navigation of the present invention is specifically a method based on multi-sensor fusion SLAM and magnetic navigation. Multi-sensor fusion SLAM is used in tunnels, combined with centerline control and prior map positioning and navigation to solve problems such as laser degradation in narrow corridors. Magnetic navigation is used outside the tunnel to avoid the impact of environmental changes on positioning and navigation. The combination of the two navigation methods can effectively avoid the impact of narrow similar pipes and scenes with large environmental changes on SLAM. Attached Figure Description

[0112] Figure 1 This is a flowchart of the method of the present invention;

[0113] Figure 2 This is a schematic diagram of multi-sensor fusion based on EKF in an embodiment of the present invention;

[0114] Figure 3 This is a schematic diagram of the coarse-to-fine inter-frame matching algorithm according to an embodiment of the present invention;

[0115] Figure 4 This is a schematic diagram of line control in an embodiment of the present invention. Detailed Implementation

[0116] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, 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 some embodiments of the present invention, but not all embodiments.

[0117] The robot has the following types of sensors: odom odometry, IMU inertial navigation, depth camera, and LiDAR. Among them, odom encoder and IMU are low-noise, high-drift sensors, especially inertial measurement units. Although the transient measurement values ​​are very accurate, the position data drifts significantly over time. The depth camera can calculate visual odometry by offset between adjacent frames of data. Compared with traditional odometry, it is not affected by wheel slippage, but due to the computational burden, its frequency and accuracy cannot reach the level of traditional odometry.

[0118] When mapping and localizing, data from LiDAR and depth cameras can be used. Given that LiDAR data is relatively accurate but the point cloud is relatively sparse, while depth camera data has a limited detection range and relatively poor accuracy but relatively dense point cloud features, the data from the two sensors can be fused and used to correct the data, thereby obtaining more accurate feature data and ensuring the accuracy of mapping and localization.

[0119] like Figure 1 The entire process of the present invention shown includes a combination of navigation inside and outside the tunnel, and the robot will switch maps and navigation modes at the corresponding locations.

[0120] The tunnel includes the following steps:

[0121] First, the prior map is created. The mapping algorithm combines odometry data, IMU inertial navigation data, depth camera and LiDAR.

[0122] 1.1 Correct the odometry data. The input data includes odometry data collected by the encoder, IMU inertial navigation data, and visual odometry data calculated in real time by the depth camera. These three types of data are fused and estimated to obtain more accurate odometry data.

[0123] 1.2 The data from the depth camera and LiDAR are fused to obtain more accurate point cloud data. The position coordinates of the depth camera and LiDAR must be calibrated beforehand. Then, the LiDAR data is filtered and converted to the depth camera coordinate system based on the calibrated coordinates. Finally, the data from both are smoothed to obtain the fused point cloud data.

[0124] 1.3 Perform coarse-to-fine inter-frame matching mapping. Use the more accurate odometry data obtained in 1.1 to predict the robot's real-time pose and obtain the robot's current coarse pose. Then, perform ICP matching calculation based on the current frame point cloud data and the current map data to correct the coarse pose and obtain a more accurate robot pose.

[0125] II. During inspections, the robot uses real-time positioning and navigation, employing a combination of centerline navigation and prior map positioning and navigation.

[0126] 2.1 The centerline navigation method is adopted to ensure that the robot always walks along the middle of the tunnel and does not collide with the side walls of the tunnel; the data of the front depth camera is detected in real time to find the side walls, and the distance of the robot from the side walls and the angle with the parallel line of the wall are calculated. Based on the distance and the angle, the robot is controlled to strictly walk along the middle.

[0127] 2.2 Based on the prior map created in section (I), a Monte Carlo localization strategy is adopted, and a particle filter algorithm combining the likelihood domain model and the beamforming model is used to calculate the robot's real-time posterior pose. The odometry data and point cloud data used for localization calculation are the optimized data from sections 1.1 and 1.2.

[0128] 2.3 Combined localization and navigation logic: 2.1 Centerline navigation controls the robot's forward and backward movement; 2.2 Calculates the robot's real-time pose to determine if the robot has reached the target point and controls the robot to stop.

[0129] The following steps are included outside the tunnel:

[0130] The first step is to lay out magnetic strips and RFID tags. Based on the inspection task, the inspection route is determined, and then magnetic strips and RFID tags are laid out and the RFID tags are numbered.

[0131] During navigation, the magnetic induction device on the bottom of the robot calculates and controls the speed in real time based on the distance of the magnetic strip from the center of the device; when the detected RFID number matches the target RFID number, it indicates that the destination has been reached;

[0132] The following describes in detail the combined navigation method based on multi-sensor fusion SLAM and magnetic permeability described in this embodiment, which includes the following steps:

[0133] First, the creation of the map inside the pipeline.

[0134] 1.1 One method is multi-sensor fusion based on EKF, such as... Figure 2 As shown;

[0135] Given the advantages and disadvantages of the various sensors mentioned above, traditional odometry data, IMU data, and visual odometry data can be fused to obtain more accurate odometry data.

[0136] Step 111. The first step is to acquire multi-sensor data, which serves as the front-end input for robot mapping and localization. ODOM and IMU data can be directly read from the encoder. The visual ODOM requires ICP point cloud registration based on two consecutive visual point clouds to obtain its offset. X′=R 3x3 X+T 3x1 Where X′ represents the point cloud of the current frame, X represents the point cloud of the previous frame, and R 3x3 T represents the rotation matrix of the point cloud transformation. 3x1 The point cloud transformation translation matrix is ​​represented by the rotation matrix and the translation matrix is ​​constrained in a plane to obtain the visual odometry (odom).

[0137] in This represents the x-coordinate, y-coordinate, and angle measurement values ​​of the odometer, (ρ imu () represents the measurement value of the IMU. This represents the calculated x-coordinate, y-coordinate, and angle values ​​of the visual odometry. This indicates the odometer readings after fusion;

[0138] Step 112. Perform EKF fusion calculation on odom, IMU, and visual odom.

[0139] In estimation theory, the Extended Kalman Filter (EKF) is a recursive filter estimation algorithm derived from the Hidden Markov Model (HMM). It makes Gaussian assumptions about confidence and noise and linearizes the motion and observation models. For some mildly nonlinear non-Gaussian systems, the EKF can provide good results.

[0140] The nonlinear motion and observation model is as follows:

[0141] x k =f(x) k-1 v k )+w k (1)

[0142] y k =g(x k )+n k (2)

[0143] x k For state variables, y k For the measurement, v k As control variables, both f() and g() are nonlinear functions. Assume x k The confidence function is a Gaussian distribution:

[0144]

[0145] Assumption: w k and nk Both are white noise, and f() and g() are differentiable.

[0146] w k ~N(0, Q) k )

[0147] n k ~N(0, R) k (4)

[0148] in v is the prior predicted state variable at time 0. 1：k Let y be the control quantity from time 1 to time k. 0：k The measurement quantity is from time 0 to time k. Let be the posterior state estimate at time k. Let be the covariance estimate at time k, and N be the mathematical representation of the Gaussian distribution, representing the expected value. covariance is Gaussian white noise, Q k Let R be the white noise covariance of the state equation. k For the white noise covariance of the observation equation, the entire expression represents w k The expected value is 0 and the covariance is Q. k Gaussian white noise, n k The expected value is 0 and the covariance is R. k Gaussian white noise;

[0149] Due to the nonlinear characteristics of f() and g(), closed-form solutions cannot be calculated. Therefore, it is necessary to linearize the motion and observation models by performing a first-order Taylor expansion of f() and g().

[0150] Taylor expansion of the equations of motion:

[0151]

[0152] Taylor expansion of the observation equation:

[0153]

[0154] in:

[0155] w′ k For the first-order Taylor expansion of the motion white noise, F k-1 Let f() be the Jacobian matrix of the motion function f(). Let be the prior state quantity at time k. Let be the posterior state estimate at time k-1. Let k be the prior observations. Let G be the prior state variable at time k. kLet n′ be the Jacobian matrix of the observation equation g(). k Observational white noise for first-order Taylor expansion

[0156] The EKF algorithm can be broadly divided into two steps: prediction and update.

[0157] predict:

[0158]

[0159]

[0160] renew

[0161]

[0162]

[0163]

[0164] in Let be the prior covariance at time k. Let the posterior estimate of the covariance be at time k-1. Let Q′ be the transpose of the Jacobian matrix of the motion function at time k-1. k K is the covariance of the error between the actual and predicted values ​​of the state variables. k Let K be the Kalman gain matrix at time k. Let R′ be the transpose of the Jacobian matrix of the observation equation at time k. k Let k be the covariance of the error between the actual and predicted values ​​of the measured quantity. Let be the posterior estimated covariance at time k;

[0165] Prediction updates the mean and variance based on the motion equation using the current posterior value, while the update adjusts the mean and variance based on observation information. This allows for the estimation of accurate pose data. The classic EKF recursive update equation allows for the estimation of pose data from... Calculate

[0166] This invention uses Odom data as the observation and IMU as the measurement. An EKF fusion is performed in the robot's Odom coordinate system to ensure the continuity of the fused data. Then, visual Odom data is used as the observation, and the fused data from IMU and IMU is used as the measurement for a second EKF fusion in the global map coordinate system, thereby obtaining the fused pose in the global coordinate system.

[0167] 1.2 One type is sensor fusion based on coordinate transformation

[0168] Step 121. Based on the layout of the LiDAR and depth camera on the robot, use a calibration algorithm to perform calibration and obtain the accurate coordinate transformation T1 = {x, y, z, roll, pitch, yaw} of the LiDAR and depth camera, where x represents the distance along the x-axis, y represents the distance along the y-axis, z represents the distance along the z-axis, roll represents the rotation angle around the x-axis, pitch represents the rotation angle around the y-axis, and yaw represents the rotation angle around the z-axis.

[0169] Step 122. During real-time mapping and localization, the LiDAR data is filtered. The filtering rule is that the range should not exceed the detection range of the depth camera, resulting in filtered LiDAR data; U lidar = <P1|P2|...|P n >, where P i ={x i y i , z i} represents a specific point cloud, and x i <Max x y i <Max y , z i <Max z U lidar P represents the lidar data set. i Represents a specific point cloud, x i Represents the x-axis coordinates and y-axis coordinates of the point cloud. i Represents the y-axis coordinates and z-axis coordinates of the point cloud. i Represents the z-axis coordinate of the point cloud, Max x Max represents the maximum detection range along the x-axis of the depth camera. y Max represents the maximum detection range along the y-axis of the depth camera. z This indicates the maximum detection distance along the z-axis of the depth camera;

[0170] Step 123. Transform the LiDAR data to the depth camera coordinate system according to the coordinate transformation between sensors; U′ lidar =U lidar *T1; where U′ lidar Represents the laser point cloud in the depth camera coordinate system;

[0171] Step 124. Compare the depth values ​​of the LiDAR data and the depth camera data, perform least squares, smooth the depth of nearby points, and correct the depth camera data to obtain more accurate point cloud data.

[0172] U final =f(U′) lidar U vision ), where U finalU represents the final, more accurate point cloud. vision This represents depth camera data, and f() represents the smoothing function.

[0173] 1.3 A coarse-to-fine inter-frame matching algorithm is proposed.

[0174] Using the precise ODOM data obtained after EKF multi-sensor fusion, the robot pose is pre-estimated to obtain the robot's current coarse localization coordinates. Then, based on the current frame point cloud data and the current map, ICP matching calculation is performed to obtain the pose deviation. The coarse localization coordinates are then precisely corrected to obtain a more accurate robot pose.

[0175] The precise point cloud data obtained from the fusion in section 1.2 is added to the point cloud map, and Qk represents the point cloud map built at time k. This indicates that at time k+1, the current frame has been transformed to the world coordinate system. This represents the robot's pose in the world coordinate system at time k. This represents the robot's pose increment at time k+1 (i.e., the output during coarse matching). For example... Figure 3 As shown, the robot pose generated by coarse matching clearly deviates from the true pose, which is manifested in... With Q k There is no overlap. The fine matching steps are as follows:

[0176] Step 131. Regarding Q k Using KD-tree description, for Each laser point in the search is located in Q. k The nearest point in the FLANN algorithm is used to establish pairings.

[0177] Step 132. Establish nonlinear equations with the objective of minimizing the distance between all pairs.

[0178] Step 133. Solve the nonlinear optimization problem using the LM method, and solve for T. opt = (R, t)

[0179] Step 134. The robot pose at time k+1 is:

[0180] After the above two steps, a relatively accurate robot pose can be generated.

[0181] Second, there is the positioning and navigation within the pipeline.

[0182] A method combining centerline positioning and navigation with prior map positioning and navigation is proposed to control robot inspection.

[0183] For real-time positioning and navigation within pipelines, to address the issue of narrow and similar pipelines, centerline positioning and navigation control is used to control the robot's forward and backward movement to ensure that the robot always travels along the centerline of the pipeline, thus preventing it from colliding with the sides. At the same time, a priori map is used to calculate the robot's pose in real time and confirm the robot's stopping point. The combination of these two methods ensures that the robot can perform inspection and navigation operations safely and accurately.

[0184] 2.1 A method for centerline control is proposed, such as... Figure 4 As shown;

[0185] Step 211. The depth camera detects the pillars or walls on both sides in real time, and finds and extracts the edge information of the left and right sides of the tunnel;

[0186] Step 212. Calculate the coordinate information of the left and right sides, and perform offset calculation with the robot's current posture to obtain the distance difference dist between the robot and the left and right sides, and the angle θ between the robot and the center line;

[0187] Step 213. Then, based on the distance difference dist and the included angle 0, the robot's control speed (v, w) is calculated using a PID control algorithm;

[0188] e(t) = (dist t θ t ), where e t This represents the PID control input, namely the distance difference and the included angle.

[0189] u(t)=(v t w t ), where u(t) represents the output robot speed control quantity.

[0190] Where K represents the proportionality coefficient, and T i T represents the integral coefficient. d Describing the differential coefficients

[0191] 2.2 A real-time positioning method based on prior maps

[0192] Prior map localization employs a Monte Carlo particle filter (MCL) strategy. Monte Carlo localization is a nonlinear recursive Bayesian filtering algorithm that uses the law of large numbers to solve for the posterior probability of pose. MCL localization, based on given prior map information and perceived state variables, samples a set of weighted random particles to represent the posterior probability of pose estimation, and calculates and corrects the current state variables based on these particles and weights. Its advantage is the absence of model and Gaussian assumption limitations. For the SLAM process, it can be mathematically summarized as motion equations and observation equations; the SLAM process involves optimal state estimation of these two equations.

[0193]

[0194] Equation of motion: The pose x at the current moment is estimated by using the x at the previous moment, the value u of the current motion measurement, and the noise w.

[0195] Observation equation: The observation data z is obtained by using the current pose x, the landmark point y, and the noise.

[0196] For the observation equation h(), there is a corresponding observation model, and the complete definition of the observation model is p(z). t |x t , m), where m represents the environmental map. In mobile robots, commonly used sensors are ranging sensors, including ultrasonic and laser sensors. Regardless of the type of sensor, noise is unavoidable. The observation model aims to quantitatively describe the various types of noise from the sensors.

[0197] A combination of beam model and likelihood domain model was used for particle filtering calculation;

[0198] Step 221. First, the odometry obtained from the multi-sensor fusion in 1.1 is used as the input to the motion model to update the pose of all particles. U represents the pose of particle m at time k. k This represents the odometer noise at time k;

[0199] Step 222. Then load the fused point cloud data from step 1.2 as input to the particle filter observation model;

[0200] Step 223. Use the updated particles from Step 221 and the point cloud data from Step 222 to perform beam model particle filtering estimation;

[0201] The beam model employs four types of measurement errors: errors caused by obstacles, errors caused by unexpected objects, random measurement errors, and errors caused by undetected objects. The beam model is a mixture of these four types of errors. The model considers what happens to the laser along this ray, and the expected value is also found along this ray. The probability is then calculated using the actual and expected values. The calculation uses a ray projection function. The model can then be expressed as the following equation based on the four types of measurement errors.

[0202]

[0203] Where α hit α represents the variance of the error caused by obstacles. unexp α represents the variance of the error caused by unexpected objects. max α represents the variance of the error caused by the failure to detect an object. randP represents the variance of random measurement error. hit P represents the obstacle error observation model. unexp P represents the unexpected object error observation model. max This indicates that no object error observation model was detected, P rand This represents a random measurement error observation model;

[0204] Obtain the particle weight W k，m =ηP(z|x,m); where η is the normalization coefficient.

[0205] Step 224. Use the updated particles to perform particle filter estimation for the likelihood domain model;

[0206] The main idea of ​​the likelihood domain model is to map the endpoint Zt of the sensor scan onto a grid map, calculate the distance between it and the nearest obstacle on the grid map, and then calculate the probability based on this distance. The expected value is not on the ray itself, but rather on the obstacle closest to the endpoint of the ray; the calculation uses a nearest neighbor function. First, the obstacle points detected by the sensor must be mapped onto a known map.

[0207]

[0208] Where x t =(xy θ) T It is the robot's pose state, (x k，sens y k，sens ) T θ represents the local coordinate position of the installed sensor. k，sens This represents the deflection angle of the sensor beam relative to the robot's heading angle. Of course, this process requires the sensor to actually detect the obstacle; that is, the measured value cannot be the maximum value, so the maximum value measurement result is discarded. We first assume that the sensor only has measurement error, and represent this error using Gaussian noise. Then, we calculate the obstacle point corresponding to the obtained sensor data. Next, find the nearest obstacle on the map and denote the point as dist. The distance to the nearest obstacle, therefore the probability of the sensor's measurement can be represented by a Gaussian function with a mean of 0:

[0209]

[0210] Likelihood domain model particle weights Where η is the normalization coefficient.

[0211] Step 225. Based on the two model algorithms, particle filtering is performed on the real-time point cloud data frame to obtain their respective weights and poses. Then, weighted calculation is performed based on their respective weights to obtain the final pose Pk.

[0212] Step 226. Based on particle weights and poses, resample all particles, remove particles with lower weights, and re-sprinkle with new particles. Repeat the above operation; {P k，m W k，m}——>{P′ k，m , W′ k，m}, where P k，m W represents the pose of particle m at time k. k，m P′ represents the weight of particle m at time k. k，m W′ represents the pose of particle m after resampling at time k. k，m This represents the weight of the m particles after resampling at time k;

[0213] III. A magnetically guided integrated navigation scheme is proposed.

[0214] In the area outside the pipeline, traditional SLAM positioning and navigation algorithms cannot meet the normal inspection requirements, so magnetic navigation is used for navigation and inspection control.

[0215] Step 31. Use magnetic navigation outside the pipeline. Attach magnetic strips to the ground near the inspection route, and attach RFID tags to the magnetic strips as stopping points. Number the RFID tags.

[0216] Step 32. The distance between the robot's center and the magnetic strip is detected in real time using the magnetic induction device on the bottom of the robot.

[0217] Step 33. Calculate the linear velocity v and angular velocity w of the magnetic strip based on its width d;

[0218] The calculation formula is as follows:

[0219] factor = dist / d, where factor represents the proportionality factor of dist's deviation from the center of the magnetic strip;

[0220] w t =v t-1 *(2 factor -1), v t =v t-1 *abs(1-factor)

[0221] Where w t v represents the angular velocity at time t. t Represents the linear velocity at time t;

[0222] Step 34. Control the robot to move based on the speed calculated in the previous step.

[0223] Step 35. Detect the current RFID tag number in real time and determine if it is the same as the target RFID tag.

[0224] Step 36. Once the target number is detected, stop the vehicle and begin the task inspection.

[0225] Example 1

[0226] 1. First, based on the actual scene, confirm the magnetic navigation area and the multi-sensor fusion SLAM navigation area;

[0227] 2. Lay magnetic strips along the inspection route in the magnetic navigation area for magnetic navigation use, and affix RFID sensor carriers near the inspection equipment;

[0228] 3. The robot moves inside the pipeline using centerline navigation and performs real-time mapping of the top of the pipeline to obtain a priori map of the top of the pipeline;

[0229] 4. Implement the project based on the location of the magnetic navigation RFID deployment and the device location in the prior map, confirm the map cut-off point, and associate the map cut-off point with the other points;

[0230] 5. After all equipment points have been implemented, import the equipment point information XML file into the robot;

[0231] 6. The robot plans its inspection route based on the equipment location list;

[0232] 7. The robot first uses a magnetic navigation algorithm to inspect along the magnetic strips until all RFID tags have been inspected;

[0233] 8. After the magnetic navigation area inspection is completed, navigate to the map switching point, switch maps, and load the prior map;

[0234] 9. Use centerline positioning and navigation to control the robot to walk along the center. First, use the map at the top of the pipe to confirm whether the inspection point is in place. After it is in place, stop the robot and perform the inspection operation.

[0235] 10. Repeat the above steps until all equipment points have been inspected.

[0236] As can be seen from the above, the combined navigation method based on multi-sensor fusion SLAM and magnetic navigation of the present invention is specifically a method based on multi-sensor fusion SLAM and magnetic navigation. Multi-sensor fusion SLAM is used in tunnels, combined with centerline control and prior map positioning and navigation to solve problems such as laser degradation in narrow corridors. Magnetic navigation is used outside the tunnel to avoid the impact of environmental changes on positioning and navigation. The combination of the two navigation methods can effectively avoid the impact of narrow similar pipes and scenes with large environmental changes on SLAM.

[0237] In another aspect, the present invention also discloses a computer-readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the steps of any of the methods described above.

[0238] In another aspect, the present invention also discloses a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor performs the steps of any of the methods described above.

[0239] In another embodiment provided in this application, a computer program product containing instructions is also provided, which, when run on a computer, causes the computer to perform the steps of any of the methods described in the above embodiments.

[0240] It is understood that the system provided in the embodiments of the present invention corresponds to the method provided in the embodiments of the present invention, and the explanation, examples and beneficial effects of the relevant content can be referred to the corresponding parts of the above methods.

[0241] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments described above. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0242] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0243] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A combined navigation method based on multi-sensor fusion SLAM and magnetic guidance, including combined navigation methods inside and outside tunnels, wherein the robot switches maps and navigation modes at corresponding locations, characterized in that... Includes the following steps, The process inside the tunnel first involves creating a priori map. The mapping algorithm combines odometry data, IMU inertial navigation data, depth camera and LiDAR. During inspection, the robot provides real-time positioning and navigation, using a combination of centerline navigation and priori map positioning and navigation. The creation of the prior map, the mapping algorithm combining odometry data, IMU inertial navigation data, depth camera and LiDAR, includes the following steps. S1.1 Correct the odometry data. The input data includes: odometry data collected by the encoder, IMU inertial navigation data, and visual odometry data calculated in real time by the depth camera. These three types of data are fused and estimated to obtain more accurate odometry data. S1.

2. The data from the depth camera and the LiDAR are fused and calculated to obtain more accurate point cloud data. The position coordinates of the depth camera and the LiDAR must be calibrated beforehand. Then, the LiDAR data is filtered and converted to the depth camera coordinate system based on the calibrated coordinates. Then, the data of the two are smoothed to obtain the fused point cloud data. S1.3 Perform coarse-to-fine inter-frame matching mapping and use the more accurate odometry data obtained in step S1.1 to predict the robot's real-time pose and obtain the robot's current coarse pose. Then, ICP matching calculation is performed based on the current frame point cloud data and the current map data to correct the coarse pose and obtain a more accurate robot pose. The robot's real-time positioning and navigation during inspection uses a combination of centerline navigation and prior map positioning and navigation, including the following steps. S2.

1. The centerline navigation method is adopted to ensure that the robot always walks along the middle of the tunnel and does not hit the side walls of the tunnel; the data of the front depth camera is detected in real time to find the side walls, and the distance of the robot from the side walls and the angle with the parallel line of the wall are calculated. Based on the distance and the angle, the robot is controlled to strictly walk along the middle. S2.2 Based on the pre-created prior map, a Monte Carlo localization strategy is adopted, and a particle filter algorithm combining the likelihood domain model and the beam model is used to calculate the robot's real-time posterior pose; the odometry data and point cloud data used for localization calculation are the data optimized in steps S1.1 and S1.

2. S2.

3. Using the centerline navigation control of step S2.1 to move the robot forward and backward and the calculation of the robot's real-time pose in step S2.2, determine whether the robot has reached the target point and control the robot to stop. Outside the tunnel, the first step is to determine the inspection route based on the inspection task, then lay magnetic strips and RFID tags, and number the RFID tags. During navigation, the magnetic induction device on the bottom of the robot calculates and controls the speed in real time based on the distance of the magnetic strip from the center of the device; when the detected RFID number matches the target RFID number, it indicates that the destination has been reached.

2. The combined navigation method based on multi-sensor fusion SLAM and magnetic permeability according to claim 1, characterized in that: S1.1 involves correcting the odometry data. The input data includes odometry data collected by the encoder, IMU inertial navigation data, and visual odometry data calculated in real-time by the depth camera. These three types of data are fused and estimated to obtain more accurate odometry data. Specifically, this includes: Step 111. The first step is to collect multi-sensor data as the front-end input for robot mapping and localization; odom and IMU data are directly read through the encoder, and the visual odom needs to be offset by ICP point cloud registration based on the visual point cloud of the previous and next frames. ,in X represents the point cloud of the current frame, and X represents the point cloud of the previous frame. This represents the rotation matrix of the point cloud transformation. The point cloud transformation translation matrix is ​​represented by the rotation matrix and the translation matrix is ​​constrained in a plane to obtain the visual odometry (odom). in This represents the x-coordinate, y-coordinate, and angle measurement values ​​of the odometer. This represents the measurement value from the IMU. This represents the calculated x-coordinate, y-coordinate, and angle values ​​of the visual odometry. This indicates the odometer readings after fusion; Step 112. Perform EKF fusion calculation on odom, IMU, and visual odom. The nonlinear motion and observation model is as follows: （1） （2） For state variables, For observation purposes, To control the quantity, and They are all nonlinear functions, assuming The confidence function is a Gaussian distribution: （3） Assumption: and They are all white noise. , Differentiable, in The prior predicted state quantity at time 0. The control quantity is from time 1 to time k. The measurement quantity is from time 0 to time k. Let be the posterior state estimate at time k. Let be the covariance estimate at time k, and N be the mathematical representation of the Gaussian distribution, representing the expected value. covariance is Gaussian white noise, The state equation is the white noise covariance. The white noise covariance of the observation equation is expressed as follows: The expected value is 0, and the covariance is Gaussian white noise, The expected value is 0, and the covariance is Gaussian white noise; Linearization is applied to the motion and observation models, and Perform a first-order Taylor expansion; Taylor expansion of the equations of motion: （5） Taylor expansion of the observation equation: （6） in: , , ; The motion white noise is a first-order Taylor expansion. Motion function Jacobian matrix, Let be the prior state quantity at time k. Let be the posterior state estimate at time k-1. Let k be the prior observations. Let be the prior state quantity at time k. For the observation equation Jacobian matrix, The observational white noise is a first-order Taylor expansion. The EKF algorithm consists of two steps: prediction and update. predict: （7） （8） renew （9） （10） （11） in Let be the prior covariance at time k. Let the posterior estimate of the covariance be at time k-1. Let be the transpose of the Jacobian matrix of the motion function at time k-1. The error covariance between the actual and predicted values ​​of the state variables. Let K be the Kalman gain matrix at time k. Let be the transpose of the Jacobian matrix of the observation equation at time k. Let k be the covariance of the error between the actual and predicted values ​​of the measured quantity. Let be the posterior estimate of the covariance at time k; Prediction is based on updating the mean and variance according to the motion equation using the current posterior value. The update, in turn, corrects the mean and variance based on observation information, thereby estimating accurate pose data. The classic EKF recursive update equation is used to estimate the pose data. Calculate ; Using Odom data as the observation and IMU as the measurement, an EKF fusion is performed in the robot's Odom coordinate system to ensure the continuity of the fused data. Then, visual Odom data is used as the observation, and the IMU and Odom fused data are used as the measurement to perform a second EKF fusion in the global map coordinate system, thereby obtaining the global fused pose. .

3. The combined navigation method based on multi-sensor fusion SLAM and magnetic permeability according to claim 1, characterized in that: S1.2 involves fusing the data from the depth camera and LiDAR to obtain more accurate point cloud data. This requires calibrating the position coordinates of the depth camera and LiDAR beforehand, then filtering the LiDAR data based on the calibrated coordinates and converting it to the depth camera coordinate system. Finally, the data from both systems is smoothed to obtain the fused point cloud data, specifically including: Step 121: Based on the layout of the LiDAR and depth camera on the robot, use a calibration algorithm to perform calibration and obtain the accurate coordinate transformation of the LiDAR and depth camera. Where x represents the distance along the x-axis, y represents the distance along the y-axis, z represents the distance along the z-axis, roll represents the rotation angle around the x-axis, pitch represents the rotation angle around the y-axis, and yaw represents the rotation angle around the z-axis; Step 122: During real-time mapping and localization, the LiDAR data is filtered. The filtering rule is that the range does not exceed the detection range of the depth camera, resulting in filtered LiDAR data. ,in , representing a specific point cloud, and , , ,in Represents the set of lidar data. Represents a specific point cloud. Represents the x-axis coordinates of the point cloud. Represents the y-axis coordinate of the point cloud. Represents the z-axis coordinate of the point cloud. This indicates the maximum detection range along the x-axis of the depth camera. This indicates the maximum detection range of the depth camera along the y-axis. This indicates the maximum detection distance along the z-axis of the depth camera; Step 123: Transform the LiDAR data into the depth camera coordinate system according to the coordinate transformation between sensors; ;in Represents the laser point cloud in the depth camera coordinate system; Step 124. Compare the depth values ​​of the LiDAR data and the depth camera data, perform least squares, smooth the depth of nearby points, and correct the depth camera data to obtain more accurate point cloud data. ,in This represents the final, more accurate point cloud. Represents depth camera data, This represents a smoothing function.

4. The combined navigation method based on multi-sensor fusion SLAM and magnetic permeability according to claim 1, characterized in that: In step S1.3, perform coarse-to-fine inter-frame matching mapping and use the more accurate odometry data obtained in step S1.1 to predict the robot's real-time pose and obtain the robot's current coarse pose. Then, based on the current frame point cloud data and the current map data, ICP matching calculation is performed to correct the coarse pose and obtain a more accurate robot pose, specifically including: Using the precise ODOM data obtained after EKF multi-sensor fusion, the robot pose is pre-estimated to obtain the robot's current coarse localization coordinates. Then, based on the current frame point cloud data and the current map, ICP matching calculation is performed to obtain the pose deviation. The coarse localization coordinates are then precisely corrected to obtain a more accurate robot pose. The fused, precise point cloud data is added to the point cloud map, using... This represents the point cloud map established at time k. This indicates that at time k+1, the current frame has been transformed to the world coordinate system. This represents the robot's pose in the world coordinate system at time k. This represents the robot's pose increment at time k+1, which is also the output during the coarse matching process; the fine matching steps are as follows: Step 131, To Using KD-tree description, for Each laser point in the search is in Establish a pairing based on the nearest points in the data; Step 132: Establish nonlinear equations with the goal of minimizing the distance between all pairs; Step 133: Solve the nonlinear optimization problem using the LM method. ; Step 134, then the robot pose at time k+1 is: .

5. The combined navigation method based on multi-sensor fusion SLAM and magnetic permeability according to claim 1, characterized in that: S2.1 employs centerline navigation to ensure the robot always travels along the center of the tunnel without encountering the side walls; it continuously monitors data from the front depth camera to locate the side walls and calculates the robot's distance from the walls and the angle between the robot and the parallel lines of the walls. Based on this distance and angle, it controls the robot to strictly follow the center line. Specifically, this includes: Step 211. The depth camera detects the pillars or walls on both sides in real time, and finds and extracts the edge information of the left and right sides of the tunnel; Step 212. Calculate the coordinate information of the left and right sides, and perform offset calculation with the robot's current posture to obtain the distance difference dist between the robot and the left and right sides, and the angle θ between the robot and the center line; Step 213. Then, based on the distance difference dist and the included angle θ, the robot's control speed (v, w) is calculated using a PID control algorithm. ,in This represents the PID control input, namely the distance difference and the included angle. ,in This represents the output robot speed control quantity. Where K represents the proportionality coefficient. Represents the integral coefficient. This represents the differential coefficient.

6. The combined navigation method based on multi-sensor fusion SLAM and magnetic permeability according to claim 1, characterized in that: S2.2, based on a pre-created prior map, employs a Monte Carlo localization strategy, using a particle filter algorithm combining a likelihood domain model and a beamforming model to calculate the robot's real-time posterior pose; the odometry data and point cloud data used for localization calculations are the optimized data from steps S1.1 and S1.2; specifically including: The SLAM process can be summarized by mathematical formulas as motion equations and observation equations. The SLAM process is to perform optimal state estimation on the two equations. Equation of motion: The pose x at the current moment is estimated by using the x at the previous moment, the value u of the current motion measurement, and the noise w. Observation equation: The observation data z is obtained by using the current pose x, the landmark point y, and the noise. For the observation equation There is a corresponding observation model, and the complete definition of the observation model is: Where m represents the environment map, and in mobile robots, the sensors used are ranging sensors, including ultrasonic and laser sensors; A combination of beam model and likelihood domain model was used for particle filtering calculation; Step 221. First, the odometer obtained by fusing multiple sensors is used as the input to the motion model to update the pose of all particles. , This represents the pose of particle m at time k. This represents the odometer noise at time k; Step 222. Then load the fused point cloud data as input to the particle filter observation model; Step 223. Use the updated particle and point cloud data to perform beam model particle filter estimation; The beam model employs four types of measurement errors: errors caused by obstacles, errors caused by unexpected objects, random measurement errors, and errors caused by undetected objects. The beam model is a mixture of these four types of errors. The model considers what happens to the laser along this ray, and the expected value is also found along this ray. The probability is then calculated using the actual value and the expected value. The calculation uses a ray projection function. The model can then be expressed as the following equation based on the four types of measurement errors. in This represents the variance of the error caused by the obstacle. This represents the variance of the error caused by unexpected objects. This represents the variance of the error caused by the failure to detect an object. The variance represents the error of random measurements. This represents the obstacle error observation model. This represents the unexpected object error observation model. This indicates that no object error observation model was detected. Represent the observation model of random measurement error; obtain the particle weights. Where η is the normalization coefficient; Step 224. Use the updated particles to perform particle filter estimation for the likelihood domain model; The likelihood domain model is the endpoint Z of the sensor scan. t Mapping the ray onto a grid map, calculating the distance between the ray and the nearest obstacle on the grid map, and then calculating the probability based on this distance, with the expected value being the obstacle closest to the ray's endpoint; the calculation uses a nearest neighbor function; firstly, the obstacle points detected by the sensor must be mapped onto a known map: in It refers to the robot's pose state. This indicates the local coordinate position of the installed sensor. This represents the deflection angle of the sensor beam relative to the robot's heading angle. Of course, this process requires the sensor to actually detect the obstacle; that is, the measured value cannot be the maximum value, so the maximum value measurement result is discarded. We first assume that the sensor only has measurement error, and represent this error with Gaussian noise, then calculate the obstacle point corresponding to the sensor data. Next, find the nearest obstacle on the map and denote the point as dist. The distance to the nearest obstacle is used to determine the sensor's measurement probability, which is represented by a Gaussian function with a mean of 0. ； Where dist is the distance difference; Likelihood domain model particle weights , where η is the normalization coefficient; Step 225. Based on the two model algorithms, particle filtering is applied to the real-time point cloud data frames to obtain their respective weights and poses. Then, a weighted calculation is performed based on their respective weights to obtain the final pose. ; Step 226. Based on particle weights and poses, resample all particles, remove particles with lower weights, and re-sprinkle new particles. Repeat the above operations. ,in This represents the pose of particle m at time k. This represents the weight of particle m at time k. This represents the pose of particle m after resampling at time k. This represents the weight of the m particles after resampling at time k.

7. The combined navigation method based on multi-sensor fusion SLAM and magnetic permeability according to claim 1, characterized in that: Outside the tunnel, the inspection path is first determined based on the inspection task, then magnetic strips and RFID tags are laid and numbered. During navigation, the magnetic induction device on the bottom of the robot calculates and controls the speed in real time based on the distance of the magnetic strip from the center of the device. When the detected RFID tag number matches the target RFID tag number, it indicates that the destination has been reached. Specifically, this includes: Step 31. Use magnetic navigation outside the pipeline. Stick magnetic strips on the ground near the inspection route and attach RFID carriers to the magnetic strips as parking points. Number the RFID carriers. Step 32. The distance (dist) between the robot's center and the magnetic strip is detected in real time using the magnetic induction device on the bottom of the robot; Step 33. Calculate the linear velocity v and angular velocity w of the magnetic strip based on its width d; The calculation formula is as follows: , where factor represents the scaling factor of dist's deviation from the center of the magnetic strip; ， in Represents the angular velocity at time t. Represents the linear velocity at time t; Step 34. Control the robot to move according to the speed calculated in the previous step; Step 35. Detect the current RFID number in real time and determine whether it is the same as the target RFID; Step 36. Once the target number is detected, stop the vehicle and begin the task inspection.

8. A computer-readable storage medium storing a computer program that, when executed by a processor, causes the processor to perform the steps of the method as claimed in any one of claims 1 to 7.

Images