Global positioning method for tank wall-climbing robot based on visual weld pose initialization

Abstract

The application is a kind of global positioning method of tank wall-climbing robot based on visual weld pose initialization, first, fix RGB-D camera and inertial measurement unit on the wall-climbing robot, and meanwhile, adsorb the wall-climbing robot on the tank surface; collect weld images before the robot moves, extract weld intersection and straight weld vector from the images, and convert the two to the initial coordinate system of the robot; then, align the Z axis of the initial coordinate system of the robot with the straight weld vector of the tank, obtain the initial rotation matrix from the initial coordinate system of the robot to the coordinate system of the tank, and further obtain the initial pose transformation matrix between the initial coordinate system of the robot and the coordinate system of the tank; finally, use visual inertial odometer to calculate the motion increment of the wall-climbing robot, obtain the best estimated pose of the wall-climbing robot in the initial coordinate system of the robot, and convert the best estimated pose to the coordinate system of the tank through the initial pose transformation matrix, to obtain the global pose of the wall-climbing robot in the coordinate system of the tank at any time. The method fully utilizes the weld information and three-dimensional model information of the tank itself, and solves the problem of global positioning difficulty of the wall-climbing robot on the tank.

Description

Technical Field

[0001] This invention belongs to the field of robot autonomous positioning technology, specifically a global positioning method for a tank climbing robot based on visual weld pose initialization. Technical Background

[0002] Wall-climbing robots are widely used in tasks such as tank welding, magnetic flux leakage detection, and rust removal. Currently, most research on wall-climbing robots focuses on adsorption methods and mechanical structures. In comparison, their positioning technology is still immature, and autonomous positioning is an important prerequisite for wall-climbing robots to achieve autonomous movement.

[0003] Based on the sensor information acquisition method, wall-climbing robot localization methods can be broadly categorized into two types: those based on onboard sensors and those based on external sensors. Localization based on external sensors requires pre-calibration and deployment and is significantly affected by the environment. Localization based on onboard sensors is similar to SLAM's state estimation and environmental perception; however, given the unknown initial global position and pose, it can only obtain a relative odometry estimate, making global localization of the wall-climbing robot impossible. Furthermore, the wall-climbing robot adheres to the storage tank, limiting its field of vision, and the tank wall's texture is monotonous, resulting in degraded visual features. Obtaining robust relative odometry is also a challenge. Therefore, to address the problems of existing technologies, this invention proposes a global localization method for storage tank wall-climbing robots based on visual weld pose initialization. Summary of the Invention

[0004] To address the shortcomings of existing technologies, the technical problem this invention aims to solve is to provide a global localization method for a tank-climbing robot based on visual weld pose initialization.

[0005] The present invention solves the aforementioned technical problem by adopting the following technical solution:

[0006] A global localization method for a tank-climbing robot based on visual weld pose initialization, characterized by the following steps:

[0007] S1. Fix the RGB-D camera and inertial measurement unit to the wall-climbing robot, and at the same time, attach the wall-climbing robot to the surface of the tank; construct the tank coordinate system with the geometric center of the tank as the origin, the tank axis as the Z-axis, the tank radial direction as the X-axis, and the Y-axis following the right-hand rule.

[0008] S2. Before the wall-climbing robot moves, use an RGB-D camera to acquire weld seam images, process the weld seam images, obtain the weld seam intersection points and straight weld seam vectors in the camera coordinate system, and transform the weld seam intersection points and straight weld seam vectors in the camera coordinate system to the robot's initial coordinate system.

[0009] S3. Align the Z-axis of the robot's initial coordinate system with the weld line vector of the storage tank to obtain the initial rotation matrix from the robot's initial coordinate system to the storage tank's coordinate system.

[0010] S4. Obtain the initial pose transformation matrix between the robot's initial coordinate system and the tank's initial coordinate system;

[0011] S41. Based on the initial rotation matrix and the weld intersection in the tank coordinate system, obtain the translation vector from the robot's initial coordinate system to the tank coordinate system, and further obtain the initial pose transformation matrix between the robot's initial coordinate system and the tank coordinate system.

[0012] S42. Obtain the contact point and contact point normal vector between the wall-climbing robot and the wall in the tank coordinate system;

