A Multi-Sensor Collaborative Precision Positioning Method for Pipeline Robots

Abstract

This invention discloses a multi-sensor collaborative method for precise positioning of a pipeline robot. This method utilizes an inertial measurement unit (IMU) and a wheel encoder to acquire the robot's acceleration, angular velocity, and local wheel speed information in real time. Kalman filtering is used to update the angular velocity information by using the IMU's accelerometer output as the observed value. Pre-integration of the angular velocity and wheel speed information is used to achieve a preliminary motion estimate of the pipeline robot. Next, the extrinsic parameters of the camera and IMU are calibrated, and ORB feature extraction and matching are performed on each frame captured by the camera. Camera, IMU, and wheel speed information are fused in a tightly coupled manner. A nonlinear optimization method is used to obtain more accurate robot pose information. Finally, the method is optimized by fusing the three-dimensional position information returned by the BeiDou positioning sensor to obtain locally accurate and globally drift-free robot pose information, thus achieving precise positioning of the pipeline robot.

Description

Technical Field

[0001] This invention relates to a method for precise positioning of a pipeline robot using multi-sensor collaboration, belonging to the field of computer vision. Background Technology

[0002] Pipelines are widely used as a crucial means of material transportation in fields such as natural gas, petroleum, chemicals, nuclear facilities, and military equipment. However, due to environmental and human factors, pipelines can develop cracks and leaks caused by heavy pressure, corrosion, and wear. Therefore, to prevent such accidents and extend pipeline lifespan, regular inspection and maintenance are necessary. Furthermore, the environment in which most pipelines are located is inaccessible to maintenance personnel. Traditional excavation methods for inspection are not only labor-intensive but also pose safety hazards. Therefore, pipeline robots are now widely used for pipeline inspection and for locating damage and defects.

[0003] Accurate localization of pipeline robots is crucial for operators to monitor their real-time position and accurately locate and repair damaged areas. Current pipeline robot localization methods largely rely on cameras to photograph the inside of the pipeline and use visual odometry to estimate the robot's position. However, inside pipelines, the highly repetitive environment and uneven lighting make it difficult to obtain accurate odometry data using camera sensors. To improve the accuracy and robustness of pipeline robot state estimation, an increasing number of localization methods depend on data from multiple complementary sensors.

[0004] Camera sensors and IMUs are highly complementary in pose estimation. IMUs provide better pose estimation for fast movements, while camera sensors can correct for IMU data drift during slow movements. Therefore, the joint localization method of vision and IMU has been applied to the localization of pipeline robots. However, when the pipeline robot moves at a constant speed or along an arc, the joint localization of vision and IMU suffers from scale unknowns, and the pose estimation drifts over time due to the accumulation of errors.

[0005] Therefore, a multi-sensor collaborative method for precise positioning of pipeline robots is needed to solve the problems of scale unobservability and cumulative drift in vision and IMU joint positioning, realize the complementary advantages of different sensors, and be more suitable for complex and ever-changing pipeline environments. Summary of the Invention

[0006] The technical problem to be solved by this invention is to propose a multi-sensor collaborative method for precise positioning of pipeline robots, which integrates camera, IMU, wheel speed information and Beidou positioning sensor to obtain pipeline robot pose information that is observable at scale, accurate locally and drift-free globally, thereby achieving precise positioning of pipeline robots.

[0007] To address the aforementioned technical problems, this invention provides a multi-sensor collaborative method for precise positioning of a pipeline robot, comprising the following steps:

[0008] 1) Utilize IMU, wheel encoder, camera and Beidou positioning sensor to acquire real-time information on the acceleration, angular velocity, local wheel speed, environmental information around the robot and satellite positioning information;

[0009] 2) The Kalman filter algorithm is used to update the angular velocity information by using the accelerometer output of the IMU as the observation value, and the pre-integration of angular velocity and wheel speed information is fused.

[0010] 3) Calibrate the extrinsic parameters of the camera and IMU, perform ORB feature point detection and description on the image sequence captured by the camera, and match feature point pairs;

[0011] 4) The angular velocity and wheel velocity integral information obtained in step 2) and the matching feature point pair information obtained in step 3) are fused in a tightly coupled manner, and the robot pose information with higher accuracy is obtained through nonlinear optimization;

[0012] 5) Constrain the robot pose information obtained in step 4) based on the three-dimensional position information returned by the Beidou positioning sensor to eliminate the cumulative error of robot pose estimation and achieve accurate positioning of the pipeline robot.

[0013] Furthermore, in step 1), the acceleration, angular velocity, local wheel speed information, environmental information around the robot, and satellite positioning information of the pipeline robot are acquired in real time using an IMU, wheel encoder, camera, and Beidou positioning sensor. The specific steps are as follows:

[0014] Using a pipeline robot as a platform, an IMU is installed at the robot's center of mass, a camera and a Beidou positioning sensor are installed on the robot's head, and a wheel encoder is mounted on the drive wheel. The system operates based on the radius r of the drive wheel and the pulse time interval d of the encoder wheel's return. t Calculate the velocity V of the pipeline robot:

[0015]

[0016] When the pipeline robot starts, it acquires information from each sensor in real time.

[0017] Furthermore, in step 2), the Kalman filter algorithm is used to update the angular velocity information by using the IMU's accelerometer output as the observation value, and the pre-integration of angular velocity and wheel speed information is fused. The specific steps are as follows:

[0018] 21) Kalman filtering eliminates the accumulated error of angular velocity integration; the mean angular velocity and mean acceleration of the pipeline robot at the current time t and the next time t+1 are calculated as the average angular velocity and average acceleration within the IMU data acquisition interval Δt; the three-dimensional angle information of the robot during its motion is obtained through angular velocity integration. Due to the accumulation of errors caused by noise and integration, the accelerometer output is used as the observation value to update the angle information using Kalman filtering; the angle information U(k) obtained by integrating the Y-axis angular velocity Gyro_Y within the acquisition interval is used as the state estimate of the Kalman filter.

[0019]

