A positioning method for underground pipeline robots based on Beidou, inertial navigation and vision collaboration

Abstract

The invention discloses a Beidou, inertial navigation and vision-coordinated underground pipeline robot positioning method, which comprises the following steps: firstly, an inertial measurement unit is used to obtain the acceleration and angular velocity information of the robot and correct its output, and then an odometer is used to obtain the mileage data of the robot and identify the pipeline connection point so as to correct its output, and then the angular velocity and mileage information are fused to realize the preliminary prediction of the posture state of the pipeline robot; then, the internal and external parameters of the IMU and the visual sensor are calibrated, and SIFT feature extraction and matching are performed on each captured frame of the image; then, the data of the IMU, mileage information and the binocular camera are fused in a tightly coupled manner, and a nonlinear optimization method is used to obtain the robot posture information with higher accuracy; finally, the three-dimensional position information returned by the Beidou positioning sensor is used to perform posture constraints, and the robot posture information with local accuracy and global drift-free is obtained, so as to realize the precise positioning of the pipeline robot.

Description

Technical Field

[0001] The present invention belongs to the technical field of computer vision, and in particular relates to an underground pipeline robot positioning method that cooperates with Beidou, inertial navigation and vision. Background Art

[0002] With the rapid development of urban infrastructure modernization, the construction of underground pipelines is becoming more and more intensive, and it is an important part of the national infrastructure network. However, during the use of underground pipelines, abnormal problems often occur inside the pipelines, which will seriously affect the safety and function of the facilities and need to be inspected regularly. Due to the concealment and difficulty of underground pipeline construction, the positioning of pipeline robots has become a problem that troubles technicians. Therefore, how to accurately and quickly realize the positioning of robots in pipelines has become a technical problem that needs to be studied. Summary of the invention

[0003] In order to solve the above problems, the present invention provides an underground pipeline robot positioning method that cooperates with Beidou, inertial navigation and vision, which can accurately and quickly realize the positioning of the robot in the pipeline.

[0004] The present invention specifically provides a Beidou, inertial navigation and vision-coordinated underground pipeline robot positioning method, the underground pipeline robot positioning method comprising the following steps:

[0005] Step S1: Use IMU, odometer, binocular camera and Beidou positioning sensor to obtain the acceleration, angular velocity, mileage, environmental information around the robot and satellite positioning information of the pipeline robot in real time;

[0006] Step S2: Using a fast intelligent method to update the angular velocity information using the accelerometer output of the IMU as an observation value;

[0007] Step S3: Use the wavelet transform modulus maximum method and the acceleration data of the IMU to identify the pipeline connection point, and then correct the output of the odometer. Finally, use the angular velocity and mileage information to preliminarily estimate the position information of the pipeline robot;

[0008] Step S4: calibrate the external parameters of the camera and IMU, detect and describe the SIFT feature points of the image sequence taken by the camera, and match the feature point pairs;

[0009] Step S5: The angular velocity obtained in step S2, the mileage information obtained in step S3, and the matching feature point pair information obtained in step S4 are fused in a tightly coupled manner, and the robot posture information with higher accuracy is obtained through nonlinear optimization;

[0010] Step S6: Constrain the robot posture information obtained in step S5 according to the three-dimensional position information returned by the Beidou positioning sensor, eliminate the accumulated error of the robot posture estimation, and achieve accurate positioning of the pipeline robot.

[0011] Compared with the prior art, the present invention has the following beneficial effects:

[0012] (1) The present invention proposes to use IMU, odometer and camera for collaborative processing to obtain pipeline robot positioning results with high accuracy. It combines the advantages of IMU to provide better pose estimation for fast movement and visual sensor to correct IMU data drift in slow movement, thus solving the problem that camera sensors are difficult to perform effective pose estimation in pipeline environments with weak textures.

[0013] (2) The present invention first uses a fast intelligent algorithm to use the accelerometer output of the IMU as an observation value to update the angular velocity information, reduce the angular velocity measurement noise and the error caused by integration, and then integrates the pre-integration of the angular velocity and mileage information, making the subsequent robot posture estimation simpler and the result more accurate.

[0014] (3) The present invention first uses the wavelet transform modulus maximum method to process the acceleration data to identify the pipeline connection point, and then uses the known length of each pipeline section to correct the mileage information.

[0015] (4) The present invention adds a pose graph optimization algorithm based on the collaborative estimation of the robot's pose by IMU, odometer, and camera sensors, considers the global robot pose constraints, and uses pose graph optimization to further correct the pose estimation of the pipeline robot.

[0016] (5) Due to the cumulative error in the pose estimation of the pipeline robot, the positioning result of the pipeline robot will drift over time. The present invention constrains the robot pose obtained by the IMU, odometer, and visual sensor in collaboration based on the three-dimensional position information returned by the Beidou positioning sensor, eliminates the cumulative error in the robot pose estimation, and can achieve accurate positioning of the pipeline robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 is a flow chart of the method of the present invention;

[0018] Figure 2 is the spatial relationship between the camera, IMU and calibration board;

[0019] Figure 3 This is a schematic diagram of the camera projection model;

[0020] Figure 4 Schematic diagram of the global pose graph optimization algorithm. DETAILED DESCRIPTION

[0021] The following is a detailed description of a specific implementation method of an underground pipeline robot positioning method using Beidou, inertial navigation and vision collaboration according to the present invention in conjunction with the accompanying drawings.

[0022] like Figure 1 As shown, the underground pipeline robot positioning method of the present invention comprises the following steps:

