A perception method for a magnetic four-legged robot based on binocular vision and elevation map fusion

Abstract

This invention belongs to the field of robot perception technology, specifically relating to a perception method for a magnetic quadruped robot based on binocular vision and elevation map fusion. The method includes the following steps: synchronously acquiring image data and angular velocity and linear acceleration data, and preprocessing them; extracting and tracking features from the images, and pre-integrating IMU data between consecutive keyframes to obtain relative motion increments; constructing a joint optimization objective function within a sliding window to solve for the robot's six-degree-of-freedom pose; using the pose to transform the current depth point cloud to the world coordinate system, and using probabilistic fusion to update the two-dimensional grid elevation map to obtain a local terrain map containing the elevation mean and uncertainty. This invention deeply integrates high-precision state estimation and terrain mapping, effectively solving the problems of positioning drift and inaccurate perception of small terrains in magnetic climbing robots on vertical curved surfaces and in environments with weak texture, significantly improving the robot's autonomous operation capability and safety.

Description

Technical Field

[0001] This invention belongs to the field of robot perception technology, specifically relating to a perception method for a magnetic quadruped robot based on the fusion of binocular vision and elevation map. Background Technology

[0002] Magnetic quadruped robots are specialized robots that combine the mobility of quadruped robots with magnetic adsorption capabilities. They can stably walk on vertical, inclined, or even inverted magnetic surfaces, thus replacing humans in performing inspection, maintenance, and operational tasks in high-risk, large-scale industrial facilities. Typical applications include: non-destructive testing of weld quality, coating thickness measurement, or rust removal on the hulls of large ships; corrosion inspection and structural integrity assessment of the outer walls of large spherical or vertical storage tanks; inspection of steel box girders of bridges or steel curtain walls of skyscrapers to identify cracks and loose connections; and inspection of the outer surfaces of wind turbine towers to assess structural fatigue and external damage.

[0003] In the aforementioned applications, the robot must possess precise environmental perception and self-localization capabilities to ensure the safe and reliable landing of its magnetically attached feet and to plan efficient operational paths. However, existing perception technologies face extremely severe and unique challenges when applied to magnetically attached quadruped robots: First, the working surfaces of magnetically attached quadruped robots are typically large curved surfaces such as ship hulls and tank walls, rather than standard planes, and these surfaces are often covered with welds, bolts, rust patches, or slight unevenness. These minute geometric features are crucial for preventing the magnetically attached feet from slipping or losing their footing; however, traditional sensors struggle to accurately model them. For example, LiDAR accuracy decreases at long distances or on curved surfaces, while ordinary monocular vision struggles to recover dense and accurately measured 3D information, ultimately resulting in terrain maps that cannot provide a reliable basis for foot planning. Second, when climbing vertical or inverted surfaces, the robot's gravity direction is perpendicular to or opposite to the plane of motion; any slight slippage will be misinterpreted by the IMU as a violent movement, causing the robot's pose estimation to diverge rapidly. In addition, large-area, single-color industrial coating surfaces often lack sufficient visual texture features, making it easy for pure vision or visual odometry methods to fail in tracking. The positioning drift accumulated over long periods and large areas of operation becomes increasingly severe, eventually causing the robot to be unable to close the operation trajectory.

[0004] Therefore, there is an urgent need for a perception method specifically designed for magnetic quadruped robots. This method needs to be able to accurately perceive the minute terrain features in curved environments and tightly couple them with the robot's robust positioning in vertical and inverted postures, so as to fundamentally improve the accuracy and safety of magnetic quadruped robots in autonomous operation in high-risk industrial environments. Summary of the Invention

[0005] The purpose of this invention is to provide a perception method for a magnetic quadruped robot based on the fusion of binocular vision and elevation map, which can construct a local terrain elevation map in the robot's base coordinate system in real time and accurately, and achieve high-precision and robust self-state estimation, providing a reliable basis for robot decision-making.

