Pipeline robot positioning method, system and device based on odometer wheel and IMU and medium

Abstract

The invention discloses a pipeline robot positioning method, system and device based on an odometer wheel and an IMU and a medium, and relates to the technical field of robot positioning and pipeline detection. The method comprises the following steps: determining an initial attitude angle of the pipeline robot in a static state according to acceleration data and magnetic force data collected by an IMU (Inertial Measurement Unit); based on a compensated angular velocity obtained by denoising angular velocity data collected by an IMU by using a complementary filtering algorithm, updating an initial attitude angle by using an Euler angle method to obtain an attitude angle matrix, and performing optimal estimation by using a nonlinear state estimation algorithm; performing consistency verification on the pulse data of the plurality of odometer wheels, determining corresponding effective pulse data through weighted correction, and then determining a mileage increment; and based on the optimized attitude angle matrix, the mileage increment is projected to a navigation coordinate system so as to solve the real-time position coordinate of the pipeline robot. The problems that the pipeline robot is poor in trafficability and low in positioning precision when facing a complex pipe network can be solved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning and pipeline detection, and in particular to a pipeline robot positioning method, system, device and medium based on an odometer wheel and an IMU. Background Art

[0002] With the development of urbanization, the scale of urban underground pipeline networks continues to expand, and pipeline robots are playing an increasingly important role in pipeline inspection and maintenance. However, the complex characteristics of underground pipelines, such as the lack of Global Navigation Satellite System (GNSS) signals, highly restricted internal spaces, and slippery and sediment-laden pipe walls, pose a serious challenge to the robots' precise positioning.

[0003] Existing pipeline positioning technology has significant shortcomings when dealing with complex pipe networks. This is especially true when encountering complex conditions such as right-angle bends, multi-way pipes (such as tees and crosses), variable-diameter pipes, or inclined pipes. Traditional positioning devices are often bulky and inflexible, making it difficult to adapt to varying pipe diameters and orientations. This severely limits their maneuverability and makes comprehensive mapping impossible. Furthermore, their reliance on external traction further restricts the pipeline robot's ability to operate autonomously within complex pipe networks.

[0004] In terms of positioning sensors, the problem of error accumulation is particularly prominent. The measurement method based on the odometry wheel is very prone to slipping when the pipeline is flooded, covered with sediment, or traveling on a curve, resulting in displacement measurement deviation, and this deviation continues to accumulate over time. Although the inertial measurement unit (IMU) can provide real-time attitude information, its inherent sensor zero bias drift characteristics will cause the positioning error to show nonlinear growth over long periods of operation. Relying solely on the odometry wheel or IMU is difficult to meet the engineering requirements of high-precision and long-term positioning in complex and narrow pipeline environments. Therefore, there is an urgent need to develop new positioning solutions to solve the current problem of precise positioning of robots in complex pipeline networks. Summary of the Invention

[0005] In order to solve the problems existing in the prior art, the present invention provides a pipeline robot positioning method and system based on odometer and IMU, which can solve the problems of poor passability and low positioning accuracy faced by pipeline robots in complex pipeline networks.

[0006] To achieve the above objectives, the present invention provides a pipeline robot positioning method based on odometry wheels and an IMU; the pipeline robot comprises a first segment, a second segment, a third segment, and a fourth segment hinged in series, and odometry wheels are arranged on both sides of each hinge; the method comprises: Determining an initial attitude angle of the pipeline robot in a stationary state based on acceleration data and magnetic data collected by the IMU; De-noising the angular velocity data collected by the IMU using a complementary filtering algorithm to obtain a compensated angular velocity; Based on the compensation angular velocity, the initial attitude angle is updated using the Euler angle method to obtain an attitude angle matrix; Using a nonlinear state estimation algorithm to optimally estimate the attitude angle matrix to obtain an optimized attitude angle matrix; Performing consistency check on the pulse data of the plurality of odometer wheels and determining corresponding valid pulse data through weighted correction; determining a mileage increment based on the valid pulse data; Based on the optimized attitude angle matrix, the mileage increment is projected from the carrier coordinate system to the navigation coordinate system to solve the real-time position coordinates of the pipeline robot.

[0007] Optionally, each end of the first segment and the fourth segment is provided with a passive wheel mechanism, and an elastic component is provided between the first segment and the second segment, and between the third segment and the fourth segment; performing consistency check on the pulse data of the plurality of odometer wheels and determining the corresponding valid pulse data through weighted correction includes: Determine the maximum difference between any two of the pulse data; If the maximum difference value is less than or equal to a preset threshold, taking the average value of all the pulse data as the valid pulse data; If the maximum difference value is greater than a preset threshold, obtaining the rotation signal of each of the passive wheel mechanisms and the tension feedback data of each of the elastic components; determining the actual movement direction of the pipeline robot according to the rotation signal, and identifying the slipping odometer wheel in combination with the tension feedback data; The pulse data corresponding to the slipping odometer wheel is weightedly corrected based on the tension feedback data to generate effective pulse data.

[0008] Optionally, the initial attitude angle includes an initial pitch angle, an initial roll angle, and an initial heading angle; and determining the initial attitude angle of the pipeline robot in a stationary state according to acceleration data and magnetic data collected by an IMU includes: Calculate the initial pitch angle and initial roll angle of the pipeline robot in the carrier coordinate system when it is stationary based on the gravity component of the acceleration data collected by the IMU in the inertial navigation coordinate system; The initial heading angle is calculated based on the local magnetic declination and the magnetic data collected by the IMU.