[0023] Step S1: Use IMU, odometer, camera and Beidou positioning sensor to obtain the acceleration, angular velocity, mileage information, environmental information around the robot and satellite positioning information of the pipeline robot in real time;

[0024] Step S2: Using a fast intelligent method to update the angular velocity information using the accelerometer output of the IMU as an observation value;

[0025] Step S3: Use the wavelet transform modulus maximum method and the accelerometer output data of the IMU to identify the pipeline connection point, and then correct the output of the odometer. Finally, use the angular velocity and mileage information to preliminarily estimate the position information of the pipeline robot;

[0026] Step S4: calibrate the external parameters of the camera and IMU, detect and describe the SIFT feature points of the image sequence taken by the camera, and match the feature point pairs;

[0027] Step S5: The angular velocity obtained in step S2, the mileage information obtained in step S3, and the matching feature point pair information obtained in step S4 are fused in a tightly coupled manner, and the robot posture information with higher accuracy is obtained through nonlinear optimization;

[0028] Step S6: Constrain the robot posture information obtained in step S5 according to the three-dimensional position information returned by the Beidou positioning sensor, eliminate the accumulated error of the robot posture estimation, and achieve accurate positioning of the pipeline robot.

[0029] Specifically, step S1 includes:

[0030] A binocular camera and Beidou positioning sensor are installed in front of the robot, an IMU is installed at the center of mass, and an odometer is installed on the driving wheel.

[0031] Specifically, step S2 includes:

[0032] The robot's gyro drift potential value is encoded as a decimal number into the initial particles, and the gyro drift encoded in each particle is subtracted from the gyro output to generate corrected gyro data for navigation solution based on inertial navigation differential equations;

[0033] Virtually extend the update interval of the corrected gyroscope data and the acceleration data output by the accelerometer in the IMU;

[0034] Among them, T sIndicates the robot data update interval, which is extended by k0 times. The extended update interval is:

[0035]

[0036] The accumulated error in longitude is expressed as:

[0037] δλ(t)=-(ε N oeLh t +ε U oeLh t )t (2)

[0038] Among them, L t represents the position; n represents the local horizontal navigation coordinate system, that is, the orthogonal reference coordinate system aligned with the northeast-up (ENU) geodetic coordinate axis; b represents the robot body coordinate system, that is, the orthogonal reference coordinate system aligned with the IMU axis; e represents the earth coordinate system, that is, the orthogonal reference coordinate system centered on the center of the earth and fixed to the earth (ECEF); ε n =[ε E ,ε N ,ε U ] represents the gyroscope drift in the n coordinate system, where ε E ,ε N ,ε U denote the eastward drift, northward drift and celestial drift respectively, ε b =[ε x ,ε y ,ε z ] represents the gyroscope drift in the b coordinate system;

[0039] Collect a period of inertial navigation solution data, the duration is T m , then the linear term of the longitude error is:

[0040] δλ(T m )=-(ε N oeLh t +ε U oeLh t )T m (3)

[0041] When the update interval is virtually extended to k0 times the original, the linear term of the longitude error is:

[0042] δλ(k0T m )=-(ε N oeLh t +ε U oeLh t )k0T m =k0δλ(T m ) (4)

[0043] k0 represents the multiple by which the update interval is extended.

[0044] The fitness function of PSO is established based on the position error and velocity error of the robot to evaluate the performance of each particle;

[0045] The expression of the fitness function is as follows:

[0046] J=α1J1+α2J2+α4J4+α8J8+α 16 J 16 (5)

[0047] Among them α1, α2, α4, α8 and α 16 is the weighted value;

[0048]

[0049] Where t0 represents the starting time of gyroscope drift estimation, t e Indicates the estimated end time; represents the eastward velocity error, represents the north velocity error, e Lat (t) represents the latitude error; e Lon (t) represents the longitude error, ρ1, ρ2, ρ3, ρ4 are weighted values;

[0050] Refresh individual best value p best and the global optimal value g best , refresh the velocity and coordinates of each particle in the problem space;

[0051] The particle velocity and coordinate update algorithm is expressed as follows:

[0052]

[0053]

[0054] In the formula, and are the speed and coordinates of the particle respectively; m is the number of generations of the current population; d=1,2,3,…,N is the dimension of the search space; i=1,2,3,…,N is the population size; r1 and r2 are two random numbers between [0,1]; c1 and c2 are learning factors, which represent the learning ability of the particle from itself and other particles respectively, and generally c1 and c2 are between [0,2]; is the inertia weight coefficient, which is used to adjust the diversity of particles; and are the individual optimal value and the global optimal value of the current population respectively;

[0055] If the termination condition is not met, the newly created population is re-evaluated and the process is repeated. After 50 generations of searching, the process ends. The optimal particle compensates the gyro output using the estimated gyro drift to obtain the corrected gyro data.

[0056] Step S3 includes:

[0057] S3-1: First, the acceleration data is obtained from the IMU, and then the wavelet transform modulus maximum (WTMM) is used to identify the pipeline connection points under noise interference; the denoising results are subjected to wavelet decomposition based on different levels j to obtain a series of pipeline connection locations;

[0058] If f(t) satisfies the following conditions at t0:

[0059]

[0060] Among them, h1 is a small quantity, A is a constant, P n is the nth-order Taylor expansion polynomial of f(t) at time t0, α L is the Lipschitz index, which can characterize the singularity of f(t) at time t0, and with α L As the singularity increases, it becomes weaker; if t∈[a,b], the mother wavelet is continuously differentiable and The form of decay is as follows:

[0061]

[0062] Among them, the Lipschitz index of f(t) is α L , A is a constant, is the modulus maximum, j is the scale; the signal satisfies α L >0, the noise satisfies α L <0, that is, as j changes, the signal and noise can be separated;

[0063] As j increases, the wavelet coefficients are reconstructed by the alternating projection method, and then the inverse wavelet transform is performed; when constructing the modulus maximum sequence, the detail coefficients are reduced to zero by the soft threshold to ensure the smoothness of the reconstructed signal; the amplitude peak is selected as the location of the pipeline connection point;

[0064] S3-2: Calculate the mileage difference between the acceleration peak position and the corresponding reference peak position, and then adjust the odometer output data between the acceleration peak at this position and the previous peak or starting point; if the accumulated distance of the odometer is greater than the actual distance, truncate the odometer output data; if the accumulated distance of the odometer is less than the actual distance, compensate the odometer output data; finally obtain the mileage information P at time t t :

[0065]

[0066] Where P t ′ is the original data of the odometer, ΔP is the mileage difference between the acceleration peak position and the corresponding reference peak position, n a is the number of peak accelerations, L pipe is the pipeline length, θ is the constraint factor, and only when the mileage difference is within a certain range can it be considered a reasonable error;

[0067] S3-3: Obtained mileage information P t and the angular velocity ω at time t and time t+1 t Preliminary estimate of the motion information of the pipeline robot at time t+1; using the average angular velocity data ω t ′ and the robot posture Q at time t t Obtain the posture Q at time t+1 t+1 :

[0068] Q t+1 =Q t (ω t ′Δt) (12)

[0069]

[0070] The posture contains the three-dimensional direction information of the pipeline robot movement, where B g represents the angular velocity bias, Δt represents the time interval between two frames of angular velocity data; the robot posture at the initial moment is defined as the unit matrix, and the IMU acceleration data a t It is measured in its own coordinate system b, so it needs to be converted to the world coordinate system e according to the robot's posture at the corresponding moment:

[0071] a t,e =Q t (a t,b -B a )-g (14)

[0072] a t+1,e =Q t+1 (a t+1,b -B a )-g (15)

[0073] Among them B a and g represent the acceleration bias and gravitational acceleration, respectively;

[0074] Finally, according to the robot's posture and mileage information at time t, the direction and distance of the pipeline robot's movement are obtained, and a preliminary estimation of the pipeline robot's motion state is achieved.

[0075] Specifically, step S4 includes:

[0076] Step S4-1: Calibrate the camera and IMU external parameters; the time-invariant parameter that needs to be estimated during the calibration experiment is the gravity direction g relative to the world coordinate system e , the time offset d between the camera and IMU measurement data t , the spatial transformation S of the camera and IMU c,b , the time-varying parameter to be estimated is the 6-DOF IMU pose S e,b , acceleration and angular velocity bias b a and b w ; Figure 2 The figure shows the spatial relationship between the coordinate systems in the calibration experiment, where e, c, and b represent the world coordinate system, camera coordinate system, and IMU coordinate system, respectively. S represents the spatial transformation between the coordinate systems, and the relationship between them is:

[0077] S e,b =S e,c S c,b (16)

[0078] First, a set of camera and IMU measurement data is collected in a short period of time, the time period is T1 = [t1, t k ] (usually 1-2 minutes), the timestamp of the picture acquired by the camera sensor is set to t j , where the pixel coordinate of each corner point is y mj , m k is the number of corner points in a single frame image; since the structure of the calibration plate is known, the world coordinates corresponding to each corner point It can be obtained according to the actual size of the calibration plate; therefore, the pixel coordinates of the corner points and the k The measured values ​​of acceleration and angular velocity α sampled at k , Defined as:

[0079]

[0080] Among them (S e,b ) r represents the rotation component of the IMU posture, a(t k ) and w(t k ) represents the acceleration and angular velocity generated during the IMU movement. Indicates the world coordinates corresponding to each corner point, Indicates the compensation amount of the corner pixel coordinates, n ak Indicates the compensation amount of acceleration, n wkRepresents the compensation amount of angular velocity, H represents the observation model of the corner point; each n and the bias of acceleration and angular velocity are assumed to obey a Gaussian distribution with a mean of 0 and a variance of R and are statistically independent; then the quadratic integral of acceleration and the first integral of angular velocity are used to obtain the pose estimation between the adjacent measurement data frames of the IMU, that is, the n-1th frame and the nth frame, and the pose estimation of the current frame nth frame IMU relative to the world coordinate system is obtained by combining the pose relationship of the n-1th frame IMU relative to the world coordinate system. Finally, the error term related to the measurement is constructed as the difference between the measurement and the predicted measurement given the current state estimate, and the error function f of the variable to be estimated is i Defined as:

[0081]

[0082] The subscripts represent the error functions of the corresponding variables. represents the acceleration bias matrix, represents the angular velocity bias matrix, t k Indicates the time of the last frame; the Levenberg-Marquardt (LM) algorithm is used to minimize the cumulative sum of all error terms, and the maximum likelihood estimate of all unknown parameters is found through iteration; during the iteration process, the time offset is initialized to 0, and the initial relative position of the camera and IMU can be set according to the hardware installation direction;

[0083] Step S4-2: SIFT feature point extraction; scale space L of source point cloud set and target point cloud set S (x,y,z,σ) and L D (x, y, z, σ) are expressed as:

[0084] L S (x,y,z,σ)=G S (x,y,z,σ)f S (x,y,z) (19)

[0085] L D (x,y,z,σ)=G D (x,y,z,σ)f D (x,y,z) (20)

[0086] In the formula, G S (x,y,z,σ) and G D (x, y, z, σ) are the variable-scale Gaussian functions of the source point cloud set, the target point cloud set, and the target point cloud set, respectively, where σ represents the scale space factor, which is used to measure the image blur; f S (x,y,z) and f D (x, y, z) are the curvature value functions of the source point cloud and the target point cloud respectively;

[0087]

[0088]

[0089] Then, find the extreme points in the scale space of the source point cloud and the target point cloud respectively; construct the Gaussian difference function D of the source point cloud and the target point cloud respectively. S (x,y,z,σ) and D D (x,y,z,σ).

[0090]

[0091]

[0092] Where τ represents the scale factor of the adjacent Gaussian scale space, L S (x, y, z, σ) is the scale space of the source point cloud, L D (x, y, z, σ) represents the scale space of the target point cloud;

[0093] Use DOG to detect feature points in each scale space, and compare the DOG function value of each sampling point with the adjacent points in the image space and scale space. If the DOG function value of the sampling point in the current scale space and the adjacent scale space is the largest or smallest, then the sampling point is an extreme point in the scale space;

[0094] Step S4-3: SIFT descriptor calculation and feature matching; using binary descriptor BRIEF, the description vector consists of 0 and 1; taking the detected SIFT key point p as the center, select a pixel area, then select n pixel pairs in the area according to a certain rule, and for each pixel pair (I i ,I j ) makes the following judgment:

[0095]

[0096] The binary data τ(p,i,j) obtained after judgment is written into the form of an N-dimensional vector to obtain the SIFT descriptor; then the Hamming distance between descriptors is used as a measure to match the feature points using the fast approximate nearest neighbor algorithm, and the progressive sampling consistency (PRSAC) algorithm is used to purify and optimize the feature points to obtain accurate feature matching point pairs between images.

[0097] Specifically, step S5 includes:

[0098] Step S5-1: Construct a motion state estimation model of the pipeline robot; according to the sensor fusion theory, the angular velocity, mileage integral information and matching feature point pair information are fused in a tightly coupled manner, and the resulting dynamic model is:

[0099]

[0100] where q k Represents the robot state, including the rotation matrix R and the translation matrix T, m k represents the observation of the model, that is, the pixel coordinates of the feature points obtained by matching, h represents the robot state observation model, and f k-1 represents the state calculation function, u k-1 represents the model input, w k-1 represents the random process noise, υ k represents random observation noise, all of which are assumed to have a mean of 0 and a variance of R k and Q k Gaussian white noise:

[0101] w k ~N(0,R k ),υ k ~N(0,Q k )(27)

[0102] Then, the motion state of the pipeline robot to be estimated is inferred through the noisy observations and the input of the model, that is, the maximum a posteriori probability P(q k |u k-1 ,m k ), according to Bayes’ rule, we can get:

[0103]

[0104] Convert the maximum a posteriori probability problem of the robot state into solving the robot state q k The maximum likelihood estimate of is:

[0105]

[0106] Step S5-2: Construct a model of the objective function and camera projection; first, Figure 3 The figure shows the camera projection imaging process. c is the optical center of the camera, f is the focal length of the camera, and point P in the camera coordinate system in the real space is projected onto the physical imaging plane through the optical center O, and point P′ is obtained in the image coordinate system; let the coordinates of point P be (X, Y, Z), and the coordinates of point P′ be (X c ,Y c ), Z represents the distance from point P to the camera plane XY, that is, the depth; according to the camera projection model and similar triangles:

[0107]

[0108] Then, the image coordinate system is converted to the pixel coordinate system; this process includes sampling, quantization, scaling, translation, and unit conversion; the u-axis coordinate in the pixel coordinate system is scaled by α relative to the image coordinate system cp times, the v-axis coordinate is scaled by β relative to the image coordinate system cp times, origin O P The relative image coordinate system is translated by [d x ,d y ]; therefore, the relationship between the point P′ and the pixel coordinates [u,v] in the image coordinate system is:

[0109]

[0110] Combining equations (30) and (31), we obtain:

[0111]

[0112] Let α cp f is f x , β cp f is f y The following formula is obtained:

[0113]

[0114] Where K represents the camera intrinsic parameter matrix, which is obtained through camera calibration experiments; assuming that there are n pairs of matched feature points (p1, p1′), (p2, p2′), ···, (p n ,p n ′) and the depth value Z corresponding to the feature point has been obtained. It is necessary to obtain the rotation matrix R and translation matrix T between adjacent image frames, which include the spatial relationship between the camera and IMU and the robot motion state obtained by solving the angular velocity and mileage information. The error term e is defined i for:

[0115] e i =p i -(Rp i ′+T) (34)

[0116] Then construct the error square sum loss function as the objective function F in the robot state optimization process:

[0117]

[0118] Step S5-3: Solve the robot posture by nonlinear optimization; the above objective function can be defined as the least squares problem in the overall sense, and the nonlinear optimization method and matching feature point pairs are used to obtain R and T that minimize the sum of squared errors; the Levenberg-Marquardt algorithm is used to solve the problem, and the error function is Taylor expanded:

[0119] e(x+Δx)=e(x)+e′(x)Δx (36)