[0013] S43. Project the contact point between the wall-climbing robot and the wall onto the surface of the storage tank to obtain the projection point and the normal vector of the projection point.

[0014] S44. Calculate the yaw angle error based on the contact point normal vector and the projection point normal vector, and update the initial rotation matrix based on the yaw angle error;

[0015] Repeat steps S41 to S44, iterating multiple times until the yaw angle error is small enough to update the initial rotation matrix, and update the initial pose transformation matrix between the robot's initial coordinate system and the tank coordinate system based on the updated initial rotation matrix.

[0016] S5. Use visual inertial odometry to calculate the motion increment of the wall-climbing robot, obtain the best estimated pose of the wall-climbing robot in the robot's initial coordinate system, and then use the initial pose transformation matrix to transform the best estimated pose to the tank coordinate system to obtain the global pose of the wall-climbing robot in the tank coordinate system at any time.

[0017] Furthermore, in the process of calculating the motion increment of the wall-climbing robot, visual line feature residuals and adsorption constraint residuals are introduced for optimization. The objective function is then:

[0018]

[0019] In the formula, χ is the state vector of the sliding window, {r p ,Η p} represents marginalized prior information; B is the IMU pre-integration residual, and B is the set of all IMU pre-integration measurements in the sliding window. These are the observations pre-integrated by the IMU from time k to k+1. ρ is the covariance of the IMU pre-integration noise term from time k to k+1; ρ is the robust kernel function used to suppress outliers. F is the visual point feature residual, and F is the set of all point features in the sliding window. The s-th feature point in the k-th frame of image c k The observed values ​​in It is the noise covariance of the visual point features; It represents the visual line feature residuals, where L is the set of all line features within the sliding window. It is the observation value of the l-th line feature in the k-th frame image. It is the noise covariance of the visual line features; It is the adsorption constraint residual, P r It is the set of projection points of the contact points in the sliding window onto the surface of the storage tank. P is the adsorption constraint observation value in the k-th frame. Pr It is the projection-constrained covariance;

[0020] The visual line feature residual and the adsorption constraint residual are expressed as follows:

[0021]

[0022]

[0023] In the formula, and These are the k-th frame images c k The two endpoints of the l-th line feature and Features up to the l-th projection line distance, Let be the projection of the wall-climbing robot's pose in the storage tank coordinate system in the k-th frame image. It represents the pose increment of the wall-climbing robot in the robot's initial coordinate system at time k during the visual-inertial odometry estimation process. Let be the initial pose transformation matrix between the robot's initial coordinate system and the tank's initial coordinate system, (·). xy Represents the xy-plane components, Let the projection point vector be... This is the normal vector of the contact point.

[0024] Furthermore, in step S4, the contact point between the wall-climbing robot and the wall surface in the tank coordinate system... and contact point normal vector Represented as:

[0025]

[0026]

[0027] in, and These represent the contact point between the wall-climbing robot and the wall, and the normal vector of the contact point, respectively, in the robot's initial coordinate system. R0 is the translation vector from the robot's initial coordinate system to the tank coordinate system, and R0 is the initial rotation matrix from the robot's initial coordinate system to the tank coordinate system.

[0028] Projection point and projection point normal vector Represented as:

[0029]

[0030]

[0031] In the formula, (·) z Represents the z-axis component, (·) x Represents the x-axis component, (·) y This represents the y-axis component.

[0032] Furthermore, in step S4, the update formulas for the yaw angle error Δyaw and the initial rotation matrix are as follows:

[0033]

[0034]

[0035] Where R is the updated initial rotation matrix.

[0036] Furthermore, the straight weld vector is a straight weld along the axial direction on the surface of the storage tank.

[0037] Compared with the prior art, the present invention has the following technical advantages:

[0038] 1. This invention, based on weld seam image recognition, initializes the global pose of a wall-climbing robot using known weld seam information and iterative alignment vectors. This achieves the transformation between the robot's initial coordinate system and the tank's coordinate system, obtaining the initial transformation relationship between the two systems. Then, using this initial transformation relationship and odometry to estimate the pose, the global pose of the wall-climbing robot in the tank's coordinate system is obtained, enabling global localization of the robot on the tank. This method is simple and effective, fully utilizing the tank's own weld seam information and 3D model information. It can adapt to various tank models with different radii of curvature, solving the problem of difficult global localization of wall-climbing robots on tanks.