[0009] Optionally, the IMU is fixedly mounted on the third section and is arranged offset from the axis of the pipeline robot.

[0010] Optionally, the nonlinear state estimation algorithm is specifically an extended Kalman filter algorithm or a particle filter algorithm.

[0011] The present invention also provides a pipeline robot positioning system based on odometers and an IMU, wherein the pipeline robot comprises a first segment, a second segment, a third segment, and a fourth segment hinged in series, and odometers are arranged on both sides of each hinge; the system comprises: Attitude angle determination unit, used for: Determining an initial attitude angle of the pipeline robot in a stationary state based on acceleration data and magnetic data collected by the IMU; De-noising the angular velocity data collected by the IMU using a complementary filtering algorithm to obtain a compensated angular velocity; Based on the compensation angular velocity, the initial attitude angle is updated using the Euler angle method to obtain an attitude angle matrix; Using a nonlinear state estimation algorithm to optimally estimate the attitude angle matrix to obtain an optimized attitude angle matrix; Mileage determination unit, used for: Performing consistency check on the pulse data of the plurality of odometer wheels and determining corresponding valid pulse data through weighted correction; determining a mileage increment based on the valid pulse data; The position determination unit is used to project the mileage increment from the carrier coordinate system to the navigation coordinate system based on the optimized attitude angle matrix to solve the real-time position coordinates of the pipeline robot.

[0012] The present invention also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the above-mentioned positioning method is implemented.

[0013] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects: The pipeline robot positioning method based on odometer wheels and IMU provided by the present invention significantly improves the pipeline robot's passability in complex pipeline networks such as reducers, right-angle bends and multi-pass pipelines through the design of a multi-section serial structure and odometer wheels at the hinges, solving the problem of limited passability of traditional positioning devices due to their large size and poor flexibility.

[0014] Initial IMU alignment is used to determine the static attitude angle. Complementary filtering is then used to denoise the gyroscope angular velocity data and suppress drift. The Euler angle method is then used to update the attitude matrix, and an extended Kalman filter is used to optimize the attitude matrix. This effectively overcomes the long-term nonlinear accumulation of attitude errors caused by zero-bias drift in the IMU, significantly improving attitude angle measurement accuracy. To address the displacement error accumulation caused by odometer wheels slipping in water, sediment, or on curves, a multi-odometer pulse data consistency check and weighted correction mechanism is used to dynamically identify slipping wheel groups, reduce their data weights, and fuse them to generate valid pulse data, significantly suppressing displacement measurement deviations caused by single odometer wheel slip.

[0015] Finally, based on the optimized attitude angle matrix, the mileage increments are projected from the carrier coordinate system to the navigation coordinate system to resolve the position. This combines the advantages of the IMU's high-dynamic attitude data with the odometry wheel displacement data, enabling high-precision dead reckoning even in pipeline environments without GNSS signals. This invention not only addresses the lack of reliability in single-sensor positioning, but also, through the collaborative optimization of multi-source data, achieves long-term, high-precision autonomous positioning in complex and narrow pipeline environments, providing reliable technical support for pipeline inspection and maintenance. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] The above and other objects, features and advantages of the present invention will become more apparent through a more detailed description of exemplary embodiments of the present invention with reference to the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present invention.

[0017] Figure 1 This is a schematic diagram of a method flow chart of a pipeline robot positioning method based on an odometer wheel and an IMU according to an embodiment of the present invention; Figure 2 Schematic diagram of a pipeline robot passing through various complex pipeline network working conditions shown in an embodiment of the present invention; Figure 3 This is a structural flow chart of a pipeline robot positioning method according to an embodiment of the present invention; Figure 4 This is a schematic diagram showing the contact between each odometer wheel and the pipe wall during pipeline positioning and mapping according to an embodiment of the present invention; Figure 5 A schematic diagram of the data fusion positioning algorithm structure shown in an embodiment of the present invention; Figure 6 This is a schematic flow chart of a pipeline robot positioning method according to an embodiment of the present invention; Figure 7 This is a schematic diagram showing the definitions of various coordinate systems in the pipeline robot positioning algorithm according to an embodiment of the present invention; Figure 8 This is a schematic diagram of the module structure of a pipeline robot positioning system according to an embodiment of the present invention; Figure 9 The figure is a schematic structural diagram of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0018] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0019] See Figure 1 , Figure 1 Schematic diagram of the flow chart of the pipeline robot positioning method based on odometry wheels and IMU.

[0020] The pipeline robot provided by the present invention comprises a plurality of segment units connected in series, and the joints of adjacent segment units are provided with mileage wheels. Specifically, the pipeline robot comprises a first segment, a second segment, a third segment, and a fourth segment that are hinged in series in sequence, and mileage wheels are provided on both sides of each hinge. Figure 2 , Figure 2 This is a scene diagram of the pipeline robot of the present invention passing through various complex pipeline conditions. The pipeline robot can pass through complex pipeline conditions such as reducers, elbows, tees, and crosses. The pipeline robot has strong passing ability and can be used for positioning and mapping of complex pipeline networks.

[0021] Pipeline robot positioning methods include: S101: Determine the initial posture angle of the pipeline robot in a stationary state based on the acceleration data and magnetic data collected by the IMU.

[0022] The initial attitude angles include the initial pitch angle, initial roll angle, and initial heading angle. Before the pipeline robot performs a positioning task, it must be stationary to allow for precise initial attitude alignment. This is when the microelectromechanical system (MEMS) and inertial measurement unit (IMU), mounted on the robot's segments and offset from the robot's axis, begin to operate. The IMU comprises a three-axis accelerometer and a three-axis magnetometer; it is fixedly mounted on a segment, offset from the pipeline robot's axis, such as the third segment.