[0120] Where Δx represents the descending vector of the substitution variable in the iterative optimization process, and e′ is the derivative of the error function with respect to the substitution variable; then the minimization problem of the objective function is transformed into solving Δx that minimizes the objective function. * :

[0121]

[0122] Let the derivative of the objective function with respect to Δx be 0, solve the vector Δx that minimizes the objective function, and obtain the vector equation:

[0123] e′(x) T e′(x)Δx=-e′(x) T e(x) (38)

[0124] The initial value of the robot state is set to the unit matrix, and the error and descent vector are solved in each iteration process. If Δx is small enough, the iteration is stopped and x is set. k+1 =x k +Δx.

[0125] Specifically, step S6 includes:

[0126] Step S6-1: pose graph optimization; the pipeline robot state obtained by nonlinear optimization in step S5 does not take into account the pose constraints of the robot at the previous moment, and the relative motion between the poses deviates from the true value. The pose estimation of the pipeline robot is corrected again by using pose graph optimization; Figure 4 As shown in the figure, the pose graph consists of nodes and edges. The nodes represent the pose information of the pipeline robot. The two pose nodes P i and P j The relative motion ΔP ij Represents the edge of the pose graph, and the constraint relationship between poses is The optimization variables are the positions of each vertex, the edges come from the position observation constraints, and the least squares error e is constructed ij :

[0127]

[0128] Where ∨ represents the operation of converting the matrix into a vector, and the optimization variable is P i and P j ; The derivative of the disturbance term δξ of the error with respect to the optimization variable is:

[0129]

[0130] where ξ represents the Lie algebra corresponding to the pose matrix in the special Euclidean group, f rrepresents the Jacobian matrix, Ad represents the adjoint transformation, ρ k and Represents the rotation component and the translation component. The corresponding relationship between the variables is:

[0131]

[0132] Where ∧ represents the operation from vector to matrix, and I is the unit matrix. According to the constructed error function and the derivative of the error with respect to the posture, the Levenberg-Marquardt algorithm is used to obtain the posture information after the posture graph is optimized.

[0133] Step S6-2: Integrate Beidou positioning information to eliminate the cumulative error of the robot's pose estimation; the pose estimation of the pipeline robot obtained through the above steps will drift as the robot's track becomes longer, resulting in cumulative errors; according to the longitude lati, latitude longi and altitude information alti returned by the Beidou positioning sensor, it is converted into a global three-dimensional coordinate (X-axis) with the earth as the reference system. g ,Y g ,Z g ):

[0134]

[0135] Where L e Represents the radius of the earth, takes the global three-dimensional coordinates as constraints, combines the acquired pipeline robot posture information for global optimization, and establishes the overall error model:

[0136]

[0137] Where k represents a certain moment. The initial three-dimensional position of the robot is set to the three-dimensional coordinates returned by the Beidou positioning device. The rotation matrix R and translation matrix T after pose graph optimization are used as the initial values ​​of global optimization. The Levenberg-Marquardt algorithm is used for iterative optimization to obtain locally accurate and globally drift-free robot pose information, thus achieving precise positioning of the pipeline robot.

[0138] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention and are not intended to limit the present invention. A person skilled in the art should understand that the specific implementation of the present invention can be modified or replaced by equivalents, but these modifications or changes are within the scope of protection of the pending claims.

Claims

1. A Beidou, inertial navigation and vision-coordinated underground pipeline robot positioning method, characterized in that: The following steps are involved: Step S1: Use IMU, odometer, binocular camera and Beidou positioning sensor to obtain the acceleration, angular velocity, mileage, environmental information around the robot and satellite positioning information of the pipeline robot in real time; Step S2: Using a fast intelligent method to update the angular velocity information using the accelerometer output of the IMU as an observation value; Step S3: Use the wavelet transform modulus maximum method and the acceleration data of the IMU to identify the pipeline connection point, and then correct the output of the odometer; finally, use the angular velocity and mileage information to preliminarily estimate the position information of the pipeline robot; Step S4: calibrate the external parameters of the camera and IMU, detect and describe the SIFT feature points of the image sequence taken by the camera, and match the feature point pairs; Step S5: The angular velocity obtained in step S2, the mileage information obtained in step S3, and the matching feature point pair information obtained in step S4 are fused in a tightly coupled manner, and the robot posture information with higher accuracy is obtained through nonlinear optimization; Step S6: Constrain the robot posture information obtained in step S5 according to the three-dimensional position information returned by the Beidou positioning sensor, eliminate the accumulated error of the robot posture estimation, and achieve accurate positioning of the pipeline robot.

2. According to claim 1, a Beidou, inertial navigation and vision coordinated underground pipeline robot positioning method is characterized in that: Step S1 includes: installing a binocular camera and a Beidou positioning sensor in front of the robot, installing an IMU at the center of mass, and installing an odometer on the driving wheel.

