Inspection robot positioning method based on multi-sensor fusion

Abstract

The present invention discloses a patrol robot positioning method based on multi-sensor fusion, and the steps are as follows: step S1, collecting multi-sensor data, including absolute positioning information of the positioning system, data of the encoder in the patrol robot and data of the inertial measurement unit; step S2, EKF fusion of multi-sensor data: the data of the encoder and the measurement of the inertial measurement unit are subjected to an EKF data fusion; a fused posture is obtained, and then the fused data is subjected to a secondary EKF data fusion with the absolute positioning information of the positioning system to obtain a secondary fused posture; step S3, according to the fused posture, combined with a 3D prior map, Monte Carlo positioning is performed. Based on the EKF fusion algorithm, the present invention can provide a posture solution with no cumulative error, high reliability and stability in real time; and combined with the MCL positioning algorithm, the positioning accuracy requirements in some occluded environments are guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of electric power inspection robots, and in particular to an inspection robot positioning method based on multi-sensor fusion. Background Art

[0002] There are many drawbacks in the manual power inspection operation mode. For example, the manual mode determines its low degree of automation and efficiency, and high cost; and manual operation cannot guarantee the timely communication and uploading of input data to the cloud, resulting in information lag and backup difficulties; manual operation cannot guarantee full coverage of the entire station and all-weather inspection. However, inspection robots, with their powerful and reliable positioning navigation and intelligent recognition technology, are gradually replacing manpower and becoming the main force of power inspection.

[0003] At present, the mainstream positioning solution for inspection robots is Lidar-scan, which is to solve the real-time posture of the robot by registering the current frame point cloud data with the map data. The main algorithms include the iterative nearest neighbor point (ICP) algorithm and its variants, the normal distribution transformation algorithm (NDT) and the feature matching algorithm. In addition, there are many algorithms based on deep learning to improve the matching speed and stability. For example, Nicolai A et al. used convolutional neural network (CNN) network training to obtain end-to-end matching results, and their matching efficiency is much higher than ICP. Compared with other matching algorithms, the Monte Carlo positioning method (MCL) has become the most popular positioning solution due to its stability and re-positioning.

[0004] The laser registration algorithm can give the optimal pose estimation when the surrounding environment features are rich, but the laser registration algorithm still has its drawbacks. For example, during the driving process, it may cause mismatch due to large turning angles, fast driving speeds, interference between moving objects and transparent objects, or in an open environment with fewer surrounding environment features, which will directly lead to estimated pose errors. The substation environment is complex, and the solution relying on a single laser positioning cannot meet its actual application requirements.

[0005] The multi-sensor fusion solution relies on its high data redundancy and fault tolerance to ensure the consistent matching of the estimated results and the actual results. This is an inevitable trend for unmanned inspection to achieve precise positioning. Multi-sensor data fusion technology is to combine the redundant or complementary information of different sensors in space or time according to a certain criterion to obtain a consistent explanation or description of the object being measured. At present, the theoretical methods of multi-sensor fusion include Bayesian criterion method, Kalman filtering method, DS evidence theory method, fuzzy set theory method, artificial neural network method, etc.

[0006] In order to ensure the positioning robustness and accuracy of the inspection robot in complex and unstructured environments, the multi-sensor data fusion technology of the EKF algorithm is combined with the positioning scheme of the MCL algorithm to obtain a reliable and stable estimated pose, thereby achieving precise positioning in a large range, open environment, and complex scenes, and ensuring the safety and stability of the inspection robot. Summary of the invention

[0007] The purpose of the present invention is to provide a patrol robot positioning method based on multi-sensor fusion, and to perform local and global pose estimation based on a multi-sensor fusion strategy based on an extended Kalman filter (EKF) algorithm. Combined with the fusion of the posterior pose probability distribution, the importance sampling and resampling process of the Monte Carlo positioning (MCL) algorithm is optimized, and the positioning and fusion pose dead reckoning of the MCL algorithm are switched based on the particle matching degree of the prior map. This ensures the robustness and accuracy of patrol navigation positioning.

[0008] The present invention is implemented by the following technical solution: a patrol robot positioning method based on multi-sensor fusion, the steps are as follows:

[0009] Step S1, collecting multi-sensor data, including absolute positioning information of the positioning system, data of the encoder in the inspection robot and data of the inertial measurement unit; the absolute positioning information is: (x bds ,y bds ,ρ bds ), where (x bds ,y bds ) is the position, ρ bds is the heading; the measurement quantity of the inertial measurement unit

[0010]

[0011] in is the x-axis acceleration, is the y-axis acceleration, is the z-axis acceleration, is the x-axis angular velocity, is the y-axis angular velocity, is the z-axis angular velocity; the encoder data includes the encoder position information (x odom ,y odom ) and the encoder's motion direction ρ odom ;

[0012] Step S2, EKF fusion of multi-sensor data: Encoder data (x odom ,y odom ,ρ odom ) and the measurement quantity ρ of the inertial measurement unit imuPerform an EKF data fusion; obtain a fused pose, and then compare the fused data with the absolute positioning information (x bds ,y bds ,ρ bds ) performs secondary EKF data fusion to obtain the secondary fusion pose (x kal ,y kal ,ρ kal );