[0023] The above method determines the initial attitude angle of the pipeline robot in a static state based on the acceleration data and magnetic data collected by the IMU, specifically including: Based on the gravity component of the acceleration data collected by the IMU in the inertial coordinate system, the initial pitch angle and initial roll angle of the pipeline robot in the carrier coordinate system when it is stationary are calculated; The initial heading angle is calculated based on the local magnetic declination and magnetic data collected by the IMU.

[0024] Specifically, the IMU's three-axis accelerometer is first used to collect acceleration data when the robot is stationary. In this stationary state, the robot is only affected by gravity, and the theoretical output of the accelerometer in the inertial coordinate system should be the gravity acceleration vector (0, 0, g). By analyzing the gravity acceleration component actually measured by the accelerometer in the carrier coordinate system, the initial pitch and roll angles of the pipeline robot within the current pipeline space can be calculated. Specifically, by calculating the relationship between the measured value in the inertial coordinate system and the theoretical gravity vector in the carrier coordinate system, these two attitude angles can be solved using inverse trigonometric functions. They reflect the robot's forward and backward tilt and left and right tilt angles relative to the horizontal plane.

[0025] Secondly, the IMU's three-axis magnetometer is used to collect magnetic data (environmental magnetic field strength vector) in a stationary state. Combined with the known local magnetic declination (i.e., the angular difference between the magnetic north direction and the geographic true north direction), the magnetometer data can be used to solve the initial heading angle of the pipeline robot. The magnetometer measurement value provides the geomagnetic field vector information in the carrier coordinate system. By converting it to the horizontal plane and comparing it with the geographic north reference (taking into account the magnetic declination correction), the initial heading angle can be calculated. , which represents the orientation of the robot head relative to geographic north.

[0026] In addition, in order to obtain a more reliable initial heading in complex underground pipeline environments (such as those with strong electromagnetic interference or metal structures), a fusion mechanism can be further introduced. Specifically, a high-precision MEMS inertial navigation module (which is sensitive to the Coriolis force or geomagnetic field gradient caused by the rotation of the earth) can directly output a heading angle relative to the true north direction. The final initial heading angle is obtained by weighted fusion and get: ;in It is the heading angle proportional coefficient. In application, it can be automatically switched or adjusted according to the electromagnetic interference of the pipeline environment. When the interference is weak, it mainly depends on the magnetometer results ( Smaller); in areas with strong interference or dense metal, increase the weight of the high-precision north guidance module result ( larger).

[0027] For example, the intensity of electromagnetic interference (EMI) can be determined by real-time monitoring of fluctuations in magnetometer measurements. A strong EMI environment is identified when the variance of the magnetic data exceeds a preset value (e.g., 10μT) over three consecutive sampling periods. Furthermore, the system automatically identifies metallic structures within the pipeline using a pre-set pipeline material database (including characteristic parameters of magnetically permeable materials such as cast iron and stainless steel) or camera-generated images of the pipeline environment. Based on this environmental assessment, the heading angle scaling factor is dynamically adjusted. In strong EMI environments, a higher first factor (e.g., 0.8) is used to increase the weight of the north guidance module. In metallic environments, a medium second factor (e.g., 0.6) is used to balance data source reliability. Under normal operating conditions, the default factor (e.g., 0.3) is maintained. Furthermore, the coefficient can be dynamically fine-tuned in preset steps (e.g., 0.05) based on the duration and rate of change of interference intensity, ensuring a smooth transition in heading angle fusion.

[0028] S102: De-noise the angular velocity data collected by the IMU using a complementary filtering algorithm to obtain a compensated angular velocity.

[0029] When the pipeline robot enters motion, its inertial measurement unit (IMU) continuously collects angular velocity data at a rate of 100Hz. However, this raw gyroscope angular velocity data has inherent flaws: it contains high-frequency noise (caused by the sensor itself or environmental interference) and low-frequency bias drift (error that changes slowly over time). If this data is used directly for attitude integration, the noise will cause attitude angle jitter, while the drift will cause significant attitude errors that accumulate over time, seriously affecting subsequent positioning accuracy.

[0030] To address these issues, the present invention utilizes a complementary filtering algorithm to process the raw angular velocity data collected by the IMU. Specifically, gyroscopes offer excellent dynamic response and high accuracy in short periods of time (excellent high-frequency characteristics), but their output exhibits integral drift (poor low-frequency characteristics). Accelerometers provide relatively accurate attitude references when stationary or in slow motion (excellent low-frequency characteristics), but their outputs contain non-gravitational acceleration interference during robot acceleration, resulting in poor dynamic response and high noise (poor high-frequency characteristics). The complementary filtering algorithm fuses these two types of data: High-pass filtering is performed on the gyroscope data to retain its reliable high-frequency dynamic information and remove low-frequency drift components; low-pass filtering is performed on the attitude information calculated by the accelerometer (primarily pitch and roll angles) to extract its stable low-frequency attitude reference and remove high-frequency noise and motion interference. The high-pass filtered gyroscope angular velocity and low-pass filtered accelerometer attitude information are then superimposed and fused in the frequency domain.