3. The underground pipeline robot positioning method based on Beidou, inertial navigation and vision collaboration according to claim 1 is characterized in that: Step S2 includes: The robot's gyro drift potential value is encoded as a decimal number into the initial particles, and the gyro drift encoded in each particle is subtracted from the gyro output to generate corrected gyro data for navigation solution based on inertial navigation differential equations; Virtually extend the update interval of the corrected gyroscope data and the acceleration data output by the accelerometer in the IMU; Among them, T s Indicates that the robot data update interval is extended to k0 times the original, and the extended update interval is: The accumulated error in longitude is expressed as: δλ(t)=-(ε N whatL t +ε U whatL t )t (2) Among them, L t represents the position; n represents the local horizontal navigation coordinate system, that is, the orthogonal reference coordinate system aligned with the northeast-up (ENU) geodetic coordinate axis; b represents the robot body coordinate system, that is, the orthogonal reference coordinate system aligned with the IMU axis; e represents the earth coordinate system, that is, the orthogonal reference coordinate system centered on the center of the earth and fixed to the earth (ECEF); ε n =[ε E ,ε N ,ε U ] represents the gyroscope drift in the n coordinate system, where ε E ,ε N ,ε U denote the eastward drift, northward drift and celestial drift respectively, ε b =[ε x ,ε y ,ε z ] represents the gyroscope drift in the b coordinate system; Collect a period of inertial navigation solution data, the duration is T m , then the linear term of the longitude error is: δλ(T m )=-(ε N whatL t +ε U whatL t )T m (3) When the update interval is virtually extended to k0 times the original, the linear term of the longitude error is: dl(k0T m )=-(e N cosL t +e U cosL t )k0T m =k0δλ(T m ) (4) k0 represents the multiple by which the update interval is extended; The fitness function of PSO is established based on the position error and velocity error of the robot to evaluate the performance of each particle; The expression of the fitness function is as follows: J=α1J1+α2J2+α4J4+α8J8+α 16 J 16 (5) Among them α1, α2, α4, α8 and α 16 is the weighted value; Where t0 represents the starting time of gyroscope drift estimation, t e Indicates the estimated end time; e VE (t) represents the eastward velocity error, e VN (t) represents the north velocity error, e Lat (t) represents the latitude error; e Lon (t) represents the longitude error, ρ1, ρ2, ρ3, ρ4 are weighted values; Refresh individual best value p best and the global optimal value g best , refresh the velocity and coordinates of each particle in the problem space; The particle velocity and coordinate update algorithm is expressed as follows: In the formula, and are the speed and coordinates of the particle respectively; m is the number of generations of the current population; d=1,2,3,…,N is the dimension of the search space; i=1,2,3,…,N is the population size; r1 and r2 are two random numbers between [0,1]; c1 and c2 are learning factors, which represent the learning ability of the particle from itself and other particles respectively, and generally c1 and c2 are between [0,2]; ω a It is an inertial weight coefficient, used to adjust the diversity of particles; and They are the individual optimal value and the global optimal value of the current population respectively; If the termination condition is not met, the newly created population is re-evaluated and the process is repeated. After 50 generations of searching, the process ends. The optimal particle compensates the gyro output using the estimated gyro drift to obtain the corrected gyro data.

4. The underground pipeline robot positioning method according to claim 1, characterized in that, Step S3 includes: S3-1: First, the acceleration data is obtained from the IMU, and then the wavelet transform modulus maximum (WTMM) is used to identify the pipeline connection points under noise interference; the denoising results are subjected to wavelet decomposition based on different levels j to obtain a series of pipeline connection locations; If f(t) satisfies the following conditions at t0: Among them, h1 is a small quantity, A is a constant, P n is the nth-order Taylor expansion polynomial of f(t) at time t0, α L is the Lipschitz index, which can characterize the singularity of f(t) at time t0, and with α L As the singularity increases, it becomes weaker; if t∈[a,b], the mother wavelet is continuously differentiable and The form of decay is as follows: Among them, the Lipschitz index of f(t) is α L , A is a constant, is the modulus maximum, j is the scale; the signal satisfies α L >0, the noise satisfies α L <0, that is, as j changes, the signal and noise can be separated; As j increases, the wavelet coefficients are reconstructed by the alternating projection method, and then the inverse wavelet transform is performed; when constructing the modulus maximum sequence, the detail coefficients are reduced to zero by the soft threshold to ensure the smoothness of the reconstructed signal; the amplitude peak is selected as the location of the pipeline connection point; S3-2: Calculate the mileage difference between the acceleration peak position and the corresponding reference peak position, and then adjust the odometer output data between the acceleration peak at this position and the previous peak or starting point; if the accumulated distance of the odometer is greater than the actual distance, truncate the odometer output data; if the accumulated distance of the odometer is less than the actual distance, compensate the odometer output data; finally obtain the mileage information P at time t t : Where P t ′ is the original data of the odometer, ΔP is the mileage difference between the acceleration peak position and the corresponding reference peak position, n a is the number of peak accelerations, L pipe is the pipeline length, θ is the constraint factor, and only when the mileage difference is within a certain range can it be considered a reasonable error; S3-3: Obtained mileage information P t and the angular velocity ω at time t and time t+1 t Preliminary estimate of the motion information of the pipeline robot at time t+1; using the average angular velocity data ω t ′ and the robot posture Q at time t t Obtain the posture Q at time t+1 t+1 : Q t+1 =Q t (ω t ′Δt) (12) The posture contains the three-dimensional direction information of the pipeline robot movement, where b w represents the angular velocity bias, Δt represents the time interval between two frames of angular velocity data; the robot posture at the initial moment is defined as the unit matrix, and the IMU acceleration data a t It is measured in its own coordinate system b, so it needs to be converted to the world coordinate system e according to the robot's posture at the corresponding moment: a t,e =Q t (a t,b -b a )-g (14) a t+1,e =Q t+1 (a t+1,b -b a )-g (15) where b a and g represent the acceleration bias and gravitational acceleration, respectively; Finally, according to the robot's posture and mileage information at time t, the direction and distance of the pipeline robot's movement are obtained, and a preliminary estimation of the pipeline robot's motion state is achieved.