[0020] Pitch angle Y Angle As an observation of the filter, this value is determined by the IMU's X-axis and Z-axis accelerations ACCEL_X and ACCEL_Z:

[0021] Y Angle =arctan(ACCEL_X / ACCEL_Z)

[0022] Establish the equations for the pitch angle measurement model at time t and time t-1:

[0023]

[0024] Where Y bias dt represents the current angular velocity offset, and dt represents the IMU data update frequency interval. The IMU is continuously corrected and smoothed by the observation of the pitch angle to obtain a more accurate and stable angle value. The X-axis and Z-axis angle updates are the same as the Y-axis.

[0025] 22) Pre-integration fusion of angular velocity and wheel speed information; by obtaining the wheel speed information and the angular velocity w at time t and t+1. t Preliminary estimation of the pipeline robot's motion information at time t+1, using average angular velocity data w t Robot pose Q at times ′ and t t Find the attitude Q at time t+1. t+1 :

[0026] Q t+1 =Q t (w t ′Δt)

[0027]

[0028] This pose contains three-dimensional directional information about the pipeline robot's motion, where B g This represents the angular velocity bias, Δt represents the time interval between two frames of angular velocity data, the robot's initial pose is defined as the identity matrix, and the IMU's acceleration data a... tIt was measured in its own coordinate system b, and needs to be transformed to the world coordinate system w according to the robot's posture at the corresponding moment:

[0029] a t,w =Q t (a t,b -B a )-g

[0030] a t+1,w =Q t+1 (a t+1,b -B a )-g

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

[0032] 23) Based on the robot's posture and position P at time t t The velocity V of the pipeline robot at the current moment is calculated from the wheel speed information V. t,w and the distance P moved in time Δt. t+1 :

[0033] V t,w =Q t (V)

[0034] P t+1 =P t +V t,w Δt

[0035] The direction and distance of the pipeline robot's movement are obtained, enabling a preliminary estimation of the pipeline robot's motion state.

[0036] Furthermore, in step 3), the extrinsic parameters of the camera and IMU are calibrated, ORB feature points are detected and described in the image sequence captured by the camera, and feature point pairs are matched. The specific steps are as follows:

[0037] 31) Camera and IMU extrinsic parameter calibration; the time-invariant parameter that needs to be estimated during the calibration experiment is the direction of gravity g relative to the world coordinate system. w The time offset d between camera and IMU measurement data, and the spatial transformation T between camera and IMU. c,i The time-varying parameters to be estimated are the IMU pose T with 6 degrees of freedom. w,i The bias of acceleration and angular velocity b a and b w The subscripts w, c, and i represent the world coordinate system, camera coordinate system, and IMU coordinate system, respectively, and T represents the spatial transformation between the coordinate systems. The relationships between them are as follows:

[0038] T w,i =T w,c T c,i

[0039] First, collect a set of measurement data from the camera and IMU, T = [t1, t2]. K The timestamp of the image acquired by the camera sensor is set to t. j The pixel coordinates of each corner point are y mj m is the number of corner points in a single frame image; since the structure of the calibration plate is known, the world coordinates of each corner point are... The pixel coordinates of the corner points are obtained based on the actual size of the calibration board, therefore the pixel coordinates of the corner points at time t are... k The measured values ​​of acceleration and angular velocity α obtained from the sampling point. k , Defined as:

[0040] α k =(T w,i ) r (a(t k )-g w )+b a (t k )+n ak

[0041]

[0042]

[0043] Among them (T) w,i ) r a(t) represents the rotational component of the IMU pose. k ) and w(t) k Let n represent the acceleration and angular velocity generated during the IMU's motion, and h represent the observation model at the corner point. Each n and the biases of acceleration and angular velocity are assumed to follow a Gaussian distribution with mean 0 and variance R, and are statistically independent. Then, the pose estimation between adjacent measurement data frames of the IMU is obtained using the quadratic integral of acceleration and the first integral of angular velocity. Combined with the pose relationship of the IMU relative to the world coordinate system in the previous frame, the pose estimation of the IMU relative to the world coordinate system in the current frame is obtained. Finally, the measurement-related error term is constructed as the difference between the measurement and the predicted measurement given the current state estimate. The error function e of the variable to be estimated is defined as:

[0044]

[0045]

[0046]

[0047]

[0048]

[0049] The subscripts represent the error functions of the corresponding variables; the sum of all error terms is minimized using the Levenberg-Marquardt algorithm, 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 pose of the camera and IMU is set according to the hardware installation orientation.

[0050] 32) ORB Feature Point Extraction; ORB feature points consist of keypoints and descriptors. First, select pixel p in the image, whose brightness is known to be I. p A threshold T is manually set; then, 16 pixels on a circle with a radius of 3 centered on pixel p are selected, and it is determined whether there are 9 consecutive pixels with a brightness greater than I. p +T or less than I p -T, if present, then point p is considered a feature point; construct an image pyramid using the input image, and perform FAST corner detection at each layer of the pyramid to achieve feature point scale invariance; achieve feature point rotation invariance using the gray-scale centroid method: define the moments of an image patch B:

[0051]

[0052] Where I(x,y) represents the pixel brightness, and (x,y) represents the pixel coordinates. The centroid of the image patch is obtained through moment calculation.

[0053]

[0054] Connect the centroid C of image patch B to the geometric center O to obtain the vector. The orientation angle θ of the feature point is defined as:

[0055] θ = arctan(m) 01 / m 10 );

[0056] 33) ORB descriptor calculation and feature matching; using binary descriptors (BRIEF), the description vector consists of 0s and 1s. A pixel region of size d×d is selected centered on the detected ORB keypoint p. n pixel pairs within this region are selected according to a certain rule. For each pixel pair (I... i ,I j Make the following judgment:

[0057]

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

[0059] Furthermore, in step 4), the angular velocity and wheel velocity integral information obtained in step 2) and the matching feature point pair information obtained in step 3) are fused in a tightly coupled manner, and higher-precision robot pose information is obtained through nonlinear optimization. The specific steps are as follows:

[0060] 41) Construct a motion state estimation model for the pipeline robot; based on sensor fusion theory, fuse angular velocity, wheel speed integral information, and matching feature point pair information in a tightly coupled manner to form the dynamic model:

[0061]

[0062] Where x k Representing the robot's state, including the rotation matrix R and the translation matrix t, y k The observations of the model, i.e., the pixel coordinates of the matched feature points, u k-1 w represents the model input. k-1 V represents noise in a random process. k This represents random observation noise, all assumed to have a mean of 0 and a variance of R. k and Q k Gaussian white noise:

[0063] w k ~N(0,R k ),v k ~N(0,Q) k )

[0064] The motion state of the pipeline robot to be estimated is inferred by using noisy observations and model inputs, that is, by finding the maximum posterior probability P(x) of the robot state given the known sensor input data and observation data. k |u k-1 ,y k According to Bayes' theorem, we get:

[0065]

[0066] The maximum a posteriori probability problem of robot state is transformed into solving for robot state x. k Maximum likelihood estimation:

[0067] x * MLE =argm ax(P(uk-1 ,y k |x k ));

[0068] 42) Construction of camera projection model and objective function during robot state optimization; To characterize the camera projection model during robot state optimization, pixel coordinate system, image coordinate system, and camera coordinate system are used to describe how to project the object in the camera coordinate system onto the pixel coordinate system, where O c Let O be the optical center of the camera, and f be the focal length of the camera. A point P in the camera coordinate system in real space is projected onto the physical imaging plane through the optical center O, resulting in point P′ in the image coordinate system. Let the coordinates of point P be [X,Y,Z], and the coordinates of point P′ be [X′,Y′]. Z represents the distance from point P to the XY plane of the camera, i.e., the depth. Based on the camera projection model and similar triangles, the following can be derived:

[0069]

[0070] By sampling and quantizing, points in the image coordinate system are transformed to the pixel coordinate system. This transformation involves a scaling and translation transformation. In the pixel coordinate system, the u-axis is scaled by a factor of α relative to the image coordinate system, the v-axis is scaled by a factor of β relative to the image coordinate system, and the origin o′ is translated by a factor of [c]. x ,c y Therefore, the relationship between point P′ and pixel coordinates [u,v] in the image coordinate system is as follows:

[0071]

[0072] Combine αf into f x βf merges into f y This yields the transformation relationship between the pixel coordinate system and the camera coordinate system:

[0073]

[0074] Where K represents the camera intrinsic parameter matrix, obtained through camera calibration experiments; assuming there are n pairs of matched feature points (p1, p1′), (p2, p2′)...(p...). n ,p n The depth values ​​corresponding to the feature points have been obtained. The rotation matrix R and translation matrix t between adjacent image frames need to be calculated. These matrixes contain the spatial relationship between the camera and the IMU, as well as the robot's motion state obtained from the angular velocity and wheel speed information. An error term e is defined. i for:

[0075] e i =p i -(Rp i ′+t)

[0076] Construct the sum of squared errors loss function as the objective function F in the robot state optimization process:

[0077]

[0078] 43) Nonlinear optimization to solve robot pose; the objective function is defined as a least squares problem in the overall sense. R and t, which minimize the sum of squared errors, are obtained using nonlinear optimization methods and matching feature point pairs; the Levenberg-Marquardt algorithm is used to solve the problem, and the error function is expanded using Taylor series.

[0079] e(x+Δx)=e(x)+J(x)Δx

[0080] Where Δx represents the descent vector of the variable being solved during the iterative optimization process, and J is the derivative of the error function with respect to the variable being solved, the problem of minimizing the objective function is transformed into solving for Δx that minimizes the objective function. * :

[0081]

[0082] Setting the derivative of the objective function with respect to Δx to 0, we find the vector Δx that minimizes the objective function, yielding the vector equation:

[0083] J(x) T J(x)Δx=-J(x) T e(x)

[0084] The robot's initial state is set as the identity matrix. The error and descent vector are calculated for each iteration. If Δx is sufficiently small, the iteration stops, and x is set to... k+1 =x k +Δx.

[0085] Furthermore, in step 5), the robot pose information obtained in step 4) is constrained by the three-dimensional position information returned by the Beidou positioning sensor to eliminate the cumulative error of the robot pose estimation and achieve precise positioning of the pipeline robot. The specific steps are as follows:

[0086] 51) Pose graph optimization; The pipe robot state obtained through nonlinear optimization in step 4) did not consider the robot's pose constraints at previous moments, and the relative motion between poses deviates from the true value. Pose graph optimization is used to correct the pose estimation of the pipe robot again; The pose graph consists of nodes and edges. Nodes represent the pose information of the pipe robot, and two pose nodes T i and T j The relative motion ΔT between them ij The edges of the pose graph represent the constraints between poses, denoted by ΔT. ij =T i-1 T j The optimization variables are the poses of each vertex, and the edges are derived from the pose observation constraints. The least squares error e is constructed. ij :

[0087] e ij =ln(ΔT) ij -1 T i -1 T j ) ∨

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

[0089]

[0090] Where ξ represents the Lie algebra corresponding to the pose matrix in the special Euclidean group, and ρ and Representing rotation and translation components, the correspondence between the variables is as follows:

[0091]

[0092]

[0093] Where ^ represents the operation from vector to matrix, and I is the identity matrix; based on the constructed error function and the derivative of the error with respect to the pose, the Levenberg-Marquardt algorithm is used to obtain the pose information after the pose graph is optimized.

[0094] 52) Integrating BeiDou positioning information eliminates the cumulative error in robot pose estimation; integrating camera, IMU, and wheel encoder information and processing it through a pipeline. The robot's pose estimation will drift as the robot's trajectory lengthens, generating cumulative errors. Based on the longitude (lati), latitude (longi), and altitude (alti) information returned by the BeiDou positioning sensors, it is converted into global three-dimensional coordinates (X-coordinates) with Earth as the reference frame. g ,Y g Z g ):

[0095]

[0096] Where L represents the Earth's radius, global three-dimensional coordinates are used as constraints, and global optimization is performed in conjunction with the acquired pose information of the pipeline robot to establish an overall error model:

[0097]

[0098] Where k represents a certain moment, the robot's initial three-dimensional position is set as 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 ​​for global optimization, and the Levenberg-Marquardt algorithm is used for iterative optimization to obtain locally accurate and globally drift-free robot pose information, thereby achieving precise positioning of the pipeline robot.

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

[0100] (1) This invention proposes to use IMU, wheel encoder and camera to perform collaborative processing to obtain high-precision positioning results of pipeline robot. It combines the advantages of IMU to provide better pose estimation for fast movement and camera sensor to correct IMU data drift in slow movement, which solves the problem that camera sensor is difficult to perform effective pose estimation in pipeline environment with weak texture. It also uses the speed information returned by wheel encoder to solve the problem of unknown scale in vision and IMU joint positioning.

[0101] (2) This invention first uses the Kalman filter algorithm to update the angular velocity information by using the accelerometer output of the IMU as the observation value, thereby reducing the error caused by angular velocity measurement noise and integration. Then, it fuses the pre-integration of angular velocity and wheel speed information, making the subsequent robot pose estimation simpler and the result more accurate.

[0102] (3) The present invention adds a pose graph optimization algorithm to the robot pose estimation based on the collaborative estimation of robot pose by IMU, wheel encoder and camera sensor, takes into account the global robot pose constraints, and uses pose graph optimization to correct the pose estimation of the pipeline robot again.

[0103] (4) Due to the cumulative error in the pose estimation of the pipeline robot, the positioning result of the pipeline robot will drift over time. This invention constrains the robot pose obtained by the IMU, wheel encoder and camera sensor in collaboration based on the three-dimensional position information returned by the Beidou positioning sensor, thereby eliminating the cumulative error in the robot pose estimation and realizing the accurate positioning of the pipeline robot. Attached Figure Description

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

[0105] Figure 2 The spatial relationship between the camera, IMU, and calibration board;

[0106] Figure 3 A framework diagram for fusion of IMU, vision, and wheel speed information to estimate robot pose;

[0107] Figure 4 This is a schematic diagram of a camera projection model;

[0108] Figure 5 This is a schematic diagram of the global pose graph optimization algorithm. Detailed Implementation

[0109] The following detailed description, in conjunction with the accompanying drawings, of the specific implementation method of the multi-sensor collaborative pipeline robot precise positioning method of the present invention.

[0110] The operation flow of the multi-sensor collaborative pipeline robot precise positioning method of the present invention is as follows: Figure 1 As shown, the specific steps include:

[0111] 1) Real-time acquisition of acceleration, angular velocity, local wheel speed information, environmental information, and satellite positioning information of the pipeline robot is achieved using an IMU, wheel encoder, camera, and BeiDou positioning sensor. The pipeline robot is used as a carrier, with an IMU installed at its center of mass and a camera and BeiDou positioning sensor mounted on its head. The camera captures images with a resolution of 1280×960. A wheel encoder is mounted on the drive wheel, and the data is based on the drive wheel radius r and the pulse time interval d of the encoder wheel's return. t Calculate the velocity V of the pipeline robot:

[0112]

[0113] When the pipeline robot starts, it acquires information from each sensor in real time.

[0114] 2) The Kalman filter algorithm is used to update the angular velocity information by using the accelerometer output of the IMU as the observation value, and the pre-integration of angular velocity and wheel speed information is fused.

[0115] 21) Kalman filtering eliminates the accumulated error of angular velocity integration. The average angular velocity and average acceleration of the pipeline robot at the current time t and the next time t+1 are calculated and used as the average angular velocity and average acceleration within the IMU data acquisition interval Δt. Three-dimensional angle information during robot motion is obtained through angular velocity integration. Due to the accumulation of errors caused by noise and integration, Kalman filtering is used to update the angle information using the accelerometer output as the observed value. The angle information U(k) obtained by integrating the Y-axis angular velocity Gyro_Y within the acquisition interval is used as the state estimate of the Kalman filter.

[0116]

[0117] Pitch angle Y Angle As an observation of the filter, this value is determined by the IMU's X-axis and Z-axis accelerations ACCEL_X and ACCEL_Z:

[0118] Y Angle =arctan(ACCEL_X / ACCEL_Z) (3)

[0119] Establish the equations for the pitch angle measurement model at time t and time t-1:

[0120]

[0121] Where Y bias This represents the current angular velocity offset, and dt represents the IMU data update frequency interval. The IMU is continuously corrected and smoothed using elevation angle observations to obtain more accurate and stable angle values. X-axis and Z-axis angle updates are the same as the Y-axis.

[0122] 22) Pre-integration fusion of angular velocity and wheel speed information. This is achieved by combining the obtained wheel speed information with the angular velocities w at times t and t+1. t Preliminary estimation of the pipeline robot's motion information at time t+1. This is achieved using the average angular velocity data w. t Robot pose Q at times ′ and t t Find the attitude Q at time t+1. t+1 :

[0123] Q t+1 =Q t (w t ′Δt) (5)

[0124]

[0125] This pose contains three-dimensional directional information about the pipeline robot's motion, where B g This represents the angular velocity bias, and Δt represents the time interval between two frames of angular velocity data. The robot's initial pose is defined as an identity matrix. The IMU's acceleration data a... t The measurements were taken in the robot's own coordinate system b, therefore it needs to be transformed to the world coordinate system w based on the robot's pose at the corresponding moment:

[0126] a t,w =Q t (a t,b -B a )-g (7)

[0127] a t+1,w =Q t+1 (a t+1,b -B a )-g (8)

[0128] Among them B a g and g represent acceleration bias and gravitational acceleration, respectively.

[0129] 23) Based on the robot's posture and position P at time t t The velocity V of the pipeline robot at the current moment is calculated from the wheel speed information V. t,w and the distance P moved in time Δt.t+1 :

[0130] V t,w =Q t (V) (9)

[0131] P t+1 =P t +V t,w Δt (10)

[0132] The direction and distance of the pipeline robot's movement are obtained, enabling a preliminary estimation of the pipeline robot's motion state.