[0013] Step S3: Monte Carlo positioning is performed based on the fused pose and the 3D prior map: the particles in the Monte Carlo positioning are positioned according to the secondary fused pose (x kal ,y kal ,ρ kal ) and variance information are sampled in a normal distribution in the prior map, and the secondary fusion pose (x kal ,y kal ,ρ kal ) as the prior pose of Monte Carlo positioning, and during the operation of the robot, according to the secondary fusion pose (x kal ,y kal ,ρ kal ) to update the particle weights; when the number of particles is too small, the secondary fusion pose (x kal ,y kal ,ρ kal ) to perform dead reckoning.

[0014] Specifically, in step S2, the position information (x odom ,y odom ) and the encoder's direction of motion ρ odom As the observed quantity, the measurement quantity ρ of the inertial measurement unit is used imu As the measurement quantity, an EKF fusion is performed in the robot Odom coordinate system to obtain a fusion pose (x′ kal , y′ kal , ρ′ kal ); and then use the absolute positioning information (x bds ,y bds ,ρ bds ) as the observation quantity, and a fusion pose (x′ kal , y′ kal , ρ′ kal ) is the measurement quantity, and the secondary EKF fusion is performed in the global map coordinate system, thereby obtaining the secondary fusion pose (x kal ,y kal ,ρ kal ).

[0015] Specifically, in step S3, each particle in the Monte Carlo positioning has corresponding positioning information (x k-1,m,y k-1,m , ρ k-1,m ) and weight w k-1,m , the final positioning information of the robot is obtained by weighted calculation based on the positioning information and weights of all particles (X k-1 , Y k-1 , W k-1 ); After two EKF fusion calculations, the secondary fusion pose (x kal ,y kal ,ρ kal ) and the corresponding variance information According to the obtained positioning and variance information, the existing particles are sampled in a normal distribution, that is, the positioning information of each particle is updated in a normal distribution manner to obtain the positioning information of each particle at the current k moment (x k,m ,y k,m , ρ k,m ), and then update the weight w of each particle according to the updated particle pose and the current prior map measurement k,m , and finally obtain the precise positioning information of the robot at the current moment (X k , Y k , W k ), where x k-1,m is the x-axis positioning data of particle number m at time k-1, y k-1,m is the y-axis positioning data of particle number m at time k-1, ρ k-1,m is the angular positioning data of particle number m at time k-1, w k-1,m is the weight of particle number m at time k-1, X k-1 is the robot x-axis positioning data at time k-1, Y k-1 is the robot y-axis positioning data at time k-1, W k-1 is the robot angle positioning data at time k-1, x k,m is the x-axis positioning data of particle number m at time k, y k,m is the y-axis positioning data of particle number m at time k, ρ k,m is the angular positioning data of particle number m at time k, w k,m is the weight of particle number m at time k-1, X k is the robot x-axis positioning data at time k, Y k is the robot y-axis positioning data at time k, W k It is the robot angle positioning data at moment k-1.

[0016] Specifically, the specific process of step S3 is:

[0017] S3-1. Adjust the importance parameters of all samples according to the Euclidean distance between all sample particles and the fusion pose;

[0018] S3-2. During the initial sampling and resampling process, samples are extracted based on the Gaussian distribution of the fused pose;

[0019] S3-3. Use the fused pose as a sample subset to replace particles with low importance in the sample set;

[0020] S3-4. Adjust the fusion strategy according to the particle matching degree. In the open area, use the incremental information of EKF fusion pose to perform track calculation until the particle filter estimated pose regains a higher matching degree, and then use the MCL positioning algorithm.

[0021] The specific process of step S3-1 is as follows: according to the positioning information (x k-1,m ,y k-1,m , ρ k-1,m ) and secondary fusion pose (x kal ,y kal ,ρ kal ) to update the position information of all particles in the sample at the current moment; (Δx kal,k ,Δy kal,k ,Δρ kal,k )

[0022] That is (x k,m ,y k,m , ρ k,m )=(x k-1,m ,y k-1,m , ρ k-1,m )+(Δx kal,k ,Δy kal,k ,Δρ kal,k )+Z k,m ;

[0023] where Δx kal,k =x kal,k -x kal,k-1 , Δy kal,k =y kal,k -y kal,k-1 , Δρ kal,k =ρ kal,k -ρ kal,k-1 ,

[0024] Δx kal,k is the increment of the x-axis of the fused positioning pose at time k, Δy kal,k is the increment of the y-axis of the fused positioning pose at time k, Δρ kal,k is the increment of the fusion positioning pose angle at time k, Z k,m is the sampling noise of particle number m at time k, x kal,k is the value of the x-axis of the fusion pose at time k, and y kal,k is the value of the y-axis of the fusion pose at time k, ρkal,k is the value of the fusion pose angle at moment k.