5. The underground pipeline robot positioning method based on Beidou, inertial navigation and vision collaboration according to claim 1 is characterized in that: Step S4 includes: Step S4-1: Calibrate the camera and IMU external parameters; the time-invariant parameter that needs to be estimated during the calibration experiment is the gravity direction g relative to the world coordinate system e , the time offset d between the camera and IMU measurement data t , the spatial transformation S of the camera and IMU c,b , the time-varying parameter to be estimated is the 6-DOF IMU pose S e,b , acceleration and angular velocity bias b a and b w ; S represents the spatial transformation between the coordinate systems, and the relationship between them is: S e,b =S e,c S c,b (16) First, a set of camera and IMU measurement data is collected in a short period of time, the time period is T1 = [t1, t k ], the timestamp of the picture acquired by the camera sensor is set to t j , where the pixel coordinate of each corner point is y mj , m k is the number of corner points in a single frame image; since the structure of the calibration plate is known, the world coordinates corresponding to each corner point It can be obtained according to the actual size of the calibration plate; therefore, the pixel coordinates of the corner points and the k The measured values ​​of acceleration and angular velocity α sampled at k , Defined as: Among them (S e,b ) r represents the rotation component of the IMU posture, a(t k ) and w(t k ) represents the acceleration and angular velocity generated during the IMU movement, g e represents the acceleration due to gravity, Indicates the world coordinates corresponding to each corner point, Indicates the compensation amount of the corner pixel coordinates, n ak Indicates the compensation amount of acceleration, n wk represents the compensation amount of angular velocity, H represents the observation model of the corner point; each n and the bias of acceleration and angular velocity are assumed to obey a Gaussian distribution with a mean of 0 and a variance of R and are statistically independent; then the quadratic integral of acceleration and the first integral of angular velocity are used to obtain the pose estimation between the adjacent measurement data frames of the IMU, that is, the n-1th frame and the nth frame, and the pose estimation of the current frame nth frame IMU relative to the world coordinate system is obtained by combining the pose relationship of the n-1th frame IMU relative to the world coordinate system; finally, the error term related to the measurement is constructed as the difference between the measurement and the predicted measurement given the current state estimate, and the error function f of the variable to be estimated is i Defined as: The subscripts represent the error functions of the corresponding variables. represents the acceleration bias matrix, represents the angular velocity bias matrix, t k Indicates the time of the last frame; the Levenberg-Marquardt (LM) algorithm is used to minimize the cumulative sum of all error terms, and the maximum likelihood estimate of all unknown parameters is found through iteration; during the iteration process, the time offset is initialized to 0, and the initial relative position of the camera and IMU can be set according to the hardware installation direction; Step S4-2: SIFT feature point extraction; scale space L of source point cloud set and target point cloud set S (x,y,z,σ) and L D (x, y, z, σ) are expressed as: L S (x,y,z,σ)=G S (x,y,z,σ)f S (x,y,z) (19) L D (x,y,z,σ)=G D (x,y,z,σ)f D (x,y,z) (20) In the formula, G S (x,y,z,σ) and G D (x, y, z, σ) are the variable-scale Gaussian functions of the source point cloud set, the target point cloud set, and the target point cloud set, respectively, where σ represents the scale space factor, which is used to measure the image blur; f S (x,y,z) and f D (x, y, z) are the curvature value functions of the source point cloud and the target point cloud respectively; Then, find the extreme points in the scale space of the source point cloud and the target point cloud respectively; construct the Gaussian difference function D of the source point cloud and the target point cloud respectively. S (x,y,z,σ) and D D (x,y,z,σ). Where τ represents the scale factor of the adjacent Gaussian scale space, L S (x, y, z, σ) is the scale space of the source point cloud, L D (x, y, z, σ) represents the scale space of the target point cloud; Use DOG to detect feature points in each scale space, and compare the DOG function value of each sampling point with the adjacent points in the image space and scale space. If the DOG function value of the sampling point in the current scale space and the adjacent scale space is the largest or smallest, then the sampling point is an extreme point in the scale space; Step S4-3: SIFT descriptor calculation and feature matching; using binary descriptor BRIEF, the description vector consists of 0 and 1; taking the detected SIFT key point p as the center, select a pixel area, then select n pixel pairs in the area according to a certain rule, and for each pixel pair (I i ,I j ) makes the following judgment: The binary data τ(p,i,j) obtained after judgment is written into the form of an N-dimensional vector to obtain the SIFT descriptor; then the Hamming distance between descriptors is used as a measure to match the feature points using the fast approximate nearest neighbor algorithm, and the progressive sampling consistency (PRSAC) algorithm is used to purify and optimize the feature points to obtain accurate feature matching point pairs between images.