[0031] The application receives raw angular velocity data streams from the IMU and accelerometer in real time. The robot's current pitch and roll angles are estimated in real time using the accelerometer data (the relationship between the gravity component and the navigation coordinate system). This estimated angle is then compared with the angle obtained by integrating the gyroscope to determine the angular error. This angular error passes through a scaling factor (which can be considered a low-pass filter) to generate a correction to the gyroscope measurement (the compensated angular velocity). Finally, the raw angular velocity is added to this correction to obtain the compensated angular velocity. This compensated angular velocity retains the gyroscope's fast response and excellent dynamic performance while leveraging the accelerometer's low-frequency stability to effectively suppress gyroscope bias drift. The scaling factor determines the filter's cutoff frequency and can be optimized based on sensor characteristics and the application scenario to balance dynamic response speed and drift suppression.

[0032] S103: Based on the compensated angular velocity, the initial attitude angle is updated using the Euler angle method to obtain an attitude angle matrix.

[0033] In the application, after obtaining the compensated angular velocity after complementary filtering, the pipeline robot's posture update process begins. Based on this precisely compensated angular velocity information, the robot's posture angle in three-dimensional space is calculated and updated in real time, ultimately forming a posture angle matrix that represents its spatial orientation.

[0034] Specifically, as the pipeline robot moves within the pipeline, its attitude (i.e., pitch, roll, and heading) changes continuously over time. Discrete Euler angle differential equations enable real-time iterative calculations. Within each discrete time step (corresponding to the IMU's 100Hz sampling frequency), the change in attitude angle at the next moment is accurately calculated based on the current attitude angle (initially, the initial attitude angle determined in step S101) and the compensated angular velocity measured at that moment. By integrating and superimposing this change with the attitude angle at the previous moment, the updated attitude angle at the current moment, including the updated pitch, roll, and heading angles, is obtained in real time. The attitude angle matrix, the result of the attitude update, accurately describes the rotational transformation between the pipeline robot's carrier coordinate system (frame b, fixed to the robot body) and the navigation coordinate system (frame n, the northeast celestial geographic coordinate system). Each element of this matrix is ​​calculated from the updated real-time pitch, roll, and heading angles using a specific combination of trigonometric functions.

[0035] S104: Optimally estimating the attitude angle matrix using a nonlinear state estimation algorithm to obtain an optimized attitude angle matrix.

[0036] In applications, although the compensated angular velocity and Euler angle method updates after complementary filtering can provide basic attitude information, there are still many factors that affect the attitude accuracy during the actual movement of the pipeline robot. Sensor measurement noise, the interference of motion acceleration on the perception of gravity components, the disturbance of the complex metal pipe wall environment on the magnetometer, and the non-rigid vibration caused by the robot's own articulated structure will all introduce errors in the attitude angle matrix. If these errors are not suppressed, they will continue to accumulate over time, seriously affecting the accuracy of the final positioning results. To obtain more accurate and robust attitude estimation, the present invention introduces a nonlinear state estimation algorithm to further optimize and correct the attitude angle matrix initially updated by the Euler angle method.

[0037] Specifically, this nonlinear state estimation algorithm (which can use either an extended Kalman filter or a particle filter) constructs a state vector containing key parameters such as attitude angle error and sensor bias, and establishes a nonlinear state equation that describes the time evolution of these states. Simultaneously, the nonlinear observation equation is constructed using real-time accelerometer and magnetometer data provided by the IMU as observations.

[0038] During the filtering iteration process, the optimal estimated state at the previous moment is first predicted based on the state equation, resulting in the current state prediction value and its covariance matrix. Next, the actual observations (accelerometer and magnetometer data) at the current moment are compared with the observation prediction value calculated based on the state prediction value to generate an observation residual. The observation residual is then used to optimally correct the state prediction value by calculating the Kalman gain based on the covariance matrix (in the extended Kalman filter) or by updating the particle weights (in the particle filter) to obtain the optimal estimate of the state vector at the current moment. This optimal estimate includes an optimized attitude angle error estimate.

[0039] Ultimately, the estimated attitude angle error is used to inversely compensate or directly update the attitude angle matrix initially updated by the Euler angle method, resulting in an optimized attitude angle matrix. This optimization process effectively integrates multi-source sensor information (gyroscope, accelerometer, magnetometer), dynamically estimating and correcting attitude errors introduced by sensor bias drift, motion acceleration interference, magnetometer perturbations, and mechanical vibration. This significantly suppresses the effect of error accumulation and significantly improves the long-term stability and accuracy of the attitude angle matrix, laying a solid foundation for the subsequent precise projection of mileage increments into the navigation coordinate system for position resolution.

[0040] S105: performing consistency check on the pulse data of multiple odometer wheels, and determining corresponding valid pulse data through weighted correction.

[0041] As the robot moves, the omnidirectional wheels at different articulated locations continuously generate pulse signals. These signals should theoretically be consistent, reflecting the robot's actual displacement. However, in complex operating conditions such as waterlogged pipes, sediment-covered roads, or on bends or roadways with varying diameters, some omnidirectional wheels may slip, causing their pulse data to deviate significantly from the data from other wheels. To identify and correct these anomalies and ensure accurate odometer measurements, the pulse data from all omnidirectional wheels must be checked for consistency and fused.

[0042] In one embodiment, the ends of the fore and aft segment units are each provided with a passive wheel mechanism, and an elastic component is provided between adjacent segment units; the above-mentioned consistency check of the pulse data of multiple odometer wheels and determination of the corresponding valid pulse data through weighted correction include: Determine the maximum difference between two pulse data; If the maximum difference value is less than or equal to the preset threshold, the average value of all pulse data is taken as the valid pulse data; If the maximum difference value is greater than a preset threshold, the rotation signal of each passive wheel mechanism and the tension feedback data of each elastic component are obtained; The actual movement direction of the pipeline robot is determined based on the rotation signal, and the slipping odometer wheel is identified in combination with the tension feedback data; The pulse data corresponding to the slipping odometer wheel is weightedly corrected based on the tension feedback data to generate effective pulse data.

[0043] In the application, the difference between the pulse data of all two omnidirectional wheels is first calculated, and the maximum difference is determined. This maximum difference is then compared with a preset threshold. If the maximum difference does not exceed the threshold, it indicates that all omnidirectional wheels are operating normally with no noticeable slippage. The arithmetic mean of all pulse data is then output as the final valid pulse data. If the maximum difference exceeds the preset threshold, it indicates that one or more omnidirectional wheels are slipping or operating abnormally. At this point, the rotation signal generated by the passive wheel mechanism and the real-time tension feedback data of each elastic component are required. The rotation signal of the passive wheel mechanism is used to assist in determining the actual overall motion direction of the pipeline robot. The tension feedback data of the elastic component directly reflects the positive pressure exerted by the corresponding wheel assembly on the pipe wall. When the tension of the elastic component corresponding to a particular omnidirectional wheel falls below the preset safety threshold, the wheel assembly is determined to be slipping because it has lost sufficient friction support.

[0044] After identifying a slipping omnidirectional wheel, the abnormal pulse data is weighted and corrected. Specifically, the pulse data corresponding to the slipping wheel is assigned a lower weight (the weight value can be dynamically adjusted based on the degree to which the tension falls below a threshold or the degree of deviation from the data of other wheel groups), while the data of the normal wheel group is assigned a higher weight. Then, a weighted average is calculated for all pulse data (including the corrected data of the slipping wheel) to generate a fused valid pulse data. This significantly reduces the negative impact of the abnormal data from the slipping wheel on the overall mileage calculation.

[0045] S106: Determine the mileage increment based on the valid pulse data.

[0046] After obtaining reliable and valid pulse data through consistency verification and weighted correction, it needs to be converted into a physical quantity reflecting the pipeline robot's actual displacement, namely, the mileage increment. The mileage increment represents the distance the pipeline robot moves along each axis in its own carrier coordinate system within a specific time period.

[0047] Specifically, each omnidirectional wheel (odometer) is equipped with a Hall effect magnetic switch. As the wheel rolls along the inner wall of the pipe, the magnets embedded in the wheel hub periodically trigger the Hall effect magnetic switch, generating an electrical pulse signal. Each pulse corresponds to a fixed rotation angle of the wheel, which is directly related to the number of magnets arranged on the wheel hub. Therefore, by counting the number of valid pulses received within a fixed sampling interval (for example, a 100Hz sampling period synchronized with the IMU data), the actual arc length of the omnidirectional wheel during that period can be accurately calculated.

[0048] Because valid pulse data is verified and corrected, it incorporates information from all properly functioning omnidirectional wheels and mitigates the impact of slipping wheels. When calculating the mileage increment, a weighted average is taken of the arc lengths calculated for all participating omnidirectional wheels (i.e., wheelsets with non-zero weights). These weights correspond to the weights assigned to the pulse data of each wheel set in step S105. Wheel sets in good condition and with sufficient tension receive higher weights, and their calculated results contribute more significantly to the final mileage increment. This weighted fusion strategy further minimizes the impact of individual wheel manufacturing errors, minor slippage, or measurement noise on the overall displacement calculation.

[0049] Ultimately, the arc length calculated from this weighted average is the actual displacement of the pipeline robot along its forward direction in the carrier coordinate system during that time period, also known as the mileage increment. This mileage increment reflects the actual distance the robot has traveled in the complex pipeline environment after overcoming the interference of wheel slip.

[0050] S107: Based on the optimized attitude angle matrix, the mileage increment is projected from the carrier coordinate system to the navigation coordinate system to solve the real-time position coordinates of the pipeline robot.

[0051] After obtaining accurate odometry increments and an attitude angle matrix optimized using the Extended Kalman Filter (EKF), the pipeline robot's real-time position coordinates can be calculated. In applications, the pipeline robot's real-time position coordinates are determined by accumulating displacement increments in the navigation coordinate system. The positioning process typically begins with a known initial position point; for example, the starting point of the pipeline can be measured at the pipeline entrance using the Global Navigation Satellite System (GNSS). Its coordinates in the world coordinate system are recorded, and its coordinates in the navigation coordinate system are converted using coordinate transformations. Whenever a new odometry increment is obtained and successfully projected into the navigation coordinate system, this displacement increment is added to the previous position coordinates to update the pipeline robot's real-time position coordinates in the navigation coordinate system at the current moment. This iterative calculation continuously updates the robot's position coordinates as it moves, ultimately forming a complete trajectory of the robot's movement within the pipeline.

[0052] In one embodiment, during the position update phase, the pipeline robot rotates around the pipeline axis by controlling the passive wheel mechanisms at the head and tail to rotate in the same direction; or controlling the passive wheel mechanisms at the head and tail to rotate in opposite directions to adjust the angle between the pipeline robot and the pipeline axis.

[0053] During the position update phase of the robot's positioning task (i.e., the process of performing dead reckoning to determine its real-time position coordinates), the passive wheel mechanisms at the ends of the head and tail segments can be actively controlled to perform specific rotational movements to actively adjust the robot's overall posture. Specifically, when the passive wheel mechanisms at the head and tail are controlled to rotate in the same direction (for example, both rotating clockwise or counterclockwise), the friction between the passive wheels and the pipe wall generates a torque that rotates the robot as a whole about the pipe axis. This rotational motion does not change the robot's axial position within the pipe, but it can adjust its azimuth angle (i.e., heading angle) about its own central axis. This is important when changing the observation direction is required for detection (such as adjusting the camera's viewing angle) or when correcting heading deviations caused by accumulated errors.