[0006] The specific technical solution adopted by this invention is as follows: A perception method for a magnetic quadruped robot based on binocular vision and elevation map fusion includes the following steps: Step 1: Synchronous Acquisition and Preprocessing of Multi-Source Data The binocular depth camera and inertial measurement unit (IMU) mounted on the magnetic quadruped robot are activated to simultaneously acquire binocular image pairs, depth images, and angular velocity and linear acceleration data from the IMU. The images are then processed to remove distortion using the intrinsic parameter matrix and distortion parameters obtained from the binocular camera calibration. The IMU and camera are connected to the same trigger signal via physical circuitry, and the IMU data and image data are strictly time-stamped aligned.

[0007] Step 2: Visual front-end feature extraction and IMU pre-integration: First, IMU pre-integration is performed. The IMU installed on the magnetic robot body can provide the robot's angular velocity. and linear acceleration The measured values. In the world coordinate system, the robot's pose and velocity changes with time, which can be described by the following differential equations: ; in, This represents the robot's position in the world coordinate system at time t. This represents the robot's velocity in the world coordinate system at time t. The superscript indicates the robot's pose at time t. and Representing the world coordinate system and the robot body coordinate system respectively. For robot positioning, For robot speed, For robot linear acceleration, For the robot's pose quaternion, Let ω be the robot's angular velocity. The above differential equations and the notation used are well-known continuous-time kinematic models in the fields of robotics, inertial navigation, and visual inertial odometry (VIO), used to describe the evolution of the robot's pose and velocity over time. The IMU measurements are also included. and It is in the body coordinate system and includes offset. and Gaussian white noise : ; in, These are the linear acceleration and angular velocity measured by the IMU, respectively. Let represent the rotation matrix from the world system to the home system at time t. It is the acceleration due to gravity. These represent zero bias in linear acceleration and zero bias in angular velocity, respectively. , These are Gaussian white noise used in linear acceleration and angular velocity measurements, respectively, and are well-known sensor noise models in the field of inertial navigation.

[0008] Direct integration of these measurements will affect the robot's posture. The estimation changes and needs to be repeated, resulting in a huge computational load. Therefore, this invention uses IMU pre-integration technology to perform the estimation on two consecutive visual keyframes. and Between these points, we integrate the IMU measurements in the body coordinate system of the k-th frame to obtain the relative motion increment: ; in, , , These are the pre-integral increments for position, velocity, and rotation quaternions, respectively. Let be the rotation matrix from the body coordinate system at time t to the body coordinate system at the corresponding time of the k-th frame. The above integral expression adopts standard mathematical notation: For continuous time variables, For time differential elements, Indicates the integration time interval, superscript denoted by vector transpose, both of which are well-known symbols in the fields of mathematical analysis and robot state estimation.

[0009] These pre-integral values ​​depend only on the IMU bias and constitute the measurements of the state change between two keyframes.

[0010] Step 3: Visual-Inertial Tightly Coupled Nonlinear Optimization within the Sliding Window: This step utilizes the feature point observation data obtained in step two to construct the visual reprojection error. Specifically, for a feature point with coordinates in the world coordinate system... 3D feature points When it is observed by the camera in the kth frame, its pixel coordinates on the image Model can be projected through a camera The calculation yielded the result.

[0011] This process involves transforming points from the world coordinate system to the camera coordinate system: ; in, These are the coordinates of the feature point in the camera coordinate system at frame k. It is the external parameter from the world to the camera. It is the rotation matrix from the world coordinate system to the camera coordinate system. It is the position vector of the origin of the world coordinate system in the camera coordinate system.

[0012] Visual reprojection error Defined as the actual observed pixel coordinates The difference between the pixel coordinates estimated from the current state and the pixel coordinates obtained by reprojection: ; Finally, nonlinear optimization and state solving are performed. This invention constructs a joint optimization objective function within a sliding window to solve for the optimal robot state and feature point positions. The state vector to be optimized is... This includes the robot state of all keyframes within the sliding window and the 3D coordinates of all observed feature points: ; in, It refers to the robot state of all keyframes within the window. These are the three-dimensional coordinates of all observed feature points. Robot states include... , which are the robot's position, linear velocity, rotational quaternion, accelerometer zero bias vector, and gyroscope zero bias vector in the world coordinate system, respectively.