[0133] 3) Calibrate the extrinsic parameters of the camera and IMU, perform ORB feature point detection and description on the image sequence captured by the camera, and match feature point pairs.

[0134] 31) Camera and IMU extrinsic parameter calibration. The time-invariant parameter that needs to be estimated during the calibration experiment is the direction of gravity g relative to the world coordinate system. w The time offset d between camera and IMU measurement data, and the spatial transformation T between camera and IMU. c,i The time-varying parameters to be estimated are the IMU pose T with 6 degrees of freedom. w,i The bias of acceleration and angular velocity b a and b w .like Figure 2 As shown, the spatial relationship of each coordinate system in the calibration experiment is illustrated, where F... w F c F i Let represent the world coordinate system, camera coordinate system, and IMU coordinate system, respectively. T represents the spatial transformation between these coordinate systems, and the relationships between them are as follows:

[0135] T w,i =T w,c T c,i (11)

[0136] First, a set of measurement data from the camera and IMU is collected within a short period of time, T = [t1, t2]. K (Typically 1-2 minutes), the timestamp of the images acquired by the camera sensor is set to t. j The pixel coordinates of each corner point are y mj Where m is the number of corner points in a single frame image. Since the structure of the calibration plate is known, the world coordinates of each corner point are... This can be obtained based on the actual size of the calibration board. Therefore, the pixel coordinates of the corner points and at time t... k The measured values ​​of acceleration and angular velocity α obtained from the sampling point. k , It can be defined as:

[0137]

[0138] Among them (T) w,i ) r a(t) represents the rotational component of the IMU pose. k ) and w(t) k Let n represent the acceleration and angular velocity generated during the IMU's motion, and h represent the observation model at the corner point. Each n and the biases of acceleration and angular velocity are assumed to follow a Gaussian distribution with mean 0 and variance R, and are statistically independent. Then, the pose estimation between adjacent measurement data frames of the IMU is obtained using the quadratic integral of acceleration and the first integral of angular velocity. This is combined with the pose relationship of the IMU relative to the world coordinate system in the previous frame to obtain the pose estimation of the IMU relative to the world coordinate system in the current frame. Finally, the measurement-related error term is constructed as the difference between the measurement and the predicted measurement given the current state estimate. The error function e of the variable to be estimated is defined as:

[0139]

[0140] The subscripts denote the error function of the corresponding variable. The Levenberg-Marquardt (LM) algorithm is used to minimize the sum of all error terms, and the maximum likelihood estimate of all unknown parameters is found iteratively. During the iteration process, the time offset is initialized to 0, and the initial relative pose between the camera and the IMU can be set according to the hardware mounting orientation.

[0141] 32) ORB Feature Point Extraction. ORB feature points consist of keypoints and descriptors. First, select pixel p in the image, whose brightness is known to be I. p A threshold T is manually set. Then, 16 pixels within a circle with a radius of 3, centered on pixel p, are selected. Finally, it is determined whether there are nine consecutive pixels with a brightness greater than I. p +T or less than I p -T, if present, indicates that point p is considered a feature point. An image pyramid is constructed using the input image, and FAST corner detection is performed at each level of the pyramid to achieve scale invariance of feature points. Rotation invariance of feature points is achieved using the gray-scale centroid method. This method first defines the moments of an image patch B:

[0142]

[0143] Where I(x,y) represents the pixel brightness, and (x,y) represents the pixel coordinates. The centroid of the image patch is obtained through moment calculation:

[0144]

[0145] Connect the centroid C of image patch B to the geometric center O to obtain the vector. The direction of the feature point is then defined as:

[0146] θ = arctan(m) 01 / m 10 (16)

[0147] 33) ORB descriptor calculation and feature matching. A binary descriptor (BRIEF) is used, with the descriptor vector consisting of 0s and 1s. A pixel region of size d×d is selected centered on the detected ORB keypoint p. Then, n pixel pairs within this region are selected according to a certain rule. For each pixel pair (I... i ,I j Make the following judgment:

[0148]

[0149] The binary data τ(p; i, j) obtained after the judgment is written into the form of an N-dimensional vector to obtain the ORB descriptor. Then, based on the Hamming distance between the descriptors, the fast approximate nearest neighbor algorithm is used for feature point matching, and the progressive sampling consistency (PRSAC) algorithm is used to refine and optimize the feature points to obtain accurate feature matching point pairs between images.

[0150] 4) The angular velocity, wheel velocity integral information and matching feature point pair information obtained in the above steps are fused in a tightly coupled manner, and the robot pose information with higher accuracy is obtained through nonlinear optimization.

[0151] 41) Construct a motion state estimation model for the pipeline robot. The framework for fusing visual, IMU, and wheel speed information is as follows: Figure 3 As shown. According to sensor fusion theory, the dynamic model formed by fusing angular velocity, wheel speed integral information, and matched feature point pair information in a tightly coupled manner is as follows:

[0152]

[0153] Where x k Representing the robot's state, including the rotation matrix R and the translation matrix t, y k The observations of the model, i.e., the pixel coordinates of the matched feature points, u k-1 w represents the model input. k-1 V represents noise in a random process. k This represents random observation noise, all assumed to have a mean of 0 and a variance of R. k and Q k Gaussian white noise:

[0154] w k ~N(0,R k ),v k ~N(0,Q) k(19)

[0155] Next, the motion state of the pipeline robot to be estimated is inferred through noisy observations and model inputs, that is, by finding the maximum posterior probability P(x) of the robot state given the known sensor input data and observation data. k |u k-1 ,y k According to Bayes' theorem, we can obtain:

[0156]

[0157] The maximum a posteriori probability problem of robot state is transformed into solving for robot state x. k Maximum likelihood estimation:

[0158] x * MLE =argm ax(P(u k-1 ,y k |x k )) (twenty one)