[0039] 2. This invention addresses the problems of sparse wall features and limited field of view by introducing visual line feature constraints and adsorption constraints into visual inertial odometry estimation. Through optimized solution, it effectively suppresses the accumulation and drift of odometry errors, and achieves robust odometry estimation under special wall conditions.

[0040] 3. The device of this invention is simple and convenient to build, has low cost, is easier to implement, and does not rely on external sensors, making it less affected by the external environment. It only requires mounting a visual inertial sensor on the wall-climbing robot, and then attaching the robot to the vicinity of the weld seam. Attached Figure Description

[0041] Figure 1 This is an overall flowchart of the present invention;

[0042] Figure 2 This is a schematic diagram of the various coordinate systems of the present invention;

[0043] Figure 3 This is a schematic diagram of the yaw angle error of the present invention. Detailed Implementation

[0044] Specific embodiments are given below with reference to the accompanying drawings. These specific embodiments are only used to describe the technical solution of the present invention in detail, and are not intended to limit the scope of protection of this application.

[0045] This invention provides a global localization method for a tank-climbing robot based on visual weld pose initialization (hereinafter referred to as the method, see [link]). Figures 1-3 The process includes the following steps:

[0046] S1. Fix the RGB-D camera and inertial measurement unit (IMU) to the wall-climbing robot, allowing the robot to adhere to the surface of the tank; construct a tank coordinate system O with the geometric center of the tank as the origin, the tank axis as the Z-axis, the tank radial direction as the X-axis, and the Y-axis following the right-hand rule. g _X g Y g Z g The initial position of the wall-climbing robot should be close to the weld intersection and the Z-axis of the robot's initial coordinate system should be aligned with the straight weld vector of the tank (the straight weld vector is a straight weld along the axial direction on the surface of the tank) to ensure that the RGB-D camera can capture the weld image.

[0047] The coordinates of the weld intersection point in the tank coordinate system are P. j g = (0, r, 0), the vector of the straight weld seam is represented as V j g = (0,0,1), the true coordinates of any point on the storage tank are P. g = (x, y, h) and the true normal vector of any point are denoted as Where r is the radius of the storage tank, and x, y, and h are the x, y, and z coordinates of any point;

[0048] S2. Before the wall-climbing robot moves, an RGB-D camera is used to acquire weld seam images. The weld seam images are processed to obtain the weld seam intersection points and straight weld seam vectors in the camera coordinate system, and then transformed to the robot's initial coordinate system. The weld seam intersection points and straight weld seam vectors in the robot's initial coordinate system are denoted as follows: and V j b ;

[0049] S3. Align the Z-axis of the robot's initial coordinate system with the weld line vector of the storage tank to obtain the initial rotation matrix R0 from the robot's initial coordinate system to the storage tank coordinate system. The initial rotation matrix R0 is a calculated value, and there is a yaw angle error between the calculated value and the actual value.

[0050] S4. Compensate for the yaw angle by using the error between the contact point normal vector and the projection point normal vector. Update the initial rotation matrix from the robot's initial coordinate system to the tank coordinate system based on the yaw angle error. Update the initial pose transformation matrix between the robot's initial coordinate system and the tank coordinate system based on the updated initial rotation matrix to obtain the initial global pose of the wall-climbing robot in the tank coordinate system.

[0051] S41. Based on the initial rotation matrix R0 from the robot's initial coordinate system to the tank coordinate system and the intersection of the weld seam in the tank coordinate system, obtain the translation vector from the robot's initial coordinate system to the tank coordinate system. The initial pose transformation matrix between the robot's initial coordinate system and the tank coordinate system is obtained according to equation (2).

[0052]

[0053]

[0054] S42. Calculate the contact point between the wall-climbing robot and the wall in the tank coordinate system according to equations (3) and (4). and contact point normal vector

[0055]

[0056]

[0057] in, and These are the contact point between the wall-climbing robot and the wall in the robot's initial coordinate system and the normal vector of the contact point, respectively. These two parameters are known from the robot model parameters.

[0058] S43, the contact point between the wall-climbing robot and the wall surface. Projecting onto the surface of the storage tank to obtain the projection point. and projection point normal vector Among them, projection points and projection point normal vector They are represented as follows:

[0059]

[0060]

[0061] In the formula, (·) xy Represents the xy-plane components, (·) z Represents the z-axis component, (·) x Represents the x-axis component, (·) y Indicates the y-axis component;

[0062] S44. Due to the yaw angle error in the initial rotation matrix R0, the contact point and contact point normal vector in the tank coordinate system may not necessarily meet the constraints of the tank surface curvature and normal vector. In order to make the contact point and contact point normal vector between the wall-climbing robot and the wall satisfy the constraints of the tank surface curvature and normal vector, the contact point normal vector is calculated according to equation (7). With the normal vector of the projection point The yaw angle is compensated for in the xy plane to obtain the yaw angle error Δyaw, and the initial rotation matrix is ​​updated according to the yaw angle error Δyaw. The formula for updating the initial rotation matrix is ​​as shown in equation (8).

[0063]

[0064]

[0065] Where R is the updated initial rotation matrix;

[0066] Repeat steps S41 to S44, iterating multiple times until the yaw error Δyaw is sufficiently small, then update the initial rotation matrix. Substitute the updated initial rotation matrix into equation (2) to update the initial pose transformation matrix between the robot's initial coordinate system and the tank coordinate system. Obtain the initial global pose of the wall-climbing robot in the coordinates of the storage tank;

[0067] S5. Use visual inertial odometry to calculate the motion increment of the wall-climbing robot, obtain the best estimated pose of the wall-climbing robot in the robot's initial coordinate system, and then use the initial pose transformation matrix. Transform to the tank coordinate system to obtain the global pose of the wall-climbing robot in the tank coordinate system at any time;

[0068] When calculating the motion increment of the wall-climbing robot, nonlinear optimization based on a sliding window model is used to reduce errors, and visual line feature residuals and adsorption constraint residuals are introduced. The objective function is then:

[0069]

[0070] In the formula, χ is the state vector of the sliding window, {r p ,Η p} represents marginalized prior information; B is the IMU pre-integration residual, and B is the set of all IMU pre-integration measurements in the sliding window. These are the observations pre-integrated by the IMU from time k to k+1. ρ is the covariance of the IMU pre-integration noise term from time k to k+1; ρ is the robust kernel function used to suppress outliers. F is the visual point feature residual, and F is the set of all point features in the sliding window. The s-th feature point in the k-th frame of image c k The observed values ​​in It is the noise covariance of the visual point features; It represents the visual line feature residuals, where L is the set of all line features within the sliding window. It is the observation value of the l-th line feature in the k-th frame image. It is the noise covariance of the visual line features; It is the adsorption constraint residual, P r It is the set of projection points of the contact points in the sliding window onto the surface of the storage tank. P is the adsorption constraint observation value in the k-th frame. Pr It is the projection-constrained covariance;

[0071] In equation (9), the visual line feature residual and the adsorption constraint residual are shown in the following equations:

[0072]

[0073]

[0074] In the formula, and These are the k-th frame images c k The two endpoints of the l-th line feature and Features up to the l-th projection line distance, Let be the projection of the wall-climbing robot's pose in the storage tank coordinate system in the k-th frame image. It is the pose increment of the wall-climbing robot in the robot's initial coordinate system at time k during the visual inertial odometry estimation process;

[0075] The Levenberg-Marquardt algorithm is used to solve the objective function, obtaining the optimal estimated pose of the wall-climbing robot in the robot's initial coordinate system. Then, the initial pose transformation matrix is ​​used... Transform to the tank coordinate system to obtain the global pose of the wall-climbing robot in the tank coordinate system at any time;

[0076]

[0077] in, Let k be the global pose of the wall-climbing robot in the tank coordinate system at time k. Let k be the optimal estimated pose of the wall-climbing robot in the robot's initial coordinate system at time k.

[0078] Any aspects not covered in this invention are applicable to existing technologies.

Claims