[0054] On the other hand, when the passive wheel mechanisms controlling the head and tail rotate in opposite directions (for example, the head passive wheel rotates clockwise, the tail passive wheel rotates counterclockwise, or vice versa), the friction forces applied by the head and tail passive wheels to the pipe wall are in opposite directions, thereby generating lateral forces in opposite directions at the head and tail ends of the robot. The action of this force will cause the head and tail segments of the robot to deflect relative to each other, thereby adjusting the angle between the central axis of the pipeline robot and the axis of the pipe. This ability to actively adjust the angle is crucial for the robot to smoothly pass through variable-diameter pipes, miter pipes, or certain complex non-standard connection parts. By adjusting this angle in a timely manner, the robot can better adapt to sudden changes in the pipe geometry, optimize the contact state between the odometer wheel and the pipe wall, reduce the risk of jamming, and ensure that the odometer wheel (omnidirectional wheel) maintains effective contact and rolling with the pipe wall as much as possible, thereby obtaining more reliable odometer data.

[0055] See also Figure 3 , Figure 3 The following is a flow chart of the pipeline robot positioning method. In the application, the pipeline robot is first placed in the pipeline to be inspected. The tension force of the elastic component (tension spring) makes the angles between the first and second segments, and between the third and fourth segments maintain an elastic trend of decreasing, driving the mileage wheel and the passive wheel mechanism to stick to the inner wall of the pipeline and adaptively adjust the segment angle according to the pipe diameter. The walking process is shown in the figure below. Figure 4 As shown in the figure, the IMU is mounted on the second section, off-axis, and samples motion data at a frequency of 100Hz. The omnidirectional wheels generate pulse signals as the robot moves, sampling at a frequency of 10Hz. The passive wheel mechanisms at the front and rear rotate in contact with the inner wall of the pipe. The active rotation direction controls the pipeline robot's rotation around the pipe axis. The data acquisition board receives and stores the data, which is then uploaded to the host computer via USB after the robot completes its movement. Data processing then proceeds. Initial alignment is performed using the accelerometer and magnetometer data at rest, and the initial attitude angle is obtained after correcting for magnetic declination. During the attitude update phase, the real-time attitude angle is calculated using the Euler angle method based on the gyroscope data. Accelerometer data is then fused through complementary filtering to suppress drift, and then further optimized using an EKF. Furthermore, odometry data processing is performed. The omnidirectional wheel pulse count is first verified, and abnormal data is corrected by combining the rotation signals of the passive wheel mechanisms and the tension feedback of the tension spring. Finally, a position update is performed. Based on the updated attitude matrix, the odometry increments are projected into the navigation coordinate system, and the trajectory is gradually calculated from the starting point. Finally, the angle encoder, tension sensor, and IMU attitude information are monitored in real time throughout the entire process to compensate for mechanical structure errors. The sensor and calculation errors are optimized through algorithms to ultimately achieve high-precision positioning.