[0025] Specifically, in step S3-2, during the initial sampling process, based on the prior map information and the perceived state, a Gaussian distribution is used to extract samples and the particle weights at the current moment are updated. The real-time posture of the robot is weightedly calculated according to the latest weight and the posture information of particle k at the moment updated in step 1);

[0026]

[0027] in represents the posterior estimated state of the robot at time k, ∑ represents the weighted calculation of all particles, is the a posteriori estimated state quantity of particle number m at time k, w k,m is the weight of particle m at time k.

[0028] Specifically, in step S3-3, the robot posterior estimated state quantity at time k obtained by the final calculation is used To update the current particle sample set, the weight w k,m Replace the low-quality particles.

[0029] Specifically, in step S3-4, the fusion strategy is adjusted according to the matching degree of the particles. When calculating the particle weight and the final real-time posture in step S3-2, the matching degree information of the current posture is obtained synchronously. If the matching degree meets the requirements, the robot posture at the current moment is directly used. As the final state quantity; if the matching degree does not meet the requirements and it is considered to be an open area, the incremental information ΔX of the positioning posture after the secondary fusion of EKF is used kal,k To perform dead reckoning, until the particle filter estimates the pose to regain a high degree of match, then use the MCL positioning algorithm, that is, in represents the robot posture at time k-1, ΔX kal,k =(Δx kal,k ,Δy kal,k ,Δρ kal,k ) is the increment of the two EKF fusion positioning poses.

[0030] Beneficial effects of the present invention: In view of the fact that today's inspection robots need to cope with inspection tasks in complex unstructured environments of substations and inspection navigation requirements in large-scale, open environments, this paper uses sensors such as the Beidou Positioning System (BDS), Inertial Measurement Unit (IMU) and Encoder (Odom), and uses a multi-sensor fusion strategy based on the Extended Kalman Filter (EKF) algorithm to perform local and global pose estimation. Combined with the fusion of the posterior pose probability distribution, the importance sampling and resampling process of the Monte Carlo Positioning (MCL) algorithm is optimized, and the positioning and fusion pose track calculation of the MCL algorithm are switched based on the particle matching degree of the prior map. In this way, the robustness and accuracy of the inspection navigation positioning are guaranteed. Experiments have shown that the loosely coupled positioning and navigation strategy based on EKF+MCL has a positioning accuracy of ±1cm, and its positioning data meets the requirements of mapping and navigation, and can ensure safe and reliable navigation in some complex open environments and shielded environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a schematic diagram of the present invention. DETAILED DESCRIPTION

[0032] The present invention will be further described in detail below with reference to the accompanying drawings.

[0033] Reference Figure 1 , a patrol robot positioning method based on multi-sensor fusion, the steps are as follows:

[0034] Step S1, collecting multi-sensor data, including the absolute positioning information of the Beidou Positioning System (BDS), the data of the encoder (Odom) in the inspection robot, and the data of the inertial measurement unit (IMU).

[0035] In the BeiDou Positioning System (BDS), the data reference coordinate system is the World Geodetic System Coordinate System (WGS84), and its data includes longitude, latitude, altitude, positioning standard deviation, timing, etc. However, in order to ensure the availability and uniformity of data, it is necessary to unify the arc system of longitude and latitude into distance units, that is, the meter system. Therefore, it is necessary to convert the BDS measurement data into a plane Cartesian reference coordinate system. The present invention uses the Universal Transverse Mercator Grid System (UTM) to realize the metric conversion from angle to distance. That is, the BDS data coordinate system is the UTM coordinate system.

[0036] In order to ensure the accuracy of BDS measurement data, differential positioning is used to improve Beidou positioning accuracy. The differential data center provides differential data, and its data is transmitted through HTTP. It can access services through devices such as wireless data transmission terminals. Its protocol follows the Ntrip protocol, and the differential data communication format is RTCM3.0. The solved position information is obtained through the serial port in the form of a message, which complies with the NMEA message standard format. Because the Beidou Positioning System (BDS) not only provides position information, but also provides heading information, it is necessary to read the positioning message and heading message separately. The absolute positioning information solved is: (x bds ,y bds ,ρ bds ), where (x bds ,y bds ) is the position, ρ bds For the heading, in the local navigation strategy, there is no need to consider the elevation information, and the error of the altitude information of positioning is large, so the altitude information is filtered out, and the positioning data obeys the normal distribution.

[0037] In order to facilitate subsequent data fusion and unified coordinate system, it is necessary to know the robot's true north yaw angle, which can generally be given by an inertial measurement unit. However, the present invention uses an inertial navigation unit of a consumer-grade product, which cannot give the true north angle. Therefore, dual antennas are used to meet the dual needs of positioning and orientation, that is, two antennas are used to form a master-slave dual antenna, and the pointing information from the master antenna to the slave antenna is the true north heading information.

[0038] The inertial measurement unit (IMU) can provide accurate transient information, which is divided into three-axis acceleration information and three-axis angular velocity information, ρ imu is the measurement quantity of the inertial measurement unit, expressed as follows

[0039]