[0013] The objective function is a least-squares problem encompassing all constraints, weighted by Mahalanobis distances of IMU pre-integration errors and visual reprojection errors: ; in, It is the collection of all keyframes within the sliding window; It is the IMU pre-integration error term, which consists of the difference between the pre-integrated measurement and the relative motion calculated based on the state to be optimized; It is the set of all valid feature point observations. For the index pairs defined in this invention, representing the first... The map point at the ... Observations in keyframes; It is the Huber kernel function, used to suppress outliers in the visual part of the image and enhance robustness; , This is the covariance matrix corresponding to the error term, used to weight measurements from different sources and of different qualities. Solving this nonlinear least squares problem yields the optimal estimate of the robot's state within the sliding window.

[0014] Step 4: Update the probabilistic elevation map based on the robot's pose: This step utilizes the robot's six-DOF pose estimated in step three to accurately fuse the local 3D point cloud information acquired by the binocular depth camera into the global or local elevation map.

[0015] For depth cameras For each pixel generated at any given time, we can obtain its coordinate system in the camera coordinate system. Three-dimensional coordinates and its measurement covariance The transformation from the camera frame to the world frame is obtained using the robot pose estimated in step three. and its covariance We can transform this point to the world coordinate system. Down: ; in, The transformation matrix is ​​a rigid body transformation matrix, and the transformation matrix symbol is... This is well-known in the fields of robotics and computer vision; this invention adopts a left-to-right transformation convention, i.e., subscripts. Indicates from the camera coordinate system To the world coordinate system The transformation.

[0016] At the same time, according to the first-order Taylor expansion, we can propagate the uncertainty of this point in the world coordinate system, i.e., the covariance: ; in and These are the Jacobian matrices for coordinate transformation with respect to pose and with respect to the original point coordinates, respectively, with Jacobian symbols. Known in the fields of mathematical optimization and SLAM, subscript This is a variable distinguishing identifier defined in this invention.

[0017] This invention maintains a two-dimensional raster map, where each raster cell... Store the elevation distribution of this horizontal location using a Gaussian distribution. These are expressed as mean (elevation value) and variance (uncertainty). When a point is transformed to the world coordinate system... and Variance in the axial direction Fall into the grid At that time, we use a Kalman filter to update the elevation information of the raster: ; The above equation is the well-known Kalman filter update equation in the field of state estimation, describing the recursive calculation process of the optimal linear estimate, where, For Kalman gain, and The first The mean and variance of the estimated elevation. These are sensor observations. To observe the noise variance; subscript and The recursive step is represented by a well-known notation in the field of discrete-time filtering. This probabilistic fusion method can effectively handle sensor noise and reduce the uncertainty of elevation estimation as the number of observations increases, resulting in smoother and more robust topographic maps.

[0018] A magnetic quadruped robot includes a robot body, four mechanical legs, and foot-end magnetic suction devices disposed at the ends of the mechanical legs. The robot body is equipped with a binocular depth camera and an inertial measurement unit.

[0019] The technical effects achieved by this invention are as follows: (1) The present invention provides a perception method for a magnetic quadruped robot based on binocular vision and elevation map fusion. It organically combines vision-inertial tight coupling state estimation with probabilistic elevation map update, uses high-precision pose to guide terrain mapping, and indirectly verifies positioning consistency through terrain mapping results, forming a virtuous cycle and significantly improving the overall perception performance of the robot in complex curved environment.

[0020] (2) The magnetic quadruped robot perception method based on binocular vision and elevation map fusion of the present invention can effectively suppress IMU measurement anomalies caused by slight sliding in vertical and inverted postures by jointly optimizing visual reprojection error and IMU pre-integration error within a sliding window. It overcomes the tracking loss problem of pure visual odometry on weak textured industrial surfaces, greatly reduces positioning drift during long-term operation, and enhances trajectory closure capability.