[0056] See also Figure 5 , Figure 5The diagram of the data fusion positioning algorithm is as follows: First, the body attitude angle is updated. The attitude update is the basis of dead reckoning. The attitude angle accuracy is guaranteed through multi-step coordination. Specifically, the IMU is used to complete the initial alignment and obtain the initial attitude angle of the pipeline robot body. , providing a starting benchmark for subsequent calculations. Next, collect the fuselage angular velocity , processed by the complementary filtering algorithm, the algorithm can effectively remove sensor noise, suppress inertial navigation integral drift, and output the compensated angular velocity for attitude update. Subsequently, the Euler angle method is used to update the initial attitude angle based on the compensated angular velocity. Perform iterative updates to obtain the attitude angle matrix at time t To further improve accuracy, the Extended Kalman Filter (EKF) algorithm is introduced to optimally estimate the updated attitude angle, correct errors, ensure the reliability of attitude data, and provide an accurate attitude basis for position estimation.

[0057] In the dead reckoning system, the odometer uses the distance increment output method, and the odometer coordinate system coincides with the carrier coordinate system. Assuming that the odometer is The distance increment measured during the time period is , its projection in the fuselage coordinate system {B} is After the attitude matrix is ​​updated, The posture matrix of the moment carrier , project the distance increment of the odometer from the carrier coordinate system to the navigation coordinate system {N}, , based on the position update algorithm of distance increment, let the starting point coordinates be , the global position coordinates in the navigation coordinate system at time t are obtained through iterative calculation .

[0058] The flow chart of the positioning method of the present invention is as follows: Figure 6As shown in the figure, first, using RTK real-time dynamic positioning technology and the GNSS global positioning system, the three-dimensional coordinates of the pipeline's starting and ending points in the world coordinate system are obtained, establishing a global benchmark for subsequent positioning. Next, wheel odometer pulse increments are collected, and the IMU module is used to obtain angular velocity, acceleration, and attitude angle data during robot motion in real time, forming a positioning data foundation. During the initial alignment phase, the robot first calculates initial pitch and roll angles. These are then combined with magnetometer measurements and local magnetic declination information to calculate the initial heading angle, completing initial attitude calibration. Subsequently, the attitude angle is calculated using the Euler angle method. Complementary filtering and extended Kalman filtering algorithms are then used to further optimize the body attitude angle and improve attitude data accuracy. Finally, using the optimized attitude angle matrix, the odometer increments are projected into the navigation coordinate system for position calculation, generating the robot's motion track. This track is then projected into world coordinates, completing pipeline mapping and constructing a global pipeline trajectory from local motion data.