[0040] in is the x-axis acceleration, is the y-axis acceleration, is the z-axis acceleration, is the x-axis angular velocity, is the y-axis angular velocity, is the angular velocity of the z-axis. In addition, the inertial measurement unit is a self-reference measurement unit. In addition to its own measurement information, there is no other observation to provide redundant constraints. Therefore, the position and rotation information obtained by integration will have an increasing cumulative error due to the time scale, resulting in data unusable. Therefore, its transient quantity or increment is generally used for fusion. Because the fusion algorithm is based on the robot coordinate system, it is necessary to exclude the measurement information of gravity acceleration. In addition, it needs to be calibrated, assuming that the data obeys a zero-mean normal distribution.

[0041] The encoder uses the photoelectric sensors on the two wheels to realize the position trajectory calculation according to the robot dynamics model. For the encoder, its static error is much smaller than the dynamic error. The position data information is greatly affected by speed. In order to reflect the influence of speed, the data variance of the encoder is regulated by speed-related factors. The basic strategy is that the variance is positively correlated with speed.

[0042]

[0043] where σ k,odom is the real-time variance of the encoder, h(·) is the linear equation about the speed; σ x is the encoder x-axis variance, σ y is the encoder y-axis variance, σ ρ is the encoder angle value variance, σ x,init is the initial variance of the x-axis, σ y,init is the initial variance of the y-axis, σ ρ,init is the initial variance of the angle value, v x,odom is the encoder x-axis speed, v y,odom is the encoder y-axis speed, ω ρ,odom is the encoder angular velocity, h x (v x,odom ) is the linear equation of the encoder about the x-axis speed, h y (v y,odom ) is the linear equation of the encoder about the y-axis speed, h ρ (ω ρ,odom ) is the linear equation of the encoder about the angular velocity; the position information of the encoder (x odom ,y odom ) and the encoder's motion direction ρ odom is the real-time data of the encoder. The total amount of movement of the odometer from the initial moment to the current moment is calculated based on the real-time data of the encoder and the Ackerman steering inverse kinematics model.

[0044] Step S2, EKF fusion of multi-sensor data. Because encoders and inertial measurement units are both low-noise, high-drift sensors, especially inertial measurement units, although the transient measurement value is very accurate, the position data drifts seriously over time. On the contrary, the absolute positioning data of BDS has small drift and large noise. By using their different error characteristics, the accuracy and stability of fusion positioning can be improved. First, the data (x odom ,y odom ,ρ odom ) and the measurement quantity ρ of the inertial measurement unit imu Perform an EKF data fusion to obtain a fused pose, and then compare the fused data with the absolute positioning information (x bds ,y bds,ρ bds ) performs secondary EKF data fusion and obtains the secondary fusion pose (x kal ,y kal ,ρ kal ). Absolute positioning information (x bds ,y bds ,ρ bds ) is the Beidou positioning data, which is the real-time coordinates of the robot in the UTM coordinate system obtained by the Beidou module from the differential server. It can be used directly for positioning, but its accuracy is not as high as that of fusion positioning.

[0045] In estimation theory, the extended Kalman algorithm is a recursive filter estimation algorithm derived from the hidden Markov model, which makes Gaussian assumptions on confidence and noise and linearizes the motion model and observation model. For some mildly nonlinear non-Gaussian systems, the extended Kalman algorithm can give better results.

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

[0047] x k =f(x k-1 , v k )+w k (2)

[0048] y k =g(x k )+n k (3)

[0049] Among them, x k is the state quantity, y k is the observed quantity, v k is the control variable, f(·) and g(·) are both nonlinear functions, assuming x k The confidence function of is a Gaussian distribution:

[0050]

[0051] in, is the prior predicted state quantity at time 0, v 1:k is the control quantity from 1 to k, y 0:k is the measurement quantity from time 0 to time k, is the posterior state estimate at time k, is the estimated covariance value at time k, N is the mathematical representation of Gaussian distribution, indicating that the expected value is The covariance is Gaussian white noise;

[0052] Assumption: w k and n k It's all white noise.

[0053]

[0054] Among them, Q k is the state equation white noise covariance, R k is the white noise covariance of the observation equation, and formula (5) represents w k The expected value of the state equation is 0, and the white noise covariance Q k is Gaussian white noise, n k The expected value of is 0, and the observation equation white noise covariance R k is Gaussian white noise;

[0055] f(·) and g(·) are differentiable. Due to the nonlinear characteristics of f(·) and g(·), a closed-form solution cannot be calculated. Therefore, it is necessary to linearize the motion and observation models and perform a first-order Taylor expansion of f(·) and g(·).

[0056] Taylor expansion of the equation of motion:

[0057]

[0058] in,

[0059]

[0060] w′ k is the first-order Taylor expansion of motion white noise, F k-1 is the Jacobian matrix of the motion function f(·), is the posterior state estimate at time k-1

[0061] Taylor expansion of the observation equation:

[0062]

[0063] in: is the prior observation at time k, G k is the Jacobian matrix of the observation equation g(·), n′ k is the first-order Taylor expansion of observed white noise;

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

[0065] predict:

[0066]

[0067]