[0021] (3) The perception method of magnetic quadruped robot based on binocular vision and elevation map fusion of the present invention uses Kalman filter to perform probabilistic fusion update of elevation map, which can make full use of multiple observation information, reduce the influence of sensor noise, and achieve high-fidelity modeling of small geometric features such as welds, bolts, and rust, providing reliable terrain basis for foot landing point planning, and effectively preventing magnetic feet from stepping into the air or slipping.

[0022] (4) The magnetic quadruped robot perception method based on binocular vision and elevation map fusion of the present invention obtains accurate self-pose and detailed local topographic map. When performing tasks such as structural inspection, corrosion inspection and thickness measurement, the robot can autonomously plan safe foot trajectory and avoid obstacles in real time, which greatly enhances the robustness and safety of operation on high-risk industrial facilities such as large ships, storage tanks and bridges.

[0023] (5) The elevation mean and variance stored in each grid cell of the magnetic quadruped robot perception method based on binocular vision and elevation map fusion of the present invention not only provide terrain height information, but also quantify its reliability, enabling the robot to adjust the planning strategy according to uncertainty, and improving the system's fault tolerance in complex environments. Attached Figure Description

[0024] Figure 1 This is a flowchart of the perception algorithm of the present invention; Figure 2 This is a schematic diagram of the structure of the magnetic quadruped robot of the present invention; Figure 3 This is a positioning effect diagram of the present invention; Figure 4 This is a diagram illustrating the topographic map construction effect of the present invention.

[0025] The attached diagram lists the components represented by each number as follows: 1. Main body of the magnetic quadruped robot; 2. Legs of the magnetic quadruped robot; 3. Magnetic suction device at the foot end. Detailed Implementation

[0026] To make the objectives and advantages of this invention clearer, the invention will be specifically described below with reference to embodiments. It should be understood that the following text is merely used to describe one or more specific embodiments of the invention and does not strictly limit the scope of protection specifically claimed by the invention.

[0027] like Figure 1-4 As shown, a perception method for a magnetic quadruped robot based on binocular vision and elevation map fusion is applicable to magnetic quadruped robots. The magnetic quadruped robot includes a robot body 1, four mechanical legs 2, and foot-end magnetic suction devices 3 set at the ends of the mechanical legs 2. The robot body 1 is equipped with a binocular depth camera and an inertial measurement unit (IMU). The foot-end magnetic suction device 3 includes an electromagnet and a rubber sheet. The rubber sheet covers the bottom surface of the electromagnet, and friction is generated between the electromagnet and the contact surface by the attraction force of the electromagnet. By using a de-energized electromagnet and a universal joint adaptive system, the robot can climb on uneven metal surfaces.

[0028] The magnetic quadruped robot also includes a memory and a processor, with the memory storing computer programs.

[0029] like Figure 1-4 As shown, a perception method for a magnetic quadruped robot based on the fusion of binocular vision and elevation map includes the following steps: Step 1: Synchronous Acquisition and Preprocessing of Multi-Source Data Simultaneously acquire image data from the binocular depth camera mounted on the magnetic quadruped robot and angular velocity and linear acceleration data from the inertial measurement unit, perform distortion correction on the image data, and align the IMU data with the image data using timestamps. Specifically, in step one, two data buffers are established to store image and IMU data respectively. The algorithm uses the acquisition time of the stereo image as the base timestamp, indexes it in the IMU data queue, and uses linear interpolation to calculate the IMU data precisely corresponding to the image exposure time. Simultaneously, during the algorithm initialization phase, the pre-calibrated camera intrinsic parameter matrix K and distortion coefficients are loaded. Each frame of the acquired left and right infrared images is then processed using the OpenCV library to remove distortion and eliminate the impact of lens distortion on subsequent feature point extraction and matching.

[0030] Step 2: Visual front-end feature extraction and IMU pre-integration: Feature points are extracted and tracked on the image processed in step one to establish visual data association between frames; at the same time, IMU measurements are pre-integrated between two consecutive visual keyframes to obtain relative motion increments. Among them, feature point extraction of the image after step one refers to extracting stable environmental texture features from a continuous image stream through visual front-end feature extraction, and establishing visual data association is constructed through optical flow tracing.