[0059] See also Figure 7 , Figure 7 Define schematic diagrams for each coordinate system; Figure 7 The fuselage coordinate system, navigation coordinate system and inertial navigation coordinate system are shown in the figure. Among them, the fuselage coordinate system (carrier coordinate system) takes the centroid or center of mass of the pipeline robot as the coordinate origin, and the forward direction of the pipeline robot as the positive direction of the X-axis. The inertial navigation coordinate system takes the centroid or center of mass of the IMU as the coordinate origin, and the forward direction of the pipeline robot as the positive direction of the Y-axis. In the application, acceleration data and magnetic data are collected by the IMU, and these data can be projected from the inertial navigation coordinate system to the carrier coordinate system or navigation coordinate system through coordinate transformation.

[0060] Corresponding to the aforementioned application function implementation method embodiment, the present invention also provides a pipeline robot positioning system based on odometer and IMU and corresponding embodiments.

[0061] See Figure 8 , Figure 8 Schematic diagram of the module structure of the pipeline robot positioning system based on odometry wheels and IMU.

[0062] The pipeline robot includes a first section, a second section, a third section, and a fourth section that are hinged in series, and odometer wheels are arranged on both sides of each hinge. The pipeline robot positioning system includes: The attitude angle determination unit 81 is used to: Determine the initial attitude angle of the pipeline robot in a static state based on the acceleration data and magnetic data collected by the IMU; The complementary filtering algorithm is used to denoise the angular velocity data collected by the IMU to obtain the compensated angular velocity; Based on the compensated angular velocity, the initial attitude angle is updated using the Euler angle method to obtain the attitude angle matrix; The nonlinear state estimation algorithm is used to optimally estimate the attitude angle matrix to obtain the optimized attitude angle matrix; The mileage determination unit 82 is configured to: Perform consistency check on the pulse data of multiple odometer wheels and determine the corresponding valid pulse data through weighted correction; Determine the mileage increment based on the valid pulse data; The position determination unit 83 is used to project the mileage increment from the carrier coordinate system to the navigation coordinate system based on the optimized attitude angle matrix to calculate the real-time position coordinates of the pipeline robot.

[0063] In one embodiment, a passive wheel mechanism is provided at each end of the first segment and the fourth segment, and elastic components are provided between the first segment and the second segment, and between the third segment and the fourth segment. In performing consistency check on the pulse data of multiple odometer wheels and determining the corresponding valid pulse data through weighted correction, the odometer determination unit 82 is specifically configured to: Determine the maximum difference between two pulse data; If the maximum difference value is less than or equal to the preset threshold, the average value of all pulse data is taken as the valid pulse data; If the maximum difference value is greater than a preset threshold, the rotation signal of each passive wheel mechanism and the tension feedback data of each elastic component are obtained; The actual movement direction of the pipeline robot is determined based on the rotation signal, and the slipping odometer wheel is identified in combination with the tension feedback data; The pulse data corresponding to the slipping odometer wheel is weightedly corrected based on the tension feedback data to generate effective pulse data.

[0064] In one embodiment, the initial attitude angle includes an initial pitch angle, an initial roll angle, and an initial heading angle. In determining the initial attitude angle of the pipeline robot in a stationary state based on the acceleration data and magnetic data collected by the IMU, the attitude angle determination unit 81 is specifically used to: Based on the gravity component of the acceleration data collected by the IMU in the inertial coordinate system, the initial pitch angle and initial roll angle of the pipeline robot in the carrier coordinate system when it is stationary are calculated; The initial heading angle is calculated based on the local magnetic declination and magnetic data collected by the IMU.

[0065] Regarding the system in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated again here.

[0066] An embodiment of the present invention also provides a pipeline robot positioning device based on an odometer wheel and an IMU, including a processor and a memory, wherein a computer program is stored in the memory, and the above-mentioned positioning method is implemented when the processor executes the computer program.

[0067] Positioning device (electronic device), see Figure 9 , the electronic device 9000 includes a memory 9010 and a processor 9020.

[0068] The processor 9020 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0069] Memory 9010 may include various types of storage units, such as system memory, read-only memory (ROM), and permanent storage. ROM may store static data or instructions required by processor 9020 or other modules of the computer. Permanent storage may be a readable and writable storage device. A permanent storage device may be a non-volatile storage device that retains stored instructions and data even when the computer is powered off. In some embodiments, the permanent storage device utilizes a mass storage device (e.g., a magnetic or optical disk, flash memory). In other embodiments, the permanent storage device may be a removable storage device (e.g., a floppy disk, optical drive). System memory may be a readable and writable storage device or a volatile readable and writable storage device, such as dynamic random access memory (DRAM). System memory may store some or all instructions and data required by the processor during operation. Furthermore, memory 9010 may include any combination of computer-readable storage media, including various types of semiconductor memory chips (e.g., DRAM, SRAM, SDRAM, flash memory, programmable read-only memory), as well as magnetic disks and / or optical disks. In some embodiments, the memory 9010 may include a readable and / or writable removable storage device, such as a compact disc (CD), a read-only digital versatile disc (e.g., DVD-ROM, double-layer DVD-ROM), a read-only Blu-ray disc, an ultra-density optical disc, a flash memory card (e.g., SD card, mini SD card, Micro-SD card, etc.), a magnetic floppy disk, etc. Computer-readable storage media do not include carrier waves and transient electronic signals transmitted wirelessly or wired.