[0068] in, is the prior covariance at time k, is the posterior estimated covariance at time k-1, is the transposed matrix of the Jacobian matrix of the motion function at time k-1, Q′ k is the error covariance between the true value and the predicted value of the state quantity;

[0069] renew

[0070]

[0071]

[0072]

[0073] in: is the update amount, K k is the Kalman gain matrix at time k, is the transposed matrix of the Jacobian matrix of the observation equation at time k, R′ k is the error covariance between the true value and the predicted value of the measured quantity at time k, is the posterior estimated covariance at time k;

[0074] The prediction is based on the current posterior quantity to update the mean and variance according to the motion equation, and the update is to correct the mean and variance according to the observation information. In this way, accurate pose data can be estimated. The classic recursive update equation of EKF, through the update equation, we can get Calculate

[0075] The present invention uses the position information (x odom ,y odom ) and the encoder's direction of motion ρ odom As the observed quantity, the measurement quantity ρ of the inertial measurement unit is used imu As the measurement quantity, an EKF fusion is performed in the robot Odom coordinate system to obtain a fusion pose (x′ kal , y′ kal , ρ′ kal ), to ensure the continuity of the fused data, and then use the absolute positioning information (x bds ,y bds ,ρ bds ) as the observation quantity, and a fusion pose (x′ kal , y′ kal , ρ′ kal ) is the measurement quantity, and the secondary EKF fusion is performed in the global map coordinate system, thereby obtaining the secondary fusion pose (x kal ,y kal ,ρ kal ).

[0076] Step S3: Monte Carlo localization (MCL) is performed based on the secondary fusion pose and the 3D prior map. The particles in the Monte Carlo localization can be based on the secondary fusion pose (x kal ,y kal ,ρ kal ) and variance information are sampled in a normal distribution in the prior map, and the secondary fusion pose (x kal ,y kal ,ρ kal ) as the prior pose of Monte Carlo positioning, and during the operation of the robot, according to the secondary fusion pose (x kal ,y kal ,ρ kal ) to update the particle weights to make the positioning data more accurate. In order to ensure better robustness of positioning and avoid the danger of positioning loss, the present invention uses the secondary fusion pose (x kal ,y kal ,ρ kal ) to perform track calculation and ensure the reliability of positioning in open environments.

[0077] Each particle in Monte Carlo localization has corresponding localization information (x k-1,m ,y k-1,m , ρ k-1,m ) and weight w k-1,m , the final positioning information of the robot is obtained by weighted calculation based on the positioning information and weights of all particles (X k-1 , Y k-1 , W k-1 ); After two EKF fusion calculations, the secondary fusion pose (x kal ,y kal ,ρ kal ) and the corresponding variance information According to the obtained positioning and variance information, the existing particles are sampled in a normal distribution, that is, the positioning information of each particle is updated in a normal distribution manner to obtain the positioning information of each particle at the current k moment (x k,m ,y k,m , ρ k,m ), and then update the weight w of each particle according to the updated particle pose and the current prior map measurement k,m , and finally obtain the precise positioning information of the robot at the current moment (X k , Y k , W k ), making the positioning data more accurate. In order to ensure better robustness of positioning and avoid the danger of positioning loss, the present invention uses the secondary fusion pose (x kal ,ykal ,ρ kal ) increments to perform track dead reckoning to ensure the reliability of positioning in open environments.

[0078] where x k-1,m is the x-axis positioning data of particle number m at time k-1, y k-1,m is the y-axis positioning data of particle number m at time k-1, ρ k-1,m is the angular positioning data of particle number m at time k-1, w k-1,m is the weight of particle number m at time k-1, X k-1 is the robot x-axis positioning data at time k-1, Y k-1 is the robot y-axis positioning data at time k-1, W k-1 is the robot angle positioning data at time k-1, x k,m is the x-axis positioning data of particle number m at time k, y k,m is the y-axis positioning data of particle number m at time k, ρ k,m is the angular positioning data of particle number m at time k, w k,m is the weight of particle number m at time k-1, X k is the robot x-axis positioning data at time k, Y k is the robot y-axis positioning data at time k, W k It is the robot angle positioning data at moment k-1.

[0079] The MCL algorithm, also known as the particle filter algorithm, is a nonlinear recursive Bayesian filter algorithm that solves the posterior probability of posture through the statistical perspective of the law of large numbers. MCL positioning is based on the given prior map information and perception state quantity. According to the state quantity, a group of random particles with weights are sampled to represent the posterior probability of posture estimation, and the current state quantity is calculated and corrected based on these particles and weights. Its advantage is that it is not restricted by the model and Gaussian assumption.

[0080] Bel(x)≈{x (i) , w (i)}i=1,...,n (13)

[0081] Among them, Bel(x) is the particle sample of the random state x, which includes the position and rotation. The weight of each sample particle can be determined by a non-negative parameter, which is also called the importance factor of the particle. It represents the probability of estimating the pose at this sample, w (i) represents the non-negative parameter of the ith particle, x (i) Represents the random state of the i-th particle. The basic MCL recursive update is implemented by the following steps:

[0082] 1) Sampling: Particles are sampled based on the joint probability density of the Bayesian posterior estimate and the noise at the previous moment:

[0083]

[0084] Where: is the estimated value of the posterior state of particle number m at time k-1, w k,m is the weight of particle number m at time k, is the probability density function of the robot state at time k-1, p(w k ) is the particle weight probability density at time k.

[0085] 2) Prediction: Substitute each prior particle and noise sample into the nonlinear motion model:

[0086]

[0087] is the prior predicted state of the particle with sequence number m at time k. These predicted particles together approximately describe the probability density

[0088] 3) Combined with the correction of the posterior probability, it is mainly divided into two steps:

[0089] Step 1: Assign a weight w to each particle based on the expected posterior and predicted posterior of each particle. k,m :

[0090]

[0091] Where η is the normalization coefficient, is the posterior probability density function of the particle state quantity at time k with sequence number m, is the prior probability density function of the particle state quantity with sequence number m at time k;

[0092] Step 2: According to the assigned weight, importance resample each particle:

[0093]

[0094] The EKF fusion pose data based on BDS observations is integrated into the MCL positioning algorithm through the following steps:

[0095] S3-1. Adjust the importance parameters of all samples according to the Euclidean distance between all sample particles and the fusion pose.

[0096] According to the positioning information of all particles at time k-1 (x k-1,m ,y k-1,m , ρ k-1,m ) and secondary fusion pose (xkal ,y kal ,ρ kal ) to update the position information of all particles in the sample at the current moment; (Δx kal,k ,Δy kal,k ,Δρ kal,k )

[0097] That is (x k,m ,y k,m , ρ k,m )=(x k-1,m ,y k-1,m , ρ k-1,m )+(Δx kal,k ,Δy kal,k ,Δρ kal,k )+Z k,m ;

[0098] where Δx kal,k =x kal,k -x kal,k-1 , Δy kal,k =y kal,k -y kal,k-1 , Δρ kal,k =ρ kal,k -ρ kal,k-1 ,

[0099] Δx kal,k is the increment of the x-axis of the fused positioning pose at time k, Δy kal,k is the increment of the y-axis of the fused positioning pose at time k, Δρ kal,k is the increment of the fusion positioning pose angle at time k, Z k,m is the sampling noise of particle number m at time k, x kal,k is the value of the x-axis of the fusion pose at time k, and y kal,k is the value of the y-axis of the fusion pose at time k, ρ kal,k is the value of the fusion pose angle at moment k.

[0100] S3-2. During the initial sampling and resampling process, samples are extracted based on the Gaussian distribution of the fused posture; during the initial sampling process, samples are extracted using the Gaussian distribution according to the prior map information and the perceived state, and the particle weights at the current moment are updated. The real-time posture of the robot is weightedly calculated according to the latest weight and the posture information of particle k at the moment updated in step 1);

[0101] in represents the posterior estimated state of the robot at time k, ∑ represents the weighted calculation of all particles, is the a posteriori estimated state quantity of particle number m at time k, w k,mis the weight of particle m at time k.

[0102] S3-3. Use the fused pose as a sample subset to replace particles with low importance in the sample set;

[0103] During the resampling process, the robot posterior estimated state quantity at the final calculated k moment is used To update the current particle sample set, the weight w k,m Replace the low-quality particles.

[0104] S3-4. Adjust the fusion strategy according to the particle matching degree. That is, when the pose matching degree is low, it means that there are few reference objects nearby and it belongs to an open area. Then, the incremental information of the EKF fusion pose can be used to perform track calculation until the particle filter estimates the pose to regain a higher matching degree, and then use the MCL positioning algorithm.

[0105] The fusion strategy is adjusted according to the matching degree of the particles. When calculating the particle weight and the final real-time posture in step S3-2, the matching degree information of the current posture will be obtained synchronously. If the matching degree is high, the robot posture at the current moment will be used directly. As the final state quantity; if the matching degree is low, such as in a relatively open area or an area with large environmental changes, it means that there are few valid reference objects nearby, and the incremental information ΔX of the positioning pose after the secondary fusion of EKF can be used kal,k To perform dead reckoning, until the particle filter estimates the pose to obtain a high degree of match again, then use the MCL positioning algorithm;

[0106] Right now in represents the robot posture at time k-1, ΔX kal,k =(Δx kal,k ,Δy kal,k ,Δρ kal,k ) is the increment of the two EKF fusion positioning poses.

[0107] The present invention verifies the reliability of the algorithm through two parts. On the one hand, fused data is used for mapping, because the real-time localization and mapping (SLAM) algorithm uses inter-frame registration as an odometer, and back-end optimization and loop detection are used to ensure the long-term reliability of pose estimation and the global consistency of mapping. In particular, loop detection is the most important means to reduce errors in the mapping process, but the dependence of loop conditions and the cumulative registration errors lead to low reliability of loop judgment. Using fused pose data as a loop judgment condition can improve the accuracy of the judgment. In addition, the fused pose as a priori pose can also give the SLAM algorithm a good initial value for iteration, reduce the amount of calculation, and avoid falling into a local optimal solution. Therefore, the algorithm is constructed. Figure 1The consistency is a means to verify the EKF fusion algorithm. The experiment found that the secondary fusion pose (x kal ,y kal ,ρ kal ) as the prior pose for mapping, which can ensure the consistency of mapping, and the map has no ghosting and breaks. And the mapping of open areas is also completed by fusion pose, which shows that fusion pose can improve mapping.