[0159] 42) Construction of the camera projection model and objective function during robot state optimization. To characterize the camera projection model during robot state optimization, pixel coordinates, image coordinates, and camera coordinates are generally used to describe how to project the object from the camera coordinate system onto the pixel coordinate system. For example... Figure 4 The image shown illustrates the camera projection imaging process. Where O... c Let O be the optical center of the camera, and f be the focal length of the camera. A point P in the camera coordinate system in real space is projected onto the physical imaging plane through the optical center O, resulting in point P′ in the image coordinate system. Let the coordinates of point P be [X,Y,Z], and the coordinates of point P′ be [X′,Y′], where Z represents the distance from point P to the XY plane of the camera, i.e., the depth. Based on the camera projection model and similar triangles, we can derive:

[0160]

[0161] Points in the image coordinate system are transformed to the pixel coordinate system through sampling and quantization. The transformation between the image and pixel coordinate systems involves a scaling and translation transformation. In the pixel coordinate system, the u-axis coordinate is scaled by a factor of α relative to the image coordinate system, the v-axis coordinate is scaled by a factor of β relative to the image coordinate system, and the origin o′ is translated by a factor of [c]. x ,c y Therefore, the relationship between point P′ and pixel coordinates [u,v] in the image coordinate system is as follows:

[0162]

[0163] Substitute into equations (22) and (23) and combine αf into fx βf merges into f y This yields the transformation relationship between the pixel coordinate system and the camera coordinate system:

[0164]

[0165] Where K represents the camera intrinsic parameter matrix, which can be obtained through camera calibration experiments. Assume there are n pairs of matched feature points (p1, p1′), (p2, p2′)...(p...). n ,p n The depth values ​​corresponding to the feature points have been obtained. The rotation matrix R and translation matrix t between adjacent image frames need to be calculated. These matrixes contain the spatial relationship between the camera and the IMU, as well as the robot's motion state obtained from the angular velocity and wheel speed information. An error term e is defined. i for:

[0166] e i =p i -(Rp i (25)

[0167] Next, the sum of squared errors loss function is constructed as the objective function in the robot state optimization process:

[0168]

[0169] 43) Nonlinear optimization for robot pose solving. The objective function described above can be defined as a least-squares problem in the overall sense. Nonlinear optimization methods 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 this problem, and the error function is expanded using Taylor series:

[0170] e(x+Δx)=e(x)+J(x)Δx (27)

[0171] Where Δx represents the descent vector of the variable being optimized during the iterative optimization process, and J is the derivative of the error function with respect to the variable being optimized. Therefore, the problem of minimizing the objective function is transformed into finding the Δx that minimizes the objective function. * :

[0172]

[0173] Setting the derivative of the objective function with respect to Δx to 0, we find the vector Δx that minimizes the objective function, yielding the vector equation:

[0174] J(x) T J(x)Δx=-J(x) T e(x) (29)

[0175] The robot's initial state is set as the identity matrix. The error and descent vector are calculated for each iteration. If Δx is sufficiently small, the iteration stops, and x is set to... k+1 =x k +Δx.

[0176] 5) Constrain the robot pose information obtained in the above steps based on the three-dimensional position information returned by the Beidou positioning sensor to eliminate the cumulative error of robot pose estimation and achieve accurate positioning of the pipeline robot.

[0177] 51) Pose Graph Optimization. The pipe robot state obtained through nonlinear optimization in the above steps did not consider the robot's pose constraints at previous moments, resulting in deviations between the relative motions of the poses and the true values. Pose graph optimization is used to further correct the pipe robot's pose estimation. For example... Figure 5 As shown, the pose graph consists of nodes and edges. Nodes represent the pose information of the pipeline robot. Two pose nodes T i and T j The relative motion ΔT between them ij The edges of the pose graph represent the constraints between poses, denoted by ΔT. ij =T i -1 T j The optimization variables are the poses of each vertex, and the edges are derived from the pose observation constraints. The least squares error e is constructed. ij :

[0178] e ij =ln(ΔT) ij -1 T i -1 T j ) ∨ (30)

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

[0180]

[0181] Where ξ represents the Lie algebra corresponding to the pose matrix in the special Euclidean group, and ρ and Representing rotation and translation components, the correspondence between the variables is as follows:

[0182]

[0183] Where ^ denotes the vector-to-matrix operation, and I is the identity matrix. Based on the constructed error function and the derivative of the error with respect to the pose, the Levenberg-Marquardt algorithm is used to obtain the pose information after the pose graph optimization.

[0184] 52) Integrate BeiDou positioning information to eliminate cumulative errors in robot pose estimation. The pose estimation of the pipeline robot, obtained by integrating camera, IMU, and wheel encoder information and following the above steps, will drift as the robot trajectory lengthens, resulting in cumulative errors. Based on the longitude (lati), latitude (longi), and altitude (alti) information returned by the BeiDou positioning sensors, convert it into global three-dimensional coordinates (X, Y, L, Z) with Earth as the reference frame. g ,Y g Z g ):

[0185]

[0186] Where L represents the Earth's radius, global three-dimensional coordinates are used as constraints, and global optimization is performed in conjunction with the acquired pose information of the pipeline robot to establish an overall error model:

[0187]

[0188] Where k represents a certain moment. The robot's initial 3D position is set as the 3D 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 ​​for global optimization. The Levenberg-Marquardt algorithm is used for iterative optimization to obtain locally accurate and globally drift-free robot pose information, thereby achieving precise positioning of the pipeline robot.

[0189] Finally, it should be noted that the above embodiments are merely illustrative of the technical solutions of the present invention and not intended to limit it. Those skilled in the art should understand that modifications or equivalent substitutions can be made to the specific embodiments of the present invention, but such modifications or alterations are all within the scope of protection of the pending claims.

Claims