[0031] In step two, the FAST corner detection algorithm is used to extract feature points from the distortion-free image sequence, and the KLT optical flow method is used to track the extracted feature points between consecutive image frames. Between two consecutive visual keyframes, there are multiple sets of IMU measurement data. These IMU data are integrated in the body coordinate system of the previous frame to obtain the measurement value of the motion state between the two keyframes, and the relative motion increment is calculated to complete the pre-integration process.

[0032] Step 3: Visual-Inertial Tightly Coupled Nonlinear Optimization within the Sliding Window: Construct a robot state vector containing all keyframes within the sliding window. Using the visual data association and IMU pre-integration increment obtained in step two as constraints, establish a joint optimization objective function containing visual reprojection error term and IMU pre-integration error term. Solve the robot's six-degree-of-freedom pose through nonlinear least squares optimization. Specifically, in step three, a sliding window containing the latest ten keyframes will be maintained. The state vector within the sliding window includes the pose, velocity, IMU bias, and inverse depth of all 3D feature points observed in these ten keyframes.

[0033] Construct the objective function for the optimization problem, including: IMU pre-integration error term and visual reprojection error term. For every two adjacent keyframes within the sliding window... and The theoretical relative displacement calculated based on the current state to be optimized is compared with the measured value obtained from IMU pre-integration in step two to construct a residual term. The Mahalanobis distance of this residual term is added to the total cost function. For each 3D feature point observed within the sliding window, it is projected onto each keyframe that observed it. On the image plane, the projected pixel coordinates are calculated. The difference between these projected pixel coordinates and the actual observed feature point pixel coordinates is the reprojection error. Similarly, the Mahalanobis distance of this error term is added to the total cost function. Solving this nonlinear least squares problem yields the optimal estimates of the pose, velocity, and IMU bias for all keyframes within the sliding window. The robot's pose at the current moment is the pose of the latest keyframe within the sliding window. Moving the camera in the horizontal and vertical planes, the pose estimation results are as follows... Figure 3 As shown.

[0034] Step 4: Update the probabilistic elevation map based on the robot's pose: Using the six-degree-of-freedom pose calculated in step three, the dense three-dimensional point cloud acquired by the binocular depth camera at the current moment is transformed into the world coordinate system, and a Kalman filter is used to perform probabilistic fusion update on each grid cell in the two-dimensional grid elevation map to obtain a local topographic map containing the elevation mean and uncertainty.

[0035] Specifically, in step four, a two-dimensional grid map will be maintained in the robot's base coordinate system, with each grid storing a mean value. The variance is The elevation information at that location is represented by a Gaussian distribution. During initialization, the mean elevation of all grid cells is set to zero, and the variance is set to ten, indicating that the height is uncertain. The transformation from the robot's world frame to its own frame is utilized. And the external parameters from the camera to the robot body obtained through pre-calibration. The complete transformation chain from the camera coordinate system to the world coordinate system is constructed as follows: ; Point cloud in camera coordinate system Transforming to the world coordinate system yields the point cloud in the world coordinate system: ; For every point transformed to the world coordinate system The elevation map grid in which the point cloud is located is determined by its position. The measurement uncertainty at this point is... variance of direction The noise level is determined by both the inherent noise model of the depth camera and the uncertainty propagation of pose estimation. In this embodiment, to simplify the calculation, a noise model related to the depth value is used. ,in These are preset parameters. The raster is updated using the standard linear Kalman filter update formula. The elevation distribution, after multiple observations and fusion, shows the elevation variance of the raster. Significantly reduced, mean elevation Converging to the true value, ultimately as Figure 4 As shown, a smooth, accurate local topographic elevation map containing information about uncertainties is formed.

[0036] The above description is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention. Structures, devices, and operating methods not specifically described or explained in this invention are implemented according to conventional methods in the art unless otherwise specified or limited.

Claims