[0108] On the other hand, the reliability and robustness of the overall navigation and positioning framework are verified. The present invention verifies the fusion effect based on the map constructed in the previous step as a priori map. The fusion algorithm verification strategy adopts the closed path method, that is, the starting and ending positions of the robot coincide, and the cumulative error is verified by letting the robot walk along a predetermined trajectory. The robot speed is set to 0.3m / s. The experimental results show that the cumulative error of the fused pose data is small and a closed curve can be formed. This proves the correctness of the EKF fusion pose.

[0109] The present invention fuses sensor data based on a loosely coupled strategy, so it is necessary to test the fusion situation in various situations, such as testing the positioning and navigation accuracy of all sensors under normal conditions, the positioning and navigation accuracy in an open environment, and the positioning and navigation accuracy in a shielded environment.

[0110] The navigation positioning accuracy test is now carried out according to the EKF+MCL positioning strategy. The average value of 10 groups of experiments is taken. The navigation positioning accuracy test results under different environments are shown in Table 1.

[0111] Table 1 Navigation positioning accuracy test results in different environments

[0112]

[0113] It can be concluded from the data in Table 1 that the EKF+MCL positioning strategy can provide a better positioning accuracy requirement. For complex environments, the positioning accuracy can also be guaranteed, thereby ensuring the safety of navigation.

[0114] The present invention uses sensors such as Beidou module, encoder and inertial measurement unit, based on EKF fusion algorithm, to provide real-time posture solution with no cumulative error, high reliability and stability. It enables the inspection robot to complete accurate autonomous navigation in an open environment. In addition, combined with the MCL positioning algorithm, it ensures the positioning accuracy requirements in some obstructed environments and meets the navigation requirements in the complex environment of the substation.

[0115] The present invention has the following characteristics: 1. Based on multi-sensor KEF fusion, Beidou absolute positioning information is used to ensure that the data has no cumulative error in the global situation, and the fusion of other sensors also ensures the real-time positioning requirements; 2. Combined with the MCL positioning algorithm, the fused posture is used as a particle reference for sampling to ensure the reliability of the estimated posture, and combined with the characteristics of laser positioning that is suitable for environments with rich features and complex structures, but not suitable for open environments, Beidou fusion data is used to ensure positioning accuracy and reliability in open environments. The navigation robustness of the inspection robot is improved, making it applicable to the complex and changeable substation environment today.