1. A global localization method for a tank-climbing robot based on visual weld pose initialization, characterized in that, The method includes the following steps: S1. Fix the RGB-D camera and inertial measurement unit to the wall-climbing robot, and at the same time, attach the wall-climbing robot to the surface of the tank; construct the tank coordinate system with the geometric center of the tank as the origin, the tank axis as the Z-axis, the tank radial direction as the X-axis, and the Y-axis following the right-hand rule. S2. Before the wall-climbing robot moves, use an RGB-D camera to acquire weld seam images, process the weld seam images, obtain the weld seam intersection points and straight weld seam vectors in the camera coordinate system, and transform the weld seam intersection points and straight weld seam vectors in the camera coordinate system to the robot's initial coordinate system. S3. Align the Z-axis of the robot's initial coordinate system with the weld line vector of the storage tank to obtain the initial rotation matrix from the robot's initial coordinate system to the storage tank's coordinate system. S4. Obtain the initial pose transformation matrix between the robot's initial coordinate system and the tank's initial coordinate system; S41. Based on the initial rotation matrix and the weld intersection in the tank coordinate system, obtain the translation vector from the robot's initial coordinate system to the tank coordinate system, and further obtain the initial pose transformation matrix between the robot's initial coordinate system and the tank coordinate system. S42. Obtain the contact point and contact point normal vector between the wall-climbing robot and the wall in the tank coordinate system; Contact point between the wall-climbing robot and the wall in the tank coordinate system and contact point normal vector Represented as: (3) (4) in, and These represent the contact point between the wall-climbing robot and the wall, and the normal vector of the contact point, respectively, in the robot's initial coordinate system. Let be the translation vector from the robot's initial coordinate system to the tank's coordinate system. This is the initial rotation matrix from the robot's initial coordinate system to the tank's coordinate system; S43. Project the contact point between the wall-climbing robot and the wall onto the surface of the storage tank to obtain the projection point and the normal vector of the projection point. Projection point and projection point normal vector Represented as: (5) (6) In the formula, Represents the z-axis component. express x Axial components, express y Axial components, Where the radius is the storage tank radius; S44. Calculate the yaw angle error based on the contact point normal vector and the projection point normal vector, and update the initial rotation matrix based on the yaw angle error; Yaw angle error The formulas for updating the initial rotation matrix are as follows: (7) (8) in, This is the updated initial rotation matrix; Repeat steps S41 to S44, iterating multiple times until the yaw angle error is small enough to update the initial rotation matrix, and update the initial pose transformation matrix between the robot's initial coordinate system and the tank coordinate system based on the updated initial rotation matrix. S5. Use visual inertial odometry to calculate the motion increment of the wall-climbing robot, obtain the best estimated pose of the wall-climbing robot in the robot's initial coordinate system, and then use the initial pose transformation matrix to transform the best estimated pose to the tank coordinate system to obtain the global pose of the wall-climbing robot in the tank coordinate system at any time. In calculating the motion increment of the wall-climbing robot, visual line feature residuals and adsorption constraint residuals are introduced for optimization. The objective function is then: (9) In the formula, It is the state vector of the sliding window. It is marginalized prior information; It is the IMU pre-integration residual. It is the set of all IMU pre-integrated measurements in the sliding window. yes arrive The observations pre-integrated by the IMU at time [time]. yes arrive Covariance of the IMU pre-integrated noise term at time step; It is a robust kernel function used to suppress outliers; It is the visual point feature residual. It is the set of features of all points in the sliding window. It is the first The feature point at the th ... Frame Image The observed values ​​in It is the noise covariance of the visual point features; It is a visual line feature residual. It is the set of all line features in the sliding window. It is the first The line feature in the first Observations in the frame image It is the noise covariance of the visual line features; It is the adsorption constraint residual. It is the set of projection points of the contact points in the sliding window onto the surface of the storage tank. It is the first Adsorption constraint observations in the frame. It is the projection-constrained covariance; The visual line feature residual and the adsorption constraint residual are expressed as follows: (10) (11) In the formula, and They are the first Frame Image Middle The two endpoints of a line feature and To the Each projection line feature distance, For the first The projection of the wall-climbing robot's pose in the tank coordinate system in the frame image. In the process of visual inertial odometry estimation The pose increment of the wall-climbing robot in the robot's initial coordinate system at all times. Let be the initial pose transformation matrix between the robot's initial coordinate system and the storage tank's initial coordinate system. express xy Planar components, Let the projection point vector be... This is the normal vector of the contact point.

2. The global localization method for a tank-climbing robot based on visual weld pose initialization according to claim 1, characterized in that, The straight weld vector is a straight weld along the axial direction on the surface of the storage tank.

Images