1. A method for precise positioning of a pipeline robot using multi-sensor collaboration, characterized in that, Includes the following steps: 1) Utilize IMU, wheel encoder, camera and Beidou positioning sensor to acquire real-time information on the acceleration, angular velocity, local wheel speed, environmental information around the robot and satellite positioning information of the pipeline robot; 2) The Kalman filter algorithm is used to update the angular velocity information by using the accelerometer output of the IMU as the observation value, and the pre-integration of angular velocity and wheel speed information is fused. 3) Calibrate the extrinsic parameters of the camera and IMU, perform ORB feature point detection and description on the image sequence captured by the camera, and match feature point pairs; 4) The angular velocity and wheel velocity integral information obtained in step 2) and the matching feature point pair information obtained in step 3) are fused in a tightly coupled manner, and the robot pose information with higher accuracy is obtained through nonlinear optimization; 5) Based on the 3D position information returned by the Beidou positioning sensor, constrain the robot pose information obtained in step 4) to eliminate the cumulative error of robot pose estimation and achieve precise positioning of the pipeline robot; the specific steps are as follows: 51) Pose graph optimization; The state of the pipeline robot obtained through nonlinear optimization in step 4) did not consider the pose constraints of the robot at previous time steps, and the relative motion between poses deviates from the true value. Pose graph optimization is used to correct the pose estimation of the pipeline robot again; The pose graph consists of nodes and edges. Nodes represent the pose information of the pipeline robot, and two pose nodes... and relative motion between The edges of the pose graph represent the constraints between poses. The optimization variables are the poses of each vertex, and the edges are derived from the pose observation constraints. A least-squares error is constructed. : ； in This represents the operation of converting a matrix into a vector, with the optimization variable being... and The error's disturbance term to the optimization variable The derivative is: ； in This represents the Lie algebra corresponding to the pose matrix in a special Euclidean group. and Representing rotation and translation components, the correspondence between the variables is as follows: ， in This represents operations from vector to matrix. The identity matrix is ​​used; based on the constructed error function and the derivative of the error with respect to the pose, the Levenberg-Marquardt algorithm is used to obtain the pose information after the pose graph is optimized. 52) Integrating BeiDou positioning information to eliminate cumulative errors in robot pose estimation; The robot's pose estimation will drift as the robot's trajectory lengthens, generating cumulative errors, based on the longitude returned by the BeiDou positioning sensor. lati ,latitude longi and height information alti Convert it to global 3D coordinates with Earth as the reference frame. : ， in L Representing the Earth's radius, and using global three-dimensional coordinates as constraints, a global optimization is performed based on the acquired pose information of the pipeline robot to establish an overall error model: ， in k At a certain moment, the robot's initial three-dimensional position is set as 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 ​​for global optimization. The Levenberg-Marquardt algorithm is used for iterative optimization to obtain locally accurate and globally drift-free robot pose information, thereby achieving precise positioning of the pipeline robot.

2. The multi-sensor collaborative pipeline robot precise positioning method according to claim 1, characterized in that, In step 1), the acceleration, angular velocity, local wheel speed information, environmental information around the robot, and satellite positioning information of the pipeline robot are acquired in real time using an IMU, wheel encoder, camera, and Beidou positioning sensor. The specific steps are as follows: Using a pipeline robot as a platform, an IMU is installed at the robot's center of mass, a camera and a Beidou positioning sensor are mounted on the robot's head, and a wheel encoder is mounted on the drive wheel. The encoder's position is determined based on the radius of the drive wheel. and the pulse time interval of the encoding round return Calculate the movement speed of the pipeline robot : ， When the pipeline robot starts, it acquires information from each sensor in real time.

3. The multi-sensor collaborative pipeline robot precise positioning method according to claim 1, characterized in that, In step 2), the Kalman filter algorithm is used to update the angular velocity information by using the IMU's accelerometer output as the observation value, and the pre-integration of angular velocity and wheel speed information is fused. The specific steps are as follows: 21) Kalman filtering eliminates the cumulative error of the angular velocity integral; calculate the current time. and the next moment The mean angular velocity and mean acceleration of the pipeline robot are used as the IMU data acquisition interval. Average angular velocity and average acceleration over time; The robot's three-dimensional angle information during motion is obtained by integrating the angular velocity. Due to the accumulation of errors caused by noise and integration, the accelerometer output is used as the observation value to update the angle information by Kalman filtering. by Y axial angular velocity Angle information obtained by integration within the acquisition interval As a state estimator for the Kalman filter: ， Pitch angle As the observed value of the filter, this value is determined by the IMU. X shaft and Z Axial acceleration and Decide: ， Establish Time and Equation for pitch angle measurement model at time: ， in Indicates the current angular velocity offset. This indicates the IMU data update frequency interval; by continuously correcting and smoothing the IMU using elevation angle observations, more accurate and stable angle values ​​can be obtained. X shaft and Z Axis angle update same Y axis; 22) Pre-integration fusion of angular velocity and wheel speed information; by obtaining the wheel speed information and time, angular velocity at time t Preliminary estimate Motion information of the pipeline robot at all times, using average angular velocity data and robot posture at all times Seeking The posture of the moment : ， This pose contains three-dimensional directional information about the pipeline robot's motion, where Indicates angular velocity offset. This represents the time interval between two frames of angular velocity data. The robot's initial pose is defined as an identity matrix, and the IMU's acceleration data... In its own coordinate system The measurements taken below need to be transformed into the world coordinate system based on the robot's pose at the corresponding moment. Down: ， in and These represent acceleration bias and gravitational acceleration, respectively. 23) Based on the robot's posture, Location at any moment Wheel speed information Find the velocity of the pipeline robot in its current pose. as well as Distance traveled in time : ， The direction and distance of the pipeline robot's movement are obtained, enabling a preliminary estimation of the pipeline robot's motion state.