1. A magnetic four-legged robot perception method based on binocular vision and height map fusion, characterized in that: Includes the following steps: Step 1: Synchronous Acquisition and Preprocessing of Multi-Source Data Simultaneously acquire image data from the binocular depth camera mounted on the magnetic quadruped robot and angular velocity and linear acceleration data from the inertial measurement unit, perform distortion correction on the image data, and align the timestamps of the IMU data with the image data. Step 2: Visual front-end feature extraction and IMU pre-integration: Feature points are extracted and tracked on the image processed in step one to establish visual data association between frames; at the same time, IMU measurements are pre-integrated between two consecutive visual keyframes to obtain relative motion increments. Step 3: Visual-Inertial Tightly Coupled Nonlinear Optimization within the Sliding Window: Construct a robot state vector containing all keyframes within the sliding window. Using the visual data association and IMU pre-integration increment obtained in step two as constraints, establish a joint optimization objective function containing visual reprojection error term and IMU pre-integration error term. Solve the robot's six-degree-of-freedom pose through nonlinear least squares optimization. Step 4: Update the probabilistic elevation map based on the robot's pose: Using the six-degree-of-freedom pose calculated in step three, the dense three-dimensional point cloud acquired by the binocular depth camera at the current moment is transformed into the world coordinate system, and a Kalman filter is used to perform probabilistic fusion update on each grid cell in the two-dimensional grid elevation map to obtain a local topographic map containing the elevation mean and uncertainty.

2. The magnetic four-legged robot perception method based on binocular vision and height map fusion according to claim 1, characterized in that: In step two, the FAST corner detection algorithm is used to extract feature points, and the KLT optical flow method is used to track the extracted feature points between consecutive image frames.

3. The magnetic four-legged robot perception method based on binocular vision and height map fusion according to claim 1, characterized in that: In step two, the IMU measurement values ​​are pre-integrated by integrating all IMU data in the body coordinate system of the previous frame between two consecutive visual keyframes, and using the result as the measurement value of the motion state between the two keyframes.

4. The magnetic four-legged robot perception method based on binocular vision and height map fusion according to claim 1, characterized in that: In step three, the robot state vector includes the pose, velocity, IMU bias, and inverse depth of all observed 3D feature points for each keyframe within the sliding window.

5. The perception method for a magnetic quadruped robot based on binocular vision and elevation map fusion as described in claim 1, characterized in that: The visual re-projection error in step three is the difference between the projected pixel coordinates and the actually observed feature point pixel coordinates, which are calculated by projecting the three-dimensional feature point onto the image plane of each key frame where it is observed .

6. The magnetic four-legged robot perception method based on binocular vision and height map fusion according to claim 1, characterized in that: In step three, the objective function for joint optimization uses Mahalanobis distance to weight the visual reprojection error term and the IMU pre-integration error term.

7. The magnetic four-legged robot perception method based on binocular vision and height map fusion according to claim 1, characterized in that: Step four includes the following steps: Using the robot's six-DOF pose in step three, the local 3D point cloud information acquired by the binocular depth camera is accurately fused into the global or local elevation map; For each pixel point generated at time t, we get its 3D coordinates in the camera coordinate system and its measurement covariance Using the robot pose estimated in step three, we get the transformation from the camera frame to the world frame and its covariance and transform the point to the world coordinate system ​​​ ； Meanwhile, according to the first-order Taylor expansion, the uncertainty of this point in the world coordinate system, i.e., the covariance, is propagated as follows: ； wherein and Jp and J0 are the Jacobians of the coordinate transformation for the pose and for the original point coordinates, respectively; Each grid cell of a two-dimensional grid map The elevation profile of this horizontal position is stored as a Gaussian distribution with mean and variance; When a point is transformed to the world coordinate system and Variance in the axial direction Fall into the grid At that time, a Kalman filter is used to update the elevation information of the raster: ； in, For Kalman gain, and The first The mean and variance of the estimated elevation. These are sensor observations. To observe the noise variance; subscript and This indicates the recursive step.

8. A magnetic quadruped robot, employing the magnetic quadruped robot perception method according to any one of claims 1-7, characterized in that: The magnetic quadruped robot includes a robot body (1), four mechanical legs (2) and foot magnetic suction devices (3) set at the ends of the mechanical legs (2). The robot body (1) is equipped with a binocular depth camera and an inertial measurement unit.

Images