[0116] The above is a preferred embodiment of the present invention. It should be noted that the above embodiments are only used to illustrate the present invention and are not limiting. The present invention is not limited to the above examples. All technical solutions and improvements that do not depart from the spirit and scope of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A patrol robot positioning method based on multi-sensor fusion, characterized in that: Here are the steps: Step S1, collecting multi-sensor data, including absolute positioning information of the positioning system, data of the encoder in the inspection robot, and data of the inertial measurement unit; the absolute positioning information is: ,in For location, is the heading; the measurement quantity of the inertial measurement unit ,in is the x-axis acceleration, is the y-axis acceleration, is the z-axis acceleration, is the x-axis angular velocity, is the y-axis angular velocity, is the z-axis angular velocity; the encoder data includes the encoder position information and the encoder's direction of motion ; Step S2, EKF fusion of multi-sensor data: Encoder data (x odom ,y odom , ρ odom ) and the measurement quantity ρ of the inertial measurement unit imu Perform an EKF data fusion; obtain a fused pose, and then compare the fused data with the absolute positioning information of the positioning system (x bds , y bds ,ρ bds ) performs secondary EKF data fusion to obtain the secondary fusion pose (x kal , y kal ,ρ kal ); Step S3: Monte Carlo positioning is performed based on the fused pose and the 3D prior map: the particles in the Monte Carlo positioning are based on the secondary fused pose (x kal , y kal ,ρ kal ) and variance information are sampled in a normal distribution in the prior map, and the secondary fusion pose (x kal , y kal ,ρ kal ) as the prior pose of Monte Carlo positioning, and during the operation of the robot, according to the secondary fusion pose (x kal , y kal ,ρ kal ) to update the particle weights; when the number of particles is too small, the secondary fusion pose (x kal ,y kal ,ρ kal ) to perform dead reckoning; In step S3, each particle in the Monte Carlo positioning has corresponding positioning information (x k-1,m ,y k-1,m , ρ k-1,m ) and weight w k-1,m , the final positioning information of the robot is obtained by weighted calculation based on the positioning information and weights of all particles (X k-1 , Y k-1 , W k-1 ); After two EKF fusion calculations, the secondary fusion pose (x kal , y kal ,ρ kal ) and the corresponding variance information According to the obtained positioning and variance information, the existing particles are sampled in a normal distribution, that is, the positioning information of each particle is updated in a normal distribution manner to obtain the positioning information of each particle at the current k moment (x k,m ,y k,m , ρ k,m ), and then update the weight w of each particle according to the updated particle pose and the current prior map measurement k,m , and finally obtain the precise positioning information of the robot at the current moment (X k , Y k , W k ); where x k-1,m is the x-axis positioning data of particle number m at time k-1, y k-1,m is the y-axis positioning data of particle number m at time k-1, ρ k-1,m is the angular positioning data of particle number m at time k-1, w k-1,m is the weight of particle number m at time k-1, X k-1 is the robot x-axis positioning data at time k-1, Y k-1 is the robot y-axis positioning data at time k-1, W k-1 is the robot angle positioning data at time k-1, x k,m is the x-axis positioning data of particle number m at time k, y k,m is the y-axis positioning data of particle number m at time k, ρ k,m is the angular positioning data of particle number m at time k, w k,m is the weight of particle number m at time k-1, X k is the robot x-axis positioning data at time k, Y k is the robot y-axis positioning data at time k, W k The robot angle positioning data at time k-1; The specific process of step S3 is: S3-1. Adjust the importance parameters of all samples according to the Euclidean distance between all sample particles and the fusion pose; S3-2. During the initial sampling and resampling process, samples are extracted based on the Gaussian distribution of the fused pose; S3-3. Use the fused pose as a sample subset to replace particles with low importance in the sample set; S3-4. Adjust the fusion strategy according to the particle matching degree. In the open area, use the incremental information of EKF fusion pose to perform dead reckoning until the particle filter estimated pose regains a high matching degree, and then use the MCL positioning algorithm; The specific process of step S3-1 is as follows: according to the positioning information (x k-1,m ,y k-1,m , ρ k-1,m ) and secondary fusion pose (x kal , y kal ,ρ kal ) to update the position information of all particles in the sample at the current moment; (Δx kal,k ,Δy kal,k , Δρ kal,k ) That is, (x k,m , y k,m , ρ k,m ) = (x k-1,m , y k-1,m , ρ k-1,m ) + (Δx kal,k , Δy kal,k , Δρ kal,k ) + Z k,m ; among themΔx kal,k =x kal,k -x kal,k-1 ,No kal,k =y kal,k -y kal,k-1 ,Dr. kal,k =p kal,k -r kal,k-1 , Δx kal,k is the increment of the x-axis of the fused positioning pose at time k, Δy kal,k is the increment of the y-axis of the fused positioning pose at time k, Δρ kal,k is the increment of the fusion positioning pose angle at time k, Z k,m is the sampling noise of particle number m at time k, x kal,k is the value of the x-axis of the fusion pose at time k, and y kal,k is the value of the y-axis of the fusion pose at time k, ρ kal,k is the value of the fusion pose angle at moment k.

2. The inspection robot positioning method based on multi-sensor fusion according to claim 1 is characterized in that: Step S2 uses the encoder position information (x odom ,y odom ) and the encoder's direction of motion ρ odom As the observed quantity, the measurement quantity ρ of the inertial measurement unit is used imu As a measurement quantity, an EKF fusion is performed in the robot Odom coordinate system to obtain a fusion pose ; Then use the absolute positioning information (x bds , y bds ,ρ bds ) as the observation quantity, and a fusion pose The measured quantity is subjected to secondary EKF fusion in the global map coordinate system, thereby obtaining the secondary fusion pose (x kal , y kal ,ρ kal ).

3. The inspection robot positioning method based on multi-sensor fusion according to claim 1 is characterized in that: In step S3-2, during the initial sampling process, based on the prior map information and the perceived state, a Gaussian distribution is used to extract samples and the particle weights at the current moment are updated. , and weightedly calculate the real-time posture of the robot according to the latest weight and the posture information of particle k updated in step 1); , in represents the posterior estimated state of the robot at time k, represents the weighted calculation of all particles, is the a posteriori estimated state quantity of particle number m at time k, w k,m is the weight of particle m at time k.

4. The inspection robot positioning method based on multi-sensor fusion according to claim 1 is characterized in that: In step S3-3, the robot posterior estimated state quantity at time k obtained by the final calculation is used To update the current particle sample set, the weight w k,m Replace the low-quality particles.

5. The inspection robot positioning method based on multi-sensor fusion according to claim 1 is characterized in that: In step S3-4, the fusion strategy is adjusted according to the matching degree of the particles. When calculating the particle weight and the final real-time posture in step S3-2, the matching degree information of the current posture is obtained synchronously. If the matching degree meets the requirements, the robot posture at the current moment is directly used. As the final state quantity; if the matching degree does not meet the requirements and it is considered to be an open area, the incremental information ΔX of the positioning posture after the secondary fusion of EKF is used kal,k To perform dead reckoning, until the particle filter estimates the pose to obtain a high degree of match again, then use the MCL positioning algorithm, that is, ,in represents the robot posture at time k-1, ΔX kal,k = (Δx kal,k ,Δy kal,k , Δρ kal,k ) is the increment of the two EKF fusion positioning poses.

Images