6. The underground pipeline robot positioning method based on Beidou, inertial navigation and vision collaboration according to claim 1 is characterized in that: Step S5 includes: Step S5-1: Construct a motion state estimation model of the pipeline robot; according to the sensor fusion theory, the angular velocity, mileage integral information and matching feature point pair information are fused in a tightly coupled manner, and the resulting dynamic model is: where q k Represents the robot state, including the rotation matrix R and the translation matrix T, m k represents the observation of the model, that is, the pixel coordinates of the feature points obtained by matching, h represents the robot state observation model, and f k-1 represents the state calculation function, u k-1 represents the model input, w k-1 represents the random process noise, υ k represents random observation noise, all of which are assumed to have a mean of 0 and a variance of R k and Q k Gaussian white noise: w k ~N(0,R k ),υ k ~N(0,Q k )(27) Then, the motion state of the pipeline robot to be estimated is inferred through the noisy observations and the input of the model, that is, the maximum a posteriori probability P(q k |u k-1 ,m k ), according to Bayes’ rule, we can get: Convert the maximum a posteriori probability problem of the robot state into solving the robot state q k The maximum likelihood estimate of is: Step S5-2: Construct the model of the objective function and camera projection; First, the camera projection imaging process; wherein O c is the optical center of the camera, f is the focal length of the camera, and point P in the camera coordinate system in the real space is projected onto the physical imaging plane through the optical center O, and point P′ is obtained in the image coordinate system; let the coordinates of point P be (X, Y, Z), and the coordinates of point P′ be (X c ,Y c ), Z represents the distance from point P to the camera plane XY, that is, the depth; according to the camera projection model and similar triangles: Then, the image coordinate system is converted to the pixel coordinate system; this process includes sampling, quantization, scaling, translation, and unit conversion; the u-axis coordinate in the pixel coordinate system is scaled by α relative to the image coordinate system cp times, the v-axis coordinate is scaled by β relative to the image coordinate system cp times, origin O P The relative image coordinate system is translated by [d x ,d y ]; therefore, the relationship between the point P′ and the pixel coordinates [u,v] in the image coordinate system is: Combining equations (30) and (31), we obtain: Let α cp f is f x , β cp f is f y The following formula is obtained: Where K represents the camera intrinsic parameter matrix, which is obtained through camera calibration experiments; assuming that there are n pairs of matched feature points (p1, p1′), (p2, p2′), ···, (p n ,p n ′) and the depth value Z corresponding to the feature point has been obtained. It is necessary to obtain the rotation matrix R and translation matrix T between adjacent image frames, which include the spatial relationship between the camera and IMU and the robot motion state obtained by solving the angular velocity and mileage information. The error term e is defined i for: e i =p i -(Rp i ′+T) (34) Then construct the error square sum loss function as the objective function F in the robot state optimization process: Step S5-3: Solve the robot posture by nonlinear optimization; the above objective function can be defined as the least squares problem in the overall sense, and the nonlinear optimization method and matching feature point pairs are used to obtain R and T that minimize the sum of squared errors; the Levenberg-Marquardt algorithm is used to solve the problem, and the error function is Taylor expanded: e(x+Δx)=e(x)+e′(x)Δx (36) Where Δx represents the descending vector of the substitution variable in the iterative optimization process, and e′ is the derivative of the error function with respect to the substitution variable; then the minimization problem of the objective function is transformed into solving Δx that minimizes the objective function. * : Let the derivative of the objective function with respect to Δx be 0, solve the vector Δx that minimizes the objective function, and obtain the vector equation: e′(x) T e′(x)Δx=-e′(x) T e(x) (38) The initial value of the robot state is set to the unit matrix, and the error and descent vector are solved in each iteration process. If Δx is small enough, the iteration is stopped and x is set. k+1 =x k +Δx.

7. The underground pipeline robot positioning method based on Beidou, inertial navigation and vision collaboration according to claim 1 is characterized in that: Step S6 includes: Step S6-1: pose graph optimization; the pipeline robot state obtained by nonlinear optimization in step S5 does not take into account the pose constraints of the robot at the previous moment, and the relative motion between the poses deviates from the true value. The pose estimation of the pipeline robot is corrected again by using pose graph optimization; the pose graph consists of nodes and edges, and the nodes represent the pose information of the pipeline robot. The two pose nodes P i and P j The relative motion ΔP ij Represents the edge of the pose graph, and the constraint relationship between poses is ΔP ij =P i - 1 P j The optimization variables are the positions of each vertex, the edges come from the position observation constraints, and the least squares error e is constructed ij : Where ∨ represents the operation of converting the matrix into a vector, and the optimization variable is P i and P j ; The derivative of the disturbance term δξ of the error with respect to the optimization variable is: where ξ represents the Lie algebra corresponding to the pose matrix in the special Euclidean group, f r represents the Jacobian matrix, Ad represents the adjoint transformation, ρ k and Represents the rotation component and the translation component. The corresponding relationship between the variables is: Where ∧ represents the operation from vector to matrix, and I is the unit matrix. According to the constructed error function and the derivative of the error with respect to the posture, the Levenberg-Marquardt algorithm is used to obtain the posture information after the posture graph is optimized. Step S6-2: Integrate Beidou positioning information to eliminate the cumulative error of the robot's pose estimation; the pose estimation of the pipeline robot obtained through the above steps will drift as the robot's track becomes longer, resulting in cumulative errors; according to the longitude lati, latitude longi and altitude information alti returned by the Beidou positioning sensor, it is converted into a global three-dimensional coordinate (X-axis) with the earth as the reference system. g ,Y g ,Z g ): Where L e Represents the radius of the earth, takes the global three-dimensional coordinates as constraints, combines the acquired pipeline robot posture information for global optimization, and establishes the overall error model: Where k represents a certain moment. The initial three-dimensional position of the robot is set to the three-dimensional coordinates returned by the Beidou positioning device. The rotation matrix R and translation matrix T after pose graph optimization are used as the initial values ​​of global optimization. The Levenberg-Marquardt algorithm is used for iterative optimization to obtain locally accurate and globally drift-free robot pose information, thus achieving precise positioning of the pipeline robot.

Images