4. The multi-sensor collaborative pipeline robot precise positioning method according to claim 1, characterized in that, In step 3), the extrinsic parameters of the camera and IMU are calibrated, ORB feature points are detected and described in the image sequence captured by the camera, and feature point pairs are matched. The specific steps are as follows: 31) Camera and IMU extrinsic parameter calibration; the time-invariant parameter that needs to be estimated during the calibration experiment is the direction of gravity relative to the world coordinate system. Time offset between camera and IMU measurement data Spatial transformation between camera and IMU The time-varying parameters to be estimated are the pose of the 6-DOF IMU. Bias of acceleration and angular velocity and The subscripts w, c, and i represent the world coordinate system, camera coordinate system, and IMU coordinate system, respectively. The spatial transformations between the coordinate systems are represented by the following relationships: ， First, a set of measurement data from the camera and IMU was collected. The timestamp of the images acquired by the camera sensor is set to The pixel coordinates of each corner point are , This represents 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 are... The pixel coordinates of the corner points are obtained based on the actual size of the calibration board, therefore the pixel coordinates of the corner points and the time... The measured values ​​of acceleration and angular velocity obtained from the sampling point. , Defined as: ， in Represents the rotational component of the IMU pose. and Let n represent the acceleration and angular velocity generated during IMU motion, and h represent the observation model at the corner point. Each n and the biases of acceleration and angular velocity are assumed to follow a Gaussian distribution with mean 0 and variance R, and are statistically independent. Then, the pose estimation between adjacent measurement data frames of the IMU is obtained using the second integral of acceleration and the first integral of angular velocity. Combined with the pose relationship of the IMU relative to the world coordinate system in the previous frame, the pose estimation of the IMU relative to the world coordinate system in the current frame is obtained. Finally, the measurement-related error term is constructed as the difference between the measurement and the predicted measurement given the current state estimate. The error function of the variable to be estimated is... Defined as: ， The subscripts represent the error functions of the corresponding variables; the sum of all error terms is minimized using the Levenberg-Marquardt algorithm, 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 pose of the camera and IMU is set according to the hardware installation orientation. 32) ORB Feature Point Extraction; ORB feature points consist of keypoints and descriptors. First, select pixels in the image. Its brightness is known to be Artificially set a threshold Then, using pixels Sixteen pixels on a circle with a radius of 3 centered at a certain point are selected, and then it is determined whether there are nine consecutive pixels with a brightness greater than a certain value. or less than If it exists, then the point is considered to be true. For feature points; construct an image pyramid using the input image, and perform FAST corner detection at each layer of the pyramid to achieve feature point scale invariance; use the gray-level centroid method to achieve feature point rotation invariance: in a certain image patch The moments of an image patch are defined in the following way: ， in For pixel brightness, Given pixel coordinates, the centroid of the image patch is obtained through moment calculation: ， Image blocks center of mass With geometric center Connect them to obtain a vector. Then the direction angle of the feature point Defined as: ； 33) ORB descriptor computation and feature matching; using binary descriptors (BRIEF), the description vector consists of 0s and 1s, to detect ORB keypoints. Centered on a point of size A pixel region is selected based on a certain rule. Each pixel pair Make the following judgment: ， The binary data obtained after judgment Written The ORB descriptor is obtained in the form of a dimensional vector. Using the Hamming distance between the descriptors as a metric, the fast approximate nearest neighbor algorithm is used for feature point matching, and the progressive sampling consensus algorithm is used to refine and optimize the feature points to obtain accurate feature matching point pairs between images.

5. The multi-sensor collaborative pipeline robot precise positioning method according to claim 1, characterized in that, In step 4), the angular velocity and wheel velocity integral information obtained in step 2) and the matching feature point pair information obtained in step 3) are fused in a tightly coupled manner. More accurate robot pose information is obtained through nonlinear optimization. The specific steps are as follows: 41) Construct a motion state estimation model for the pipeline robot; based on sensor fusion theory, fuse angular velocity, wheel speed integral information, and matching feature point pair information in a tightly coupled manner to form the dynamic model: ， in Represents the robot's state, including the rotation matrix. R Translation matrix t , The observations representing the model are the pixel coordinates of the matched feature points. Indicates the model input, Represents noise in a random process. This represents random observation noise, all assumed to have a mean of 0 and a variance of . and Gaussian white noise: ， The motion state of the pipeline robot to be estimated is inferred by using noisy observations and model inputs, that is, by finding the maximum posterior probability of the robot's state given known sensor input data and observation data. According to Bayes' theorem: ， The maximum a posteriori probability problem of robot state is transformed into solving the robot state problem. Maximum likelihood estimation: ； 42) Construction of the camera projection model and objective function during robot state optimization; To characterize the camera projection model during robot state optimization, pixel coordinates, image coordinates, and camera coordinates are used to describe how to project the object in the camera coordinates onto the pixel coordinates. For the camera optical center, The focal length of the camera, a point in the camera's coordinate system in real space. Passing through the light core Projected onto the physical imaging plane, a point is obtained in the image coordinate system. Set up points The coordinates are ,point The coordinates are , Point to camera plane The distance, i.e., the depth, is derived from the camera projection model and similar triangles: ， Sampling and quantization transform points in the image coordinate system to the pixel coordinate system. This transformation involves scaling and translation. The axis coordinates are scaled relative to the image coordinate system. times, The axis coordinates are scaled relative to the image coordinate system. times, origin The image coordinate system was translated. Therefore, points in the image coordinate system and pixel coordinates The relationship is: ， Will merged into , merged into This yields the transformation relationship between the pixel coordinate system and the camera coordinate system: ， in This represents the camera intrinsic parameter matrix, obtained through camera calibration experiments; assuming we have... For the matched feature points Having obtained the depth values ​​corresponding to the feature points, we need to calculate the rotation matrix between adjacent image frames. Translation matrix This includes the spatial relationship between the camera and the IMU, as well as the robot's motion state obtained by solving for angular velocity and wheel speed information, and defines the error term. for: ， Constructing the sum of squared errors loss function as the objective function in the robot state optimization process. F : ； 43) Nonlinear optimization to solve robot pose; The objective function is defined as a least squares problem in the overall sense. It is solved using nonlinear optimization methods and matching feature point pairs to minimize the sum of squared errors. and The Levenberg-Marquardt algorithm is used to solve the problem, and the error function is expanded using Taylor series. ， in This represents the descent vector of the variable being replaced during the iterative optimization process. If we take the derivative of the error function with respect to the substituted variables, then the problem of minimizing the objective function is transformed into finding the solution that minimizes the objective function. : ， Let the objective function be... Given that the derivative is 0, find the vector that minimizes the objective function. The vector equation is obtained as follows: ， The robot's initial state is set as the identity matrix. The error and descent vector are solved in each iteration. If it is small enough, stop iterating and let .

Images