[0070] The memory 9010 stores executable codes. When the executable codes are processed by the processor 9020 , the processor 9020 may execute part or all of the methods described above.

[0071] In addition, the method according to the present invention may also be implemented as a computer program or a computer program product, which includes computer program code instructions for executing some or all of the steps in the above method of the present invention.

[0072] Alternatively, the present invention may also be implemented as a computer-readable storage medium (or a non-transitory machine-readable storage medium or a machine-readable storage medium) on which an executable code (or a computer program or a computer instruction code) is stored. When the executable code (or a computer program or a computer instruction code) is executed by a processor of an electronic device (or a server, etc.), the processor executes part or all of the steps of the above-mentioned method according to the present application.

[0073] While various embodiments of the present invention have been described above, the above descriptions are intended to be illustrative, non-exhaustive, and not limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments. The terminology used herein is selected to best explain the principles of the embodiments, their practical applications, or improvements to existing technologies, or to enable others skilled in the art to understand the embodiments disclosed herein.

Claims

1. A pipeline robot positioning method based on odometry wheels and IMU, characterized in that: The pipeline robot comprises a first section, a second section, a third section and a fourth section which are hinged in series in sequence, and mileage wheels are arranged on both sides of each hinge; the method comprises: Determining an initial attitude angle of the pipeline robot in a stationary state based on acceleration data and magnetic data collected by the IMU; De-noising the angular velocity data collected by the IMU using a complementary filtering algorithm to obtain a compensated angular velocity; Based on the compensation angular velocity, the initial attitude angle is updated using the Euler angle method to obtain an attitude angle matrix; Using a nonlinear state estimation algorithm to optimally estimate the attitude angle matrix to obtain an optimized attitude angle matrix; Performing consistency check on the pulse data of the plurality of odometer wheels and determining corresponding valid pulse data through weighted correction; determining a mileage increment based on the valid pulse data; Based on the optimized attitude angle matrix, the mileage increment is projected from the carrier coordinate system to the navigation coordinate system to solve the real-time position coordinates of the pipeline robot.

2. The pipeline robot positioning method based on odometry wheel and IMU according to claim 1 is characterized in that: The ends of the first and fourth sections are each provided with a passive wheel mechanism, and elastic components are provided between the first and second sections, and between the third and fourth sections; Performing consistency check on the pulse data of the plurality of odometer wheels and determining corresponding valid pulse data through weighted correction includes: Determine the maximum difference between any two of the pulse data; If the maximum difference value is less than or equal to a preset threshold, taking the average value of all the pulse data as the valid pulse data; If the maximum difference value is greater than a preset threshold, obtaining the rotation signal of each of the passive wheel mechanisms and the tension feedback data of each of the elastic components; determining the actual movement direction of the pipeline robot according to the rotation signal, and identifying the slipping odometer wheel in combination with the tension feedback data; The pulse data corresponding to the slipping odometer wheel is weightedly corrected based on the tension feedback data to generate effective pulse data.

3. The pipeline robot positioning method based on odometry wheels and IMU according to claim 1 is characterized in that: The initial attitude angles include an initial pitch angle, an initial roll angle, and an initial heading angle; Determining the initial attitude angle of the pipeline robot in a stationary state based on acceleration data and magnetic data collected by the IMU includes: Calculate the initial pitch angle and initial roll angle of the pipeline robot in the carrier coordinate system when it is stationary based on the gravity component of the acceleration data collected by the IMU in the inertial navigation coordinate system; The initial heading angle is calculated based on the local magnetic declination and the magnetic data collected by the IMU.

4. The pipeline robot positioning method based on odometry wheels and IMU according to claim 1 is characterized in that: The IMU is fixedly mounted on the third section and is arranged away from the axis of the pipeline robot.

5. The pipeline robot positioning method based on odometry wheels and IMU according to claim 1 is characterized in that: The nonlinear state estimation algorithm is specifically an extended Kalman filter algorithm or a particle filter algorithm.

6. A pipeline robot positioning system based on odometer and IMU, characterized in that: The pipeline robot comprises a first section, a second section, a third section and a fourth section which are hinged in series in sequence, and mileage wheels are arranged on both sides of each hinge; the system comprises: Attitude angle determination unit, used for: Determining an initial attitude angle of the pipeline robot in a stationary state based on acceleration data and magnetic data collected by the IMU; De-noising the angular velocity data collected by the IMU using a complementary filtering algorithm to obtain a compensated angular velocity; Based on the compensation angular velocity, the initial attitude angle is updated using the Euler angle method to obtain an attitude angle matrix; Using a nonlinear state estimation algorithm to optimally estimate the attitude angle matrix to obtain an optimized attitude angle matrix; Mileage determination unit, used for: Performing consistency check on the pulse data of the plurality of odometer wheels and determining corresponding valid pulse data through weighted correction; determining a mileage increment based on the valid pulse data; The position determination unit is used to project the mileage increment from the carrier coordinate system to the navigation coordinate system based on the optimized attitude angle matrix to solve the real-time position coordinates of the pipeline robot.

7. A pipeline robot positioning device based on odometer and IMU, characterized in that: The method comprises a processor and a memory, wherein a computer program is stored in the memory, and when the processor executes the computer program, the positioning method according to claim 1 is implemented.

8. A computer-readable storage medium, characterized in that A computer program is stored thereon, and when the computer program is executed by a processor, the positioning method according to claim 1 is implemented.

Images